diff --git a/docs/superpowers/plans/2026-07-30-rm75-joint-command-braking.md b/docs/superpowers/plans/2026-07-30-rm75-joint-command-braking.md index a903354..3ca298c 100644 --- a/docs/superpowers/plans/2026-07-30-rm75-joint-command-braking.md +++ b/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：检查最终差异** 运行： diff --git a/xr_rm_teleop/test/test_joint_control.py b/xr_rm_teleop/test/test_joint_control.py index 6dffa92..d88aef4 100644 --- a/xr_rm_teleop/test/test_joint_control.py +++ b/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): 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 b0c940a..413645c 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 @@ -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: - raise ValueError("joint command limits and dt must be positive") - desired_velocity = np.clip( - (np.asarray(target) - np.asarray(previous_target)) / dt, - -max_speed, - max_speed, - ) + if not all( + math.isfinite(value) and value > 0.0 + for value in (max_speed, max_acceleration, dt) + ): + raise ValueError("joint command limits and dt must be finite and positive") + 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( - desired_velocity, - np.asarray(previous_velocity) - velocity_step, - np.asarray(previous_velocity) + velocity_step, - ) - limited_target = np.asarray(previous_target) + velocity * dt + arrival_distance = velocity_step * dt + limited_target = [] + limited_velocity = [] + 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 + ) + 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,