fix: 修复 RM75 关节目标越界振荡

docs/superpowers/plans/2026-07-30-rm75-joint-command-braking.md

@@ -46,7 +46,7 @@
 - 修改：`xr_rm_teleop/test/test_joint_control.py:206`
 - 测试：`xr_rm_teleop/test/test_joint_control.py`
 
-- [ ] **步骤 1：在现有首周期加速度测试后增加固定目标测试**
+- [x] **步骤 1：在现有首周期加速度测试后增加固定目标测试**
 
 增加以下测试：
 
@@ -90,7 +90,7 @@ def test_joint_command_step_brakes_before_fixed_target_without_overshoot() -> No
 该测试同时覆盖正负方向、不同目标距离、最大速度、最大加速度、禁止越过固定目标
 和最终停止。
 
-- [ ] **步骤 2：运行新增测试并确认失败**
+- [x] **步骤 2：运行新增测试并确认失败**
 
 运行：
 
@@ -113,7 +113,7 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
   `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:1232-1263`
 - 测试：`xr_rm_teleop/test/test_joint_control.py`
 
-- [ ] **步骤 1：用离散制动逻辑替换现有限幅计算**
+- [x] **步骤 1：用离散制动逻辑替换现有限幅计算**
 
 保留方法签名和现有长度、参数校验，将
 `desired_velocity = np.clip(...)` 到返回值的部分替换为：
@@ -188,7 +188,7 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
 不要增加 ROS 参数或辅助类。制动距离直接使用当前方法已有的
 `max_acceleration`、`max_speed` 和 `dt`。
 
-- [ ] **步骤 2：运行新增测试并确认通过**
+- [x] **步骤 2：运行新增测试并确认通过**
 
 运行：
 
@@ -202,7 +202,7 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
 
 预期：`PASS`。
 
-- [ ] **步骤 3：运行关节控制测试文件**
+- [x] **步骤 3：运行关节控制测试文件**
 
 运行：
 
@@ -222,7 +222,7 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
 
 - 不修改文件。
 
-- [ ] **步骤 1：运行 `xr_rm_teleop` 全部测试**
+- [x] **步骤 1：运行 `xr_rm_teleop` 全部测试**
 
 运行：
 
@@ -230,13 +230,13 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
 source /opt/ros/humble/setup.bash
 export RM75_TEST_PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
 PYTHONPATH="${RM75_TEST_PYTHONPATH}:${PYTHONPATH:-}" \
