adjust initial pose of robotic arms

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -0,0 +1,19 @@
+from xr_rm_teleop.realman_adapter import RealManAdapter
+
+
+def test_initial_pose_uses_joint_move_only() -> None:
+    class FakeArm:
+        def __init__(self) -> None:
+            self.calls = []
+
+        def rm_movej(self, *args):
+            self.calls.append(args)
+            return 0
+
+    joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
+    adapter = RealManAdapter("127.0.0.1", 8080, 0, 1, initial_joint_pose=joints)
+    adapter._arm = FakeArm()
+
+    adapter._move_to_initial_pose()
+
+    assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -91,7 +91,6 @@ class RealManAdapter:
         joint_max_acc: float = 180.0,
         move_to_initial_pose_on_connect: bool = False,
         initial_joint_pose: list[float] | None = None,
-        initial_tcp_pose: list[float] | None = None,
         init_move_speed: int = 20,
         canfd_trajectory_mode: int = 2,
         canfd_radio: int = 0,
@@ -110,7 +109,6 @@ class RealManAdapter:
         self._joint_max_acc = joint_max_acc
         self._move_to_initial_pose_on_connect = move_to_initial_pose_on_connect
         self._initial_joint_pose = initial_joint_pose
-        self._initial_tcp_pose = initial_tcp_pose
         self._init_move_speed = init_move_speed
         self._canfd_trajectory_mode = canfd_trajectory_mode
         self._canfd_radio = canfd_radio
@@ -219,13 +217,11 @@ class RealManAdapter:
             self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
 
     def _move_to_initial_pose(self) -> None:
-        if self._initial_joint_pose is None or self._initial_tcp_pose is None:
-            raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose 和 initial_tcp_pose")
+        if self._initial_joint_pose is None:
+            raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
 
         ret = self._arm.rm_movej(self._initial_joint_pose, self._init_move_speed, 0, 0, 1)
         self._check_return(ret, "rm_movej(initial_joint_pose)")
-        ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1)
-        self._check_return(ret, "rm_movel(initial_tcp_pose)")
 
     def _try_call(self, name: str, *args: Any) -> None:
         func = getattr(self._arm, name, None)

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -193,7 +193,6 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("joint_max_acc", 180.0)
         self.declare_parameter("move_to_initial_pose_on_connect", False)
         self.declare_parameter("initial_joint_pose", [0.0] * 7)
-        self.declare_parameter("initial_tcp_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
         self.declare_parameter("init_move_speed", 20)
         self.declare_parameter("canfd_trajectory_mode", 2)
         self.declare_parameter("canfd_radio", 0)
@@ -305,7 +304,6 @@ class SingleArmVelocityTeleop(Node):
             joint_max_acc=float(self.get_parameter("joint_max_acc").value),
             move_to_initial_pose_on_connect=self._bool_parameter("move_to_initial_pose_on_connect"),
             initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7),
-            initial_tcp_pose=self._float_list_parameter("initial_tcp_pose", 6),
             init_move_speed=int(self.get_parameter("init_move_speed").value),
             canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value),
             canfd_radio=int(self.get_parameter("canfd_radio").value),