diff --git a/xr_rm_teleop/test/test_initial_joint_pose.py b/xr_rm_teleop/test/test_initial_joint_pose.py index 6572a13..b794976 100644 --- a/xr_rm_teleop/test/test_initial_joint_pose.py +++ b/xr_rm_teleop/test/test_initial_joint_pose.py @@ -30,11 +30,23 @@ def test_initial_pose_uses_joint_move_only() -> None: ) adapter._arm = FakeArm() - adapter._move_to_initial_pose() + adapter.move_to_initial_pose() assert adapter._arm.calls == [(joints, 20, 0, 0, 1)] +def test_mock_initial_pose_restores_configured_joints() -> None: + initial_degrees = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55] + adapter = MockRealManAdapter(initial_degrees) + adapter.send_joint_target([0.0] * 7, follow=False) + + adapter.move_to_initial_pose() + + assert adapter.read_joint_state().positions == pytest.approx( + [math.radians(value) for value in initial_degrees] + ) + + def test_peripheral_config_exposes_selected_tool() -> None: config = PeripheralConfig( scissorgripper=1, diff --git a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py index ce33cf6..c00d466 100755 --- a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py +++ b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py @@ -44,9 +44,10 @@ class MockRealManAdapter: math.isfinite(value) for value in initial_joint_degrees ): raise ValueError("initial joint pose must contain 7 finite values") - self._joint_positions = [ + self._initial_joint_positions = [ math.radians(value) for value in initial_joint_degrees ] + self._joint_positions = list(self._initial_joint_positions) self.last_joint_target: list[float] | None = None self.last_tool_open: bool | None = None @@ -69,6 +70,10 @@ class MockRealManAdapter: self._joint_positions = list(joints) self.last_joint_target = list(joints) + def move_to_initial_pose(self) -> None: + self._joint_positions = list(self._initial_joint_positions) + self.last_joint_target = list(self._joint_positions) + def stop(self) -> None: return @@ -178,7 +183,7 @@ class RealManAdapter: if self._configure_safety_limits: self._apply_safety_limits() if self._move_to_initial_pose_on_connect: - self._move_to_initial_pose() + self.move_to_initial_pose() self._feedback_ready.clear() self._accept_realtime_feedback = True self._realtime_callback = rm_realtime_arm_state_callback_ptr( @@ -451,11 +456,18 @@ class RealManAdapter: self._try_call("rm_set_joint_max_speed", joint_index, self._joint_max_speed) self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc) - def _move_to_initial_pose(self) -> None: + def move_to_initial_pose(self) -> None: + self._require_arm() 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) + 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)") def _try_call(self, name: str, *args: Any) -> None: