Refactor and enhance XR-RM teleoperation functionality

xr_rm_teleop/setup.py

@@ -1,6 +1,6 @@
 """xr_rm_teleop 包安装配置。
 
-该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点。
+该包提供基于 XR 相对位移的 RM75 笛卡尔位姿透传遥操作节点。
 """
 
 from setuptools import setup
@@ -19,7 +19,7 @@ setup(
     zip_safe=True,
     maintainer="Yikai Fu",
     maintainer_email="user@example.com",
-    description="XR relative-motion teleoperation controllers for RealMan RM75 arms.",
+    description="XR relative-pose teleoperation controllers for RealMan RM75 arms.",
     license="Apache-2.0",
     tests_require=["pytest"],
     entry_points={

xr_rm_teleop/xr_rm_teleop/__init__.py

@@ -1,4 +1,4 @@
 """XR 到 RM75 遥操作节点包。
 
-核心模块包括单臂速度遥操作、RealMan SDK 适配层和夹爪 trigger 桥接。
+核心模块包括单臂位姿透传遥操作、RealMan SDK 适配层和夹爪 trigger 桥接。
 """

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -1,12 +1,11 @@
 """RM75 机械臂适配层。
 
-对上提供统一的当前位姿读取、笛卡尔速度发送和停止接口；对下根据配置
+对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置
 选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
 """
 
 from __future__ import annotations
 
-import time
 from dataclasses import dataclass
 from numbers import Number
 from typing import Any
@@ -40,16 +39,17 @@ class MockRealManAdapter:
     def get_current_pose(self) -> ArmPose:
         return self._pose
 
-    def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
+    def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
         del follow
-        self.last_velocity = velocity
-        # 模拟模式只做简单积分，便于观察控制器是否在按预期更新末端位置。
-        self._pose.x += velocity[0] * self._dt
-        self._pose.y += velocity[1] * self._dt
-        self._pose.z += velocity[2] * self._dt
-        self._pose.rx += velocity[3] * self._dt
-        self._pose.ry += velocity[4] * self._dt
-        self._pose.rz += velocity[5] * self._dt
+        self.last_velocity = [
+            (pose.x - self._pose.x) / self._dt,
+            (pose.y - self._pose.y) / self._dt,
+            (pose.z - self._pose.z) / self._dt,
+            0.0,
+            0.0,
+            0.0,
+        ]
+        self._pose = pose
 
     def stop(self) -> None:
         self.last_velocity = [0.0] * 6
@@ -65,13 +65,12 @@ class MockRealManAdapter:
 
 
 class RealManAdapter:
-    """睿尔曼 Python API2 的笛卡尔速度透传适配层。"""
+    """睿尔曼 Python API2 的笛卡尔位姿透传适配层。"""
 
     def __init__(
         self,
         robot_ip: str,
         robot_port: int,
-        dt: float,
         avoid_singularity: int,
         frame_type: int,
         logger: Any | None = None,
@@ -86,13 +85,11 @@ class RealManAdapter:
         initial_joint_pose: list[float] | None = None,
         initial_tcp_pose: list[float] | None = None,
         init_move_speed: int = 20,
-        command_mode: str = "velocity",
         canfd_trajectory_mode: int = 2,
         canfd_radio: int = 0,
     ) -> None:
         self._robot_ip = robot_ip
         self._robot_port = robot_port
-        self._dt_ms = int(round(dt * 1000.0))
         self._avoid_singularity = avoid_singularity
         self._frame_type = frame_type
         self._logger = logger
@@ -107,13 +104,10 @@ class RealManAdapter:
         self._initial_joint_pose = initial_joint_pose
         self._initial_tcp_pose = initial_tcp_pose
         self._init_move_speed = init_move_speed
-        self._command_mode = command_mode
         self._canfd_trajectory_mode = canfd_trajectory_mode
         self._canfd_radio = canfd_radio
         self._scissorgripper: int | None = None
         self._arm: Any | None = None
-        if self._command_mode not in ("velocity", "pose_canfd"):
-            raise ValueError("command_mode must be one of: velocity, pose_canfd")
 
     def connect(self) -> None:
         try:
@@ -130,17 +124,12 @@ class RealManAdapter:
             "RealMan connected: "
             f"ip={self._robot_ip}, port={self._robot_port}, "
             f"avoid_singularity={self._avoid_singularity}, "
-            f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, "
-            f"command_mode={self._command_mode}"
+            f"frame_type={self._frame_type}, command=rm_movep_canfd"
         )
         if self._configure_safety_limits:
             self._apply_safety_limits()
         if self._move_to_initial_pose_on_connect:
             self._move_to_initial_pose()
-        if self._command_mode == "velocity":
-            self._init_movev_canfd()
-        else:
-            self._log_info("command_mode=pose_canfd，跳过 rm_set_movev_canfd_init。")
 
     def get_current_pose(self) -> ArmPose:
         self._require_arm()
@@ -150,17 +139,8 @@ class RealManAdapter:
             raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}")
         return ArmPose(*pose[:6])
 
-    def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
-        self._require_arm()
-        if self._command_mode == "pose_canfd":
-            return
-        ret = self._arm.rm_movev_canfd(velocity, follow, 0, 0)
-        self._check_return(ret, "rm_movev_canfd")
-
     def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
         self._require_arm()
-        if self._command_mode != "pose_canfd":
-            raise RuntimeError("send_cartesian_target requires command_mode=pose_canfd")
         ret = self._arm.rm_movep_canfd(
             [pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
             follow,
@@ -169,9 +149,6 @@ class RealManAdapter:
         )
         self._check_return(ret, "rm_movep_canfd")
 
-    def uses_pose_targets(self) -> bool:
-        return self._command_mode == "pose_canfd"
-
     def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
         self._require_arm()
         from .fun_peripheral import load_peripheral_config, peripheral_cfg
@@ -205,8 +182,6 @@ class RealManAdapter:
         if self._arm is None:
             return
         try:
-            if self._command_mode == "velocity":
-                self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0)
             self._arm.rm_set_arm_slow_stop()
         except Exception:
             pass
@@ -244,46 +219,6 @@ class RealManAdapter:
         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 _init_movev_canfd(self) -> None:
-        attempts = 3
-        candidates = [self._avoid_singularity]
-        if self._avoid_singularity != 0:
-            candidates.append(0)
-
-        # 某些现场控制器初始化避奇异模式会超时，失败后自动降级到 0 再重试。
-        last_ret: Any = None
-        for avoid_singularity in candidates:
-            if avoid_singularity != self._avoid_singularity:
-                self._log_warn("rm_set_movev_canfd_init 降级为 avoid_singularity=0 后重试。")
-
-            for attempt in range(1, attempts + 1):
-                ret = self._arm.rm_set_movev_canfd_init(
-                    avoid_singularity,
-                    self._frame_type,
-                    self._dt_ms,
-                )
-                last_ret = ret
-                code = self._return_code(ret)
-                if code == 0:
-                    self._avoid_singularity = avoid_singularity
-                    self._log_info(
-                        "rm_set_movev_canfd_init 成功："
-                        f"avoid_singularity={avoid_singularity}, "
-                        f"frame_type={self._frame_type}, dt_ms={self._dt_ms}"
-                    )
-                    return
-
-                self._log_warn(
-                    "rm_set_movev_canfd_init 失败："
-                    f"avoid_singularity={avoid_singularity}, "
-                    f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, "
-                    f"attempt={attempt}/{attempts}, ret={ret!r}"
-                )
-                if attempt < attempts:
-                    time.sleep(0.2)
-
-        self._check_return(last_ret, "rm_set_movev_canfd_init")
-
     def _try_call(self, name: str, *args: Any) -> None:
         func = getattr(self._arm, name, None)
         if func is None:

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -1,7 +1,7 @@
-"""RM75 单臂 XR 相对位移遥操作节点。
+"""RM75 单臂 XR 相对位姿透传遥操作节点。
 
 节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位移
