From e393af2d88e59fe5c3926eab83922178f02fc69b Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Thu, 3 Apr 2025 17:35:10 +0200 Subject: [PATCH] Add is_calibrated test --- tests/motors/test_dynamixel.py | 45 +++++++++++++++++++++++++++++++++- tests/motors/test_feetech.py | 41 ++++++++++++++++++++++++++++++- 2 files changed, 84 insertions(+), 2 deletions(-) diff --git a/tests/motors/test_dynamixel.py b/tests/motors/test_dynamixel.py index 83fe23bc7..9ebbbacb6 100644 --- a/tests/motors/test_dynamixel.py +++ b/tests/motors/test_dynamixel.py @@ -5,7 +5,7 @@ from unittest.mock import MagicMock, patch import dynamixel_sdk as dxl import pytest -from lerobot.common.motors import Motor, MotorNormMode +from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode from lerobot.common.motors.dynamixel import MODEL_NUMBER, DynamixelMotorsBus from lerobot.common.utils.encoding_utils import encode_twos_complement from tests.mocks.mock_dynamixel import MockMotors, MockPortHandler @@ -37,6 +37,24 @@ def dummy_motors() -> dict[str, Motor]: } +@pytest.fixture +def dummy_calibration(dummy_motors) -> dict[str, MotorCalibration]: + drive_modes = [0, 1, 0] + homings = [-709, -2006, 1624] + mins = [43, 27, 145] + maxes = [1335, 3608, 3999] + calibration = {} + for name, motor in dummy_motors.items(): + calibration[name] = MotorCalibration( + id=motor.id, + drive_mode=drive_modes[motor.id - 1], + homing_offset=homings[motor.id - 1], + range_min=mins[motor.id - 1], + range_max=maxes[motor.id - 1], + ) + return calibration + + @pytest.mark.skipif(sys.platform != "darwin", reason=f"No patching needed on {sys.platform=}") def test_autouse_patch(): """Ensures that the autouse fixture correctly patches dxl.PortHandler with MockPortHandler.""" @@ -435,6 +453,31 @@ def test_write_by_name(data_name, dxl_id, value, mock_motors, dummy_motors): assert mock_motors.stubs[stub_name].called +def test_is_calibrated(mock_motors, dummy_motors, dummy_calibration): + drive_modes = {m.id: m.drive_mode for m in dummy_calibration.values()} + encoded_homings = {m.id: encode_twos_complement(m.homing_offset, 4) for m in dummy_calibration.values()} + mins = {m.id: m.range_min for m in dummy_calibration.values()} + maxes = {m.id: m.range_max for m in dummy_calibration.values()} + drive_modes_stub = mock_motors.build_sync_read_stub("Drive_Mode", drive_modes) + offsets_stub = mock_motors.build_sync_read_stub("Homing_Offset", encoded_homings) + mins_stub = mock_motors.build_sync_read_stub("Min_Position_Limit", mins) + maxes_stub = mock_motors.build_sync_read_stub("Max_Position_Limit", maxes) + motors_bus = DynamixelMotorsBus( + port=mock_motors.port, + motors=dummy_motors, + calibration=dummy_calibration, + ) + motors_bus.connect(assert_motors_exist=False) + + is_calibrated = motors_bus.is_calibrated + + assert is_calibrated + assert mock_motors.stubs[drive_modes_stub].called + assert mock_motors.stubs[offsets_stub].called + assert mock_motors.stubs[mins_stub].called + assert mock_motors.stubs[maxes_stub].called + + def test_reset_calibration(mock_motors, dummy_motors): write_homing_stubs = [] write_mins_stubs = [] diff --git a/tests/motors/test_feetech.py b/tests/motors/test_feetech.py index 5a9298c2a..871f102e3 100644 --- a/tests/motors/test_feetech.py +++ b/tests/motors/test_feetech.py @@ -5,7 +5,7 @@ from unittest.mock import MagicMock, patch import pytest import scservo_sdk as scs -from lerobot.common.motors import Motor, MotorNormMode +from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode from lerobot.common.motors.feetech import MODEL_NUMBER, FeetechMotorsBus from lerobot.common.utils.encoding_utils import encode_sign_magnitude from tests.mocks.mock_feetech import MockMotors, MockPortHandler @@ -37,6 +37,23 @@ def dummy_motors() -> dict[str, Motor]: } +@pytest.fixture +def dummy_calibration(dummy_motors) -> dict[str, MotorCalibration]: + homings = [-709, -2006, 1624] + mins = [43, 27, 145] + maxes = [1335, 3608, 3999] + calibration = {} + for name, motor in dummy_motors.items(): + calibration[name] = MotorCalibration( + id=motor.id, + drive_mode=0, + homing_offset=homings[motor.id - 1], + range_min=mins[motor.id - 1], + range_max=maxes[motor.id - 1], + ) + return calibration + + @pytest.mark.skipif(sys.platform != "darwin", reason=f"No patching needed on {sys.platform=}") def test_autouse_patch(): """Ensures that the autouse fixture correctly patches scs.PortHandler with MockPortHandler.""" @@ -456,6 +473,28 @@ def test_write_by_name(data_name, dxl_id, value, mock_motors, dummy_motors): assert mock_motors.stubs[stub_name].called +def test_is_calibrated(mock_motors, dummy_motors, dummy_calibration): + encoded_homings = {m.id: encode_sign_magnitude(m.homing_offset, 11) for m in dummy_calibration.values()} + mins = {m.id: m.range_min for m in dummy_calibration.values()} + maxes = {m.id: m.range_max for m in dummy_calibration.values()} + offsets_stub = mock_motors.build_sync_read_stub("Homing_Offset", encoded_homings) + mins_stub = mock_motors.build_sync_read_stub("Min_Position_Limit", mins) + maxes_stub = mock_motors.build_sync_read_stub("Max_Position_Limit", maxes) + motors_bus = FeetechMotorsBus( + port=mock_motors.port, + motors=dummy_motors, + calibration=dummy_calibration, + ) + motors_bus.connect(assert_motors_exist=False) + + is_calibrated = motors_bus.is_calibrated + + assert is_calibrated + assert mock_motors.stubs[offsets_stub].called + assert mock_motors.stubs[mins_stub].called + assert mock_motors.stubs[maxes_stub].called + + def test_reset_calibration(mock_motors, dummy_motors): write_homing_stubs = [] write_mins_stubs = []