Avoid replaying feedback torque during startup and calibration

i2rt/motor_drivers/dm_driver.py

@@ -485,9 +485,7 @@ def __init__(
 
             self.absolute_positions = None
             self._motor_on()
-        starting_command = []
-        for motor_state in self.state:
-            starting_command.append(MotorCmd(torque=motor_state.torque))
+        starting_command = [MotorCmd() for _ in self.state]
         logging.info(f"Initializing motorchain with starting command: {starting_command}")
         self.commands = starting_command
         self.command_lock = threading.RLock()

i2rt/motor_drivers/tests/test_startup.py

@@ -0,0 +1,26 @@
+from types import SimpleNamespace
+
+import pytest
+
+from i2rt.motor_drivers.dm_driver import DMChainCanInterface, MotorCmd
+
+
+def test_startup_uses_zero_torque_instead_of_measured_feedback(monkeypatch: pytest.MonkeyPatch) -> None:
+    import numpy as np
+
+    from i2rt.motor_drivers import dm_driver
+
+    interface = SimpleNamespace(close=lambda: None)
+    monkeypatch.setattr(dm_driver, "run_startup_checks", lambda *_args, **_kwargs: None)
+    monkeypatch.setattr(dm_driver, "DMSingleMotorCanInterface", lambda **_kwargs: interface)
+
+    def motor_on(chain: DMChainCanInterface) -> None:
+        chain.state = [SimpleNamespace(torque=7.0)]
+        chain.running = True
+
+    monkeypatch.setattr(DMChainCanInterface, "_motor_on", motor_on)
+    chain = DMChainCanInterface([(1, "DM4310")], np.zeros(1), np.ones(1), start_thread=False)
+    try:
+        assert chain.commands == [MotorCmd()]
+    finally:
+        chain.close()

i2rt/robots/tests/test_gripper_calibration.py

@@ -0,0 +1,39 @@
+from types import SimpleNamespace
+from typing import Any
+
+import numpy as np
+import pytest
+
+from i2rt.robots import utils
+
+
+def test_calibration_holds_arm_and_stops_gripper_without_replaying_feedback(
+    monkeypatch: pytest.MonkeyPatch,
+) -> None:
+    now = 0.0
+    commands = []
+
+    def sleep(seconds: float) -> None:
+        nonlocal now
+        now += seconds
+
+    def set_commands(**kwargs: Any) -> None:
+        commands.append({key: value.copy() for key, value in kwargs.items()})
+
+    chain = SimpleNamespace(
+        motor_list=[1, 2],
+        motor_direction=np.ones(2),
+        read_states=lambda: [SimpleNamespace(pos=0.3, eff=5.0), SimpleNamespace(pos=0.4, eff=6.0)],
+        set_commands=set_commands,
+    )
+    monkeypatch.setattr(utils, "time", SimpleNamespace(time=lambda: now, sleep=sleep))
+    utils.detect_gripper_limits(chain, gripper_index=1)
+
+    assert {float(command["torques"][1]) for command in commands} == {-0.2, 0.0, 0.2}
+    for command in commands:
+        np.testing.assert_array_equal(command["pos"], [0.3, 0.4])
+        np.testing.assert_array_equal(command["vel"], [0.0, 0.0])
+        np.testing.assert_array_equal(command["kp"], [5.0, 0.0])
+        np.testing.assert_array_equal(command["kd"], [0.5, 0.0])
+        assert command["torques"][0] == 0.0
+    np.testing.assert_array_equal(commands[-1]["torques"], [0.0, 0.0])

i2rt/robots/utils.py

@@ -789,6 +789,8 @@ def detect_gripper_limits(
     max_duration: float = 2.0,
     position_threshold: float = 0.01,
     check_interval: float = 0.1,
+    hold_arm_kp: float = 5.0,
+    hold_arm_kd: float = 0.5,
 ) -> Tuple[float, float]:
     """
     Detect gripper limits by applying test torques and monitoring position changes.
@@ -800,9 +802,11 @@ def detect_gripper_limits(
         max_duration: Maximum test duration for each direction (s)
         position_threshold: Minimum position change to consider motor still moving (rad)
         check_interval: Time interval between checks (s)
+        hold_arm_kp: Position gain used to hold non-gripper joints during calibration
+        hold_arm_kd: Damping gain used to hold non-gripper joints during calibration
 
     Returns:
-        List of detected limits [limit1, limit2]
+        Tuple of detected limits [limit1, limit2]
     """
     logger = logging.getLogger(__name__)
     positions = []
@@ -814,23 +818,29 @@ def detect_gripper_limits(
 
     # Record initial position
     initial_states = motor_chain.read_states()
-    init_torque = np.array([state.eff for state in initial_states])
+    hold_pos = np.array([state.pos for state in initial_states])
+    hold_vel = np.zeros(num_motors)
+    hold_kp = np.zeros(num_motors)
+    hold_kd = np.zeros(num_motors)
+    arm_indices = np.arange(num_motors) != gripper_index
+    hold_kp[arm_indices] = hold_arm_kp
+    hold_kd[arm_indices] = hold_arm_kd
     initial_pos = initial_states[gripper_index].pos
     positions.append(initial_pos)
     logger.info(f"Gripper calibration starting from position: {initial_pos:.4f}")
 
     # Test both directions
     for direction in [1, -1]:
         logger.info(f"Testing gripper direction: {direction}")
-        test_torques = init_torque
+        test_torques = zero_torques.copy()
         test_torques[gripper_index] = direction * test_torque
 
         start_time = time.time()
         last_pos = None
         position_stable_count = 0
 
         while time.time() - start_time < max_duration:
-            motor_chain.set_commands(torques=test_torques)
+            motor_chain.set_commands(torques=test_torques, pos=hold_pos, vel=hold_vel, kp=hold_kp, kd=hold_kd)
             time.sleep(check_interval)
 
             states = motor_chain.read_states()
@@ -854,6 +864,8 @@ def detect_gripper_limits(
 
         time.sleep(0.3)
 
+    motor_chain.set_commands(torques=zero_torques, pos=hold_pos, vel=hold_vel, kp=hold_kp, kd=hold_kd)
+
     # Calculate detected limits
     min_pos = min(positions)
     max_pos = max(positions)