wangzixiang
/
lerobot
mirror of https://github.com/huggingface/lerobot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Update dynamixel with motors bus & tables changes

This commit is contained in:
Simon Alibert 2025-04-08 10:47:11 +02:00
parent e998dddcfa
commit 792c3d961d
3 changed files with 61 additions and 163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
lerobot/common/motors/dynamixel/dynamixel.py
View File

@ -29,7 +29,8 @@ from .tables import (
AVAILABLE_BAUDRATES, AVAILABLE_BAUDRATES,
MODEL_BAUDRATE_TABLE, MODEL_BAUDRATE_TABLE,
MODEL_CONTROL_TABLE, MODEL_CONTROL_TABLE,
MODEL_NUMBER, MODEL_ENCODING_TABLE,
MODEL_NUMBER_TABLE,
MODEL_RESOLUTION, MODEL_RESOLUTION,
) )
@ -37,7 +38,7 @@ PROTOCOL_VERSION = 2.0
BAUDRATE = 1_000_000 BAUDRATE = 1_000_000
DEFAULT_TIMEOUT_MS = 1000 DEFAULT_TIMEOUT_MS = 1000
NORMALIZATION_REQUIRED = ["Goal_Position", "Present_Position"] NORMALIZED_DATA = ["Goal_Position", "Present_Position"]
CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"] CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"]
logger = logging.getLogger(__name__) logger = logging.getLogger(__name__)
@ -94,9 +95,10 @@ class DynamixelMotorsBus(MotorsBus):
default_timeout = DEFAULT_TIMEOUT_MS default_timeout = DEFAULT_TIMEOUT_MS
model_baudrate_table = deepcopy(MODEL_BAUDRATE_TABLE) model_baudrate_table = deepcopy(MODEL_BAUDRATE_TABLE)
model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE) model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
model_number_table = deepcopy(MODEL_NUMBER) model_encoding_table = deepcopy(MODEL_ENCODING_TABLE)
model_number_table = deepcopy(MODEL_NUMBER_TABLE)
model_resolution_table = deepcopy(MODEL_RESOLUTION) model_resolution_table = deepcopy(MODEL_RESOLUTION)
normalization_required = deepcopy(NORMALIZATION_REQUIRED) normalized_data = deepcopy(NORMALIZED_DATA)
def __init__( def __init__(
self, self,
@ -128,11 +130,25 @@ class DynamixelMotorsBus(MotorsBus):
for motor in motors: for motor in motors:
self.write("Torque_Enable", motor, TorqueMode.ENABLED.value) self.write("Torque_Enable", motor, TorqueMode.ENABLED.value)
def _encode_value(self, value: int, data_name: str | None = None, n_bytes: int | None = None) -> int: def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
return encode_twos_complement(value, n_bytes) for id_ in ids_values:
model = self._id_to_model(id_)
encoding_table = self.model_encoding_table.get(model)
if encoding_table and data_name in encoding_table:
n_bytes = encoding_table[data_name]
ids_values[id_] = encode_twos_complement(ids_values[id_], n_bytes)
def _decode_value(self, value: int, data_name: str | None = None, n_bytes: int | None = None) -> int: return ids_values
return decode_twos_complement(value, n_bytes)
def _decode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
for id_ in ids_values:
model = self._id_to_model(id_)
encoding_table = self.model_encoding_table.get(model)
if encoding_table and data_name in encoding_table:
n_bytes = encoding_table[data_name]
ids_values[id_] = decode_twos_complement(ids_values[id_], n_bytes)
return ids_values
def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]: def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]:
""" """
@ -182,13 +198,13 @@ class DynamixelMotorsBus(MotorsBus):
data_list, comm = self.packet_handler.broadcastPing(self.port_handler) data_list, comm = self.packet_handler.broadcastPing(self.port_handler)
if self._is_comm_success(comm): if self._is_comm_success(comm):
break break
logger.debug(f"Broadcast failed on port '{self.port}' ({n_try=})") logger.debug(f"Broadcast ping failed on port '{self.port}' ({n_try=})")
logger.debug(self.packet_handler.getTxRxResult(comm)) logger.debug(self.packet_handler.getTxRxResult(comm))
if not self._is_comm_success(comm): if not self._is_comm_success(comm):
if raise_on_error: if raise_on_error:
raise ConnectionError(self.packet_handler.getTxRxResult(comm)) raise ConnectionError(self.packet_handler.getTxRxResult(comm))
return data_list if data_list else None return
return {id_: data[0] for id_, data in data_list.items()} return {id_: data[0] for id_, data in data_list.items()}