-  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  python3 -m pytest \
   src/xr_rm_teleop/test -v
 ```
 
 预期：全部测试通过，无失败；真实 Placo 回归测试必须执行，不能因缺少模块而跳过。
 
-- [ ] **步骤 2：单独运行姿态控制测试**
+- [x] **步骤 2：单独运行姿态控制测试**
 
 运行：
 
@@ -249,7 +249,7 @@ PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
 
 预期：全部通过。
 
-- [ ] **步骤 3：构建工作空间**
+- [x] **步骤 3：构建工作空间**
 
 运行：
 
@@ -261,7 +261,7 @@ colcon build --symlink-install
 预期：`xr_rm_input`、`xr_rm_interfaces`、`xr_rm_teleop` 和 `xr_rm_bringup`
 全部构建成功。
 
-- [ ] **步骤 4：使用 mock 启动右臂统一 launch**
+- [x] **步骤 4：使用 mock 启动右臂统一 launch**
 
 运行：
 
@@ -279,7 +279,7 @@ timeout 10s ros2 launch xr_rm_bringup arm_debug.launch.py \
 - 不连接厂商 SDK，不发送真实 CANFD；
 - 除 `timeout` 主动结束产生的退出状态外，没有 Python 异常或 ROS 错误。
 
-- [ ] **步骤 5：检查最终差异**
+- [x] **步骤 5：检查最终差异**
 
 运行：
 

xr_rm_teleop/test/test_joint_control.py

@@ -218,6 +218,114 @@ def test_joint_command_step_limits_acceleration_from_rest() -> None:
     assert target == pytest.approx([math.radians(0.0192)] * 7)
 
 
+def test_joint_command_step_rejects_non_finite_limits() -> None:
+    for max_speed, max_acceleration, dt in (
+        (math.inf, 1.0, 0.1),
+        (1.0, math.inf, 0.1),
+        (1.0, 1.0, math.inf),
+    ):
+        with pytest.raises(ValueError, match="finite and positive"):
+            SingleArmVelocityTeleop._limit_joint_command_step(
+                target=[0.5] * 7,
+                previous_target=[0.0] * 7,
+                previous_velocity=[0.0] * 7,
+                max_speed=max_speed,
+                max_acceleration=max_acceleration,
+                dt=dt,
+            )
+
+
+def test_joint_command_step_arrival_respects_max_speed() -> None:
+    target, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
+        target=[0.5] * 7,
+        previous_target=[0.0] * 7,
+        previous_velocity=[0.0] * 7,
+        max_speed=1.0,
+        max_acceleration=100.0,
+        dt=0.1,
+    )
+
+    assert velocity == pytest.approx([1.0] * 7)
+    assert target == pytest.approx([0.1] * 7)
+
+
+def test_joint_command_step_reverses_with_acceleration_limit() -> None:
+    command = [0.0] * 7
+    velocity = [0.0] * 7
+    for _ in range(5):
+        command, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
+            target=[1.0] * 7,
+            previous_target=command,
+            previous_velocity=velocity,
+            max_speed=1.0,
+            max_acceleration=1.0,
+            dt=0.1,
+        )
+
+    previous_command = list(command)
+    previous_velocity = list(velocity)
+    command, velocity = SingleArmVelocityTeleop._limit_joint_command_step(
+        target=[-1.0] * 7,
+        previous_target=command,
+        previous_velocity=velocity,
+        max_speed=1.0,
+        max_acceleration=1.0,
+        dt=0.1,
+    )
+
+    assert previous_velocity == pytest.approx([0.5] * 7)
+    assert velocity == pytest.approx([0.4] * 7)
+    assert [
+        current - previous
+        for current, previous in zip(command, previous_command)
+    ] == pytest.approx([value * 0.1 for value in velocity])
+    assert all(
+        current > previous
+        for current, previous in zip(command, previous_command)
+    )
+
+
+def test_joint_command_step_brakes_before_fixed_target_without_overshoot() -> None:
+    dt = 1.0 / 90.0
+    max_speed = math.radians(180.0)
+    max_acceleration = math.radians(300.0)
+    target = np.radians(
+        [10.0, -10.0, 3.0, -3.0, 1.0, -1.0, 0.1]
+    ).tolist()
+    command = [0.0] * 7
+    velocity = [0.0] * 7
+
+    for _ in range(180):
+        previous_command = list(command)
+        previous_velocity = list(velocity)
+        command, velocity = (
+            SingleArmVelocityTeleop._limit_joint_command_step(
+                target=target,
+                previous_target=command,
+                previous_velocity=velocity,
+                max_speed=max_speed,
+                max_acceleration=max_acceleration,
+                dt=dt,
+            )
+        )
+
+        for index in range(7):
+            assert min(0.0, target[index]) - 1e-12 <= command[index]
+            assert command[index] <= max(0.0, target[index]) + 1e-12
+            assert abs(velocity[index]) <= max_speed + 1e-12
+            assert (
+                abs(velocity[index] - previous_velocity[index])
+                <= max_acceleration * dt + 1e-12
+            )
+            assert command[index] - previous_command[index] == pytest.approx(
+                velocity[index] * dt,
+                abs=1e-12,
+            )
+
+    assert command == pytest.approx(target, abs=1e-12)
+    assert velocity == pytest.approx([0.0] * 7, abs=1e-12)
+
+
 def test_feedback_fault_blocks_grip_until_release() -> None:
     class FakeClock:
         def now(self):

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -1244,23 +1244,51 @@ class SingleArmVelocityTeleop(Node):
             or len(previous_velocity) != 7
         ):
             raise ValueError("joint command state must contain 7 values")
-        if max_speed <= 0.0 or max_acceleration <= 0.0 or dt <= 0.0:
+        if not all(
-            raise ValueError("joint command limits and dt must be positive")
+            math.isfinite(value) and value > 0.0
-        desired_velocity = np.clip(
+            for value in (max_speed, max_acceleration, dt)
-            (np.asarray(target) - np.asarray(previous_target)) / dt,
+        ):
-            -max_speed,
+            raise ValueError("joint command limits and dt must be finite and positive")
-            max_speed,
+        values = np.asarray([target, previous_target, previous_velocity], dtype=float)
-        )
+        if not np.isfinite(values).all():
+            raise ValueError("joint command contains NaN/Inf")
+
         velocity_step = max_acceleration * dt
-        velocity = np.clip(
+        arrival_distance = velocity_step * dt
-            desired_velocity,
+        limited_target = []
-            np.asarray(previous_velocity) - velocity_step,
+        limited_velocity = []
-            np.asarray(previous_velocity) + velocity_step,
+        for desired_target, last_target, last_velocity in zip(
+            target, previous_target, previous_velocity
+        ):
+            error = desired_target - last_target
+            if abs(last_velocity) <= 1e-12 and abs(error) <= arrival_distance:
+                velocity = _clamp(error / dt, -max_speed, max_speed)
+                position = last_target + velocity * dt
+            else:
+                direction = (
+                    math.copysign(1.0, error) if abs(error) > 1e-12 else 0.0
                 )
-        limited_target = np.asarray(previous_target) + velocity * dt
+                accelerated_speed = min(abs(last_velocity) + velocity_step, max_speed)
+                braking_steps = max(0, math.ceil(accelerated_speed / velocity_step) - 1)
+                braking_distance = accelerated_speed * dt + dt * (
+                    braking_steps * accelerated_speed
+                    - velocity_step * braking_steps * (braking_steps + 1) / 2.0
+                )
+                desired_velocity = direction * max_speed
+                if last_velocity * error > 0.0 and abs(error) <= braking_distance:
+                    desired_velocity = 0.0
+                velocity = _clamp(
+                    desired_velocity,
+                    last_velocity - velocity_step,
+                    last_velocity + velocity_step,
+                )
+                velocity = _clamp(velocity, -max_speed, max_speed)
+                position = last_target + velocity * dt
+            limited_target.append(position)
+            limited_velocity.append(velocity)
         if not np.isfinite(limited_target).all():
             raise ValueError("joint command contains NaN/Inf")
-        return limited_target.tolist(), velocity.tolist()
+        return limited_target, limited_velocity
 
     def _publish_debug(
         self,