diff --git a/xr_rm_teleop/test/placo_ik_smoke.py b/xr_rm_teleop/test/placo_ik_smoke.py index 9f12c1d..e7e33b0 100644 --- a/xr_rm_teleop/test/placo_ik_smoke.py +++ b/xr_rm_teleop/test/placo_ik_smoke.py @@ -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) diff --git a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py index 4820b54..69f6842 100644 --- a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py +++ b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py @@ -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)