diff --git a/xr_rm_teleop/test/test_act_control_sample.py b/xr_rm_teleop/test/test_act_control_sample.py new file mode 100644 index 0000000..b3e8403 --- /dev/null +++ b/xr_rm_teleop/test/test_act_control_sample.py @@ -0,0 +1,194 @@ +from types import SimpleNamespace + +import numpy as np +import pytest +from builtin_interfaces.msg import Time as TimeMsg + +from xr_rm_interfaces.msg import XrController +from xr_rm_teleop.single_arm_velocity_teleop import ( + SingleArmVelocityTeleop, + _ActCycleContext, +) + + +class FakePublisher: + def __init__(self, error=None) -> None: + self.error = error + self.messages = [] + + def publish(self, message) -> None: + if self.error is not None: + raise self.error + self.messages.append(message) + + +class FakeLogger: + def __init__(self) -> None: + self.warnings = [] + + def warn(self, message, **kwargs) -> None: + del kwargs + self.warnings.append(message) + + +def _controller(*, grip=True) -> XrController: + message = XrController() + message.hand = "right" + message.grip = grip + message.trigger = 0.25 + message.primary = False + message.secondary = False + message.axis = [0.1, -0.2] + message.pose.orientation.w = 1.0 + return message + + +def _teleop(*, last_target=None, tool_state=True): + teleop = object.__new__(SingleArmVelocityTeleop) + teleop._arm_name = "right_rm75" + teleop._last_msg = _controller() + teleop._active = True + teleop._control_fault_latched = False + teleop._last_current_pose = np.eye(4) + teleop._robot_start_transform = None + teleop._last_sent_target = None + teleop._last_sent_orientation = None + teleop._last_successful_action_target = last_target + teleop._ik_solver = SimpleNamespace( + joint_position_limits=np.asarray([[-1.0, 1.0]] * 7) + ) + teleop._tool_state_snapshot = lambda: ( + True, + tool_state, + False, + False, + ) + teleop.get_clock = lambda: SimpleNamespace( + now=lambda: SimpleNamespace(to_msg=lambda: TimeMsg()) + ) + teleop.get_logger = lambda: FakeLogger() + return teleop + + +def _cycle(**overrides) -> _ActCycleContext: + values = { + "control_seq": 100, + "control_monotonic_ns": 1_000_000_000, + "feedback_monotonic_ns": 990_000_000, + "action_monotonic_ns": 1_005_000_000, + "feedback_age_ms": 10.0, + "q_actual": [0.1] * 7, + "q_qp_raw": [0.3] * 7, + "q_target": [0.2] * 7, + "current_pose": np.eye(4), + "raw_target_pose": np.eye(4), + "target_pose": np.eye(4), + "command_velocity": [0.0] * 6, + "feedback_valid": True, + "command_sent": True, + "qp_attempted": True, + "qp_success": True, + } + values.update(overrides) + return _ActCycleContext(**values) + + +def test_act_sample_uses_feedback_and_limited_target_from_one_cycle() -> None: + teleop = _teleop(last_target=[0.2] * 7) + + message = teleop._build_act_control_sample(_cycle()) + + assert message.control_seq == 100 + assert message.q_actual == pytest.approx([0.1] * 7) + assert message.q_qp_raw == pytest.approx([0.3] * 7) + assert message.q_target == pytest.approx([0.2] * 7) + assert message.joint_lower_limits == pytest.approx([-1.0] * 7) + assert message.joint_upper_limits == pytest.approx([1.0] * 7) + assert message.command_sent + assert message.action_valid + assert message.qp_attempted + assert message.qp_success + + +def test_act_sample_marks_qp_fallback_as_valid_held_action() -> None: + teleop = _teleop(last_target=[0.2] * 7) + cycle = _cycle( + q_qp_raw=[0.2] * 7, + q_target=[0.2] * 7, + qp_success=False, + ) + + message = teleop._build_act_control_sample(cycle) + + assert message.q_target == pytest.approx([0.2] * 7) + assert message.action_valid + assert message.qp_attempted + assert not message.qp_success + + +def test_act_sample_holds_last_action_while_grip_is_released() -> None: + teleop = _teleop(last_target=[0.4] * 7) + teleop._last_msg = _controller(grip=False) + teleop._active = False + cycle = _cycle( + q_qp_raw=None, + q_target=None, + command_sent=False, + qp_attempted=False, + qp_success=False, + action_monotonic_ns=-1, + ) + + message = teleop._build_act_control_sample(cycle) + + assert message.q_target == pytest.approx([0.4] * 7) + assert message.action_valid + assert not message.command_sent + assert not message.teleop_active + + +def test_act_sample_marks_send_failure_invalid() -> None: + teleop = _teleop(last_target=[0.4] * 7) + + message = teleop._build_act_control_sample( + _cycle(send_failed=True, command_sent=False) + ) + + assert not message.action_valid + + +def test_act_sample_marks_unknown_gripper_state() -> None: + teleop = _teleop(last_target=[0.2] * 7, tool_state=None) + + message = teleop._build_act_control_sample(_cycle()) + + assert not message.gripper_state_known + + +def test_act_sample_publish_failure_does_not_escape_control_path() -> None: + teleop = _teleop(last_target=[0.2] * 7) + logger = FakeLogger() + teleop._act_sample_pub = FakePublisher(RuntimeError("dds failed")) + teleop.get_logger = lambda: logger + + teleop._publish_act_control_sample(_cycle()) + + assert logger.warnings == [ + "right_rm75 ACT原子样本发布失败：dds failed" + ] + + +def test_control_tick_wraps_one_impl_call_in_one_atomic_sample() -> None: + teleop = object.__new__(SingleArmVelocityTeleop) + cycles = [] + published = [] + teleop._act_control_seq = 7 + teleop._control_tick_impl = lambda cycle: cycles.append(cycle) + teleop._publish_act_control_sample = lambda cycle: published.append(cycle) + + teleop._control_tick() + + assert len(cycles) == 1 + assert published == cycles + assert cycles[0].control_seq == 7 + assert teleop._act_control_seq == 8 diff --git a/xr_rm_teleop/test/test_joint_control.py b/xr_rm_teleop/test/test_joint_control.py index fa2614d..7646224 100644 --- a/xr_rm_teleop/test/test_joint_control.py +++ b/xr_rm_teleop/test/test_joint_control.py @@ -686,9 +686,10 @@ def test_qp_failure_returns_last_known_good_target() -> None: teleop._arm_name = "right_rm75" teleop.get_logger = lambda: FakeLogger() - target = teleop._solve_joint_target(np.eye(4)) + target, qp_success = teleop._solve_joint_target(np.eye(4)) assert target == pytest.approx([0.1] * 7) + assert not qp_success assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7) @@ -704,9 +705,10 @@ def test_qp_success_updates_last_known_good_target() -> None: teleop._arm_name = "left_rm75" teleop.get_logger = lambda: FakeLogger() - target = teleop._solve_joint_target(np.eye(4)) + target, qp_success = teleop._solve_joint_target(np.eye(4)) assert target == pytest.approx([0.2] * 7) + assert qp_success assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7) diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py index 818dde6..3ee4764 100755 --- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py +++ b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py @@ -10,17 +10,19 @@ import math import queue import threading import time +from dataclasses import dataclass from typing import Iterable import numpy as np import rclpy from geometry_msgs.msg import PoseStamped, TwistStamped from rclpy.node import Node +from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy from rclpy.time import Time from sensor_msgs.msg import JointState from std_msgs.msg import Bool -from xr_rm_interfaces.msg import XrController +from xr_rm_interfaces.msg import ActControlSample, XrController from .fun_peripheral import load_peripheral_config from .placo_ik_solver import PlacoIkSolver @@ -31,6 +33,30 @@ from .realman_adapter import ( ) +@dataclass +class _ActCycleContext: + control_seq: int + control_monotonic_ns: int + feedback_monotonic_ns: int = -1 + action_monotonic_ns: int = -1 + feedback_age_ms: float = math.inf + qp_duration_ms: float = 0.0 + q_actual: list[float] | None = None + q_qp_raw: list[float] | None = None + q_target: list[float] | None = None + current_pose: np.ndarray | None = None + raw_target_pose: np.ndarray | None = None + target_pose: np.ndarray | None = None + command_velocity: list[float] | None = None + feedback_valid: bool = False + command_sent: bool = False + send_failed: bool = False + qp_attempted: bool = False + qp_success: bool = False + target_clamped: bool = False + control_fault: bool = False + + def _norm(values: Iterable[float]) -> float: return math.sqrt(sum(value * value for value in values)) @@ -291,6 +317,8 @@ class SingleArmVelocityTeleop(Node): self._latest_joint_positions: list[float] | None = None self._last_joint_command_target: list[float] | None = None self._last_joint_command_velocity: list[float] | None = None + self._last_successful_action_target: list[float] | None = None + self._act_control_seq = 0 self._joint_feedback_ready = False self._grip_rearm_required = False self._feedback_resync_attempted = False @@ -345,6 +373,15 @@ class SingleArmVelocityTeleop(Node): f"{debug_ns}/joint_target", 10, ) + self._act_sample_pub = self.create_publisher( + ActControlSample, + f"{debug_ns}/act_control_sample", + QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=10, + reliability=ReliabilityPolicy.BEST_EFFORT, + ), + ) self._adapter = self._make_adapter() self._adapter.connect() self._initialize_joint_state() @@ -619,6 +656,18 @@ class SingleArmVelocityTeleop(Node): self._enqueue_tool_command(self._trigger_tool_open, "trigger") def _control_tick(self) -> None: + control_seq = getattr(self, "_act_control_seq", 0) + self._act_control_seq = control_seq + 1 + cycle = _ActCycleContext( + control_seq=control_seq, + control_monotonic_ns=time.monotonic_ns(), + ) + try: + self._control_tick_impl(cycle) + finally: + self._publish_act_control_sample(cycle) + + def _control_tick_impl(self, cycle: _ActCycleContext) -> None: tick_started_ns = time.perf_counter_ns() last_tick_started_ns = getattr( self, @@ -633,10 +682,12 @@ class SingleArmVelocityTeleop(Node): ) now = self.get_clock().now() if self._control_fault_latched: + cycle.control_fault = True return snapshot = self._adapter.get_latest_joint_state() if not self._joint_snapshot_is_motion_ready(snapshot): + cycle.control_fault = True self._grip_rearm_required = True if self._joint_feedback_ready: self.get_logger().warn( @@ -647,13 +698,18 @@ class SingleArmVelocityTeleop(Node): self._safe_stop(reset_active=True) return assert snapshot is not None + cycle.q_actual = list(snapshot.positions) + cycle.feedback_monotonic_ns = int(snapshot.received_at * 1e9) feedback_age = time.monotonic() - snapshot.received_at + cycle.feedback_age_ms = feedback_age * 1000.0 if feedback_age < 0.0: + cycle.control_fault = True self._grip_rearm_required = True self._joint_feedback_ready = False self._safe_stop(reset_active=True) return if feedback_age > self._command_timeout_sec: + cycle.control_fault = True self._handle_stale_joint_feedback(feedback_age) return @@ -661,6 +717,7 @@ class SingleArmVelocityTeleop(Node): try: current_pose = self._sync_joint_feedback(snapshot) except Exception as exc: + cycle.control_fault = True self.get_logger().error( f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}", throttle_duration_sec=1.0, @@ -669,6 +726,8 @@ class SingleArmVelocityTeleop(Node): self._grip_rearm_required = True self._safe_stop(reset_active=True) return + cycle.current_pose = current_pose + cycle.feedback_valid = True if not self._joint_feedback_ready: if self._grip_rearm_required: message = ( @@ -717,6 +776,7 @@ class SingleArmVelocityTeleop(Node): try: controller_quat = self._controller_quaternion(self._last_msg) except ValueError as exc: + cycle.control_fault = True self.get_logger().warn( f"{self._arm_name} XR 手柄姿态无效，停止输出：{exc}", throttle_duration_sec=1.0, @@ -761,19 +821,35 @@ class SingleArmVelocityTeleop(Node): sent_target, sent_orientation, ) + cycle.raw_target_pose = raw_target_pose + cycle.target_pose = target_pose + cycle.command_velocity = list(velocity) + cycle.target_clamped = target_clamped self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped) + cycle.qp_attempted = True qp_started_ns = time.perf_counter_ns() - joint_target = self._solve_joint_target(target_pose) + joint_target, qp_success = self._solve_joint_target(target_pose) qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6 + cycle.qp_duration_ms = qp_ms + cycle.q_qp_raw = list(joint_target) + cycle.qp_success = qp_success send_started_ns = time.perf_counter_ns() sent = self._send_joint_target(joint_target) send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6 if sent: + assert self._last_joint_command_target is not None + final_target = list(self._last_joint_command_target) + self._last_successful_action_target = final_target + cycle.q_target = final_target + cycle.action_monotonic_ns = time.monotonic_ns() + cycle.command_sent = True self._last_sent_target = sent_target self._last_sent_orientation = sent_orientation.copy() self._last_command_time = now self._stop_sent = False + else: + cycle.send_failed = True total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6 try: self._record_timing_sample( @@ -1233,7 +1309,10 @@ class SingleArmVelocityTeleop(Node): self._last_valid_joint_target = list(snapshot.positions) return current_pose - def _solve_joint_target(self, target_pose: np.ndarray) -> list[float]: + def _solve_joint_target( + self, + target_pose: np.ndarray, + ) -> tuple[list[float], bool]: if self._last_valid_joint_target is None: raise RuntimeError("valid joint feedback has not been initialized") try: @@ -1243,9 +1322,9 @@ class SingleArmVelocityTeleop(Node): f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}", throttle_duration_sec=1.0, ) - return list(self._last_valid_joint_target) + return list(self._last_valid_joint_target), False self._last_valid_joint_target = list(result) - return list(result) + return list(result), True def _safe_stop(self, reset_active: bool) -> None: if not self._stop_sent: @@ -1417,6 +1496,122 @@ class SingleArmVelocityTeleop(Node): self._cmd_vel_pub.publish(velocity_msg) self._target_clamped_pub.publish(clamped_msg) + def _build_act_control_sample( + self, + cycle: _ActCycleContext, + ) -> ActControlSample: + message = ActControlSample() + message.header.stamp = self.get_clock().now().to_msg() + message.header.frame_id = "rm_base" + message.control_seq = cycle.control_seq + message.control_monotonic_ns = cycle.control_monotonic_ns + message.feedback_monotonic_ns = cycle.feedback_monotonic_ns + message.action_monotonic_ns = cycle.action_monotonic_ns + message.feedback_age_ms = float(cycle.feedback_age_ms) + message.qp_duration_ms = float(cycle.qp_duration_ms) + + q_actual = cycle.q_actual or [0.0] * 7 + held_target = ( + cycle.q_target + or self._last_successful_action_target + or q_actual + ) + qp_target = cycle.q_qp_raw or held_target + limits = np.asarray( + self._ik_solver.joint_position_limits, + dtype=float, + ) + if limits.shape != (7, 2) or not np.isfinite(limits).all(): + raise ValueError("joint limits must have finite shape (7, 2)") + message.q_actual = [float(value) for value in q_actual] + message.q_qp_raw = [float(value) for value in qp_target] + message.q_target = [float(value) for value in held_target] + message.joint_lower_limits = limits[:, 0].tolist() + message.joint_upper_limits = limits[:, 1].tolist() + + current_pose = cycle.current_pose + if current_pose is None: + current_pose = self._debug_pose_fallback() + if current_pose is None: + current_pose = np.eye(4) + raw_target_pose = cycle.raw_target_pose + if raw_target_pose is None: + raw_target_pose = current_pose + target_pose = cycle.target_pose + if target_pose is None: + target_pose = current_pose + message.tcp_current = self._pose_msg( + message.header.stamp, + current_pose, + ).pose + message.tcp_raw_target = self._pose_msg( + message.header.stamp, + raw_target_pose, + ).pose + message.tcp_target = self._pose_msg( + message.header.stamp, + target_pose, + ).pose + velocity = cycle.command_velocity or [0.0] * 6 + if len(velocity) != 6: + raise ValueError("ACT command velocity must contain 6 values") + message.tcp_command_velocity.linear.x = float(velocity[0]) + message.tcp_command_velocity.linear.y = float(velocity[1]) + message.tcp_command_velocity.linear.z = float(velocity[2]) + message.tcp_command_velocity.angular.x = float(velocity[3]) + message.tcp_command_velocity.angular.y = float(velocity[4]) + message.tcp_command_velocity.angular.z = float(velocity[5]) + + controller = self._last_msg + if controller is not None: + message.pico_pose = controller.pose + message.pico_grip = bool(controller.grip) + message.pico_trigger = float(controller.trigger) + message.pico_primary = bool(controller.primary) + message.pico_secondary = bool(controller.secondary) + message.pico_axis = [float(value) for value in controller.axis] + + tool_target, tool_state, tool_pending, tool_failed = ( + self._tool_state_snapshot() + ) + message.gripper_target_open = tool_target + message.gripper_state_known = tool_state is not None + message.gripper_state_open = bool(tool_state) + message.gripper_command_pending = tool_pending + message.gripper_command_failed = tool_failed + + message.teleop_active = bool(self._active) + message.feedback_valid = cycle.feedback_valid + message.action_valid = bool( + self._last_successful_action_target is not None + and cycle.feedback_valid + and not cycle.send_failed + and not cycle.control_fault + ) + message.command_sent = cycle.command_sent + message.qp_attempted = cycle.qp_attempted + message.qp_success = cycle.qp_success + message.target_clamped = cycle.target_clamped + message.control_fault = bool( + cycle.control_fault or self._control_fault_latched + ) + return message + + def _publish_act_control_sample( + self, + cycle: _ActCycleContext, + ) -> None: + publisher = getattr(self, "_act_sample_pub", None) + if publisher is None: + return + try: + publisher.publish(self._build_act_control_sample(cycle)) + except Exception as exc: + self.get_logger().warn( + f"{self._arm_name} ACT原子样本发布失败：{exc}", + throttle_duration_sec=1.0, + ) + @staticmethod def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped: transform = _make_transform(pose[:3, 3], pose[:3, :3])