forked from tangger/lerobot
Adapt feetech calibration
This commit is contained in:
@@ -169,6 +169,10 @@ class DynamixelMotorsBus(MotorsBus):
|
|||||||
for motor in self.motors:
|
for motor in self.motors:
|
||||||
self.write("Return_Delay_Time", motor, 0)
|
self.write("Return_Delay_Time", motor, 0)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_calibrated(self) -> bool:
|
||||||
|
return self.calibration == self.read_calibration()
|
||||||
|
|
||||||
def read_calibration(self) -> dict[str, MotorCalibration]:
|
def read_calibration(self) -> dict[str, MotorCalibration]:
|
||||||
offsets = self.sync_read("Homing_Offset", normalize=False)
|
offsets = self.sync_read("Homing_Offset", normalize=False)
|
||||||
mins = self.sync_read("Min_Position_Limit", normalize=False)
|
mins = self.sync_read("Min_Position_Limit", normalize=False)
|
||||||
|
|||||||
@@ -227,6 +227,26 @@ class FeetechMotorsBus(MotorsBus):
|
|||||||
self.write("Maximum_Acceleration", motor, 254)
|
self.write("Maximum_Acceleration", motor, 254)
|
||||||
self.write("Acceleration", motor, 254)
|
self.write("Acceleration", motor, 254)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_calibrated(self) -> bool:
|
||||||
|
motors_calibration = self.read_calibration()
|
||||||
|
if set(motors_calibration) != set(self.calibration):
|
||||||
|
return False
|
||||||
|
|
||||||
|
same_ranges = all(
|
||||||
|
self.calibration[motor].range_min == cal.range_min
|
||||||
|
and self.calibration[motor].range_max == cal.range_max
|
||||||
|
for motor, cal in motors_calibration.items()
|
||||||
|
)
|
||||||
|
if self.protocol_version == 1:
|
||||||
|
return same_ranges
|
||||||
|
|
||||||
|
same_offsets = all(
|
||||||
|
self.calibration[motor].homing_offset == cal.homing_offset
|
||||||
|
for motor, cal in motors_calibration.items()
|
||||||
|
)
|
||||||
|
return same_ranges and same_offsets
|
||||||
|
|
||||||
def read_calibration(self) -> dict[str, MotorCalibration]:
|
def read_calibration(self) -> dict[str, MotorCalibration]:
|
||||||
if self.protocol_version == 0:
|
if self.protocol_version == 0:
|
||||||
offsets = self.sync_read("Homing_Offset", normalize=False)
|
offsets = self.sync_read("Homing_Offset", normalize=False)
|
||||||
|
|||||||
@@ -625,9 +625,10 @@ class MotorsBus(abc.ABC):
|
|||||||
raise RuntimeError("Failed to write bus baud rate.")
|
raise RuntimeError("Failed to write bus baud rate.")
|
||||||
|
|
||||||
@property
|
@property
|
||||||
|
@abc.abstractmethod
|
||||||
def is_calibrated(self) -> bool:
|
def is_calibrated(self) -> bool:
|
||||||
"""bool: ``True`` if the cached calibration matches the motors."""
|
"""bool: ``True`` if the cached calibration matches the motors."""
|
||||||
return self.calibration == self.read_calibration()
|
pass
|
||||||
|
|
||||||
@abc.abstractmethod
|
@abc.abstractmethod
|
||||||
def read_calibration(self) -> dict[str, MotorCalibration]:
|
def read_calibration(self) -> dict[str, MotorCalibration]:
|
||||||
|
|||||||
@@ -125,6 +125,7 @@ class MockMotorsBus(MotorsBus):
|
|||||||
def _handshake(self): ...
|
def _handshake(self): ...
|
||||||
def _find_single_motor(self, motor, initial_baudrate): ...
|
def _find_single_motor(self, motor, initial_baudrate): ...
|
||||||
def configure_motors(self): ...
|
def configure_motors(self): ...
|
||||||
|
def is_calibrated(self): ...
|
||||||
def read_calibration(self): ...
|
def read_calibration(self): ...
|
||||||
def write_calibration(self, calibration_dict): ...
|
def write_calibration(self, calibration_dict): ...
|
||||||
def disable_torque(self, motors, num_retry): ...
|
def disable_torque(self, motors, num_retry): ...
|
||||||
|
|||||||
Reference in New Issue
Block a user