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

Fix calibration

Browse Source
This commit is contained in:
Simon Alibert
2025-04-02 22:27:49 +02:00
parent 65569ba90e
commit 1ebd81552c
4 changed files with 154 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,7 +24,7 @@ from enum import Enum
from lerobot.common.utils.encoding_utils import decode_twos_complement, encode_twos_complement
from ..motors_bus import Motor, MotorsBus, NameOrID, Value
from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value
from .tables import (
AVAILABLE_BAUDRATES,
MODEL_BAUDRATE_TABLE,
@@ -102,8 +102,9 @@ class DynamixelMotorsBus(MotorsBus):
self,
port: str,
motors: dict[str, Motor],
calibration: dict[str, MotorCalibration] | None = None,
):
super().__init__(port, motors)
super().__init__(port, motors, calibration)
import dynamixel_sdk as dxl
self.port_handler = dxl.PortHandler(self.port)
@@ -113,12 +114,26 @@ class DynamixelMotorsBus(MotorsBus):
self._comm_success = dxl.COMM_SUCCESS
self._no_error = 0x00
def _configure_motors(self) -> None:
def configure_motors(self) -> None:
# By default, Dynamixel motors have a 500µs delay response time (corresponding to a value of 250 on
# the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0).
for id_ in self.ids:
self.write("Return_Delay_Time", id_, 0)
def _disable_torque(self, motors: list[NameOrID]) -> None:
for motor in motors:
self.write("Torque_Enable", motor, TorqueMode.DISABLED.value)
def _enable_torque(self, motors: list[NameOrID]) -> None:
for motor in motors:
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:
return encode_twos_complement(value, n_bytes)
def _decode_value(self, value: int, data_name: str | None = None, n_bytes: int | None = None) -> int:
return decode_twos_complement(value, n_bytes)
def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]:
"""
On Dynamixel Motors:
@@ -132,12 +147,6 @@ class DynamixelMotorsBus(MotorsBus):
return half_turn_homings
def _encode_value(self, value: int, data_name: str | None = None, n_bytes: int | None = None) -> int:
return encode_twos_complement(value, n_bytes)
def _decode_value(self, value: int, data_name: str | None = None, n_bytes: int | None = None) -> int:
return decode_twos_complement(value, n_bytes)
@staticmethod
def _split_int_to_bytes(value: int, n_bytes: int) -> list[int]:
# Validate input