Enhance RM75 IK Solver with Joint Limit Regularization and Step Bounds
This commit is contained in:
@ -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`，将双臂恢复到本次启动时求得的
|
||||
初始关节状态；也可调用：
|
||||
|
||||
|
||||
@ -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)
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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(
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
|
||||
Reference in New Issue
Block a user