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

feat: 使用双 RM75 局部相对逆解

Browse Source
This commit is contained in:
YikaiFu-cart
2026-08-03 16:08:07 +08:00
parent 99cb45ef6d
commit 451de103b8
2 changed files with 89 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/placo_ik_smoke.py
+16 -8
View File
@@ -11,8 +11,13 @@ from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
CASES = {
"left": [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
"right": [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
"left": [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
"right": [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
}
TOOL_CHAINS = {
"left": ("scissor_base_link", "scissor_link_7", 0.165),
"right": ("omnipic_base_link", "omnipic_link_7", 0.14),
}
@@ -37,13 +42,16 @@ def main() -> None:
urdf_path = Path(sys.argv[1]).resolve()
for arm, joint_degrees in CASES.items():
initial_joints = np.deg2rad(joint_degrees)
drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0, arm)
joints = initial_joints.tolist()
stationary_target = drift_solver.update_joint_state(joints)
flange = drift_solver._robot.get_T_world_frame("link_7")
flange_to_tcp = np.linalg.inv(flange) @ stationary_target
assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, 0.16])
assert np.allclose(flange_to_tcp[:3, :3], np.eye(3))
base_frame, flange_frame, tcp_length = TOOL_CHAINS[arm]
world_to_base = drift_solver._robot.get_T_world_frame(base_frame)
world_to_flange = drift_solver._robot.get_T_world_frame(flange_frame)
base_to_flange = np.linalg.inv(world_to_base) @ world_to_flange
flange_to_tcp = np.linalg.inv(base_to_flange) @ stationary_target
assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, tcp_length])
assert np.allclose(flange_to_tcp[:3, :3], np.eye(3), atol=1e-5)
for _ in range(250):
drift_solver.update_joint_state(joints)
joints = drift_solver.solve(stationary_target)
@@ -54,7 +62,7 @@ def main() -> None:
f"{arm} stationary target drifted {drift_degrees:.3f}deg"
)
solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0, arm)
joints = initial_joints.tolist()
current = solver.update_joint_state(joints)
assert current.shape == (4, 4)
xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py
+73 -25
View File
@@ -8,8 +8,24 @@ from pathlib import Path
import numpy as np
EXPECTED_PLACO_VERSION = "0.9.4"
RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
RM75_Q_SLICE = slice(7, 14)
ARM_CHAINS = {
"left": (
"scissor_base_link",
"scissor_scissor_tcp",
"scissor",
"omnipic",
),
"right": (
"omnipic_base_link",
"omnipic_OmniPic_tcp",
"omnipic",
"scissor",
),
}
DUAL_RM75_JOINT_NAMES = [
*[f"omnipic_joint_{index}" for index in range(1, 8)],
*[f"scissor_joint_{index}" for index in range(1, 8)],
]
QP_MAX_ITERATIONS = 30
QP_POSITION_TOLERANCE_M = 2e-3
QP_ORIENTATION_TOLERANCE_RAD = 5e-3
@@ -43,9 +59,21 @@ class PlacoIkSolver:
self,
urdf_path: str,
dt: float,
arm: str,
) -> None:
if dt <= 0.0:
raise ValueError("dt must be positive")
if arm not in ARM_CHAINS:
raise ValueError("arm must be left or right")
self._base_frame, self._tcp_frame, prefix, inactive_prefix = (
ARM_CHAINS[arm]
)
self._joint_names = [
f"{prefix}_joint_{index}" for index in range(1, 8)
]
inactive_joint_names = [
f"{inactive_prefix}_joint_{index}" for index in range(1, 8)
]
try:
installed_version = version("placo")
import placo
@@ -65,30 +93,44 @@ class PlacoIkSolver:
self._dt = dt
self._robot = placo.RobotWrapper(str(model_path))
if self._robot.state.q.shape != (14,):
if self._robot.state.q.shape != (21,):
raise RuntimeError(
f"expected Placo q shape (14,), got {self._robot.state.q.shape}"
"expected Placo q shape (21,), got "
f"{self._robot.state.q.shape}"
)
if list(self._robot.joint_names()) != RM75_JOINT_NAMES:
if list(self._robot.joint_names()) != DUAL_RM75_JOINT_NAMES:
raise RuntimeError(
f"unexpected RM75 joint order: {list(self._robot.joint_names())}"
"unexpected dual RM75 joint order: "
f"{list(self._robot.joint_names())}"
)
self._q_offsets = np.asarray(
[self._robot.get_joint_offset(name) for name in self._joint_names],
dtype=int,
)
self._v_offsets = np.asarray(
[
self._robot.get_joint_v_offset(name)
for name in self._joint_names
],
dtype=int,
)
if len(set(self._q_offsets.tolist())) != 7:
raise RuntimeError(
f"invalid RM75 q offsets: {self._q_offsets.tolist()}"
)
if len(set(self._v_offsets.tolist())) != 7:
raise RuntimeError(
f"invalid RM75 v offsets: {self._v_offsets.tolist()}"
)
offsets = [
self._robot.get_joint_offset(name) for name in RM75_JOINT_NAMES
]
if offsets != list(range(7, 14)):
raise RuntimeError(f"unexpected RM75 q offsets: {offsets}")
self._joint_limits = np.asarray(
[self._robot.get_joint_limits(name) for name in RM75_JOINT_NAMES]
[self._robot.get_joint_limits(name) for name in self._joint_names]
)
velocity_offsets = [
self._robot.get_joint_v_offset(name) for name in RM75_JOINT_NAMES
]
self._velocity_limits = np.asarray(
[
self._robot.model.velocityLimit[index]
for index in velocity_offsets
for index in self._v_offsets
]
)
self._actual_joints: np.ndarray | None = None
@@ -96,12 +138,15 @@ class PlacoIkSolver:
self._solver = placo.KinematicsSolver(self._robot)
self._solver.dt = dt
self._solver.mask_fbase(True)
for name in inactive_joint_names:
self._solver.mask_dof(name)
self._solver.enable_velocity_limits(True)
self._frame_task = self._solver.add_frame_task(
"omnipicker_tcp",
self._frame_task = self._solver.add_relative_frame_task(
self._base_frame,
self._tcp_frame,
np.eye(4),
)
self._frame_task.configure("rm75_frame", "soft", 1.0)
self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
self._solver.add_kinetic_energy_regularization_task(1e-6)
@property
@@ -114,11 +159,14 @@ class PlacoIkSolver:
raise ValueError("joint state must contain 7 finite values")
is_first_feedback = self._actual_joints is None
self._actual_joints = values.copy()
self._robot.state.q[RM75_Q_SLICE] = values
self._robot.state.q[self._q_offsets] = values
self._robot.update_kinematics()
base_to_tool = self._robot.get_T_world_frame("omnipicker_tcp")
base_to_tool = (
np.linalg.inv(self._robot.get_T_world_frame(self._base_frame))
@ self._robot.get_T_world_frame(self._tcp_frame)
)
if is_first_feedback:
self._frame_task.T_world_frame = base_to_tool.copy()
self._frame_task.T_a_b = base_to_tool.copy()
return base_to_tool.copy()
def _target_errors(self) -> tuple[float, float]:
@@ -134,11 +182,11 @@ class PlacoIkSolver:
def solve(self, target_tool_pose: np.ndarray) -> list[float]:
if self._actual_joints is None:
raise RuntimeError("joint state must be initialized before QP solve")
self._frame_task.T_world_frame = _validated_transform(
self._frame_task.T_a_b = _validated_transform(
target_tool_pose
)
result = np.asarray(
self._robot.state.q[RM75_Q_SLICE],
self._robot.state.q[self._q_offsets],
dtype=float,
).copy()
position_error, orientation_error = self._target_errors()
@@ -153,7 +201,7 @@ class PlacoIkSolver:
self._solver.solve(True)
self._robot.update_kinematics()
result = np.asarray(
self._robot.state.q[RM75_Q_SLICE],
self._robot.state.q[self._q_offsets],
dtype=float,
).copy()
self._validate_result(result, previous)