feat: 添加ACT原子控制消息

xr_rm_interfaces/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ActControlSample.msg"
   "msg/XrController.msg"
   DEPENDENCIES geometry_msgs std_msgs
 )

xr_rm_interfaces/msg/ActControlSample.msg

@@ -0,0 +1,41 @@
+std_msgs/Header header
+uint64 control_seq
+int64 control_monotonic_ns
+int64 feedback_monotonic_ns
+int64 action_monotonic_ns
+
+float32 feedback_age_ms
+float32 qp_duration_ms
+
+float64[7] q_actual
+float64[7] q_qp_raw
+float64[7] q_target
+float64[7] joint_lower_limits
+float64[7] joint_upper_limits
+
+geometry_msgs/Pose tcp_current
+geometry_msgs/Pose tcp_raw_target
+geometry_msgs/Pose tcp_target
+geometry_msgs/Twist tcp_command_velocity
+geometry_msgs/Pose pico_pose
+
+bool pico_grip
+float32 pico_trigger
+bool pico_primary
+bool pico_secondary
+float32[2] pico_axis
+
+bool gripper_target_open
+bool gripper_state_open
+bool gripper_state_known
+bool gripper_command_pending
+bool gripper_command_failed
+
+bool teleop_active
+bool feedback_valid
+bool action_valid
+bool command_sent
+bool qp_attempted
+bool qp_success
+bool target_clamped
+bool control_fault

xr_rm_teleop/test/test_placo_transforms.py

@@ -272,6 +272,20 @@ def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
         _validated_transform(transform)
 
 
+def test_joint_position_limits_returns_a_copy() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
+
+    first = solver.joint_position_limits
+    second = solver.joint_position_limits
+
+    assert first.shape == (7, 2)
+    assert np.isfinite(first).all()
+    assert np.all(first[:, 0] < first[:, 1])
+    first[0, 0] = 999.0
+    assert second[0, 0] != 999.0
+
+
 def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
     solver = object.__new__(PlacoIkSolver)
     solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -157,6 +157,10 @@ class PlacoIkSolver:
     def base_configuration(self) -> list[float]:
         return self._robot.state.q[:7].tolist()
 
+    @property
+    def joint_position_limits(self) -> np.ndarray:
+        return self._joint_limits.copy()
+
     def update_joint_state(self, joints: list[float]) -> np.ndarray:
         values = np.asarray(joints, dtype=float)
         if values.shape != (7,) or not np.isfinite(values).all():