镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
snapshot: sync mint main from current workspace 2026-03-03 16:32:56 +0800
这个提交包含在:
66
tests/motors/test_damiao.py
普通文件
66
tests/motors/test_damiao.py
普通文件
@ -0,0 +1,66 @@
|
||||
"""Minimal test script for Damiao motor with ID 3."""
|
||||
|
||||
import pytest
|
||||
|
||||
from lerobot.utils.import_utils import _can_available
|
||||
|
||||
if not _can_available:
|
||||
pytest.skip("python-can not available", allow_module_level=True)
|
||||
|
||||
from lerobot.motors import Motor
|
||||
from lerobot.motors.damiao import DamiaoMotorsBus
|
||||
|
||||
|
||||
@pytest.mark.skip(reason="Requires physical Damiao motor and CAN interface")
|
||||
def test_damiao_motor():
|
||||
motors = {
|
||||
"joint_3": Motor(
|
||||
id=0x03,
|
||||
model="damiao",
|
||||
norm_mode="degrees",
|
||||
motor_type_str="dm4310",
|
||||
recv_id=0x13,
|
||||
),
|
||||
}
|
||||
|
||||
bus = DamiaoMotorsBus(port="can0", motors=motors)
|
||||
|
||||
try:
|
||||
print("Connecting...")
|
||||
bus.connect()
|
||||
print("✓ Connected")
|
||||
|
||||
print("Enabling torque...")
|
||||
bus.enable_torque()
|
||||
print("✓ Torque enabled")
|
||||
|
||||
print("Reading all states...")
|
||||
states = bus.sync_read_all_states()
|
||||
print(f"✓ States: {states}")
|
||||
|
||||
print("Reading position...")
|
||||
positions = bus.sync_read("Present_Position")
|
||||
print(f"✓ Position: {positions}")
|
||||
|
||||
print("Testing MIT control batch...")
|
||||
current_pos = states["joint_3"]["position"]
|
||||
commands = {"joint_3": (10.0, 0.5, current_pos, 0.0, 0.0)}
|
||||
bus._mit_control_batch(commands)
|
||||
print("✓ MIT control batch sent")
|
||||
|
||||
print("Disabling torque...")
|
||||
bus.disable_torque()
|
||||
print("✓ Torque disabled")
|
||||
|
||||
print("Setting zero position...")
|
||||
bus.set_zero_position()
|
||||
print("✓ Zero position set")
|
||||
|
||||
finally:
|
||||
print("Disconnecting...")
|
||||
bus.disconnect(disable_torque=True)
|
||||
print("✓ Disconnected")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
test_damiao_motor()
|
||||
416
tests/motors/test_dynamixel.py
普通文件
416
tests/motors/test_dynamixel.py
普通文件
@ -0,0 +1,416 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import re
|
||||
import sys
|
||||
from collections.abc import Generator
|
||||
from unittest.mock import MagicMock, patch
|
||||
|
||||
import pytest
|
||||
|
||||
from lerobot.motors import Motor, MotorCalibration, MotorNormMode
|
||||
from lerobot.motors.dynamixel import MODEL_NUMBER_TABLE, DynamixelMotorsBus
|
||||
from lerobot.motors.dynamixel.tables import X_SERIES_CONTROL_TABLE
|
||||
from lerobot.motors.encoding_utils import encode_twos_complement
|
||||
|
||||
try:
|
||||
import dynamixel_sdk as dxl
|
||||
|
||||
from tests.mocks.mock_dynamixel import MockMotors, MockPortHandler
|
||||
except (ImportError, ModuleNotFoundError):
|
||||
pytest.skip("dynamixel_sdk not available", allow_module_level=True)
|
||||
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def patch_port_handler():
|
||||
if sys.platform == "darwin":
|
||||
with patch.object(dxl, "PortHandler", MockPortHandler):
|
||||
yield
|
||||
else:
|
||||
yield
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def mock_motors() -> Generator[MockMotors, None, None]:
|
||||
motors = MockMotors()
|
||||
motors.open()
|
||||
yield motors
|
||||
motors.close()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def dummy_motors() -> dict[str, Motor]:
|
||||
return {
|
||||
"dummy_1": Motor(1, "xl430-w250", MotorNormMode.RANGE_M100_100),
|
||||
"dummy_2": Motor(2, "xm540-w270", MotorNormMode.RANGE_M100_100),
|
||||
"dummy_3": Motor(3, "xl330-m077", MotorNormMode.RANGE_M100_100),
|
||||
}
|
||||
|
||||
|
||||
@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 motor, m in dummy_motors.items():
|
||||
calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=drive_modes[m.id - 1],
|
||||
homing_offset=homings[m.id - 1],
|
||||
range_min=mins[m.id - 1],
|
||||
range_max=maxes[m.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."""
|
||||
assert dxl.PortHandler is MockPortHandler
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"value, length, expected",
|
||||
[
|
||||
(0x12, 1, [0x12]),
|
||||
(0x1234, 2, [0x34, 0x12]),
|
||||
(0x12345678, 4, [0x78, 0x56, 0x34, 0x12]),
|
||||
],
|
||||
ids=[
|
||||
"1 byte",
|
||||
"2 bytes",
|
||||
"4 bytes",
|
||||
],
|
||||
) # fmt: skip
|
||||
def test__split_into_byte_chunks(value, length, expected):
|
||||
bus = DynamixelMotorsBus("", {})
|
||||
assert bus._split_into_byte_chunks(value, length) == expected
|
||||
|
||||
|
||||
def test_abc_implementation(dummy_motors):
|
||||
"""Instantiation should raise an error if the class doesn't implement abstract methods/properties."""
|
||||
DynamixelMotorsBus(port="/dev/dummy-port", motors=dummy_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]
|
||||
stub = mock_motors.build_ping_stub(id_, expected_model_nb)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
ping_model_nb = bus.ping(id_)
|
||||
|
||||
assert ping_model_nb == expected_model_nb
|
||||
assert mock_motors.stubs[stub].called
|
||||
|
||||
|
||||
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()}
|
||||
stub = mock_motors.build_broadcast_ping_stub(expected_model_nbs)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
ping_model_nbs = bus.broadcast_ping()
|
||||
|
||||
assert ping_model_nbs == expected_model_nbs
|
||||
assert mock_motors.stubs[stub].called
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"addr, length, id_, value",
|
||||
[
|
||||
(0, 1, 1, 2),
|
||||
(10, 2, 2, 999),
|
||||
(42, 4, 3, 1337),
|
||||
],
|
||||
)
|
||||
def test__read(addr, length, id_, value, mock_motors, dummy_motors):
|
||||
stub = mock_motors.build_read_stub(addr, length, id_, value)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
read_value, _, _ = bus._read(addr, length, id_)
|
||||
|
||||
assert mock_motors.stubs[stub].called
|
||||
assert read_value == value
|
||||
|
||||
|
||||
@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, dxl.ERRNUM_DATA_LIMIT)
|
||||
stub = mock_motors.build_read_stub(addr, length, id_, value, error=error)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
if raise_on_error:
|
||||
with pytest.raises(
|
||||
RuntimeError, match=re.escape("[RxPacketError] The data value exceeds the limit value!")
|
||||
):
|
||||
bus._read(addr, length, id_, raise_on_error=raise_on_error)
|
||||
else:
|
||||
_, _, read_error = bus._read(addr, length, id_, raise_on_error=raise_on_error)
|
||||
assert read_error == error
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_read_stub(addr, length, id_, value, reply=False)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
if raise_on_error:
|
||||
with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")):
|
||||
bus._read(addr, length, id_, raise_on_error=raise_on_error)
|
||||
else:
|
||||
_, read_comm, _ = bus._read(addr, length, id_, raise_on_error=raise_on_error)
|
||||
assert read_comm == dxl.COMM_RX_TIMEOUT
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_write_stub(addr, length, id_, value)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
comm, error = bus._write(addr, length, id_, value)
|
||||
|
||||
assert mock_motors.stubs[stub].called
|
||||
assert comm == dxl.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, dxl.ERRNUM_DATA_LIMIT)
|
||||
stub = mock_motors.build_write_stub(addr, length, id_, value, error=error)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
if raise_on_error:
|
||||
with pytest.raises(
|
||||
RuntimeError, match=re.escape("[RxPacketError] The data value exceeds the limit value!")
|
||||
):
|
||||
bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
|
||||
else:
|
||||
_, write_error = bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
|
||||
assert write_error == error
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_write_stub(addr, length, id_, value, reply=False)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
if raise_on_error:
|
||||
with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")):
|
||||
bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
|
||||
else:
|
||||
write_comm, _ = bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
|
||||
assert write_comm == dxl.COMM_RX_TIMEOUT
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_sync_read_stub(addr, length, ids_values)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
read_values, _ = bus._sync_read(addr, length, list(ids_values))
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_sync_read_stub(addr, length, ids_values, reply=False)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
if raise_on_error:
|
||||
with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")):
|
||||
bus._sync_read(addr, length, list(ids_values), raise_on_error=raise_on_error)
|
||||
else:
|
||||
_, read_comm = bus._sync_read(addr, length, list(ids_values), raise_on_error=raise_on_error)
|
||||
assert read_comm == dxl.COMM_RX_TIMEOUT
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_sync_write_stub(addr, length, ids_values)
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
comm = bus._sync_write(addr, length, ids_values)
|
||||
|
||||
assert mock_motors.stubs[stub].wait_called()
|
||||
assert comm == dxl.COMM_SUCCESS
|
||||
|
||||
|
||||
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(*X_SERIES_CONTROL_TABLE["Drive_Mode"], drive_modes)
|
||||
offsets_stub = mock_motors.build_sync_read_stub(*X_SERIES_CONTROL_TABLE["Homing_Offset"], encoded_homings)
|
||||
mins_stub = mock_motors.build_sync_read_stub(*X_SERIES_CONTROL_TABLE["Min_Position_Limit"], mins)
|
||||
maxes_stub = mock_motors.build_sync_read_stub(*X_SERIES_CONTROL_TABLE["Max_Position_Limit"], maxes)
|
||||
bus = DynamixelMotorsBus(
|
||||
port=mock_motors.port,
|
||||
motors=dummy_motors,
|
||||
calibration=dummy_calibration,
|
||||
)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
is_calibrated = 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 = []
|
||||
write_maxes_stubs = []
|
||||
for motor in dummy_motors.values():
|
||||
write_homing_stubs.append(
|
||||
mock_motors.build_write_stub(*X_SERIES_CONTROL_TABLE["Homing_Offset"], motor.id, 0)
|
||||
)
|
||||
write_mins_stubs.append(
|
||||
mock_motors.build_write_stub(*X_SERIES_CONTROL_TABLE["Min_Position_Limit"], motor.id, 0)
|
||||
)
|
||||
write_maxes_stubs.append(
|
||||
mock_motors.build_write_stub(*X_SERIES_CONTROL_TABLE["Max_Position_Limit"], motor.id, 4095)
|
||||
)
|
||||
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
bus.reset_calibration()
|
||||
|
||||
assert all(mock_motors.stubs[stub].called for stub in write_homing_stubs)
|
||||
assert all(mock_motors.stubs[stub].called for stub in write_mins_stubs)
|
||||
assert all(mock_motors.stubs[stub].called for stub in write_maxes_stubs)
|
||||
|
||||
|
||||
def test_set_half_turn_homings(mock_motors, dummy_motors):
|
||||
"""
|
||||
For this test, we assume that the homing offsets are already 0 such that
|
||||
Present_Position == Actual_Position
|
||||
"""
|
||||
current_positions = {
|
||||
1: 1337,
|
||||
2: 42,
|
||||
3: 3672,
|
||||
}
|
||||
expected_homings = {
|
||||
1: 710, # 2047 - 1337
|
||||
2: 2005, # 2047 - 42
|
||||
3: -1625, # 2047 - 3672
|
||||
}
|
||||
read_pos_stub = mock_motors.build_sync_read_stub(
|
||||
*X_SERIES_CONTROL_TABLE["Present_Position"], current_positions
|
||||
)
|
||||
write_homing_stubs = []
|
||||
for id_, homing in expected_homings.items():
|
||||
encoded_homing = encode_twos_complement(homing, 4)
|
||||
stub = mock_motors.build_write_stub(*X_SERIES_CONTROL_TABLE["Homing_Offset"], id_, encoded_homing)
|
||||
write_homing_stubs.append(stub)
|
||||
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
bus.reset_calibration = MagicMock()
|
||||
|
||||
bus.set_half_turn_homings()
|
||||
|
||||
bus.reset_calibration.assert_called_once()
|
||||
assert mock_motors.stubs[read_pos_stub].called
|
||||
assert all(mock_motors.stubs[stub].called for stub in write_homing_stubs)
|
||||
|
||||
|
||||
def test_record_ranges_of_motion(mock_motors, dummy_motors):
|
||||
positions = {
|
||||
1: [351, 42, 1337],
|
||||
2: [28, 3600, 2444],
|
||||
3: [4002, 2999, 146],
|
||||
}
|
||||
expected_mins = {
|
||||
"dummy_1": 42,
|
||||
"dummy_2": 28,
|
||||
"dummy_3": 146,
|
||||
}
|
||||
expected_maxes = {
|
||||
"dummy_1": 1337,
|
||||
"dummy_2": 3600,
|
||||
"dummy_3": 4002,
|
||||
}
|
||||
read_pos_stub = mock_motors.build_sequential_sync_read_stub(
|
||||
*X_SERIES_CONTROL_TABLE["Present_Position"], positions
|
||||
)
|
||||
with patch("lerobot.motors.motors_bus.enter_pressed", side_effect=[False, True]):
|
||||
bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
mins, maxes = bus.record_ranges_of_motion(display_values=False)
|
||||
|
||||
assert mock_motors.stubs[read_pos_stub].calls == 3
|
||||
assert mins == expected_mins
|
||||
assert maxes == expected_maxes
|
||||
459
tests/motors/test_feetech.py
普通文件
459
tests/motors/test_feetech.py
普通文件
@ -0,0 +1,459 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import re
|
||||
import sys
|
||||
from collections.abc import Generator
|
||||
from unittest.mock import MagicMock, patch
|
||||
|
||||
import pytest
|
||||
|
||||
from lerobot.motors import Motor, MotorCalibration, MotorNormMode
|
||||
from lerobot.motors.encoding_utils import encode_sign_magnitude
|
||||
from lerobot.motors.feetech import MODEL_NUMBER, MODEL_NUMBER_TABLE, FeetechMotorsBus
|
||||
from lerobot.motors.feetech.tables import STS_SMS_SERIES_CONTROL_TABLE
|
||||
|
||||
try:
|
||||
import scservo_sdk as scs
|
||||
|
||||
from tests.mocks.mock_feetech import MockMotors, MockPortHandler
|
||||
except (ImportError, ModuleNotFoundError):
|
||||
pytest.skip("scservo_sdk not available", allow_module_level=True)
|
||||
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def patch_port_handler():
|
||||
if sys.platform == "darwin":
|
||||
with patch.object(scs, "PortHandler", MockPortHandler):
|
||||
yield
|
||||
else:
|
||||
yield
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def mock_motors() -> Generator[MockMotors, None, None]:
|
||||
motors = MockMotors()
|
||||
motors.open()
|
||||
yield motors
|
||||
motors.close()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def dummy_motors() -> dict[str, Motor]:
|
||||
return {
|
||||
"dummy_1": Motor(1, "sts3215", MotorNormMode.RANGE_M100_100),
|
||||
"dummy_2": Motor(2, "sts3215", MotorNormMode.RANGE_M100_100),
|
||||
"dummy_3": Motor(3, "sts3215", MotorNormMode.RANGE_M100_100),
|
||||
}
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def dummy_calibration(dummy_motors) -> dict[str, MotorCalibration]:
|
||||
homings = [-709, -2006, 1624]
|
||||
mins = [43, 27, 145]
|
||||
maxes = [1335, 3608, 3999]
|
||||
calibration = {}
|
||||
for motor, m in dummy_motors.items():
|
||||
calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=0,
|
||||
homing_offset=homings[m.id - 1],
|
||||
range_min=mins[m.id - 1],
|
||||
range_max=maxes[m.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."""
|
||||
assert scs.PortHandler is MockPortHandler
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"protocol, value, length, expected",
|
||||
[
|
||||
(0, 0x12, 1, [0x12]),
|
||||
(1, 0x12, 1, [0x12]),
|
||||
(0, 0x1234, 2, [0x34, 0x12]),
|
||||
(1, 0x1234, 2, [0x12, 0x34]),
|
||||
(0, 0x12345678, 4, [0x78, 0x56, 0x34, 0x12]),
|
||||
(1, 0x12345678, 4, [0x56, 0x78, 0x12, 0x34]),
|
||||
],
|
||||
ids=[
|
||||
"P0: 1 byte",
|
||||
"P1: 1 byte",
|
||||
"P0: 2 bytes",
|
||||
"P1: 2 bytes",
|
||||
"P0: 4 bytes",
|
||||
"P1: 4 bytes",
|
||||
],
|
||||
) # fmt: skip
|
||||
def test__split_into_byte_chunks(protocol, value, length, expected):
|
||||
bus = FeetechMotorsBus("", {}, protocol_version=protocol)
|
||||
assert bus._split_into_byte_chunks(value, length) == expected
|
||||
|
||||
|
||||
def test_abc_implementation(dummy_motors):
|
||||
"""Instantiation should raise an error if the class doesn't implement abstract methods/properties."""
|
||||
FeetechMotorsBus(port="/dev/dummy-port", motors=dummy_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(addr, length, id_, expected_model_nb)
|
||||
bus = FeetechMotorsBus(
|
||||
port=mock_motors.port,
|
||||
motors=dummy_motors,
|
||||
)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
ping_model_nb = bus.ping(id_)
|
||||
|
||||
assert ping_model_nb == expected_model_nb
|
||||
assert mock_motors.stubs[ping_stub].called
|
||||
assert mock_motors.stubs[mobel_nb_stub].called
|
||||
|
||||
|
||||
def test_broadcast_ping(mock_motors, dummy_motors):
|
||||
models = {m.id: m.model for m in dummy_motors.values()}
|
||||
addr, length = MODEL_NUMBER
|
||||
ping_stub = mock_motors.build_broadcast_ping_stub(list(models))
|
||||
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)
|
||||
bus = FeetechMotorsBus(
|
||||
port=mock_motors.port,
|
||||
motors=dummy_motors,
|
||||
)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
ping_model_nbs = bus.broadcast_ping()
|
||||
|
||||
assert ping_model_nbs == expected_model_nbs
|
||||
assert mock_motors.stubs[ping_stub].called
|
||||
assert all(mock_motors.stubs[stub].called for stub in mobel_nb_stubs)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"addr, length, id_, value",
|
||||
[
|
||||
(0, 1, 1, 2),
|
||||
(10, 2, 2, 999),
|
||||
(42, 4, 3, 1337),
|
||||
],
|
||||
)
|
||||
def test__read(addr, length, id_, value, mock_motors, dummy_motors):
|
||||
stub = mock_motors.build_read_stub(addr, length, id_, value)
|
||||
bus = FeetechMotorsBus(
|
||||
port=mock_motors.port,
|
||||
motors=dummy_motors,
|
||||
)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
read_value, _, _ = bus._read(addr, length, id_)
|
||||
|
||||
assert mock_motors.stubs[stub].called
|
||||
assert read_value == value
|
||||
|
||||
|
||||
@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 = mock_motors.build_read_stub(addr, length, id_, value, error=error)
|
||||
bus = FeetechMotorsBus(
|
||||
port=mock_motors.port,
|
||||
motors=dummy_motors,
|
||||
)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
if raise_on_error:
|
||||
with pytest.raises(RuntimeError, match=re.escape("[RxPacketError] Input voltage error!")):
|
||||
bus._read(addr, length, id_, raise_on_error=raise_on_error)
|
||||
else:
|
||||
_, _, read_error = bus._read(addr, length, id_, raise_on_error=raise_on_error)
|
||||
assert read_error == error
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_read_stub(addr, length, id_, value, reply=False)
|
||||
bus = FeetechMotorsBus(
|
||||
port=mock_motors.port,
|
||||
motors=dummy_motors,
|
||||
)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
if raise_on_error:
|
||||
with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")):
|
||||
bus._read(addr, length, id_, raise_on_error=raise_on_error)
|
||||
else:
|
||||
_, read_comm, _ = bus._read(addr, length, id_, raise_on_error=raise_on_error)
|
||||
assert read_comm == scs.COMM_RX_TIMEOUT
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_write_stub(addr, length, id_, value)
|
||||
bus = FeetechMotorsBus(
|
||||
port=mock_motors.port,
|
||||
motors=dummy_motors,
|
||||
)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
comm, error = bus._write(addr, length, id_, value)
|
||||
|
||||
assert mock_motors.stubs[stub].wait_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 = mock_motors.build_write_stub(addr, length, id_, value, error=error)
|
||||
bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
if raise_on_error:
|
||||
with pytest.raises(RuntimeError, match=re.escape("[RxPacketError] Input voltage error!")):
|
||||
bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
|
||||
else:
|
||||
_, write_error = bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
|
||||
assert write_error == error
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_write_stub(addr, length, id_, value, reply=False)
|
||||
bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
if raise_on_error:
|
||||
with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")):
|
||||
bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
|
||||
else:
|
||||
write_comm, _ = bus._write(addr, length, id_, value, raise_on_error=raise_on_error)
|
||||
assert write_comm == scs.COMM_RX_TIMEOUT
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_sync_read_stub(addr, length, ids_values)
|
||||
bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
read_values, _ = bus._sync_read(addr, length, list(ids_values))
|
||||
|
||||
assert mock_motors.stubs[stub].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 = mock_motors.build_sync_read_stub(addr, length, ids_values, reply=False)
|
||||
bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
if raise_on_error:
|
||||
with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")):
|
||||
bus._sync_read(addr, length, list(ids_values), raise_on_error=raise_on_error)
|
||||
else:
|
||||
_, read_comm = 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].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 = mock_motors.build_sync_write_stub(addr, length, ids_values)
|
||||
bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
comm = bus._sync_write(addr, length, ids_values)
|
||||
|
||||
assert mock_motors.stubs[stub].wait_called()
|
||||
assert comm == scs.COMM_SUCCESS
|
||||
|
||||
|
||||
def test_is_calibrated(mock_motors, dummy_motors, dummy_calibration):
|
||||
mins_stubs, maxes_stubs, homings_stubs = [], [], []
|
||||
for cal in dummy_calibration.values():
|
||||
mins_stubs.append(
|
||||
mock_motors.build_read_stub(
|
||||
*STS_SMS_SERIES_CONTROL_TABLE["Min_Position_Limit"], cal.id, cal.range_min
|
||||
)
|
||||
)
|
||||
maxes_stubs.append(
|
||||
mock_motors.build_read_stub(
|
||||
*STS_SMS_SERIES_CONTROL_TABLE["Max_Position_Limit"], cal.id, cal.range_max
|
||||
)
|
||||
)
|
||||
homings_stubs.append(
|
||||
mock_motors.build_read_stub(
|
||||
*STS_SMS_SERIES_CONTROL_TABLE["Homing_Offset"],
|
||||
cal.id,
|
||||
encode_sign_magnitude(cal.homing_offset, 11),
|
||||
)
|
||||
)
|
||||
|
||||
bus = FeetechMotorsBus(
|
||||
port=mock_motors.port,
|
||||
motors=dummy_motors,
|
||||
calibration=dummy_calibration,
|
||||
)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
is_calibrated = bus.is_calibrated
|
||||
|
||||
assert is_calibrated
|
||||
assert all(mock_motors.stubs[stub].called for stub in mins_stubs)
|
||||
assert all(mock_motors.stubs[stub].called for stub in maxes_stubs)
|
||||
assert all(mock_motors.stubs[stub].called for stub in homings_stubs)
|
||||
|
||||
|
||||
def test_reset_calibration(mock_motors, dummy_motors):
|
||||
write_homing_stubs = []
|
||||
write_mins_stubs = []
|
||||
write_maxes_stubs = []
|
||||
for motor in dummy_motors.values():
|
||||
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)
|
||||
)
|
||||
|
||||
bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
bus.reset_calibration()
|
||||
|
||||
assert all(mock_motors.stubs[stub].wait_called() for stub in write_homing_stubs)
|
||||
assert all(mock_motors.stubs[stub].wait_called() for stub in write_mins_stubs)
|
||||
assert all(mock_motors.stubs[stub].wait_called() for stub in write_maxes_stubs)
|
||||
|
||||
|
||||
def test_set_half_turn_homings(mock_motors, dummy_motors):
|
||||
"""
|
||||
For this test, we assume that the homing offsets are already 0 such that
|
||||
Present_Position == Actual_Position
|
||||
"""
|
||||
current_positions = {
|
||||
1: 1337,
|
||||
2: 42,
|
||||
3: 3672,
|
||||
}
|
||||
expected_homings = {
|
||||
1: -710, # 1337 - 2047
|
||||
2: -2005, # 42 - 2047
|
||||
3: 1625, # 3672 - 2047
|
||||
}
|
||||
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(
|
||||
*STS_SMS_SERIES_CONTROL_TABLE["Homing_Offset"], id_, encoded_homing
|
||||
)
|
||||
write_homing_stubs.append(stub)
|
||||
|
||||
bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
bus.reset_calibration = MagicMock()
|
||||
|
||||
bus.set_half_turn_homings()
|
||||
|
||||
bus.reset_calibration.assert_called_once()
|
||||
assert mock_motors.stubs[read_pos_stub].called
|
||||
assert all(mock_motors.stubs[stub].wait_called() for stub in write_homing_stubs)
|
||||
|
||||
|
||||
def test_record_ranges_of_motion(mock_motors, dummy_motors):
|
||||
positions = {
|
||||
1: [351, 42, 1337],
|
||||
2: [28, 3600, 2444],
|
||||
3: [4002, 2999, 146],
|
||||
}
|
||||
expected_mins = {
|
||||
"dummy_1": 42,
|
||||
"dummy_2": 28,
|
||||
"dummy_3": 146,
|
||||
}
|
||||
expected_maxes = {
|
||||
"dummy_1": 1337,
|
||||
"dummy_2": 3600,
|
||||
"dummy_3": 4002,
|
||||
}
|
||||
stub = mock_motors.build_sequential_sync_read_stub(
|
||||
*STS_SMS_SERIES_CONTROL_TABLE["Present_Position"], positions
|
||||
)
|
||||
with patch("lerobot.motors.motors_bus.enter_pressed", side_effect=[False, True]):
|
||||
bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
|
||||
mins, maxes = bus.record_ranges_of_motion(display_values=False)
|
||||
|
||||
assert mock_motors.stubs[stub].calls == 3
|
||||
assert mins == expected_mins
|
||||
assert maxes == expected_maxes
|
||||
358
tests/motors/test_motors_bus.py
普通文件
358
tests/motors/test_motors_bus.py
普通文件
@ -0,0 +1,358 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import re
|
||||
from unittest.mock import patch
|
||||
|
||||
import pytest
|
||||
|
||||
from lerobot.motors.motors_bus import (
|
||||
Motor,
|
||||
MotorNormMode,
|
||||
assert_same_address,
|
||||
get_address,
|
||||
get_ctrl_table,
|
||||
)
|
||||
from tests.mocks.mock_motors_bus import (
|
||||
DUMMY_CTRL_TABLE_1,
|
||||
DUMMY_CTRL_TABLE_2,
|
||||
DUMMY_MODEL_CTRL_TABLE,
|
||||
MockMotorsBus,
|
||||
)
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def dummy_motors() -> dict[str, Motor]:
|
||||
return {
|
||||
"dummy_1": Motor(1, "model_2", MotorNormMode.RANGE_M100_100),
|
||||
"dummy_2": Motor(2, "model_3", MotorNormMode.RANGE_M100_100),
|
||||
"dummy_3": Motor(3, "model_2", MotorNormMode.RANGE_0_100),
|
||||
}
|
||||
|
||||
|
||||
def test_get_ctrl_table():
|
||||
model = "model_1"
|
||||
ctrl_table = get_ctrl_table(DUMMY_MODEL_CTRL_TABLE, model)
|
||||
assert ctrl_table == DUMMY_CTRL_TABLE_1
|
||||
|
||||
|
||||
def test_get_ctrl_table_error():
|
||||
model = "model_99"
|
||||
with pytest.raises(KeyError, match=f"Control table for {model=} not found."):
|
||||
get_ctrl_table(DUMMY_MODEL_CTRL_TABLE, model)
|
||||
|
||||
|
||||
def test_get_address():
|
||||
addr, n_bytes = get_address(DUMMY_MODEL_CTRL_TABLE, "model_1", "Firmware_Version")
|
||||
assert addr == 0
|
||||
assert n_bytes == 1
|
||||
|
||||
|
||||
def test_get_address_error():
|
||||
model = "model_1"
|
||||
data_name = "Lock"
|
||||
with pytest.raises(KeyError, match=f"Address for '{data_name}' not found in {model} control table."):
|
||||
get_address(DUMMY_MODEL_CTRL_TABLE, "model_1", data_name)
|
||||
|
||||
|
||||
def test_assert_same_address():
|
||||
models = ["model_1", "model_2"]
|
||||
assert_same_address(DUMMY_MODEL_CTRL_TABLE, models, "Present_Position")
|
||||
|
||||
|
||||
def test_assert_same_length_different_addresses():
|
||||
models = ["model_1", "model_2"]
|
||||
with pytest.raises(
|
||||
NotImplementedError,
|
||||
match=re.escape("At least two motor models use a different address"),
|
||||
):
|
||||
assert_same_address(DUMMY_MODEL_CTRL_TABLE, models, "Model_Number")
|
||||
|
||||
|
||||
def test_assert_same_address_different_length():
|
||||
models = ["model_1", "model_2"]
|
||||
with pytest.raises(
|
||||
NotImplementedError,
|
||||
match=re.escape("At least two motor models use a different bytes representation"),
|
||||
):
|
||||
assert_same_address(DUMMY_MODEL_CTRL_TABLE, models, "Goal_Position")
|
||||
|
||||
|
||||
def test__serialize_data_invalid_length():
|
||||
bus = MockMotorsBus("", {})
|
||||
with pytest.raises(NotImplementedError):
|
||||
bus._serialize_data(100, 3)
|
||||
|
||||
|
||||
def test__serialize_data_negative_numbers():
|
||||
bus = MockMotorsBus("", {})
|
||||
with pytest.raises(ValueError):
|
||||
bus._serialize_data(-1, 1)
|
||||
|
||||
|
||||
def test__serialize_data_large_number():
|
||||
bus = MockMotorsBus("", {})
|
||||
with pytest.raises(ValueError):
|
||||
bus._serialize_data(2**32, 4) # 4-byte max is 0xFFFFFFFF
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"data_name, id_, value",
|
||||
[
|
||||
("Firmware_Version", 1, 14),
|
||||
("Model_Number", 1, 5678),
|
||||
("Present_Position", 2, 1337),
|
||||
("Present_Velocity", 3, 42),
|
||||
],
|
||||
)
|
||||
def test_read(data_name, id_, value, dummy_motors):
|
||||
bus = MockMotorsBus("/dev/dummy-port", dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
addr, length = DUMMY_CTRL_TABLE_2[data_name]
|
||||
|
||||
with (
|
||||
patch.object(MockMotorsBus, "_read", return_value=(value, 0, 0)) as mock__read,
|
||||
patch.object(MockMotorsBus, "_decode_sign", return_value={id_: value}) as mock__decode_sign,
|
||||
patch.object(MockMotorsBus, "_normalize", return_value={id_: value}) as mock__normalize,
|
||||
):
|
||||
returned_value = bus.read(data_name, f"dummy_{id_}")
|
||||
|
||||
assert returned_value == value
|
||||
mock__read.assert_called_once_with(
|
||||
addr,
|
||||
length,
|
||||
id_,
|
||||
num_retry=0,
|
||||
raise_on_error=True,
|
||||
err_msg=f"Failed to read '{data_name}' on {id_=} after 1 tries.",
|
||||
)
|
||||
mock__decode_sign.assert_called_once_with(data_name, {id_: value})
|
||||
if data_name in bus.normalized_data:
|
||||
mock__normalize.assert_called_once_with({id_: value})
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"data_name, id_, value",
|
||||
[
|
||||
("Goal_Position", 1, 1337),
|
||||
("Goal_Velocity", 2, 3682),
|
||||
("Lock", 3, 1),
|
||||
],
|
||||
)
|
||||
def test_write(data_name, id_, value, dummy_motors):
|
||||
bus = MockMotorsBus("/dev/dummy-port", dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
addr, length = DUMMY_CTRL_TABLE_2[data_name]
|
||||
|
||||
with (
|
||||
patch.object(MockMotorsBus, "_write", return_value=(0, 0)) as mock__write,
|
||||
patch.object(MockMotorsBus, "_encode_sign", return_value={id_: value}) as mock__encode_sign,
|
||||
patch.object(MockMotorsBus, "_unnormalize", return_value={id_: value}) as mock__unnormalize,
|
||||
):
|
||||
bus.write(data_name, f"dummy_{id_}", value)
|
||||
|
||||
mock__write.assert_called_once_with(
|
||||
addr,
|
||||
length,
|
||||
id_,
|
||||
value,
|
||||
num_retry=0,
|
||||
raise_on_error=True,
|
||||
err_msg=f"Failed to write '{data_name}' on {id_=} with '{value}' after 1 tries.",
|
||||
)
|
||||
mock__encode_sign.assert_called_once_with(data_name, {id_: value})
|
||||
if data_name in bus.normalized_data:
|
||||
mock__unnormalize.assert_called_once_with({id_: value})
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"data_name, id_, value",
|
||||
[
|
||||
("Firmware_Version", 1, 14),
|
||||
("Model_Number", 1, 5678),
|
||||
("Present_Position", 2, 1337),
|
||||
("Present_Velocity", 3, 42),
|
||||
],
|
||||
)
|
||||
def test_sync_read_by_str(data_name, id_, value, dummy_motors):
|
||||
bus = MockMotorsBus("/dev/dummy-port", dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
addr, length = DUMMY_CTRL_TABLE_2[data_name]
|
||||
ids = [id_]
|
||||
expected_value = {f"dummy_{id_}": value}
|
||||
|
||||
with (
|
||||
patch.object(MockMotorsBus, "_sync_read", return_value=({id_: value}, 0)) as mock__sync_read,
|
||||
patch.object(MockMotorsBus, "_decode_sign", return_value={id_: value}) as mock__decode_sign,
|
||||
patch.object(MockMotorsBus, "_normalize", return_value={id_: value}) as mock__normalize,
|
||||
):
|
||||
returned_dict = bus.sync_read(data_name, f"dummy_{id_}")
|
||||
|
||||
assert returned_dict == expected_value
|
||||
mock__sync_read.assert_called_once_with(
|
||||
addr,
|
||||
length,
|
||||
ids,
|
||||
num_retry=0,
|
||||
raise_on_error=True,
|
||||
err_msg=f"Failed to sync read '{data_name}' on {ids=} after 1 tries.",
|
||||
)
|
||||
mock__decode_sign.assert_called_once_with(data_name, {id_: value})
|
||||
if data_name in bus.normalized_data:
|
||||
mock__normalize.assert_called_once_with({id_: value})
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"data_name, ids_values",
|
||||
[
|
||||
("Model_Number", {1: 5678}),
|
||||
("Present_Position", {1: 1337, 2: 42}),
|
||||
("Present_Velocity", {1: 1337, 2: 42, 3: 4016}),
|
||||
],
|
||||
ids=["1 motor", "2 motors", "3 motors"],
|
||||
)
|
||||
def test_sync_read_by_list(data_name, ids_values, dummy_motors):
|
||||
bus = MockMotorsBus("/dev/dummy-port", dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
addr, length = DUMMY_CTRL_TABLE_2[data_name]
|
||||
ids = list(ids_values)
|
||||
expected_values = {f"dummy_{id_}": val for id_, val in ids_values.items()}
|
||||
|
||||
with (
|
||||
patch.object(MockMotorsBus, "_sync_read", return_value=(ids_values, 0)) as mock__sync_read,
|
||||
patch.object(MockMotorsBus, "_decode_sign", return_value=ids_values) as mock__decode_sign,
|
||||
patch.object(MockMotorsBus, "_normalize", return_value=ids_values) as mock__normalize,
|
||||
):
|
||||
returned_dict = bus.sync_read(data_name, [f"dummy_{id_}" for id_ in ids])
|
||||
|
||||
assert returned_dict == expected_values
|
||||
mock__sync_read.assert_called_once_with(
|
||||
addr,
|
||||
length,
|
||||
ids,
|
||||
num_retry=0,
|
||||
raise_on_error=True,
|
||||
err_msg=f"Failed to sync read '{data_name}' on {ids=} after 1 tries.",
|
||||
)
|
||||
mock__decode_sign.assert_called_once_with(data_name, ids_values)
|
||||
if data_name in bus.normalized_data:
|
||||
mock__normalize.assert_called_once_with(ids_values)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"data_name, ids_values",
|
||||
[
|
||||
("Model_Number", {1: 5678, 2: 5799, 3: 5678}),
|
||||
("Present_Position", {1: 1337, 2: 42, 3: 4016}),
|
||||
("Goal_Position", {1: 4008, 2: 199, 3: 3446}),
|
||||
],
|
||||
ids=["Model_Number", "Present_Position", "Goal_Position"],
|
||||
)
|
||||
def test_sync_read_by_none(data_name, ids_values, dummy_motors):
|
||||
bus = MockMotorsBus("/dev/dummy-port", dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
addr, length = DUMMY_CTRL_TABLE_2[data_name]
|
||||
ids = list(ids_values)
|
||||
expected_values = {f"dummy_{id_}": val for id_, val in ids_values.items()}
|
||||
|
||||
with (
|
||||
patch.object(MockMotorsBus, "_sync_read", return_value=(ids_values, 0)) as mock__sync_read,
|
||||
patch.object(MockMotorsBus, "_decode_sign", return_value=ids_values) as mock__decode_sign,
|
||||
patch.object(MockMotorsBus, "_normalize", return_value=ids_values) as mock__normalize,
|
||||
):
|
||||
returned_dict = bus.sync_read(data_name)
|
||||
|
||||
assert returned_dict == expected_values
|
||||
mock__sync_read.assert_called_once_with(
|
||||
addr,
|
||||
length,
|
||||
ids,
|
||||
num_retry=0,
|
||||
raise_on_error=True,
|
||||
err_msg=f"Failed to sync read '{data_name}' on {ids=} after 1 tries.",
|
||||
)
|
||||
mock__decode_sign.assert_called_once_with(data_name, ids_values)
|
||||
if data_name in bus.normalized_data:
|
||||
mock__normalize.assert_called_once_with(ids_values)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"data_name, value",
|
||||
[
|
||||
("Goal_Position", 500),
|
||||
("Goal_Velocity", 4010),
|
||||
("Lock", 0),
|
||||
],
|
||||
)
|
||||
def test_sync_write_by_single_value(data_name, value, dummy_motors):
|
||||
bus = MockMotorsBus("/dev/dummy-port", dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
addr, length = DUMMY_CTRL_TABLE_2[data_name]
|
||||
ids_values = {m.id: value for m in dummy_motors.values()}
|
||||
|
||||
with (
|
||||
patch.object(MockMotorsBus, "_sync_write", return_value=(ids_values, 0)) as mock__sync_write,
|
||||
patch.object(MockMotorsBus, "_encode_sign", return_value=ids_values) as mock__encode_sign,
|
||||
patch.object(MockMotorsBus, "_unnormalize", return_value=ids_values) as mock__unnormalize,
|
||||
):
|
||||
bus.sync_write(data_name, value)
|
||||
|
||||
mock__sync_write.assert_called_once_with(
|
||||
addr,
|
||||
length,
|
||||
ids_values,
|
||||
num_retry=0,
|
||||
raise_on_error=True,
|
||||
err_msg=f"Failed to sync write '{data_name}' with {ids_values=} after 1 tries.",
|
||||
)
|
||||
mock__encode_sign.assert_called_once_with(data_name, ids_values)
|
||||
if data_name in bus.normalized_data:
|
||||
mock__unnormalize.assert_called_once_with(ids_values)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"data_name, ids_values",
|
||||
[
|
||||
("Goal_Position", {1: 1337, 2: 42, 3: 4016}),
|
||||
("Goal_Velocity", {1: 50, 2: 83, 3: 2777}),
|
||||
("Lock", {1: 0, 2: 0, 3: 1}),
|
||||
],
|
||||
ids=["Goal_Position", "Goal_Velocity", "Lock"],
|
||||
)
|
||||
def test_sync_write_by_value_dict(data_name, ids_values, dummy_motors):
|
||||
bus = MockMotorsBus("/dev/dummy-port", dummy_motors)
|
||||
bus.connect(handshake=False)
|
||||
addr, length = DUMMY_CTRL_TABLE_2[data_name]
|
||||
values = {f"dummy_{id_}": val for id_, val in ids_values.items()}
|
||||
|
||||
with (
|
||||
patch.object(MockMotorsBus, "_sync_write", return_value=(ids_values, 0)) as mock__sync_write,
|
||||
patch.object(MockMotorsBus, "_encode_sign", return_value=ids_values) as mock__encode_sign,
|
||||
patch.object(MockMotorsBus, "_unnormalize", return_value=ids_values) as mock__unnormalize,
|
||||
):
|
||||
bus.sync_write(data_name, values)
|
||||
|
||||
mock__sync_write.assert_called_once_with(
|
||||
addr,
|
||||
length,
|
||||
ids_values,
|
||||
num_retry=0,
|
||||
raise_on_error=True,
|
||||
err_msg=f"Failed to sync write '{data_name}' with {ids_values=} after 1 tries.",
|
||||
)
|
||||
mock__encode_sign.assert_called_once_with(data_name, ids_values)
|
||||
if data_name in bus.normalized_data:
|
||||
mock__unnormalize.assert_called_once_with(ids_values)
|
||||
在新工单中引用
屏蔽一个用户