From 3b7f7cfb22cf62abcef2b77b65b4a9548d38f95a Mon Sep 17 00:00:00 2001 From: CarolinePascal Date: Sun, 5 Jul 2026 17:16:04 +0200 Subject: [PATCH] fix(dynamixel): correct AX sign-magnitude encoding AX-series speed/load are sign-magnitude (direction at bit 10), not two's complement, and position registers are unsigned. Encode/decode now use sign-magnitude on Protocol 1 and the AX encoding table stores sign-bit indices for the speed/load registers only. --- src/lerobot/motors/dynamixel/dynamixel.py | 23 ++++++++++++++++++----- src/lerobot/motors/dynamixel/tables.py | 11 ++++++----- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/lerobot/motors/dynamixel/dynamixel.py b/src/lerobot/motors/dynamixel/dynamixel.py index 5d59d3973..a0c2b6de5 100644 --- a/src/lerobot/motors/dynamixel/dynamixel.py +++ b/src/lerobot/motors/dynamixel/dynamixel.py @@ -25,7 +25,12 @@ from typing import TYPE_CHECKING from lerobot.utils.import_utils import _dynamixel_sdk_available, require_package -from ..encoding_utils import decode_twos_complement, encode_twos_complement +from ..encoding_utils import ( + decode_sign_magnitude, + decode_twos_complement, + encode_sign_magnitude, + encode_twos_complement, +) from ..motors_bus import Motor, MotorCalibration, NameOrID, SerialMotorsBus, Value, get_address from .tables import ( AVAILABLE_BAUDRATES, @@ -271,8 +276,12 @@ class DynamixelMotorsBus(SerialMotorsBus): 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) + param = encoding_table[data_name] + # Protocol 1.0 (AX-series) uses sign-magnitude; Protocol 2.0 (X-series) two's complement. + if self.protocol_version == 1: + ids_values[id_] = encode_sign_magnitude(ids_values[id_], param) + else: + ids_values[id_] = encode_twos_complement(ids_values[id_], param) return ids_values @@ -281,8 +290,12 @@ class DynamixelMotorsBus(SerialMotorsBus): 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) + param = encoding_table[data_name] + # Protocol 1.0 (AX-series) uses sign-magnitude; Protocol 2.0 (X-series) two's complement. + if self.protocol_version == 1: + ids_values[id_] = decode_sign_magnitude(ids_values[id_], param) + else: + ids_values[id_] = decode_twos_complement(ids_values[id_], param) return ids_values diff --git a/src/lerobot/motors/dynamixel/tables.py b/src/lerobot/motors/dynamixel/tables.py index 889242a21..840412cc3 100644 --- a/src/lerobot/motors/dynamixel/tables.py +++ b/src/lerobot/motors/dynamixel/tables.py @@ -166,12 +166,13 @@ X_SERIES_ENCODINGS_TABLE = { "Present_Velocity": X_SERIES_CONTROL_TABLE["Present_Velocity"][1], } -# {data_name: size_byte} +# {data_name: sign_bit_index} +# AX-series speed/load use sign-magnitude encoding with the direction stored in bit 10. +# Position registers (0-1023) are unsigned and therefore need no sign handling. AX_SERIES_ENCODINGS_TABLE = { - "Goal_Position": AX_SERIES_CONTROL_TABLE["Goal_Position"][1], - "Moving_Speed": AX_SERIES_CONTROL_TABLE["Moving_Speed"][1], - "Present_Position": AX_SERIES_CONTROL_TABLE["Present_Position"][1], - "Present_Speed": AX_SERIES_CONTROL_TABLE["Present_Speed"][1], + "Moving_Speed": 10, + "Present_Speed": 10, + "Present_Load": 10, } MODEL_ENCODING_TABLE = {