Enhance RM75 IK Solver with Joint Limit Regularization and Step Bounds

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`，将双臂恢复到本次启动时求得的
 初始关节状态；也可调用：
 

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
-
+import numpy as np
-from rm75_ik import IkOptions, JointLimits, RM75IkSolver, RM75Kinematics
+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):
+class KinematicsSolver():
-        self.kinematics = RM75Kinematics(self._urdf_path, self._limits)
+    def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
-        self.solver = RM75IkSolver(self.kinematics)
+        """
-        self.model = self.kinematics.model
+        for realman 75b
-        self.data = self.kinematics.data
+        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]
+            max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159]
-        lower = np.asarray(min_j, dtype=float)
+        if rad_flag:
-        upper = np.asarray(max_j, dtype=float)
+            for i in range(7):
-        if not rad_flag:
+                self.model.lowerPositionLimit[i] = min_j[i]
-            lower = np.deg2rad(lower)
+                self.model.upperPositionLimit[i] = max_j[i]
-            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)
         else:
-            rotation = np.eye(3)
+            for i in range(7):
-        tool_pose = self._tool(tool)
+                self.model.lowerPositionLimit[i] = min_j[i] / 180 * pi
-        target_tool = pin.SE3(rotation, np.asarray(target_position, dtype=float))
+                self.model.upperPositionLimit[i] = max_j[i] / 180 * pi
-        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, [])
 
-    def compute_jacobian(self, joint_angles, tool="no_tool"):
+    def forward_kinematics(self, joint_angles, tool="omnipic"):
-        del tool
+        """
-        return self.kinematics.jacobian(np.asarray(joint_angles, dtype=float))
+        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)

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(
+            lower, upper = self._step_bounds(q, options)
-                -options.trust_region_rad,
-                self.kinematics.limits.lower - q,
-            )
-            upper = np.minimum(
-                options.trust_region_rad,
-                self.kinematics.limits.upper - q,
-            )
             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
-

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

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(

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:

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)

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)

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]