From 9afc4b771c62b80be2cf004a047b09add1e14cc5 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Tue, 15 Apr 2025 11:20:42 +0200 Subject: [PATCH] Motors config & disconnect fixes --- lerobot/common/motors/dynamixel/dynamixel.py | 12 +++++----- lerobot/common/motors/feetech/feetech.py | 24 +++++++++++-------- lerobot/common/motors/motors_bus.py | 6 ++--- lerobot/common/robots/so100/so100_follower.py | 5 +--- .../teleoperators/so100/so100_leader.py | 1 + 5 files changed, 25 insertions(+), 23 deletions(-) diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py index 4bcbf6b0..52a84e5e 100644 --- a/lerobot/common/motors/dynamixel/dynamixel.py +++ b/lerobot/common/motors/dynamixel/dynamixel.py @@ -142,16 +142,16 @@ class DynamixelMotorsBus(MotorsBus): 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) + for motor in self.motors: + self.write("Return_Delay_Time", motor, 0) - def disable_torque(self, motors: str | list[str] | None = None) -> None: + def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: for name in self._get_motors_list(motors): - self.write("Torque_Enable", name, TorqueMode.DISABLED.value) + self.write("Torque_Enable", name, TorqueMode.DISABLED.value, num_retry=num_retry) - def enable_torque(self, motors: str | list[str] | None = None) -> None: + def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: for name in self._get_motors_list(motors): - self.write("Torque_Enable", name, TorqueMode.ENABLED.value) + self.write("Torque_Enable", name, TorqueMode.ENABLED.value, num_retry=num_retry) def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]: for id_ in ids_values: diff --git a/lerobot/common/motors/feetech/feetech.py b/lerobot/common/motors/feetech/feetech.py index 8c31401b..bcf54972 100644 --- a/lerobot/common/motors/feetech/feetech.py +++ b/lerobot/common/motors/feetech/feetech.py @@ -164,10 +164,14 @@ class FeetechMotorsBus(MotorsBus): self._assert_same_firmware() def configure_motors(self) -> None: - # By default, Feetech motors have a 500µs delay response time (corresponding to a value of 250 on the - # 'Return_Delay' 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) + for motor in self.motors: + # By default, Feetech 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). + self.write("Return_Delay_Time", motor, 0) + # Set 'Maximum_Acceleration' to 254 to speedup acceleration and deceleration of the motors. + # Note: this address is not in the official STS3215 Memory Table + self.write("Maximum_Acceleration", motor, 254) + self.write("Acceleration", motor, 254) def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]: """ @@ -182,15 +186,15 @@ class FeetechMotorsBus(MotorsBus): return half_turn_homings - def disable_torque(self, motors: str | list[str] | None = None) -> None: + def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: for name in self._get_motors_list(motors): - self.write("Torque_Enable", name, TorqueMode.DISABLED.value) - self.write("Lock", name, 0) + self.write("Torque_Enable", name, TorqueMode.DISABLED.value, num_retry=num_retry) + self.write("Lock", name, 0, num_retry=num_retry) - def enable_torque(self, motors: str | list[str] | None = None) -> None: + def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: for name in self._get_motors_list(motors): - self.write("Torque_Enable", name, TorqueMode.ENABLED.value) - self.write("Lock", name, 1) + self.write("Torque_Enable", name, TorqueMode.ENABLED.value, num_retry=num_retry) + self.write("Lock", name, 1, num_retry=num_retry) def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]: for id_ in ids_values: diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index 1ec4a201..71016fb5 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -456,11 +456,11 @@ class MotorsBus(abc.ABC): pass @abc.abstractmethod - def disable_torque(self, motors: str | list[str] | None = None) -> None: + def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: pass @abc.abstractmethod - def enable_torque(self, motors: str | list[str] | None = None) -> None: + def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: pass def set_timeout(self, timeout_ms: int | None = None): @@ -972,7 +972,7 @@ class MotorsBus(abc.ABC): if disable_torque: self.port_handler.clearPort() self.port_handler.is_using = False - self.disable_torque() + self.disable_torque(num_retry=5) self.port_handler.closePort() logger.debug(f"{self.__class__.__name__} disconnected.") diff --git a/lerobot/common/robots/so100/so100_follower.py b/lerobot/common/robots/so100/so100_follower.py index 13c5739b..419b4047 100644 --- a/lerobot/common/robots/so100/so100_follower.py +++ b/lerobot/common/robots/so100/so100_follower.py @@ -55,6 +55,7 @@ class SO100Follower(Robot): "wrist_roll": Motor(5, "sts3215", MotorNormMode.RANGE_M100_100), "gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100), }, + calibration=self.calibration, ) self.cameras = make_cameras_from_configs(config.cameras) @@ -152,10 +153,6 @@ class SO100Follower(Robot): # Set I_Coefficient and D_Coefficient to default value 0 and 32 self.arm.write("I_Coefficient", name, 0) self.arm.write("D_Coefficient", name, 32) - # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of - # the motors. Note: this address is not in the official STS3215 Memory Table - self.arm.write("Maximum_Acceleration", name, 254) - self.arm.write("Acceleration", name, 254) self.arm.enable_torque() diff --git a/lerobot/common/teleoperators/so100/so100_leader.py b/lerobot/common/teleoperators/so100/so100_leader.py index f8f7239e..0ed5eafc 100644 --- a/lerobot/common/teleoperators/so100/so100_leader.py +++ b/lerobot/common/teleoperators/so100/so100_leader.py @@ -51,6 +51,7 @@ class SO100Leader(Teleoperator): "wrist_roll": Motor(5, "sts3215", MotorNormMode.RANGE_M100_100), "gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100), }, + calibration=self.calibration, ) @property