-映射到机器人坐标系中的目标 TCP，并通过笛卡尔速度命令跟随该目标。
+映射成机器人坐标系中的目标 TCP，并通过 rm_movep_canfd 持续下发目标位姿。
 """
 
 from __future__ import annotations
@@ -12,6 +12,7 @@ from typing import Iterable
 import rclpy
 from geometry_msgs.msg import PoseStamped, TwistStamped
 from rclpy.node import Node
+from rclpy.time import Time
 from std_msgs.msg import Bool
 
 from xr_rm_interfaces.msg import XrController
@@ -28,21 +29,24 @@ def _clamp(value: float, low: float, high: float) -> float:
 
 
 class SingleArmVelocityTeleop(Node):
-    """基于 XR 手柄相对位移的 RM75 单臂笛卡尔速度遥操节点。"""
+    """基于 XR 手柄相对位移的 RM75 单臂位姿透传遥操节点。
+
+    类名和可执行入口沿用旧名称，避免已有 launch、UI 和命令失效。
+    """
 
     def __init__(self) -> None:
         super().__init__("single_arm_velocity_teleop")
 
         self.declare_parameter("arm_name", "rm75")
         self.declare_parameter("controller_topic", "/xr/right_controller")
-        self.declare_parameter("control_rate_hz", 50.0)
+        self.declare_parameter("control_rate_hz", 90.0)
         self.declare_parameter("command_timeout_sec", 0.12)
         self.declare_parameter("scale", 1.0)
-        self.declare_parameter("kp_linear", 2.0)
-        self.declare_parameter("deadband_m", 0.002)
-        self.declare_parameter("low_pass_alpha", 0.35)
-        self.declare_parameter("velocity_zero_epsilon", 1e-6)
-        self.declare_parameter("max_linear_speed", 0.05)
+        self.declare_parameter("deadband_m", 0.001)
+        self.declare_parameter("target_filter_alpha", 0.65)
+        self.declare_parameter("target_filter_alpha_fast", 0.9)
+        self.declare_parameter("target_filter_fast_threshold_m", 0.03)
+        self.declare_parameter("max_linear_speed", 0.25)
         self.declare_parameter("enable_position_axes", [True, True, True])
         self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
         self.declare_parameter("workspace_max", [0.65, 0.35, 0.60])
@@ -50,6 +54,7 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("low_z_threshold", 0.20)
         self.declare_parameter("low_z_min_radius", 0.21)
         self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0])
+        self.declare_parameter("current_pose_poll_hz", 10.0)
         self.declare_parameter("use_mock", True)
         self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
         self.declare_parameter("robot_ip", "192.168.1.18")
@@ -68,7 +73,6 @@ class SingleArmVelocityTeleop(Node):
         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("command_mode", "velocity")
         self.declare_parameter("canfd_trajectory_mode", 2)
         self.declare_parameter("canfd_radio", 0)
         self.declare_parameter("enable_tool_control", True)
@@ -86,10 +90,12 @@ class SingleArmVelocityTeleop(Node):
         self._dt = 1.0 / control_rate_hz
         self._command_timeout_sec = float(self.get_parameter("command_timeout_sec").value)
         self._scale = float(self.get_parameter("scale").value)
-        self._kp_linear = float(self.get_parameter("kp_linear").value)
         self._deadband_m = float(self.get_parameter("deadband_m").value)
-        self._low_pass_alpha = float(self.get_parameter("low_pass_alpha").value)
-        self._velocity_zero_epsilon = float(self.get_parameter("velocity_zero_epsilon").value)
+        self._target_filter_alpha = float(self.get_parameter("target_filter_alpha").value)
+        self._target_filter_alpha_fast = float(self.get_parameter("target_filter_alpha_fast").value)
+        self._target_filter_fast_threshold_m = float(
+            self.get_parameter("target_filter_fast_threshold_m").value
+        )
         self._max_linear_speed = float(self.get_parameter("max_linear_speed").value)
         self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3)
         self._workspace_min = self._float_list_parameter("workspace_min", 3)
@@ -98,6 +104,7 @@ class SingleArmVelocityTeleop(Node):
         self._low_z_threshold = float(self.get_parameter("low_z_threshold").value)
         self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
         self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
+        self._current_pose_poll_hz = float(self.get_parameter("current_pose_poll_hz").value)
         self._follow = self._bool_parameter("follow")
         self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
         if not self._debug_topic_prefix:
@@ -105,11 +112,16 @@ class SingleArmVelocityTeleop(Node):
         self._validate_parameters()
 
         self._last_msg: XrController | None = None
-        self._last_msg_time = None
+        self._last_msg_time: Time | None = None
         self._active = False
         self._controller_start: list[float] | None = None
-        self._robot_start: list[float] | None = None
-        self._filtered_velocity = [0.0, 0.0, 0.0]
+        self._robot_start_pose: ArmPose | None = None
+        self._filtered_target: list[float] | None = None
+        self._last_sent_target: list[float] | None = None
+        self._last_command_time: Time | None = None
+        self._last_current_pose: ArmPose | None = None
+        self._last_current_pose_time: Time | None = None
+        self._stop_sent = True
 
         self._adapter = self._make_adapter()
         self._adapter.connect()
@@ -117,15 +129,20 @@ class SingleArmVelocityTeleop(Node):
 
         debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
         self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
+        self._raw_target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/raw_target_pose", 10)
         self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10)
         self._cmd_vel_pub = self.create_publisher(TwistStamped, f"{debug_ns}/cmd_vel", 10)
+        self._target_clamped_pub = self.create_publisher(Bool, f"{debug_ns}/target_clamped", 10)
 
         self.create_subscription(XrController, topic, self._on_controller, 10)
         self.create_timer(self._dt, self._control_tick)
-        self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}")
+        self.get_logger().info(
+            f"{self._arm_name} 位姿透传遥操节点已启动，监听话题：{topic}, "
+            f"dt={self._dt:.4f}s, follow={self._follow}"
+        )
 
     def _make_adapter(self):
-        # mock 和真机共享同一控制逻辑，只在适配层切换运动命令的执行方式。
+        # mock 和真机共享同一位姿目标链路，只在适配层切换执行方式。
         if self._bool_parameter("use_mock"):
             return MockRealManAdapter(
                 [float(v) for v in self.get_parameter("mock_initial_pose").value],
@@ -135,7 +152,6 @@ class SingleArmVelocityTeleop(Node):
         return RealManAdapter(
             robot_ip=self.get_parameter("robot_ip").value,
             robot_port=int(self.get_parameter("robot_port").value),
-            dt=self._dt,
             avoid_singularity=int(self.get_parameter("avoid_singularity").value),
             frame_type=int(self.get_parameter("frame_type").value),
             logger=self.get_logger(),
@@ -150,7 +166,6 @@ class SingleArmVelocityTeleop(Node):
             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),
-            command_mode=str(self.get_parameter("command_mode").value),
             canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value),
             canfd_radio=int(self.get_parameter("canfd_radio").value),
         )
@@ -196,98 +211,100 @@ class SingleArmVelocityTeleop(Node):
         self._last_msg_time = self.get_clock().now()
 
     def _control_tick(self) -> None:
+        now = self.get_clock().now()
         if self._last_msg is None or self._last_msg_time is None:
-            self._safe_stop()
+            self._safe_stop(reset_active=True)
             return
 
-        age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9
+        age = (now - self._last_msg_time).nanoseconds * 1e-9
         if age > self._command_timeout_sec:
-            self.get_logger().warn(f"{self._arm_name} XR 手柄数据超时，机械臂停止。", throttle_duration_sec=1.0)
-            self._safe_stop()
-            self._active = False
+            if self._active or not self._stop_sent:
+                self.get_logger().warn(
+                    f"{self._arm_name} XR 手柄数据超时，机械臂停止。",
+                    throttle_duration_sec=1.0,
+                )
+            self._safe_stop(reset_active=True)
+            self._last_msg = None
+            self._last_msg_time = None
             return
 
         if not self._last_msg.grip:
             if self._active:
-                self.get_logger().info(f"{self._arm_name} Grip 松开，退出相对位移遥操。")
-            self._safe_stop()
-            self._active = False
-            self._controller_start = None
-            self._robot_start = None
+                self.get_logger().info(f"{self._arm_name} Grip 松开，退出相对位姿遥操。")
+            self._safe_stop(reset_active=True)
             return
 
         controller_now = self._controller_xyz(self._last_msg)
+        if not self._active:
+            self._enter_active_control(controller_now, now)
+            return
+
+        assert self._controller_start is not None
+        assert self._robot_start_pose is not None
+
+        self._maybe_refresh_current_pose(now)
+        raw_target = self._raw_target_from_controller(controller_now)
+        workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target)
+        desired_target = self._apply_deadband(workspace_target)
+        filtered_target = self._filter_target(desired_target)
+        sent_target, step_limited = self._limit_target_step(filtered_target)
+        sent_target, final_clamped = self._clamp_workspace_with_flag(sent_target)
+
+        velocity = self._estimate_command_velocity(sent_target, now)
+        target_clamped = workspace_clamped or step_limited or final_clamped
+        target_pose = ArmPose(
+            x=sent_target[0],
+            y=sent_target[1],
+            z=sent_target[2],
+            rx=self._robot_start_pose.rx,
+            ry=self._robot_start_pose.ry,
+            rz=self._robot_start_pose.rz,
+        )
+
+        self._publish_debug(raw_target, sent_target, velocity, target_clamped)
+        if self._send_cartesian_target(target_pose):
+            self._last_sent_target = sent_target
+            self._last_command_time = now
+            self._stop_sent = False
+
+    def _enter_active_control(self, controller_now: list[float], now: Time) -> None:
         try:
-            robot_pose = self._adapter.get_current_pose()
-            robot_now = robot_pose.xyz()
+            robot_pose = self._read_current_pose_for_control(now)
         except Exception as exc:
             self.get_logger().error(
                 f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}",
                 throttle_duration_sec=1.0,
             )
-            self._safe_stop()
-            self._active = False
+            self._safe_stop(reset_active=True)
             return
 
-        if not self._active:
-            # 握持键按下的第一帧只锁定手柄和 TCP 起点，不立即运动，避免启停跳变。
-            self._active = True
-            self._controller_start = controller_now
-            self._robot_start = robot_now
-            self._filtered_velocity = [0.0, 0.0, 0.0]
-            self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
-            self._safe_stop()
-            return
-
-        assert self._controller_start is not None
-        assert self._robot_start is not None
-
-        # 核心控制：手柄相对位移 -> 机器人坐标系相对位移 -> 目标 TCP -> 速度命令。
-        controller_delta = [controller_now[i] - self._controller_start[i] for i in range(3)]
-        robot_delta = self._map_xr_delta_to_robot(controller_delta)
-        target = [self._robot_start[i] + self._scale * robot_delta[i] for i in range(3)]
-        target = self._clamp_workspace(target)
-        error = [target[i] - robot_now[i] for i in range(3)]
-        error = [error[i] if self._enable_position_axes[i] else 0.0 for i in range(3)]
-
-        if _norm(error) < self._deadband_m:
-            velocity = [0.0, 0.0, 0.0]
-        else:
-            velocity = [self._kp_linear * value for value in error]
-            velocity = self._clamp_vector_norm(velocity, self._max_linear_speed)
-
-        # 低通滤波可以削弱 XR 追踪抖动，真实机械臂测试时不要关掉。
-        alpha = self._low_pass_alpha
-        self._filtered_velocity = [
-            alpha * velocity[i] + (1.0 - alpha) * self._filtered_velocity[i]
-            for i in range(3)
-        ]
-        self._filtered_velocity = self._zero_tiny_velocity(self._filtered_velocity)
-        cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0]
-        self._publish_debug(robot_pose, target, cartesian_velocity)
-        if self._adapter_uses_pose_targets():
-            step_target = [
-                robot_now[i] + cartesian_velocity[i] * self._dt
-                for i in range(3)
-            ]
-            step_target = self._clamp_workspace(step_target)
-            self._send_cartesian_target(
-                ArmPose(
-                    x=step_target[0],
-                    y=step_target[1],
-                    z=step_target[2],
-                    rx=robot_pose.rx,
-                    ry=robot_pose.ry,
-                    rz=robot_pose.rz,
-                )
-            )
-        else:
-            self._send_cartesian_velocity(cartesian_velocity)
+        robot_xyz = robot_pose.xyz()
+        self._active = True
+        self._controller_start = controller_now
+        self._robot_start_pose = robot_pose
+        self._filtered_target = robot_xyz
+        self._last_sent_target = robot_xyz
+        self._last_command_time = now
+        self._stop_sent = True
+        self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
+        self._publish_debug(robot_xyz, robot_xyz, [0.0] * 6, False)
 
     @staticmethod
     def _controller_xyz(msg: XrController) -> list[float]:
         return [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
 
+    def _raw_target_from_controller(self, controller_now: list[float]) -> list[float]:
+        assert self._controller_start is not None
+        assert self._robot_start_pose is not None
+        controller_delta = [controller_now[i] - self._controller_start[i] for i in range(3)]
+        robot_delta = self._map_xr_delta_to_robot(controller_delta)
+        target = [
+            self._robot_start_pose.x + self._scale * robot_delta[0],
+            self._robot_start_pose.y + self._scale * robot_delta[1],
+            self._robot_start_pose.z + self._scale * robot_delta[2],
+        ]
+        return [target[i] if self._enable_position_axes[i] else self._robot_start_pose.xyz()[i] for i in range(3)]
+
     def _map_xr_delta_to_robot(self, delta: list[float]) -> list[float]:
         matrix = self._xr_to_robot_matrix
         return [
@@ -296,13 +313,61 @@ class SingleArmVelocityTeleop(Node):
             matrix[6] * delta[0] + matrix[7] * delta[1] + matrix[8] * delta[2],
         ]
 
-    def _clamp_workspace(self, target: list[float]) -> list[float]:
-        # 先做盒状工作空间限幅，再叠加底座圆柱半径限制。
+    def _apply_deadband(self, target: list[float]) -> list[float]:
+        if self._deadband_m <= 0.0 or self._last_sent_target is None:
+            return target
+        delta = [target[i] - self._last_sent_target[i] for i in range(3)]
+        if _norm(delta) < self._deadband_m:
+            return list(self._last_sent_target)
+        return target
+
+    def _filter_target(self, target: list[float]) -> list[float]:
+        if self._filtered_target is None:
+            self._filtered_target = list(target)
+            return list(target)
+
+        delta = [target[i] - self._filtered_target[i] for i in range(3)]
+        distance = _norm(delta)
+        alpha = self._adaptive_filter_alpha(distance)
+        self._filtered_target = [
+            alpha * target[i] + (1.0 - alpha) * self._filtered_target[i]
+            for i in range(3)
+        ]
+        return list(self._filtered_target)
+
+    def _adaptive_filter_alpha(self, distance: float) -> float:
+        if self._target_filter_fast_threshold_m <= 1e-9:
+            return self._target_filter_alpha_fast
+        ratio = _clamp(distance / self._target_filter_fast_threshold_m, 0.0, 1.0)
+        return (
+            self._target_filter_alpha
+            + (self._target_filter_alpha_fast - self._target_filter_alpha) * ratio
+        )
+
+    def _limit_target_step(self, target: list[float]) -> tuple[list[float], bool]:
+        if self._last_sent_target is None:
+            return target, False
+
+        delta = [target[i] - self._last_sent_target[i] for i in range(3)]
+        distance = _norm(delta)
+        max_step = self._max_linear_speed * self._dt
+        if distance <= max_step or distance <= 1e-9:
+            return target, False
+
+        scale = max_step / distance
+        return [
+            self._last_sent_target[i] + delta[i] * scale
+            for i in range(3)
+        ], True
+
+    def _clamp_workspace_with_flag(self, target: list[float]) -> tuple[list[float], bool]:
         clamped = [
             _clamp(target[i], self._workspace_min[i], self._workspace_max[i])
             for i in range(3)
         ]
-        return self._clamp_cylinder_radius(clamped)
+        clamped = self._clamp_cylinder_radius(clamped)
+        was_clamped = any(abs(clamped[i] - target[i]) > 1e-9 for i in range(3))
+        return clamped, was_clamped
 
     def _clamp_cylinder_radius(self, target: list[float]) -> list[float]:
         min_radius, max_radius = self._cyl_radius_limit
@@ -325,40 +390,80 @@ class SingleArmVelocityTeleop(Node):
 
         return target
 
-    @staticmethod
-    def _clamp_vector_norm(vector: list[float], max_norm: float) -> list[float]:
-        norm = _norm(vector)
-        if norm <= max_norm or norm <= 1e-9:
-            return vector
-        scale = max_norm / norm
-        return [value * scale for value in vector]
+    def _estimate_command_velocity(self, target: list[float], now: Time) -> list[float]:
+        if self._last_sent_target is None:
+            return [0.0] * 6
 
-    def _safe_stop(self) -> None:
-        # 所有断连、松手、退出路径都走这里，确保发送零速度。
-        self._filtered_velocity = [0.0, 0.0, 0.0]
-        self._publish_stop_debug()
-        if self._adapter_uses_pose_targets():
+        dt = self._dt
+        if self._last_command_time is not None:
+            measured_dt = (now - self._last_command_time).nanoseconds * 1e-9
+            if measured_dt > 1e-6:
+                dt = measured_dt
+        return [
+            (target[i] - self._last_sent_target[i]) / dt
+            for i in range(3)
+        ] + [0.0, 0.0, 0.0]
+
+    def _read_current_pose_for_control(self, now: Time) -> ArmPose:
+        pose = self._adapter.get_current_pose()
+        self._last_current_pose = pose
+        self._last_current_pose_time = now
+        return pose
+
+    def _maybe_refresh_current_pose(self, now: Time) -> None:
+        if self._current_pose_poll_hz <= 0.0:
             return
-        self._send_cartesian_velocity([0.0] * 6)
+        if self._last_current_pose_time is not None:
+            age = (now - self._last_current_pose_time).nanoseconds * 1e-9
+            if age < 1.0 / self._current_pose_poll_hz:
+                return
 
-    def _publish_stop_debug(self) -> None:
         try:
-            current_pose = self._adapter.get_current_pose()
-        except Exception:
-            return
-        self._publish_debug(current_pose, current_pose.xyz(), [0.0] * 6)
-
-    def _send_cartesian_velocity(self, velocity: list[float]) -> None:
-        try:
-            self._adapter.send_cartesian_velocity(velocity, self._follow)
+            self._read_current_pose_for_control(now)
         except Exception as exc:
-            self.get_logger().error(
-                f"{self._arm_name} 发送速度命令失败：{exc}",
+            self.get_logger().warn(
+                f"{self._arm_name} 低频读取 TCP 位姿失败，继续透传目标：{exc}",
                 throttle_duration_sec=1.0,
             )
-            self._active = False
 
-    def _send_cartesian_target(self, pose: ArmPose) -> None:
+    def _safe_stop(self, reset_active: bool) -> None:
+        if not self._stop_sent:
+            self._send_stop_once()
+        if reset_active:
+            self._active = False
+            self._controller_start = None
+            self._robot_start_pose = None
+            self._filtered_target = None
+            self._last_sent_target = None
+            self._last_command_time = None
+        self._publish_stop_debug()
+
+    def _send_stop_once(self) -> None:
+        try:
+            self._adapter.stop()
+        except Exception as exc:
+            self.get_logger().error(
+                f"{self._arm_name} 停止机械臂失败：{exc}",
+                throttle_duration_sec=1.0,
+            )
+        self._stop_sent = True
+
+    def _publish_stop_debug(self) -> None:
+        pose = self._debug_pose_fallback()
+        if pose is None:
+            return
+        self._publish_debug(pose.xyz(), pose.xyz(), [0.0] * 6, False)
+
+    def _debug_pose_fallback(self) -> ArmPose | None:
+        if self._last_current_pose is not None:
+            return self._last_current_pose
+        if self._robot_start_pose is not None:
+            return self._robot_start_pose
+        if self._last_sent_target is not None:
+            return ArmPose(*self._last_sent_target)
+        return None
+
+    def _send_cartesian_target(self, pose: ArmPose) -> bool:
         try:
             self._adapter.send_cartesian_target(pose, self._follow)
         except Exception as exc:
@@ -367,22 +472,33 @@ class SingleArmVelocityTeleop(Node):
                 throttle_duration_sec=1.0,
             )
             self._active = False
+            self._send_stop_once()
+            return False
+        return True
 
-    def _adapter_uses_pose_targets(self) -> bool:
-        uses_pose_targets = getattr(self._adapter, "uses_pose_targets", None)
-        return bool(uses_pose_targets is not None and uses_pose_targets())
-
-    def _zero_tiny_velocity(self, velocity: list[float]) -> list[float]:
-        if self._velocity_zero_epsilon <= 0.0:
-            return velocity
-        return [
-            0.0 if abs(value) < self._velocity_zero_epsilon else value
-            for value in velocity
-        ]
-
-    def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None:
+    def _publish_debug(
+        self,
+        raw_target_xyz: list[float],
+        target_xyz: list[float],
+        command_velocity: list[float],
+        target_clamped: bool,
+    ) -> None:
         stamp = self.get_clock().now().to_msg()
-        current_msg = self._pose_msg(stamp, current_pose)
+        current_pose = self._debug_pose_fallback()
+        if current_pose is None:
+            current_pose = ArmPose(*target_xyz)
+
+        raw_pose = self._pose_msg(
+            stamp,
+            ArmPose(
+                x=raw_target_xyz[0],
+                y=raw_target_xyz[1],
+                z=raw_target_xyz[2],
+                rx=current_pose.rx,
+                ry=current_pose.ry,
+                rz=current_pose.rz,
+            ),
+        )
         target_msg = self._pose_msg(
             stamp,
             ArmPose(
@@ -394,20 +510,24 @@ class SingleArmVelocityTeleop(Node):
                 rz=current_pose.rz,
             ),
         )
-        velocity = self._zero_tiny_velocity(velocity)
         velocity_msg = TwistStamped()
         velocity_msg.header.stamp = stamp
         velocity_msg.header.frame_id = "rm_base"
-        velocity_msg.twist.linear.x = float(velocity[0])
-        velocity_msg.twist.linear.y = float(velocity[1])
-        velocity_msg.twist.linear.z = float(velocity[2])
-        velocity_msg.twist.angular.x = float(velocity[3])
-        velocity_msg.twist.angular.y = float(velocity[4])
-        velocity_msg.twist.angular.z = float(velocity[5])
+        velocity_msg.twist.linear.x = float(command_velocity[0])
+        velocity_msg.twist.linear.y = float(command_velocity[1])
+        velocity_msg.twist.linear.z = float(command_velocity[2])
+        velocity_msg.twist.angular.x = float(command_velocity[3])
+        velocity_msg.twist.angular.y = float(command_velocity[4])
+        velocity_msg.twist.angular.z = float(command_velocity[5])
 
-        self._current_pose_pub.publish(current_msg)
+        clamped_msg = Bool()
+        clamped_msg.data = bool(target_clamped)
+
+        self._current_pose_pub.publish(self._pose_msg(stamp, current_pose))
+        self._raw_target_pose_pub.publish(raw_pose)
         self._target_pose_pub.publish(target_msg)
         self._cmd_vel_pub.publish(velocity_msg)
+        self._target_clamped_pub.publish(clamped_msg)
 
     @staticmethod
     def _pose_msg(stamp, pose: ArmPose) -> PoseStamped:
@@ -467,12 +587,18 @@ class SingleArmVelocityTeleop(Node):
             raise ValueError("command_timeout_sec must be > 0")
         if self._deadband_m < 0.0:
             raise ValueError("deadband_m must be >= 0")
-        if not 0.0 <= self._low_pass_alpha <= 1.0:
-            raise ValueError("low_pass_alpha must be in [0, 1]")
-        if self._velocity_zero_epsilon < 0.0:
-            raise ValueError("velocity_zero_epsilon must be >= 0")
-        if self._max_linear_speed < 0.0:
-            raise ValueError("max_linear_speed must be >= 0")
+        if not 0.0 <= self._target_filter_alpha <= 1.0:
+            raise ValueError("target_filter_alpha must be in [0, 1]")
+        if not 0.0 <= self._target_filter_alpha_fast <= 1.0:
+            raise ValueError("target_filter_alpha_fast must be in [0, 1]")
+        if self._target_filter_alpha_fast < self._target_filter_alpha:
+            raise ValueError("target_filter_alpha_fast must be >= target_filter_alpha")
+        if self._target_filter_fast_threshold_m < 0.0:
+            raise ValueError("target_filter_fast_threshold_m must be >= 0")
+        if self._max_linear_speed <= 0.0:
+            raise ValueError("max_linear_speed must be > 0")
+        if self._current_pose_poll_hz < 0.0:
+            raise ValueError("current_pose_poll_hz must be >= 0")
         for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):
             if low >= high:
                 raise ValueError(f"workspace_min[{axis}] must be smaller than workspace_max[{axis}]")
@@ -498,7 +624,8 @@ def main(args: list[str] | None = None) -> None:
         pass
     finally:
         node.destroy_node()
-        rclpy.shutdown()
+        if rclpy.ok():
+            rclpy.shutdown()
 
 
 if __name__ == "__main__":