diff --git a/xr_rm_interfaces/CMakeLists.txt b/xr_rm_interfaces/CMakeLists.txt index 3f70846..0f3dcde 100755 --- a/xr_rm_interfaces/CMakeLists.txt +++ b/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 ) diff --git a/xr_rm_interfaces/msg/ActControlSample.msg b/xr_rm_interfaces/msg/ActControlSample.msg new file mode 100644 index 0000000..61b6d7b --- /dev/null +++ b/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 diff --git a/xr_rm_teleop/test/test_placo_transforms.py b/xr_rm_teleop/test/test_placo_transforms.py index ae40541..23ae1d9 100644 --- a/xr_rm_teleop/test/test_placo_transforms.py +++ b/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) 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 098f9bf..f678030 100644 --- a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py +++ b/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():