diff --git a/i2rt/motor_drivers/dm_driver.py b/i2rt/motor_drivers/dm_driver.py index d9174672..c0208aaf 100644 --- a/i2rt/motor_drivers/dm_driver.py +++ b/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() diff --git a/i2rt/motor_drivers/tests/test_startup.py b/i2rt/motor_drivers/tests/test_startup.py new file mode 100644 index 00000000..81f61e1c --- /dev/null +++ b/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() diff --git a/i2rt/robots/tests/test_gripper_calibration.py b/i2rt/robots/tests/test_gripper_calibration.py new file mode 100644 index 00000000..5096d028 --- /dev/null +++ b/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]) diff --git a/i2rt/robots/utils.py b/i2rt/robots/utils.py index 77866f40..87e39c05 100644 --- a/i2rt/robots/utils.py +++ b/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,7 +818,13 @@ 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}") @@ -822,7 +832,7 @@ def detect_gripper_limits( # 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() @@ -830,7 +840,7 @@ def detect_gripper_limits( 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)