YikaiFu-cart/acRealman_xr
Template
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Enhance RM75 IK Solver with Joint Limit Regularization and Step Bounds

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-10 13:26:07 +08:00
parent 0d7e30cb7b
commit fcb0fec1ef
9 changed files with 1088 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
ik_qp/src/rm75_ik/solver.py
View File

@ -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

34
ik_qp/src/rm75_ik/teleop_config.py
View File

@ -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

18
ik_qp/src/rm75_ik/teleop_control.py
View File

@ -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(

37
ik_qp/src/rm75_ik/types.py
View File

@ -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: