From 702749b7d3109b6908f52c40349be01c9b9931de Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Thu, 17 Apr 2025 16:56:23 +0200 Subject: [PATCH] Fix setup_motor & add it to robots --- lerobot/common/motors/dynamixel/dynamixel.py | 6 +++++- lerobot/common/motors/feetech/feetech.py | 8 +++++++- lerobot/common/motors/motors_bus.py | 11 ++++++++--- lerobot/common/robots/koch/koch_follower.py | 8 +++++++- lerobot/common/robots/so100/so100_follower.py | 8 +++++++- lerobot/common/teleoperators/koch/koch_leader.py | 8 +++++++- lerobot/common/teleoperators/so100/so100_leader.py | 8 +++++++- tests/motors/test_motors_bus.py | 5 +++-- 8 files changed, 51 insertions(+), 11 deletions(-) diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py index ceeb029a2..cacc4c99f 100644 --- a/lerobot/common/motors/dynamixel/dynamixel.py +++ b/lerobot/common/motors/dynamixel/dynamixel.py @@ -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, MotorCalibration, MotorsBus, NameOrID, Value +from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value, get_address from .tables import ( AVAILABLE_BAUDRATES, MODEL_BAUDRATE_TABLE, @@ -198,6 +198,10 @@ class DynamixelMotorsBus(MotorsBus): for name in self._get_motors_list(motors): self.write("Torque_Enable", name, TorqueMode.DISABLED.value, num_retry=num_retry) + def _disable_torque(self, motor_id: int, model: str, num_retry: int = 0) -> None: + addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable") + self._write(addr, length, motor_id, TorqueMode.DISABLED.value, num_retry=num_retry) + 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, num_retry=num_retry) diff --git a/lerobot/common/motors/feetech/feetech.py b/lerobot/common/motors/feetech/feetech.py index bdfd0a326..e6dc494ce 100644 --- a/lerobot/common/motors/feetech/feetech.py +++ b/lerobot/common/motors/feetech/feetech.py @@ -19,7 +19,7 @@ from pprint import pformat from lerobot.common.utils.encoding_utils import decode_sign_magnitude, encode_sign_magnitude -from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value +from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value, get_address from .tables import ( FIRMWARE_MAJOR_VERSION, FIRMWARE_MINOR_VERSION, @@ -280,6 +280,12 @@ class FeetechMotorsBus(MotorsBus): self.write("Torque_Enable", name, TorqueMode.DISABLED.value, num_retry=num_retry) self.write("Lock", name, 0, num_retry=num_retry) + def _disable_torque(self, motor_id: int, model: str, num_retry: int = 0) -> None: + addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable") + self._write(addr, length, motor_id, TorqueMode.DISABLED.value, num_retry=num_retry) + addr, length = get_address(self.model_ctrl_table, model, "Lock") + self._write(addr, length, motor_id, 0, num_retry=num_retry) + 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, num_retry=num_retry) diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index a7ff5d967..601755bbc 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -464,13 +464,14 @@ class MotorsBus(abc.ABC): model = self.motors[motor].model target_id = self.motors[motor].id self.set_baudrate(initial_baudrate) + self._disable_torque(initial_id, model) # Set ID - addr, length = get_address(self.model_ctrl_table, "ID", model) + addr, length = get_address(self.model_ctrl_table, model, "ID") self._write(addr, length, initial_id, target_id) # Set Baudrate - addr, length = get_address(self.model_ctrl_table, "Baud_Rate", model) + addr, length = get_address(self.model_ctrl_table, model, "Baud_Rate") baudrate_value = self.model_baudrate_table[model][self.default_baudrate] self._write(addr, length, target_id, baudrate_value) @@ -485,7 +486,11 @@ class MotorsBus(abc.ABC): pass @abc.abstractmethod - def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: + def disable_torque(self, motors: int | str | list[str] | None = None, num_retry: int = 0) -> None: + pass + + @abc.abstractmethod + def _disable_torque(self, motor: int, model: str, num_retry: int = 0) -> None: pass @abc.abstractmethod diff --git a/lerobot/common/robots/koch/koch_follower.py b/lerobot/common/robots/koch/koch_follower.py index 9572024dd..5b511654d 100644 --- a/lerobot/common/robots/koch/koch_follower.py +++ b/lerobot/common/robots/koch/koch_follower.py @@ -117,7 +117,7 @@ class KochFollower(Robot): for name in self.arm.names: self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value) - input("Move robot to the middle of its range of motion and press ENTER....") + input(f"Move {self} to the middle of its range of motion and press ENTER....") homing_offsets = self.arm.set_half_turn_homings() full_turn_motors = ["shoulder_pan", "wrist_roll"] @@ -169,6 +169,12 @@ class KochFollower(Robot): self.arm.write("Position_I_Gain", "elbow_flex", 0) self.arm.write("Position_D_Gain", "elbow_flex", 600) + def setup_motors(self) -> None: + for motor in reversed(self.arm.motors): + input(f"Connect the controller board to the '{motor}' motor only and press enter.") + self.arm.setup_motor(motor) + print(f"'{motor}' motor id set to {self.arm.motors[motor].id}") + def get_observation(self) -> dict[str, Any]: if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") diff --git a/lerobot/common/robots/so100/so100_follower.py b/lerobot/common/robots/so100/so100_follower.py index d80e3075e..2b2d7472b 100644 --- a/lerobot/common/robots/so100/so100_follower.py +++ b/lerobot/common/robots/so100/so100_follower.py @@ -116,7 +116,7 @@ class SO100Follower(Robot): for name in self.arm.names: self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value) - input("Move robot to the middle of its range of motion and press ENTER....") + input(f"Move {self} to the middle of its range of motion and press ENTER....") homing_offsets = self.arm.set_half_turn_homings() full_turn_motor = "wrist_roll" @@ -154,6 +154,12 @@ class SO100Follower(Robot): self.arm.write("I_Coefficient", name, 0) self.arm.write("D_Coefficient", name, 32) + def setup_motors(self) -> None: + for motor in reversed(self.arm.motors): + input(f"Connect the controller board to the '{motor}' motor only and press enter.") + self.arm.setup_motor(motor) + print(f"'{motor}' motor id set to {self.arm.motors[motor].id}") + def get_observation(self) -> dict[str, Any]: if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") diff --git a/lerobot/common/teleoperators/koch/koch_leader.py b/lerobot/common/teleoperators/koch/koch_leader.py index 4bf33cc85..ba608bb22 100644 --- a/lerobot/common/teleoperators/koch/koch_leader.py +++ b/lerobot/common/teleoperators/koch/koch_leader.py @@ -97,7 +97,7 @@ class KochLeader(Teleoperator): self.arm.write("Drive_Mode", "elbow_flex", DriveMode.INVERTED.value) drive_modes = {name: 1 if name == "elbow_flex" else 0 for name in self.arm.names} - input("Move robot to the middle of its range of motion and press ENTER....") + input(f"Move {self} to the middle of its range of motion and press ENTER....") homing_offsets = self.arm.set_half_turn_homings() full_turn_motors = ["shoulder_pan", "wrist_roll"] @@ -146,6 +146,12 @@ class KochLeader(Teleoperator): self.arm.enable_torque("gripper") self.arm.write("Goal_Position", "gripper", self.config.gripper_open_pos) + def setup_motors(self) -> None: + for motor in reversed(self.arm.motors): + input(f"Connect the controller board to the '{motor}' motor only and press enter.") + self.arm.setup_motor(motor) + print(f"'{motor}' motor id set to {self.arm.motors[motor].id}") + def get_action(self) -> dict[str, float]: if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") diff --git a/lerobot/common/teleoperators/so100/so100_leader.py b/lerobot/common/teleoperators/so100/so100_leader.py index 2eeed8384..706c20a46 100644 --- a/lerobot/common/teleoperators/so100/so100_leader.py +++ b/lerobot/common/teleoperators/so100/so100_leader.py @@ -91,7 +91,7 @@ class SO100Leader(Teleoperator): for name in self.arm.names: self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value) - input("Move robot to the middle of its range of motion and press ENTER....") + input(f"Move {self} to the middle of its range of motion and press ENTER....") homing_offsets = self.arm.set_half_turn_homings() full_turn_motor = "wrist_roll" @@ -124,6 +124,12 @@ class SO100Leader(Teleoperator): for name in self.arm.names: self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value) + def setup_motors(self) -> None: + for motor in reversed(self.arm.motors): + input(f"Connect the controller board to the '{motor}' motor only and press enter.") + self.arm.setup_motor(motor) + print(f"'{motor}' motor id set to {self.arm.motors[motor].id}") + def get_action(self) -> dict[str, float]: start = time.perf_counter() action = self.arm.sync_read("Present_Position") diff --git a/tests/motors/test_motors_bus.py b/tests/motors/test_motors_bus.py index 39981482a..9056e8709 100644 --- a/tests/motors/test_motors_bus.py +++ b/tests/motors/test_motors_bus.py @@ -136,8 +136,9 @@ class MockMotorsBus(MotorsBus): def configure_motors(self): ... def read_calibration(self): ... def write_calibration(self, calibration_dict): ... - def disable_torque(self, motors): ... - def enable_torque(self, motors): ... + def disable_torque(self, motors, num_retry): ... + def _disable_torque(self, motor, model, num_retry): ... + def enable_torque(self, motors, num_retry): ... def _get_half_turn_homings(self, positions): ... def _encode_sign(self, data_name, ids_values): ... def _decode_sign(self, data_name, ids_values): ...