tangger/lerobot
1
0
Fork 1
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

Add more segmented tests (base motor bus & feetech), add feetech protocol 1 support

Browse Source
This commit is contained in:
Simon Alibert
2025-04-14 11:56:53 +02:00
parent e0b292ab51
commit bdbca09cb2
6 changed files with 749 additions and 350 deletions
Download Patch File Download Diff File Expand all files Collapse all files

363
tests/motors/test_feetech.py
View File

@@ -1,3 +1,4 @@
import re
import sys
from typing import Generator
from unittest.mock import MagicMock, patch
@@ -6,7 +7,8 @@ import pytest
import scservo_sdk as scs
from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode
from lerobot.common.motors.feetech import MODEL_NUMBER_TABLE, FeetechMotorsBus
from lerobot.common.motors.feetech import MODEL_NUMBER, MODEL_NUMBER_TABLE, FeetechMotorsBus
from lerobot.common.motors.feetech.tables import STS_SMS_SERIES_CONTROL_TABLE
from lerobot.common.utils.encoding_utils import encode_sign_magnitude
from tests.mocks.mock_feetech import MockMotors, MockPortHandler
@@ -109,8 +111,9 @@ def test_scan_port(mock_motors):
@pytest.mark.parametrize("id_", [1, 2, 3])
def test_ping(id_, mock_motors, dummy_motors):
expected_model_nb = MODEL_NUMBER_TABLE[dummy_motors[f"dummy_{id_}"].model]
addr, length = MODEL_NUMBER
ping_stub = mock_motors.build_ping_stub(id_)
mobel_nb_stub = mock_motors.build_read_stub("Model_Number", id_, expected_model_nb)
mobel_nb_stub = mock_motors.build_read_stub(addr, length, id_, expected_model_nb)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
@@ -126,9 +129,15 @@ def test_ping(id_, mock_motors, dummy_motors):
def test_broadcast_ping(mock_motors, dummy_motors):
models = {m.id: m.model for m in dummy_motors.values()}
expected_model_nbs = {id_: MODEL_NUMBER_TABLE[model] for id_, model in models.items()}
addr, length = MODEL_NUMBER
ping_stub = mock_motors.build_broadcast_ping_stub(list(models))
mobel_nb_stub = mock_motors.build_sync_read_stub("Model_Number", expected_model_nbs)
mobel_nb_stubs = []
expected_model_nbs = {}
for id_, model in models.items():
model_nb = MODEL_NUMBER_TABLE[model]
stub = mock_motors.build_read_stub(addr, length, id_, model_nb)
expected_model_nbs[id_] = model_nb
mobel_nb_stubs.append(stub)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
@@ -139,187 +148,209 @@ def test_broadcast_ping(mock_motors, dummy_motors):
assert ping_model_nbs == expected_model_nbs
assert mock_motors.stubs[ping_stub].called
assert mock_motors.stubs[mobel_nb_stub].called
def test_sync_read_none(mock_motors, dummy_motors):
expected_positions = {
"dummy_1": 1337,
"dummy_2": 42,
"dummy_3": 4016,
}
ids_values = dict(zip([1, 2, 3], expected_positions.values(), strict=True))
stub_name = mock_motors.build_sync_read_stub("Present_Position", ids_values)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
read_positions = motors_bus.sync_read("Present_Position", normalize=False)
assert mock_motors.stubs[stub_name].called
assert read_positions == expected_positions
assert all(mock_motors.stubs[stub].called for stub in mobel_nb_stubs)
@pytest.mark.parametrize(
"id_, position",
"addr, length, id_, value",
[
(1, 1337),
(2, 42),
(3, 4016),
(0, 1, 1, 2),
(10, 2, 2, 999),
(42, 4, 3, 1337),
],
)
def test_sync_read_single_value(id_, position, mock_motors, dummy_motors):
expected_position = {f"dummy_{id_}": position}
stub_name = mock_motors.build_sync_read_stub("Present_Position", {id_: position})
def test__read(addr, length, id_, value, mock_motors, dummy_motors):
stub_name = mock_motors.build_read_stub(addr, length, id_, value)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
read_position = motors_bus.sync_read("Present_Position", f"dummy_{id_}", normalize=False)
read_value, _, _ = motors_bus._read(addr, length, id_)
assert mock_motors.stubs[stub_name].called
assert read_position == expected_position
assert read_value == value
@pytest.mark.parametrize(
"ids, positions",
[
([1], [1337]),
([1, 2], [1337, 42]),
([1, 2, 3], [1337, 42, 4016]),
],
ids=["1 motor", "2 motors", "3 motors"],
) # fmt: skip
def test_sync_read(ids, positions, mock_motors, dummy_motors):
assert len(ids) == len(positions)
names = [f"dummy_{dxl_id}" for dxl_id in ids]
expected_positions = dict(zip(names, positions, strict=True))
ids_values = dict(zip(ids, positions, strict=True))
stub_name = mock_motors.build_sync_read_stub("Present_Position", ids_values)
@pytest.mark.parametrize("raise_on_error", (True, False))
def test__read_error(raise_on_error, mock_motors, dummy_motors):
addr, length, id_, value, error = (10, 4, 1, 1337, scs.ERRBIT_VOLTAGE)
stub_name = mock_motors.build_read_stub(addr, length, id_, value, error=error)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
read_positions = motors_bus.sync_read("Present_Position", names, normalize=False)
assert mock_motors.stubs[stub_name].called
assert read_positions == expected_positions
@pytest.mark.parametrize(
"num_retry, num_invalid_try, pos",
[
(0, 2, 1337),
(2, 3, 42),
(3, 2, 4016),
(2, 1, 999),
],
)
def test_sync_read_num_retry(num_retry, num_invalid_try, pos, mock_motors, dummy_motors):
expected_position = {"dummy_1": pos}
stub_name = mock_motors.build_sync_read_stub(
"Present_Position", {1: pos}, num_invalid_try=num_invalid_try
)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
if num_retry >= num_invalid_try:
pos_dict = motors_bus.sync_read("Present_Position", "dummy_1", normalize=False, num_retry=num_retry)
assert pos_dict == expected_position
if raise_on_error:
with pytest.raises(RuntimeError, match=re.escape("[RxPacketError] Input voltage error!")):
motors_bus._read(addr, length, id_, raise_on_error=raise_on_error)
else:
with pytest.raises(ConnectionError):
_ = motors_bus.sync_read("Present_Position", "dummy_1", normalize=False, num_retry=num_retry)
expected_calls = min(1 + num_retry, 1 + num_invalid_try)
assert mock_motors.stubs[stub_name].calls == expected_calls
@pytest.mark.parametrize(
"data_name, value",
[
("Torque_Enable", 0),
("Torque_Enable", 1),
("Goal_Position", 1337),
("Goal_Position", 42),
],
)
def test_sync_write_single_value(data_name, value, mock_motors, dummy_motors):
ids_values = {m.id: value for m in dummy_motors.values()}
stub_name = mock_motors.build_sync_write_stub(data_name, ids_values)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
motors_bus.sync_write(data_name, value, normalize=False)
assert mock_motors.stubs[stub_name].wait_called()
@pytest.mark.parametrize(
"ids, positions",
[
([1], [1337]),
([1, 2], [1337, 42]),
([1, 2, 3], [1337, 42, 4016]),
],
ids=["1 motor", "2 motors", "3 motors"],
) # fmt: skip
def test_sync_write(ids, positions, mock_motors, dummy_motors):
assert len(ids) == len(positions)
ids_values = dict(zip(ids, positions, strict=True))
stub_name = mock_motors.build_sync_write_stub("Goal_Position", ids_values)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
write_values = {f"dummy_{id_}": pos for id_, pos in ids_values.items()}
motors_bus.sync_write("Goal_Position", write_values, normalize=False)
assert mock_motors.stubs[stub_name].wait_called()
@pytest.mark.parametrize(
"data_name, dxl_id, value",
[
("Torque_Enable", 1, 0),
("Torque_Enable", 1, 1),
("Goal_Position", 2, 1337),
("Goal_Position", 3, 42),
],
)
def test_write(data_name, dxl_id, value, mock_motors, dummy_motors):
stub_name = mock_motors.build_write_stub(data_name, dxl_id, value)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
motors_bus.write(data_name, f"dummy_{dxl_id}", value, normalize=False)
_, _, read_error = motors_bus._read(addr, length, id_, raise_on_error=raise_on_error)
assert read_error == error
assert mock_motors.stubs[stub_name].called
@pytest.mark.parametrize("raise_on_error", (True, False))
def test__read_comm(raise_on_error, mock_motors, dummy_motors):
addr, length, id_, value = (10, 4, 1, 1337)
stub_name = mock_motors.build_read_stub(addr, length, id_, value, reply=False)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
if raise_on_error:
with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")):
motors_bus._read(addr, length, id_, raise_on_error=raise_on_error)
else:
_, read_comm, _ = motors_bus._read(addr, length, id_, raise_on_error=raise_on_error)
assert read_comm == scs.COMM_RX_TIMEOUT
assert mock_motors.stubs[stub_name].called
@pytest.mark.parametrize(
"addr, length, id_, value",
[
(0, 1, 1, 2),
(10, 2, 2, 999),
(42, 4, 3, 1337),
],
)
def test__write(addr, length, id_, value, mock_motors, dummy_motors):
stub_name = mock_motors.build_write_stub(addr, length, id_, value)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
comm, error = motors_bus._write(addr, length, id_, value)
assert mock_motors.stubs[stub_name].called
assert comm == scs.COMM_SUCCESS
assert error == 0
@pytest.mark.parametrize("raise_on_error", (True, False))
def test__write_error(raise_on_error, mock_motors, dummy_motors):
addr, length, id_, value, error = (10, 4, 1, 1337, scs.ERRBIT_VOLTAGE)
stub_name = mock_motors.build_write_stub(addr, length, id_, value, error=error)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
if raise_on_error:
with pytest.raises(RuntimeError, match=re.escape("[RxPacketError] Input voltage error!")):
motors_bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
else:
_, write_error = motors_bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
assert write_error == error
assert mock_motors.stubs[stub_name].called
@pytest.mark.parametrize("raise_on_error", (True, False))
def test__write_comm(raise_on_error, mock_motors, dummy_motors):
addr, length, id_, value = (10, 4, 1, 1337)
stub_name = mock_motors.build_write_stub(addr, length, id_, value, reply=False)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
if raise_on_error:
with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")):
motors_bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
else:
write_comm, _ = motors_bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
assert write_comm == scs.COMM_RX_TIMEOUT
assert mock_motors.stubs[stub_name].called
@pytest.mark.parametrize(
"addr, length, ids_values",
[
(0, 1, {1: 4}),
(10, 2, {1: 1337, 2: 42}),
(42, 4, {1: 1337, 2: 42, 3: 4016}),
],
ids=["1 motor", "2 motors", "3 motors"],
)
def test__sync_read(addr, length, ids_values, mock_motors, dummy_motors):
stub_name = mock_motors.build_sync_read_stub(addr, length, ids_values)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
read_values, _ = motors_bus._sync_read(addr, length, list(ids_values))
assert mock_motors.stubs[stub_name].called
assert read_values == ids_values
@pytest.mark.parametrize("raise_on_error", (True, False))
def test__sync_read_comm(raise_on_error, mock_motors, dummy_motors):
addr, length, ids_values = (10, 4, {1: 1337})
stub_name = mock_motors.build_sync_read_stub(addr, length, ids_values, reply=False)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
if raise_on_error:
with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")):
motors_bus._sync_read(addr, length, list(ids_values), raise_on_error=raise_on_error)
else:
_, read_comm = motors_bus._sync_read(addr, length, list(ids_values), raise_on_error=raise_on_error)
assert read_comm == scs.COMM_RX_TIMEOUT
assert mock_motors.stubs[stub_name].called
@pytest.mark.parametrize(
"addr, length, ids_values",
[
(0, 1, {1: 4}),
(10, 2, {1: 1337, 2: 42}),
(42, 4, {1: 1337, 2: 42, 3: 4016}),
],
ids=["1 motor", "2 motors", "3 motors"],
)
def test__sync_write(addr, length, ids_values, mock_motors, dummy_motors):
stub_name = mock_motors.build_sync_write_stub(addr, length, ids_values)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
comm = motors_bus._sync_write(addr, length, ids_values)
assert mock_motors.stubs[stub_name].wait_called()
assert comm == scs.COMM_SUCCESS
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)
offsets_stub = mock_motors.build_sync_read_stub(
*STS_SMS_SERIES_CONTROL_TABLE["Homing_Offset"], encoded_homings
)
mins_stub = mock_motors.build_sync_read_stub(*STS_SMS_SERIES_CONTROL_TABLE["Min_Position_Limit"], mins)
maxes_stub = mock_motors.build_sync_read_stub(*STS_SMS_SERIES_CONTROL_TABLE["Max_Position_Limit"], maxes)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
@@ -340,9 +371,15 @@ def test_reset_calibration(mock_motors, dummy_motors):
write_mins_stubs = []
write_maxes_stubs = []
for motor in dummy_motors.values():
write_homing_stubs.append(mock_motors.build_write_stub("Homing_Offset", motor.id, 0))
write_mins_stubs.append(mock_motors.build_write_stub("Min_Position_Limit", motor.id, 0))
write_maxes_stubs.append(mock_motors.build_write_stub("Max_Position_Limit", motor.id, 4095))
write_homing_stubs.append(
mock_motors.build_write_stub(*STS_SMS_SERIES_CONTROL_TABLE["Homing_Offset"], motor.id, 0)
)
write_mins_stubs.append(
mock_motors.build_write_stub(*STS_SMS_SERIES_CONTROL_TABLE["Min_Position_Limit"], motor.id, 0)
)
write_maxes_stubs.append(
mock_motors.build_write_stub(*STS_SMS_SERIES_CONTROL_TABLE["Max_Position_Limit"], motor.id, 4095)
)
motors_bus = FeetechMotorsBus(
port=mock_motors.port,
@@ -372,11 +409,15 @@ def test_set_half_turn_homings(mock_motors, dummy_motors):
2: -2005, # 42 - 2047
3: 1625, # 3672 - 2047
}
read_pos_stub = mock_motors.build_sync_read_stub("Present_Position", current_positions)
read_pos_stub = mock_motors.build_sync_read_stub(
*STS_SMS_SERIES_CONTROL_TABLE["Present_Position"], current_positions
)
write_homing_stubs = []
for id_, homing in expected_homings.items():
encoded_homing = encode_sign_magnitude(homing, 11)
stub = mock_motors.build_write_stub("Homing_Offset", id_, encoded_homing)
stub = mock_motors.build_write_stub(
*STS_SMS_SERIES_CONTROL_TABLE["Homing_Offset"], id_, encoded_homing
)
write_homing_stubs.append(stub)
motors_bus = FeetechMotorsBus(
@@ -409,7 +450,9 @@ def test_record_ranges_of_motion(mock_motors, dummy_motors):
"dummy_2": 3600,
"dummy_3": 4002,
}
read_pos_stub = mock_motors.build_sequential_sync_read_stub("Present_Position", positions)
read_pos_stub = mock_motors.build_sequential_sync_read_stub(
*STS_SMS_SERIES_CONTROL_TABLE["Present_Position"], positions
)
with patch("lerobot.common.motors.motors_bus.enter_pressed", side_effect=[False, True]):
motors_bus = FeetechMotorsBus(
port=mock_motors.port,