forked from tangger/lerobot
Adapt feetech calibration
This commit is contained in:
@@ -169,6 +169,10 @@ class DynamixelMotorsBus(MotorsBus):
|
||||
for motor in self.motors:
|
||||
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]:
|
||||
offsets = self.sync_read("Homing_Offset", 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("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]:
|
||||
if self.protocol_version == 0:
|
||||
offsets = self.sync_read("Homing_Offset", normalize=False)
|
||||
|
||||
@@ -625,9 +625,10 @@ class MotorsBus(abc.ABC):
|
||||
raise RuntimeError("Failed to write bus baud rate.")
|
||||
|
||||
@property
|
||||
@abc.abstractmethod
|
||||
def is_calibrated(self) -> bool:
|
||||
"""bool: ``True`` if the cached calibration matches the motors."""
|
||||
return self.calibration == self.read_calibration()
|
||||
pass
|
||||
|
||||
@abc.abstractmethod
|
||||
def read_calibration(self) -> dict[str, MotorCalibration]:
|
||||
|
||||
@@ -125,6 +125,7 @@ class MockMotorsBus(MotorsBus):
|
||||
def _handshake(self): ...
|
||||
def _find_single_motor(self, motor, initial_baudrate): ...
|
||||
def configure_motors(self): ...
|
||||
def is_calibrated(self): ...
|
||||
def read_calibration(self): ...
|
||||
def write_calibration(self, calibration_dict): ...
|
||||
def disable_torque(self, motors, num_retry): ...
|
||||
|
||||
Reference in New Issue
Block a user