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

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):