25
lerobot/common/motors/dynamixel/tables.py
View File

@ -1,4 +1,4 @@
# data_name: (address, size_byte) # {data_name: (address, size_byte)}
# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table # https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table
X_SERIES_CONTROL_TABLE = { X_SERIES_CONTROL_TABLE = {
"Model_Number": (0, 2), "Model_Number": (0, 2),
@ -66,6 +66,27 @@ X_SERIES_BAUDRATE_TABLE = {
6: 4_000_000, 6: 4_000_000,
} }
# {data_name: size_byte}
X_SERIES_ENCODINGS_TABLE = {
"Homing_Offset": X_SERIES_CONTROL_TABLE["Homing_Offset"][1],
"Goal_PWM": X_SERIES_CONTROL_TABLE["Goal_PWM"][1],
"Goal_Current": X_SERIES_CONTROL_TABLE["Goal_Current"][1],
"Goal_Velocity": X_SERIES_CONTROL_TABLE["Goal_Velocity"][1],
"Present_PWM": X_SERIES_CONTROL_TABLE["Present_PWM"][1],
"Present_Current": X_SERIES_CONTROL_TABLE["Present_Current"][1],
"Present_Velocity": X_SERIES_CONTROL_TABLE["Present_Velocity"][1],
}
MODEL_ENCODING_TABLE = {
"x_series": X_SERIES_ENCODINGS_TABLE,
"xl330-m077": X_SERIES_ENCODINGS_TABLE,
"xl330-m288": X_SERIES_ENCODINGS_TABLE,
"xl430-w250": X_SERIES_ENCODINGS_TABLE,
"xm430-w350": X_SERIES_ENCODINGS_TABLE,
"xm540-w270": X_SERIES_ENCODINGS_TABLE,
"xc430-w150": X_SERIES_ENCODINGS_TABLE,
}
# {model: model_resolution} # {model: model_resolution}
# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#specifications # https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#specifications
MODEL_RESOLUTION = { MODEL_RESOLUTION = {
@ -80,7 +101,7 @@ MODEL_RESOLUTION = {
# {model: model_number} # {model: model_number}
# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table-of-eeprom-area # https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table-of-eeprom-area
MODEL_NUMBER = { MODEL_NUMBER_TABLE = {
"xl330-m077": 1190, "xl330-m077": 1190,
"xl330-m288": 1200, "xl330-m288": 1200,
"xl430-w250": 1060, "xl430-w250": 1060,

163
tests/motors/test_dynamixel.py
View File

@ -6,7 +6,7 @@ import dynamixel_sdk as dxl
import pytest import pytest
from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode
from lerobot.common.motors.dynamixel import MODEL_NUMBER, DynamixelMotorsBus from lerobot.common.motors.dynamixel import MODEL_NUMBER_TABLE, DynamixelMotorsBus
from lerobot.common.utils.encoding_utils import encode_twos_complement from lerobot.common.utils.encoding_utils import encode_twos_complement
from tests.mocks.mock_dynamixel import MockMotors, MockPortHandler from tests.mocks.mock_dynamixel import MockMotors, MockPortHandler
@ -113,7 +113,7 @@ def test_abc_implementation(dummy_motors):
@pytest.mark.parametrize("id_", [1, 2, 3]) @pytest.mark.parametrize("id_", [1, 2, 3])
def test_ping(id_, mock_motors, dummy_motors): def test_ping(id_, mock_motors, dummy_motors):
expected_model_nb = MODEL_NUMBER[dummy_motors[f"dummy_{id_}"].model] expected_model_nb = MODEL_NUMBER_TABLE[dummy_motors[f"dummy_{id_}"].model]
stub_name = mock_motors.build_ping_stub(id_, expected_model_nb) stub_name = mock_motors.build_ping_stub(id_, expected_model_nb)
motors_bus = DynamixelMotorsBus( motors_bus = DynamixelMotorsBus(
port=mock_motors.port, port=mock_motors.port,
@ -129,7 +129,7 @@ def test_ping(id_, mock_motors, dummy_motors):
def test_broadcast_ping(mock_motors, dummy_motors): def test_broadcast_ping(mock_motors, dummy_motors):
models = {m.id: m.model for m in dummy_motors.values()} models = {m.id: m.model for m in dummy_motors.values()}
expected_model_nbs = {id_: MODEL_NUMBER[model] for id_, model in models.items()} expected_model_nbs = {id_: MODEL_NUMBER_TABLE[model] for id_, model in models.items()}
stub_name = mock_motors.build_broadcast_ping_stub(expected_model_nbs) stub_name = mock_motors.build_broadcast_ping_stub(expected_model_nbs)
motors_bus = DynamixelMotorsBus( motors_bus = DynamixelMotorsBus(
port=mock_motors.port, port=mock_motors.port,
@ -171,55 +171,7 @@ def test_sync_read_none(mock_motors, dummy_motors):
(3, 4016), (3, 4016),
], ],
) )
def test_sync_read_by_id(id_, position, mock_motors, dummy_motors): def test_sync_read_single_value(id_, position, mock_motors, dummy_motors):
expected_position = {id_: position}
stub_name = mock_motors.build_sync_read_stub("Present_Position", expected_position)
motors_bus = DynamixelMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
read_position = motors_bus.sync_read("Present_Position", id_, normalize=False)
assert mock_motors.stubs[stub_name].called
assert read_position == expected_position
@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_by_ids(ids, positions, mock_motors, dummy_motors):
assert len(ids) == len(positions)
expected_positions = dict(zip(ids, positions, strict=True))
stub_name = mock_motors.build_sync_read_stub("Present_Position", expected_positions)
motors_bus = DynamixelMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
read_positions = motors_bus.sync_read("Present_Position", ids, normalize=False)
assert mock_motors.stubs[stub_name].called
assert read_positions == expected_positions
@pytest.mark.parametrize(
"id_, position",
[
(1, 1337),
(2, 42),
(3, 4016),
],
)
def test_sync_read_by_name(id_, position, mock_motors, dummy_motors):
expected_position = {f"dummy_{id_}": position} expected_position = {f"dummy_{id_}": position}
stub_name = mock_motors.build_sync_read_stub("Present_Position", {id_: position}) stub_name = mock_motors.build_sync_read_stub("Present_Position", {id_: position})
motors_bus = DynamixelMotorsBus( motors_bus = DynamixelMotorsBus(
@ -243,7 +195,7 @@ def test_sync_read_by_name(id_, position, mock_motors, dummy_motors):
], ],
ids=["1 motor", "2 motors", "3 motors"], ids=["1 motor", "2 motors", "3 motors"],
) # fmt: skip ) # fmt: skip
def test_sync_read_by_names(ids, positions, mock_motors, dummy_motors): def test_sync_read(ids, positions, mock_motors, dummy_motors):
assert len(ids) == len(positions) assert len(ids) == len(positions)
names = [f"dummy_{dxl_id}" for dxl_id in ids] names = [f"dummy_{dxl_id}" for dxl_id in ids]
expected_positions = dict(zip(names, positions, strict=True)) expected_positions = dict(zip(names, positions, strict=True))
@ -271,9 +223,9 @@ def test_sync_read_by_names(ids, positions, mock_motors, dummy_motors):
], ],
) )
def test_sync_read_num_retry(num_retry, num_invalid_try, pos, mock_motors, dummy_motors): def test_sync_read_num_retry(num_retry, num_invalid_try, pos, mock_motors, dummy_motors):
expected_position = {1: pos} expected_position = {"dummy_1": pos}
stub_name = mock_motors.build_sync_read_stub( stub_name = mock_motors.build_sync_read_stub(
"Present_Position", expected_position, num_invalid_try=num_invalid_try "Present_Position", {1: pos}, num_invalid_try=num_invalid_try
) )
motors_bus = DynamixelMotorsBus( motors_bus = DynamixelMotorsBus(
port=mock_motors.port, port=mock_motors.port,
@ -282,11 +234,11 @@ def test_sync_read_num_retry(num_retry, num_invalid_try, pos, mock_motors, dummy
motors_bus.connect(assert_motors_exist=False) motors_bus.connect(assert_motors_exist=False)
if num_retry >= num_invalid_try: if num_retry >= num_invalid_try:
pos_dict = motors_bus.sync_read("Present_Position", 1, normalize=False, num_retry=num_retry) pos_dict = motors_bus.sync_read("Present_Position", "dummy_1", normalize=False, num_retry=num_retry)
assert pos_dict == {1: pos} assert pos_dict == expected_position
else: else:
with pytest.raises(ConnectionError): with pytest.raises(ConnectionError):
_ = motors_bus.sync_read("Present_Position", 1, normalize=False, num_retry=num_retry) _ = motors_bus.sync_read("Present_Position", "dummy_1", normalize=False, num_retry=num_retry)
expected_calls = min(1 + num_retry, 1 + num_invalid_try) expected_calls = min(1 + num_retry, 1 + num_invalid_try)
assert mock_motors.stubs[stub_name].calls == expected_calls assert mock_motors.stubs[stub_name].calls == expected_calls
@ -315,28 +267,6 @@ def test_sync_write_single_value(data_name, value, mock_motors, dummy_motors):
assert mock_motors.stubs[stub_name].wait_called() assert mock_motors.stubs[stub_name].wait_called()
@pytest.mark.parametrize(
"id_, position",
[
(1, 1337),
(2, 42),
(3, 4016),
],
)
def test_sync_write_by_id(id_, position, mock_motors, dummy_motors):
value = {id_: position}
stub_name = mock_motors.build_sync_write_stub("Goal_Position", value)
motors_bus = DynamixelMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
motors_bus.sync_write("Goal_Position", value, normalize=False)
assert mock_motors.stubs[stub_name].wait_called()
@pytest.mark.parametrize( @pytest.mark.parametrize(
"ids, positions", "ids, positions",
[ [
@ -346,54 +276,7 @@ def test_sync_write_by_id(id_, position, mock_motors, dummy_motors):
], ],
ids=["1 motor", "2 motors", "3 motors"], ids=["1 motor", "2 motors", "3 motors"],
) # fmt: skip ) # fmt: skip
def test_sync_write_by_ids(ids, positions, mock_motors, dummy_motors): def test_sync_write(ids, positions, mock_motors, dummy_motors):
assert len(ids) == len(positions)
values = dict(zip(ids, positions, strict=True))
stub_name = mock_motors.build_sync_write_stub("Goal_Position", values)
motors_bus = DynamixelMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
motors_bus.sync_write("Goal_Position", values, normalize=False)
assert mock_motors.stubs[stub_name].wait_called()
@pytest.mark.parametrize(
"id_, position",
[
(1, 1337),
(2, 42),
(3, 4016),
],
)
def test_sync_write_by_name(id_, position, mock_motors, dummy_motors):
id_value = {id_: position}
stub_name = mock_motors.build_sync_write_stub("Goal_Position", id_value)
motors_bus = DynamixelMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
write_value = {f"dummy_{id_}": position}
motors_bus.sync_write("Goal_Position", write_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_by_names(ids, positions, mock_motors, dummy_motors):
assert len(ids) == len(positions) assert len(ids) == len(positions)
ids_values = dict(zip(ids, positions, strict=True)) ids_values = dict(zip(ids, positions, strict=True))
stub_name = mock_motors.build_sync_write_stub("Goal_Position", ids_values) stub_name = mock_motors.build_sync_write_stub("Goal_Position", ids_values)
@ -418,29 +301,7 @@ def test_sync_write_by_names(ids, positions, mock_motors, dummy_motors):
("Goal_Position", 3, 42), ("Goal_Position", 3, 42),
], ],
) )
def test_write_by_id(data_name, dxl_id, value, mock_motors, dummy_motors): 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 = DynamixelMotorsBus(
port=mock_motors.port,
motors=dummy_motors,
)
motors_bus.connect(assert_motors_exist=False)
motors_bus.write(data_name, dxl_id, value, normalize=False)
assert mock_motors.stubs[stub_name].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_by_name(data_name, dxl_id, value, mock_motors, dummy_motors):
stub_name = mock_motors.build_write_stub(data_name, dxl_id, value) stub_name = mock_motors.build_write_stub(data_name, dxl_id, value)
motors_bus = DynamixelMotorsBus( motors_bus = DynamixelMotorsBus(
port=mock_motors.port, port=mock_motors.port,