diff --git a/ik_qp/README.md b/ik_qp/README.md index d88d6e5..ca1cb6c 100644 --- a/ik_qp/README.md +++ b/ik_qp/README.md @@ -208,6 +208,20 @@ ros2 launch xr_rm_bringup dual_arm_qp_sim.launch.py \ `initial_tcp_pose` 初始化的残差，最后出现 `dual-arm QP teleop ready`。第三阶段结果见 [STAGE3_VALIDATION.md](STAGE3_VALIDATION.md)。 +PICO 在线控制使用 `src/rm75_ik/solver.py` 中的正式 QP 求解器。每个机械臂可在 +`dual_arm_rm75.yaml` 中独立配置以下参数： + +| 参数 | 当前值 | 作用 | +|---|---:|---| +| `qp_w_limit_mid` | `0.00002` | 按关节范围归一化，将关节从软限位附近拉向范围中心 | +| `qp_joint_motion_weights` | `[1,1,1,1,0.3,0.3,0.2]` | QP 阻尼权重；越小越积极参与冗余运动 | +| `qp_joint_step_limits_rad` | `[0.05,0.05,0.05,0.05,0.08,0.08,0.10]` | 每次 QP 内部迭代的关节步长上限，单位为 rad | + +中心惩罚使用 `diag(1 / (q_upper - q_lower)^2)` 消除不同关节范围造成的量级差异，并与 +原有硬关节限位同时生效。`qp_joint_step_limits_rad` 只影响 IK 内部收敛；最终发送给 +MuJoCo 的每周期关节变化仍受 `joint_max_speed / control_rate_hz` 限制。上述参数不会为 +初始化 IK 自动启用，避免改变启动逆解支路，也不构成碰撞规避保证。 + 运行过程中可在 MuJoCo viewer 中按 `R` 或 `Home`，将双臂恢复到本次启动时求得的 初始关节状态；也可调用： diff --git a/ik_qp/kine_ctrl/rm75_kine_qp.py b/ik_qp/kine_ctrl/rm75_kine_qp.py index 26853b2..65a7e6c 100644 --- a/ik_qp/kine_ctrl/rm75_kine_qp.py +++ b/ik_qp/kine_ctrl/rm75_kine_qp.py @@ -1,104 +1,824 @@ #!/usr/bin/env python3 -"""Compatibility adapter for the original experimental import path. +import sys +import os -New code should import RM75Kinematics and RM75IkSolver from ``rm75_ik``. -""" - -from math import pi -from pathlib import Path - -import numpy as np import pinocchio as pin - -from rm75_ik import IkOptions, JointLimits, RM75IkSolver, RM75Kinematics +import numpy as np +import osqp +from scipy import sparse +from math import radians, degrees, pi, cos, sin +import time +import threading -class KinematicsSolver: - def __init__(self, urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir=None): - del mesh_dir - selected_path = Path(urdf_path) - if not selected_path.is_file() and not selected_path.is_absolute(): - selected_path = Path(__file__).resolve().parent / selected_path - self._urdf_path = selected_path - self._limits = None - self._tools = {} - self._rebuild() - def _rebuild(self): - self.kinematics = RM75Kinematics(self._urdf_path, self._limits) - self.solver = RM75IkSolver(self.kinematics) - self.model = self.kinematics.model - self.data = self.kinematics.data +class KinematicsSolver(): + def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"): + """ + for realman 75b + Initialize robotic arm kinematics using Pinocchio (ROS2 version). + unit: m, rad + """ + print(f' ------------ the qp based kinematic initialising -----------') + self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir) - def add_tool_frames(self, frames): - for name, attributes in frames.items(): - pose = np.asarray(attributes[0], dtype=float) - if pose.shape != (7,): - raise ValueError(f"tool {name!r} pose must have seven values") - quaternion = pin.Quaternion(pose[6], pose[3], pose[4], pose[5]) - quaternion.normalize() - self._tools[name] = pin.SE3(quaternion.matrix(), pose[:3]) - def cfg_j_limit(self, min_j=None, max_j=None, rad_flag=True): + + self.cfg_j_limit() + + q_range = ( + self.model.upperPositionLimit[:7] - + self.model.lowerPositionLimit[:7] + ) + + self.w_q_limit = np.diag(1.0 / (q_range ** 2)) + + self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7]) + + # ---------- for reused qp_solver ------------------ + self.nv = 7 + + # Full dense symmetric matrix structure + # P_template = np.triu(np.ones((7, 7))) + self.P_pattern = sparse.triu(np.ones((7,7))).tocsc() + + P_sparse = sparse.csc_matrix(self.P_pattern) + + A_sparse = sparse.eye(7, format='csc') + + self.osqp_solver = osqp.OSQP() + + self.osqp_solver.setup( + P=P_sparse, + q=np.zeros(7), + A=A_sparse, + l=-np.ones(7), + u=np.ones(7), + verbose=False, + warm_start=True, + polish=False + ) + + self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4]) + # Smaller value => joint moves more actively + # Larger value => joint moves less / more lazy + self.joint_motion_weight = np.diag([ + 1.0, 1.0, 1.0, 1.0, + 0.3, 0.3, 0.2 + ]) + + def add_frame(self,frame_name, position, rotationXYZ): + ''' + :param frame_name: str + :param position: [x, y, z] target position (meters) + :param rotationXYZ: [x, y, z] target rotation (rad) + ''' + camera_rotation = pin.rpy.rpyToMatrix( rotationXYZ[0], rotationXYZ[1], rotationXYZ[2] ) + camera_offset = pin.SE3( + camera_rotation, + np.array(position) + ) + self.model.addFrame( pin.Frame( frame_name, self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) ) + + def add_tool_frames(self,dict_frames): + self.tool_frames ={} + for tool_name in dict_frames: + tool_attr = dict_frames[tool_name] + position = tool_attr[0][0:3] + rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7]) + self.add_frame(tool_name, position, rotationXYZ) + self.tool_frames.update({tool_name: self.model.getFrameId(tool_name)}) + self.data = self.model.createData() + + + def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True): if min_j is None: - min_j = [-pi, -2.2689, -pi, -2.3562, -pi, -2.234, -2 * pi] + min_j = [-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159] if max_j is None: - max_j = [pi, 2.2689, pi, 2.3562, pi, 2.234, 2 * pi] - lower = np.asarray(min_j, dtype=float) - upper = np.asarray(max_j, dtype=float) - if not rad_flag: - lower = np.deg2rad(lower) - upper = np.deg2rad(upper) - self._limits = JointLimits("legacy", lower, upper) - self._rebuild() - - def _tool(self, name): - try: - return self._tools[name] - except KeyError as exc: - raise ValueError(f"unknown tool frame: {name!r}") from exc - - def forward_kinematics(self, joint_angles, tool="no_tool"): - pose = self.kinematics.forward(np.asarray(joint_angles), self._tool(tool)) - return np.concatenate( - [pose.translation.copy(), pin.rpy.matrixToRpy(pose.rotation)] - ) - - def inverse_kinematics( - self, - target_position, - target_rpy=None, - target_quat=None, - initial_guess=None, - max_iter=500, - tolerance=1e-3, - debug=False, - tool="no_tool", - ): - del debug - if target_quat is not None: - values = np.asarray(target_quat, dtype=float) - quaternion = pin.Quaternion(values[3], values[0], values[1], values[2]) - rotation = quaternion.matrix() - elif target_rpy is not None: - rotation = pin.rpy.rpyToMatrix(*target_rpy) + max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159] + if rad_flag: + for i in range(7): + self.model.lowerPositionLimit[i] = min_j[i] + self.model.upperPositionLimit[i] = max_j[i] else: - rotation = np.eye(3) - tool_pose = self._tool(tool) - target_tool = pin.SE3(rotation, np.asarray(target_position, dtype=float)) - target_flange = target_tool * tool_pose.inverse() - seed = np.zeros(7) if initial_guess is None else np.asarray(initial_guess, dtype=float) - result = self.solver.solve( - target_flange, - seed, - IkOptions( - position_tolerance_m=tolerance, - orientation_tolerance_rad=tolerance, - max_iterations=max_iter, - ), - ) - return (0, result.q.tolist()) if result.success else (-1, []) + for i in range(7): + self.model.lowerPositionLimit[i] = min_j[i] / 180 * pi + self.model.upperPositionLimit[i] = max_j[i] / 180 * pi - def compute_jacobian(self, joint_angles, tool="no_tool"): - del tool - return self.kinematics.jacobian(np.asarray(joint_angles, dtype=float)) + def forward_kinematics(self, joint_angles, tool="omnipic"): + """ + Compute forward kinematics. + + Args: + joint_angles: List or array of 7 joint angles (radians) + tool: Name of frame to compute + + Returns: + dict: Position, rotation, rpy, quaternion + unit: position: m + rpy: rad + """ + if len(joint_angles) != 7: + raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}") + + # Create configuration vector + q = pin.neutral(self.model) + for i, angle in enumerate(joint_angles): + q[i] = angle + + # Compute forward kinematics + pin.forwardKinematics(self.model, self.data, q) + pin.updateFramePlacements(self.model, self.data) + + # Get frame transform + frame_id = self.tool_frames[tool] + frame_transform = self.data.oMf[frame_id] + + # Extract results + position = frame_transform.translation.copy() + rotation = frame_transform.rotation.copy() + + # Compute RPY + rpy = pin.rpy.matrixToRpy(rotation) + + # Compute quaternion + # quat = pin.Quaternion(rotation) + pose = np.concatenate([position, rpy], axis=0) + return pose + # return { + # 'position': position, + # # 'rotation': rotation, + # 'rpy': rpy, + # 'quaternion': [quat.x, quat.y, quat.z, quat.w], + # # 'transform': frame_transform + # } + + def inverse_kinematics(self, target_position, target_rpy=None, + target_quat=None, initial_guess=None, + max_iter=500, tolerance=5e-3, debug=False, tool="ee"): + """ + Compute inverse kinematics using differential IK with multiple strategies. + + Args: + target_position: [x, y, z] target position (meters) + target_rpy: [roll, pitch, yaw] target orientation (radians) + target_quat: [x, y, z, w] target orientation as quaternion + initial_guess: Initial joint angles (radians) + max_iter: Maximum iterations + tolerance: Error tolerance + debug: Print debug information + tool: the frame name ('scissor', 'camera', 'ee') + + Returns: + tuple: (joint_angles, success, error) + """ + # Build target SE3 placement + if target_quat is not None: + quat = pin.Quaternion(target_quat[3], target_quat[0], target_quat[1], target_quat[2]) + target_rotation = quat.matrix() + elif target_rpy is not None: + target_rotation = pin.rpy.rpyToMatrix(target_rpy[0], + target_rpy[1], + target_rpy[2]) + else: + target_rotation = np.eye(3) + + target_placement = pin.SE3(target_rotation, np.array(target_position)) + + # Try multiple initial guesses + initial_guesses = [] + + if initial_guess is not None: + initial_guesses.append(initial_guess) + else: + # Try different initial configurations + initial_guesses.append([0.1] * 7) # Zero config + + + best_solution = None + best_error = float('inf') + + for guess_idx, guess in enumerate(initial_guesses): + q = pin.neutral(self.model) + for i, angle in enumerate(guess): + if i < len(q): + q[i] = np.clip(angle, self.model.lowerPositionLimit[i], + self.model.upperPositionLimit[i]) + q_ref = q.copy() + + # Differential IK with adaptive damping + damping = 0.1 + damping_reduction = 0.95 + iter_count = 0 + prev_error = float('inf') + + ee_frame_id = self.tool_frames[tool] + + J = pin.computeFrameJacobian( + self.model, + self.data, + q, + ee_frame_id, + pin.ReferenceFrame.LOCAL + ) + + pin.forwardKinematics(self.model, self.data, q) + pin.updateFramePlacements(self.model, self.data) + + current_placement = self.data.oMf[ee_frame_id] + + error_SE3 = current_placement.actInv(target_placement) + error_vec = pin.log(error_SE3).vector + + # print("\n initial error =", np.linalg.norm(error_vec)) + # print(error_vec) + + while iter_count < max_iter: + # Compute forward kinematics + + pin.computeJointJacobians(self.model, self.data, q) + pin.framesForwardKinematics(self.model, self.data, q) + + # Get current end-effector placement + current_placement = self.data.oMf[ee_frame_id] + + # Compute error + error_SE3 = current_placement.actInv(target_placement) + error_vec = pin.log(error_SE3).vector + error_norm = np.linalg.norm(error_vec) + + if error_norm < tolerance: + if error_norm < best_error: + best_error = error_norm + best_solution = q[:7].copy() + break + + # Check if error is increasing (diverging) + if error_norm > prev_error * 1.1 and iter_count > 10: + damping = min(1.0, damping * 1.5) + else: + damping = max(0.01, damping * damping_reduction) + + + J = pin.getFrameJacobian( + self.model, + self.data, + ee_frame_id, + pin.ReferenceFrame.LOCAL + ) + + # ========================= + # QP-based IK + # ========================= + w_ref = 0.0001 + w_limit_mid = 0.00002 + + J_eff = pin.Jlog6(error_SE3) @ J #J # + + H = J_eff.T @ self.W @ J_eff + + + H += damping * damping * self.joint_motion_weight + H += w_ref * np.eye(7) + H += w_limit_mid * self.w_q_limit + + H_triu = sparse.triu(H).tocsc() + + g = -J_eff.T @ self.W @ error_vec + g += w_ref * (q[:7] - q_ref[:7]) + g += w_limit_mid * self.w_q_limit @ (q[:7] - self.q_mid) + + # ------------------------- + # Joint velocity constraints + # ------------------------- + dq_limit = np.array([ 0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10 ]) # rad per iteration + + lb = -dq_limit * np.ones(7) + ub = dq_limit * np.ones(7) + + # ------------------------- + # Joint position constraints + # ------------------------- + + q_min_step = self.model.lowerPositionLimit[:7] - q[:7] + q_max_step = self.model.upperPositionLimit[:7] - q[:7] + + lb = np.maximum(lb, q_min_step) + ub = np.minimum(ub, q_max_step) + + # ------------------------- + # Solve QP + # ------------------------ + # Update solver + self.osqp_solver.update( + Px= H_triu.data, #H[np.triu_indices(7)], # + q=g, + l=lb, + u=ub + ) + + # Solve + result = self.osqp_solver.solve() + if result.info.status != 'solved': + break + + dq = result.x + + if dq is None: + break + + # Apply joint limits with scaling + alpha = 1.0 + q = pin.integrate(self.model, q, alpha * dq) + + prev_error = error_norm + iter_count += 1 + + if best_solution is not None: + # return best_solution, True, best_error, iter_count + return 0, best_solution.tolist() + else: + # return q[:7].copy(), False, error_norm, iter_count + return -1, q[:7].copy().tolist() + + def quaternion_to_euler(self, q): + """ + Convert quaternion to Euler angles (roll, pitch, yaw) + + Args: + qx, qy, qz, qw: quaternion components + + Returns: + tuple: (roll, pitch, yaw) in radians + """ + # Roll (x-axis rotation) + sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2]) + cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1]) + roll = np.arctan2(sinr_cosp, cosr_cosp) + + # Pitch (y-axis rotation) + sinp = 2.0 * (q[3] * q[1] - q[2] * q[0]) + if abs(sinp) >= 1: + pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range + else: + pitch = np.arcsin(sinp) + + # Yaw (z-axis rotation) + siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1]) + cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]) + yaw = np.arctan2(siny_cosp, cosy_cosp) + + return [roll, pitch, yaw] + + # def invese_kinematics_velocity(self, target_position, target_rpy=None, + # target_quat=None, initial_guess=None, tool="ee"): + # """ + # Compute the converging velocity (motion direction) of joints based on qp inverse kinematics. + # + # Args: + # target_position: [x, y, z] target position (meters) + # target_rpy: [roll, pitch, yaw] target orientation (radians) + # target_quat: [x, y, z, w] target orientation as quaternion + # initial_guess: Initial joint angles (radians) + # tool: the frame name ('scissor', 'camera', 'ee') + # + # Returns: + # joint_velocity: np.array() + # """ + # # Build target SE3 placement + # if target_quat is not None: + # quat = pin.Quaternion(target_quat[3], target_quat[0], + # target_quat[1], target_quat[2]) + # target_rotation = quat.matrix() + # elif target_rpy is not None: + # target_rotation = pin.rpy.rpyToMatrix(target_rpy[0], + # target_rpy[1], + # target_rpy[2]) + # else: + # target_rotation = np.eye(3) + # + # target_placement = pin.SE3(target_rotation, np.array(target_position)) + # + # # Try multiple initial guesses + # initial_guesses = [] + # + # if initial_guess is not None: + # initial_guesses.append(initial_guess) + # else: + # # Try different initial configurations + # initial_guesses.append([0.1] * 7) # Zero config + # initial_guesses.append([radians(30), radians(45), radians(30), + # radians(-45), radians(30), radians(-30), 0]) + # initial_guesses.append([radians(-30), radians(45), radians(-30), + # radians(45), radians(30), radians(30), 0]) + # + # best_solution = None + # best_error = float('inf') + # + # for guess_idx, guess in enumerate(initial_guesses): + # q = pin.neutral(self.model) + # for i, angle in enumerate(guess): + # if i < len(q): + # q[i] = np.clip(angle, self.model.lowerPositionLimit[i], + # self.model.upperPositionLimit[i]) + # + # # Differential IK with adaptive damping + # damping = 0.01 + # damping_reduction = 0.95 + # iter_count = 0 + # prev_error = float('inf') + # + # ee_frame_id = self.tool_frames[tool] + # + # J = pin.computeFrameJacobian( + # self.model, + # self.data, + # q, + # ee_frame_id, + # pin.ReferenceFrame.LOCAL_WORLD_ALIGNED + # ) + # + # while iter_count < max_iter: + # # Compute forward kinematics + # + # pin.computeJointJacobians(self.model, self.data, q) + # pin.framesForwardKinematics(self.model, self.data, q) + # + # # Get current end-effector placement + # + # current_placement = self.data.oMf[ee_frame_id] + # + # # Compute error + # error_SE3 = current_placement.actInv(target_placement) + # error_vec = pin.log(error_SE3).vector + # error_norm = np.linalg.norm(error_vec) + # + # if error_norm < tolerance: + # joint_angles = q[:7].copy() + # fk_result = self.forward_kinematics(joint_angles, tool=tool) + # position_error = np.linalg.norm(fk_result['position'] - np.array(target_position)) + # + # if position_error < best_error: + # best_error = position_error + # best_solution = joint_angles + # break + # + # # Check if error is increasing (diverging) + # if error_norm > prev_error * 1.1 and iter_count > 10: + # damping = min(1.0, damping * 1.5) + # else: + # damping = max(0.01, damping * damping_reduction) + # + # J = pin.getFrameJacobian( + # self.model, + # self.data, + # ee_frame_id, + # pin.ReferenceFrame.LOCAL_WORLD_ALIGNED + # ) + # + # # ========================= + # # QP-based IK + # # ========================= + # + # H = J.T @ self.W @ J + # H += damping * damping * np.eye(7) + # + # H_triu = sparse.triu(H).tocsc() + # + # g = -J.T @ self.W @ error_vec + # + # # ------------------------- + # # Joint velocity constraints + # # ------------------------- + # + # dq_limit = 0.05 # rad per iteration + # + # lb = -dq_limit * np.ones(7) + # ub = dq_limit * np.ones(7) + # + # # ------------------------- + # # Joint position constraints + # # ------------------------- + # + # q_min_step = self.model.lowerPositionLimit[:7] - q[:7] + # q_max_step = self.model.upperPositionLimit[:7] - q[:7] + # + # lb = np.maximum(lb, q_min_step) + # ub = np.minimum(ub, q_max_step) + # + # # ------------------------- + # # Solve QP + # # ------------------------ + # # Update solver + # self.osqp_solver.update( + # Px=H_triu.data, + # q=g, + # l=lb, + # u=ub + # ) + # + # # Solve + # result = self.osqp_solver.solve() + # + # if result.info.status != 'solved': + # break + # + # dq = result.x + # + # if dq is None: + # break + # + # # Apply joint limits with scaling + # alpha = 0.5 + # q = pin.integrate(self.model, q, alpha * dq) + # + # prev_error = error_norm + # iter_count += 1 + # + # if best_solution is not None: + # return best_solution, True, best_error + # else: + # return None, False, None + + def compute_jacobian(self, joint_angles, tool="ee"): + """Compute geometric Jacobian (6x7)""" + q = pin.neutral(self.model) + for i, angle in enumerate(joint_angles): + q[i] = angle + + pin.forwardKinematics(self.model, self.data, q) + pin.updateFramePlacements(self.model, self.data) + ee_frame_id = self.tool_frames[tool] + J = pin.computeFrameJacobian(self.model, self.data, q, ee_frame_id) + + return J + + def get_subchain_jacobian(self, + joint_angles, + frame_names + ): + + q = pin.neutral(self.model) + + all_active_joints = self.get_active_joints_from_frame(frame_names) + + for i in range(7): + q[i] = joint_angles[i] + + pin.forwardKinematics(self.model, self.data, q) + pin.updateFramePlacements(self.model, self.data) + pin.computeJointJacobians(self.model, self.data, q) + + Js = [] + + for frame_name, active_joints in zip(frame_names, all_active_joints): + frame_id = self.model.getFrameId(frame_name) + + J = pin.getFrameJacobian( + self.model, + self.data, + frame_id, + pin.ReferenceFrame.LOCAL + ) + Js.append(J[:, active_joints]) + + return Js + + def get_active_joints_from_frame(self, frame_names): + """ + Return active joint indices affecting a frame. + + Example: + frame_name='link_4' + -> [0,1,2,3] + """ + all_active_joint_ids = [] + for frame_name in frame_names: + frame_id = self.model.getFrameId(frame_name) + + # Parent joint of this frame + joint_id = self.model.frames[frame_id].parentJoint + + print(f'frame_id = {frame_id}, and joint_id = {joint_id}') + + active_joint_ids = [] + + + # Traverse upward to root + while joint_id > 0: + # Pinocchio joint indexing: + # universe joint = 0 + # robot joints start from 1 + + active_joint_ids.append(joint_id - 1) + + # Move to parent joint + joint_id = self.model.parents[joint_id] + + # Reverse so order becomes base -> tip + active_joint_ids.reverse() + all_active_joint_ids.append(active_joint_ids) + + return all_active_joint_ids + + def plan_cartesian_trajectory(self, start_pos, end_pos, + start_rpy=None, end_rpy=None, + num_steps=20, tool='ee'): + """ + Plan a Cartesian trajectory with IK for each waypoint. + """ + # Get current end-effector pose if start_rpy not provided + if start_rpy is None: + # Try to find a valid starting configuration + test_angles = [0.1] * 7 + fk_test = self.forward_kinematics(test_angles,tool=tool) + start_rpy = fk_test['rpy'] + + if end_rpy is None: + end_rpy = start_rpy + + # First, check if target is reachable + print(f"\nChecking if target is reachable...") + target_pos = end_pos + target_rpy = end_rpy + + test_solution, success, error = self.inverse_kinematics( + target_pos, target_rpy=target_rpy, initial_guess=[0.1] * 7, max_iter=500, tool=tool + ) + + if not success: + print(f"Warning: Target may be unreachable or difficult to reach") + print(f"Trying with relaxed tolerance...") + + # Initial guess for IK (start with zero configuration) + current_angles = [0.1] * 7 + trajectory = [] + + print(f"\nPlanning trajectory from ({start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f})") + print(f"To ({end_pos[0]:.2f}, {end_pos[1]:.2f}, {end_pos[2]:.2f})") + print("-" * 60) + + for i in range(num_steps + 1): + t = i / num_steps + + # Interpolate position + pos = [ + start_pos[0] + t * (end_pos[0] - start_pos[0]), + start_pos[1] + t * (end_pos[1] - start_pos[1]), + start_pos[2] + t * (end_pos[2] - start_pos[2]) + ] + + # Interpolate orientation + rpy = [ + start_rpy[0] + t * (end_rpy[0] - start_rpy[0]), + start_rpy[1] + t * (end_rpy[1] - start_rpy[1]), + start_rpy[2] + t * (end_rpy[2] - start_rpy[2]) + ] + + # Compute IK + joint_angles, success, error = self.inverse_kinematics( + pos, target_rpy=rpy, initial_guess=current_angles, max_iter=300, tool=tool + ) + + if not success: + print(f" Waypoint {i}: IK failed!") + break + + # Verify + fk_verify = self.forward_kinematics(joint_angles, tool=tool) + + trajectory.append({ + 'step': i, + 't': t, + 'position': pos, + 'rpy': rpy, + 'joint_angles': joint_angles, + 'actual_position': fk_verify['position'], + 'error': error + }) + + # Update current angles for next iteration + current_angles = joint_angles + + if i % 5 == 0 or i == num_steps: + print(f" Waypoint {i:3d}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}), " + f"error={error:.6f}m") + + return trajectory + + + +def main(): + """Main test function""" + + + rm75 = KinematicsSolver() + + # Test 1: Forward Kinematics + print("\n1. Forward Kinematics Test") + print("-" * 40) + + tool_name = "scissor" + joint_angles_zero = [0.1] * 7 + fk_result = rm75.forward_kinematics(joint_angles_zero, tool=tool_name) + + print(f"Init configuration:") + print(f" Position: ({fk_result['position'][0]:.3f}, " + f"{fk_result['position'][1]:.3f}, {fk_result['position'][2]:.3f}) m") + + # Test 2: Inverse Kinematics with more reachable target + print("\n2. Inverse Kinematics Test") + print("-" * 40) + + # Try a simpler target first + target_pos = [0.3, 0.2, 0.4] # More reachable position + target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation + + print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m") + + import time + init_joints = [0.2] * 7 + time0 = time.time() + for ii in range(100): + joint_solution, success, error = rm75.inverse_kinematics( + target_pos, target_rpy=target_rpy, initial_guess=init_joints, + max_iter=500, debug=False, tool=tool_name + ) + time1 = time.time() + print(f"Time: {time1 - time0}") + + if success: + print(f"✓ Solution found! Error: {error:.6f} m") + for i, angle in enumerate(joint_solution): + print(f" Joint {i + 1}: {degrees(angle):7.2f}°") + + # Verify + fk_verify = rm75.forward_kinematics(joint_solution,tool=tool_name) + print( + f" Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m") + else: + print("✗ IK failed to find a solution!") + + # Test 3: Jacobian + print("\n3. Jacobian Matrix") + print("-" * 40) + + J = rm75.compute_jacobian(joint_angles_zero, tool=tool_name) + print(f"Jacobian shape: {J.shape}") + for i in range(min(3, J.shape[0])): + row_str = " ".join([f"{J[i, j]:7.3f}" for j in range(7)]) + print(f" Row {i + 1}: {row_str}") + + # Test 4: Trajectory Planning with reachable positions + print("\n4. Cartesian Trajectory Planning") + print("-" * 40) + + start_pos = [0.3, 0.0, 0.4] # Start position + end_pos = [0.3, 0.0, 0.55] # End position (smaller movement) + + fk0 = rm75.forward_kinematics([0.1] * 7, tool=tool_name) + + trajectory = rm75.plan_cartesian_trajectory( + start_pos, + end_pos, + start_rpy=fk0['rpy'], + end_rpy=[ + fk0['rpy'][0] + radians(10), + fk0['rpy'][1], + fk0['rpy'][2] + ], + num_steps=10, + tool=tool_name + ) + + if trajectory: + print(f"\n✓ Generated {len(trajectory)} waypoints") + + if success: + print("✓ Inverse kinematics working (with simplified target)") + else: + print("⚠ Inverse kinematics may need tuning - try different targets") + + + print("\n" + "=" * 60) + print(f'test subchain Jacobian, for future obstacle avoidance') + frame_names = [ + "link_2", + "link_4", + "link_7" + ] + Js_sub = rm75.get_subchain_jacobian( + joint_angles=joint_angles_zero, + frame_names=frame_names + ) + print(f'Js_sub: {Js_sub}') + + return rm75, trajectory + + +if __name__ == "__main__": + rm75, trajectory = main() + + print("\n" + "=" * 60) + print("All tests completed!") + print("=" * 60) \ No newline at end of file diff --git a/ik_qp/src/rm75_ik/solver.py b/ik_qp/src/rm75_ik/solver.py index 35a52e5..adea30c 100644 --- a/ik_qp/src/rm75_ik/solver.py +++ b/ik_qp/src/rm75_ik/solver.py @@ -22,6 +22,11 @@ class RM75IkSolver: self.data = kinematics.data self.frame_id = kinematics.flange_frame_id self._n = 7 + joint_range = kinematics.limits.upper - kinematics.limits.lower + self._joint_mid = 0.5 * ( + kinematics.limits.lower + kinematics.limits.upper + ) + self._joint_limit_metric_diag = 1.0 / np.square(joint_range) pattern = sparse.triu(np.ones((self._n, self._n)), format="csc") self._p_rows = pattern.indices.copy() @@ -42,6 +47,46 @@ class RM75IkSolver: max_iter=1000, ) + def _regularization_terms( + self, + q: np.ndarray, + q_reference: np.ndarray, + options: IkOptions, + damping: float, + ) -> tuple[np.ndarray, np.ndarray]: + """Return Hessian and gradient terms unrelated to the TCP task.""" + + motion_weights = np.asarray(options.joint_motion_weights, dtype=float) + diagonal = damping * damping * motion_weights + diagonal += options.posture_weight + diagonal += ( + options.joint_limit_mid_weight * self._joint_limit_metric_diag + ) + gradient = options.posture_weight * (q - q_reference) + gradient += ( + options.joint_limit_mid_weight + * self._joint_limit_metric_diag + * (q - self._joint_mid) + ) + return np.diag(diagonal), gradient + + def _step_bounds( + self, q: np.ndarray, options: IkOptions + ) -> tuple[np.ndarray, np.ndarray]: + if options.joint_step_limits_rad is None: + step_limits = np.full(self._n, options.trust_region_rad) + else: + step_limits = np.asarray(options.joint_step_limits_rad, dtype=float) + lower = np.maximum( + -step_limits, + self.kinematics.limits.lower - q, + ) + upper = np.minimum( + step_limits, + self.kinematics.limits.upper - q, + ) + return lower, upper + def solve( self, target_se3: pin.SE3, @@ -143,20 +188,19 @@ class RM75IkSolver: ) effective_jacobian = pin.Jlog6(error_transform) @ jacobian hessian = effective_jacobian.T @ weights @ effective_jacobian - hessian += ( - damping * damping + options.posture_weight - ) * np.eye(self._n) gradient = -effective_jacobian.T @ weights @ error_vector - gradient += options.posture_weight * (q - q_reference) + regularization_hessian, regularization_gradient = ( + self._regularization_terms( + q, + q_reference, + options, + damping, + ) + ) + hessian += regularization_hessian + gradient += regularization_gradient - lower = np.maximum( - -options.trust_region_rad, - self.kinematics.limits.lower - q, - ) - upper = np.minimum( - options.trust_region_rad, - self.kinematics.limits.upper - q, - ) + lower, upper = self._step_bounds(q, options) p_values = hessian[self._p_rows, self._p_cols] self._osqp.update(Px=p_values, q=gradient, l=lower, u=upper) osqp_result = self._osqp.solve() @@ -249,4 +293,3 @@ def deterministic_recovery_seeds( while len(seeds) < count: seeds.append(rng.uniform(limits.lower, limits.upper)) return seeds - diff --git a/ik_qp/src/rm75_ik/teleop_config.py b/ik_qp/src/rm75_ik/teleop_config.py index 9e6ecfb..abe2529 100644 --- a/ik_qp/src/rm75_ik/teleop_config.py +++ b/ik_qp/src/rm75_ik/teleop_config.py @@ -49,6 +49,9 @@ class ArmTeleopProfile: low_z_threshold: float low_z_min_radius: float joint_max_speed_rad_s: np.ndarray + qp_w_limit_mid: float + qp_joint_motion_weights: np.ndarray + qp_joint_step_limits_rad: np.ndarray def __post_init__(self) -> None: if self.arm not in ("left", "right"): @@ -84,6 +87,24 @@ class ArmTeleopProfile: if np.any(speeds <= 0.0): raise ValueError("joint_max_speed_rad_s must be positive") object.__setattr__(self, "joint_max_speed_rad_s", speeds) + motion_weights = _readonly_vector( + self.qp_joint_motion_weights, + (7,), + "qp_joint_motion_weights", + ) + if np.any(motion_weights <= 0.0): + raise ValueError("qp_joint_motion_weights must be positive") + object.__setattr__(self, "qp_joint_motion_weights", motion_weights) + step_limits = _readonly_vector( + self.qp_joint_step_limits_rad, + (7,), + "qp_joint_step_limits_rad", + ) + if np.any(step_limits <= 0.0): + raise ValueError("qp_joint_step_limits_rad must be positive") + object.__setattr__(self, "qp_joint_step_limits_rad", step_limits) + if not np.isfinite(self.qp_w_limit_mid) or self.qp_w_limit_mid < 0.0: + raise ValueError("qp_w_limit_mid must be finite and non-negative") for name in ( "scale", "command_timeout_sec", @@ -183,7 +204,9 @@ def load_dual_arm_profiles( max_linear_speed_m_s=float(params["max_linear_speed"]), enable_position_axes=tuple(bool(value) for value in params["enable_position_axes"]), enable_orientation_control=bool(params["enable_orientation_control"]), - enable_orientation_axes=tuple(bool(value) for value in params["enable_orientation_axes"]), + enable_orientation_axes=tuple( + bool(value) for value in params["enable_orientation_axes"] + ), orientation_deadband_rad=float(params["orientation_deadband_rad"]), orientation_filter_alpha=float(params["orientation_filter_alpha"]), max_orientation_speed_rad_s=float(params["max_orientation_speed"]), @@ -193,5 +216,14 @@ def load_dual_arm_profiles( low_z_threshold=float(params["low_z_threshold"]), low_z_min_radius=float(params["low_z_min_radius"]), joint_max_speed_rad_s=np.full(7, np.deg2rad(joint_speed)), + qp_w_limit_mid=float(params.get("qp_w_limit_mid", 0.0)), + qp_joint_motion_weights=params.get( + "qp_joint_motion_weights", + [1.0] * 7, + ), + qp_joint_step_limits_rad=params.get( + "qp_joint_step_limits_rad", + [0.05] * 7, + ), ) return profiles diff --git a/ik_qp/src/rm75_ik/teleop_control.py b/ik_qp/src/rm75_ik/teleop_control.py index 8c08b4f..b0e865d 100644 --- a/ik_qp/src/rm75_ik/teleop_control.py +++ b/ik_qp/src/rm75_ik/teleop_control.py @@ -1,6 +1,6 @@ from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, replace from enum import Enum from typing import Dict, Mapping, Optional @@ -400,8 +400,22 @@ class DualArmQpTeleopController: flange_target = ( mapped.target_tcp * self.profiles[arm].tool_from_flange.inverse() ) + arm_ik_options = replace( + self.ik_options, + joint_limit_mid_weight=self.profiles[arm].qp_w_limit_mid, + joint_motion_weights=tuple( + float(value) + for value in self.profiles[arm].qp_joint_motion_weights + ), + joint_step_limits_rad=tuple( + float(value) + for value in self.profiles[arm].qp_joint_step_limits_rad + ), + ) result = self.solvers[arm].solve( - flange_target, q_current, self.ik_options + flange_target, + q_current, + arm_ik_options, ) if not result.success or result.q is None: return self._trip_fault( diff --git a/ik_qp/src/rm75_ik/types.py b/ik_qp/src/rm75_ik/types.py index 0500209..e0b7e53 100644 --- a/ik_qp/src/rm75_ik/types.py +++ b/ik_qp/src/rm75_ik/types.py @@ -84,6 +84,19 @@ class IkOptions: 0.4, ) posture_weight: float = 1e-5 + joint_limit_mid_weight: float = 0.0 + joint_motion_weights: Tuple[float, float, float, float, float, float, float] = ( + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + ) + joint_step_limits_rad: Optional[ + Tuple[float, float, float, float, float, float, float] + ] = None damping_initial: float = 0.1 damping_min: float = 0.01 damping_max: float = 1.0 @@ -111,6 +124,30 @@ class IkOptions: raise ValueError("task_weights must contain six positive values") if self.posture_weight < 0.0 or not np.isfinite(self.posture_weight): raise ValueError("posture_weight must be finite and non-negative") + if ( + self.joint_limit_mid_weight < 0.0 + or not np.isfinite(self.joint_limit_mid_weight) + ): + raise ValueError( + "joint_limit_mid_weight must be finite and non-negative" + ) + if len(self.joint_motion_weights) != 7 or any( + not np.isfinite(weight) or weight <= 0.0 + for weight in self.joint_motion_weights + ): + raise ValueError( + "joint_motion_weights must contain seven finite positive values" + ) + if self.joint_step_limits_rad is not None and ( + len(self.joint_step_limits_rad) != 7 + or any( + not np.isfinite(limit) or limit <= 0.0 + for limit in self.joint_step_limits_rad + ) + ): + raise ValueError( + "joint_step_limits_rad must contain seven finite positive values" + ) if not self.damping_min <= self.damping_initial <= self.damping_max: raise ValueError("damping_initial must be within damping_min and damping_max") if not 0.0 < self.damping_reduction <= 1.0: diff --git a/ik_qp/tests/test_solver.py b/ik_qp/tests/test_solver.py index 845cb74..6b6cb6b 100644 --- a/ik_qp/tests/test_solver.py +++ b/ik_qp/tests/test_solver.py @@ -1,5 +1,6 @@ import numpy as np import pinocchio as pin +import pytest from rm75_ik import IkOptions, IkStatus, RM75IkSolver, RM75Kinematics, pose_errors @@ -51,3 +52,65 @@ def test_expired_time_budget_returns_no_solution(): assert result.status is IkStatus.TIME_LIMIT assert result.q is None + +def test_joint_limit_mid_regularization_points_toward_range_center(): + kinematics = RM75Kinematics() + solver = RM75IkSolver(kinematics) + limits = kinematics.limits + midpoint = 0.5 * (limits.lower + limits.upper) + joint_range = limits.upper - limits.lower + options = IkOptions( + posture_weight=0.0, + joint_limit_mid_weight=2e-5, + joint_motion_weights=(1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2), + ) + + center_hessian, center_gradient = solver._regularization_terms( + midpoint, midpoint, options, damping=0.1 + ) + np.testing.assert_allclose(center_gradient, 0.0, atol=1e-15) + + above = midpoint + 0.4 * joint_range + below = midpoint - 0.4 * joint_range + _, above_gradient = solver._regularization_terms( + above, above, options, damping=0.1 + ) + _, below_gradient = solver._regularization_terms( + below, below, options, damping=0.1 + ) + assert np.all(above_gradient > 0.0) + np.testing.assert_allclose(below_gradient, -above_gradient, rtol=1e-12) + assert center_hessian[6, 6] < center_hessian[0, 0] + + +def test_per_joint_step_limits_and_hard_position_limits_are_combined(): + kinematics = RM75Kinematics() + solver = RM75IkSolver(kinematics) + limits = kinematics.limits + midpoint = 0.5 * (limits.lower + limits.upper) + configured = np.array([0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10]) + options = IkOptions(joint_step_limits_rad=tuple(configured)) + + lower, upper = solver._step_bounds(midpoint, options) + np.testing.assert_allclose(lower, -configured) + np.testing.assert_allclose(upper, configured) + + near_upper = midpoint.copy() + near_upper[6] = limits.upper[6] - 0.01 + lower, upper = solver._step_bounds(near_upper, options) + assert upper[6] == pytest.approx(0.01) + assert lower[6] == pytest.approx(-0.10) + + +@pytest.mark.parametrize( + "kwargs", + [ + {"joint_limit_mid_weight": -1.0}, + {"joint_motion_weights": (1.0,) * 6 + (0.0,)}, + {"joint_step_limits_rad": (0.05,) * 6}, + {"joint_step_limits_rad": (0.05,) * 6 + (float("nan"),)}, + ], +) +def test_invalid_joint_regularization_options_are_rejected(kwargs): + with pytest.raises(ValueError): + IkOptions(**kwargs) diff --git a/ik_qp/tests/test_stage3_control.py b/ik_qp/tests/test_stage3_control.py index b60105e..5df1b5a 100644 --- a/ik_qp/tests/test_stage3_control.py +++ b/ik_qp/tests/test_stage3_control.py @@ -4,10 +4,9 @@ from pathlib import Path from types import SimpleNamespace import numpy as np -import pinocchio as pin import pytest -from rm75_ik import pose_errors +from rm75_ik import IkResult, IkStatus, pose_errors from rm75_ik.mujoco_robot import MujocoRobot from rm75_ik.teleop_config import load_dual_arm_profiles from rm75_ik.teleop_control import ( @@ -50,6 +49,16 @@ def test_profiles_use_expected_tools_and_mapping(profiles): profiles["right"].xr_to_robot, [[0, 1, 0], [0, 0, 1], [1, 0, 0]], ) + for profile in profiles.values(): + assert profile.qp_w_limit_mid == pytest.approx(2e-5) + np.testing.assert_allclose( + profile.qp_joint_motion_weights, + [1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2], + ) + np.testing.assert_allclose( + profile.qp_joint_step_limits_rad, + [0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10], + ) @pytest.mark.parametrize( @@ -139,6 +148,42 @@ def test_dual_controller_has_no_grip_jump_and_moves_both_arms(profiles): controller.close() +def test_controller_passes_online_joint_regularization_options( + profiles, monkeypatch +): + robot = MujocoRobot(profiles) + controller = DualArmQpTeleopController(robot, profiles) + captured = {} + + def solve(target, seed, options): + del target + captured["options"] = options + return IkResult( + IkStatus.SUCCESS, + np.asarray(seed, dtype=float).copy(), + 0.0, + 0.0, + 0, + 0.0, + ) + + try: + monkeypatch.setattr(controller.solvers["left"], "solve", solve) + controller.update_sample(sample("left", True), 0.0) + assert controller.step(0.0).state is SafetyState.ACTIVE + + options = captured["options"] + assert options.joint_limit_mid_weight == pytest.approx(2e-5) + assert options.joint_motion_weights == pytest.approx( + (1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2) + ) + assert options.joint_step_limits_rad == pytest.approx( + (0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10) + ) + finally: + controller.close() + + def test_active_arm_timeout_faults_both_and_requires_release(profiles): robot = MujocoRobot(profiles) controller = DualArmQpTeleopController(robot, profiles) diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml index 52a8006..662f2f3 100755 --- a/xr_rm_bringup/config/dual_arm_rm75.yaml +++ b/xr_rm_bringup/config/dual_arm_rm75.yaml @@ -57,6 +57,11 @@ left_arm_teleop: max_angular_acc: 2.0 joint_max_speed: 180.0 joint_max_acc: 180.0 + + # QP 内部正则与单次迭代步长。较小的 motion weight 会让对应关节更积极参与。 + qp_w_limit_mid: 0.00002 + qp_joint_motion_weights: [0.3, 0.3, 0.5, 1.0, 1.0, 1.0, 1.0] + qp_joint_step_limits_rad: [0.10, 0.08, 0.08, 0.05, 0.05, 0.05, 0.05] move_to_initial_pose_on_connect: false initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51] initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400] @@ -112,6 +117,11 @@ right_arm_teleop: max_angular_acc: 2.0 joint_max_speed: 180.0 joint_max_acc: 180.0 + + # 与左臂采用相同初值，保留独立配置入口便于后续分别调参。 + qp_w_limit_mid: 0.00002 + qp_joint_motion_weights: [0.3, 0.3, 0.5, 1.0, 1.0, 1.0, 1.0] + qp_joint_step_limits_rad: [0.10, 0.08, 0.08, 0.05, 0.05, 0.05, 0.05] move_to_initial_pose_on_connect: false initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67] initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]