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)