Enhance RM75 IK Solver with Joint Limit Regularization and Step Bounds

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(