From 3d3a176940caff2c0abf914a900526a840109a83 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Mon, 3 Mar 2025 18:18:24 +0100 Subject: [PATCH] Move & organize motors, add base class --- lerobot/common/motors/__init__.py | 3 ++ lerobot/common/motors/dynamixel/__init__.py | 4 ++ lerobot/common/motors/dynamixel/dynamixel.py | 32 +++++++++++-- .../motors/dynamixel/dynamixel_calibration.py | 4 +- lerobot/common/motors/feetech/__init__.py | 3 ++ lerobot/common/motors/feetech/feetech.py | 24 +++++----- .../motors/feetech/feetech_calibration.py | 4 +- lerobot/common/motors/motors_bus.py | 46 +++++++++++++++++++ lerobot/common/motors/utils.py | 27 ++++------- 9 files changed, 107 insertions(+), 40 deletions(-) create mode 100644 lerobot/common/motors/__init__.py create mode 100644 lerobot/common/motors/dynamixel/__init__.py create mode 100644 lerobot/common/motors/feetech/__init__.py create mode 100644 lerobot/common/motors/motors_bus.py diff --git a/lerobot/common/motors/__init__.py b/lerobot/common/motors/__init__.py new file mode 100644 index 00000000..6c8699a1 --- /dev/null +++ b/lerobot/common/motors/__init__.py @@ -0,0 +1,3 @@ +from .motors_bus import MotorsBus + +__all__ = ["MotorsBus"] diff --git a/lerobot/common/motors/dynamixel/__init__.py b/lerobot/common/motors/dynamixel/__init__.py new file mode 100644 index 00000000..fbe65a27 --- /dev/null +++ b/lerobot/common/motors/dynamixel/__init__.py @@ -0,0 +1,4 @@ +from .dynamixel import DynamixelMotorsBus, TorqueMode, set_operating_mode +from .dynamixel_calibration import run_arm_calibration + +__all__ = ["DynamixelMotorsBus", "TorqueMode", "set_operating_mode", "run_arm_calibration"] diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py index d2b8bd4d..3c87c12a 100644 --- a/lerobot/common/motors/dynamixel/dynamixel.py +++ b/lerobot/common/motors/dynamixel/dynamixel.py @@ -8,7 +8,7 @@ from copy import deepcopy import numpy as np import tqdm -from lerobot.common.utils.robot_utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError +from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.common.utils.utils import capture_timestamp_utc PROTOCOL_VERSION = 2.0 @@ -313,7 +313,7 @@ class DynamixelMotorsBus: def connect(self): if self.is_connected: - raise RobotDeviceAlreadyConnectedError( + raise DeviceAlreadyConnectedError( f"DynamixelMotorsBus({self.port}) is already connected. Do not call `motors_bus.connect()` twice." ) @@ -670,7 +670,7 @@ class DynamixelMotorsBus: def read(self, data_name, motor_names: str | list[str] | None = None): if not self.is_connected: - raise RobotDeviceNotConnectedError( + raise DeviceNotConnectedError( f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." ) @@ -772,7 +772,7 @@ class DynamixelMotorsBus: def write(self, data_name, values: int | float | np.ndarray, motor_names: str | list[str] | None = None): if not self.is_connected: - raise RobotDeviceNotConnectedError( + raise DeviceNotConnectedError( f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." ) @@ -841,7 +841,7 @@ class DynamixelMotorsBus: def disconnect(self): if not self.is_connected: - raise RobotDeviceNotConnectedError( + raise DeviceNotConnectedError( f"DynamixelMotorsBus({self.port}) is not connected. Try running `motors_bus.connect()` first." ) @@ -857,3 +857,25 @@ class DynamixelMotorsBus: def __del__(self): if getattr(self, "is_connected", False): self.disconnect() + + +def set_operating_mode(arm: DynamixelMotorsBus): + if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any(): + raise ValueError("To run set robot preset, the torque must be disabled on all motors.") + + # Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't + # rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm, + # you could end up with a servo with a position 0 or 4095 at a crucial point See [ + # https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11] + all_motors_except_gripper = [name for name in arm.motor_names if name != "gripper"] + if len(all_motors_except_gripper) > 0: + # 4 corresponds to Extended Position on Koch motors + arm.write("Operating_Mode", 4, all_motors_except_gripper) + + # Use 'position control current based' for gripper to be limited by the limit of the current. + # For the follower gripper, it means it can grasp an object without forcing too much even tho, + # it's goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch). + # For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger + # to make it move, and it will move back to its original target position when we release the force. + # 5 corresponds to Current Controlled Position on Koch gripper motors "xl330-m077, xl330-m288" + arm.write("Operating_Mode", 5, "gripper") diff --git a/lerobot/common/motors/dynamixel/dynamixel_calibration.py b/lerobot/common/motors/dynamixel/dynamixel_calibration.py index fc1e2c4a..72056e49 100644 --- a/lerobot/common/motors/dynamixel/dynamixel_calibration.py +++ b/lerobot/common/motors/dynamixel/dynamixel_calibration.py @@ -3,12 +3,12 @@ import numpy as np -from lerobot.common.motors.dynamixel.dynamixel import ( +from ..motors_bus import MotorsBus +from .dynamixel import ( CalibrationMode, TorqueMode, convert_degrees_to_steps, ) -from lerobot.common.motors.utils import MotorsBus URL_TEMPLATE = ( "https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp" diff --git a/lerobot/common/motors/feetech/__init__.py b/lerobot/common/motors/feetech/__init__.py new file mode 100644 index 00000000..77501820 --- /dev/null +++ b/lerobot/common/motors/feetech/__init__.py @@ -0,0 +1,3 @@ +from .feetech import FeetechMotorsBus, TorqueMode + +__all__ = ["FeetechMotorsBus", "TorqueMode"] diff --git a/lerobot/common/motors/feetech/feetech.py b/lerobot/common/motors/feetech/feetech.py index cb409eff..494c834b 100644 --- a/lerobot/common/motors/feetech/feetech.py +++ b/lerobot/common/motors/feetech/feetech.py @@ -8,8 +8,7 @@ from copy import deepcopy import numpy as np import tqdm -from lerobot.common.motors.configs import FeetechMotorsBusConfig -from lerobot.common.utils.robot_utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError +from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.common.utils.utils import capture_timestamp_utc PROTOCOL_VERSION = 0 @@ -253,11 +252,10 @@ class FeetechMotorsBus: motor_index = 6 motor_model = "sts3215" - config = FeetechMotorsBusConfig( + motors_bus = FeetechMotorsBus( port="/dev/tty.usbmodem575E0031751", motors={motor_name: (motor_index, motor_model)}, ) - motors_bus = FeetechMotorsBus(config) motors_bus.connect() position = motors_bus.read("Present_Position") @@ -273,11 +271,13 @@ class FeetechMotorsBus: def __init__( self, - config: FeetechMotorsBusConfig, + port: str, + motors: dict[str, tuple[int, str]], + mock: bool = False, ): - self.port = config.port - self.motors = config.motors - self.mock = config.mock + self.port = port + self.motors = motors + self.mock = mock self.model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE) self.model_resolution = deepcopy(MODEL_RESOLUTION) @@ -294,7 +294,7 @@ class FeetechMotorsBus: def connect(self): if self.is_connected: - raise RobotDeviceAlreadyConnectedError( + raise DeviceAlreadyConnectedError( f"FeetechMotorsBus({self.port}) is already connected. Do not call `motors_bus.connect()` twice." ) @@ -693,7 +693,7 @@ class FeetechMotorsBus: import scservo_sdk as scs if not self.is_connected: - raise RobotDeviceNotConnectedError( + raise DeviceNotConnectedError( f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." ) @@ -797,7 +797,7 @@ class FeetechMotorsBus: def write(self, data_name, values: int | float | np.ndarray, motor_names: str | list[str] | None = None): if not self.is_connected: - raise RobotDeviceNotConnectedError( + raise DeviceNotConnectedError( f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." ) @@ -866,7 +866,7 @@ class FeetechMotorsBus: def disconnect(self): if not self.is_connected: - raise RobotDeviceNotConnectedError( + raise DeviceNotConnectedError( f"FeetechMotorsBus({self.port}) is not connected. Try running `motors_bus.connect()` first." ) diff --git a/lerobot/common/motors/feetech/feetech_calibration.py b/lerobot/common/motors/feetech/feetech_calibration.py index b3ec125c..4f4a6dca 100644 --- a/lerobot/common/motors/feetech/feetech_calibration.py +++ b/lerobot/common/motors/feetech/feetech_calibration.py @@ -5,12 +5,12 @@ import time import numpy as np -from lerobot.common.motors.feetech.feetech import ( +from ..motors_bus import MotorsBus +from .feetech import ( CalibrationMode, TorqueMode, convert_degrees_to_steps, ) -from lerobot.common.motors.utils import MotorsBus URL_TEMPLATE = ( "https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp" diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py new file mode 100644 index 00000000..ca95a4da --- /dev/null +++ b/lerobot/common/motors/motors_bus.py @@ -0,0 +1,46 @@ +import abc + + +class MotorsBus(abc.ABC): + """The main LeRobot class for implementing motors buses.""" + + def __init__( + self, + motors: dict[str, tuple[int, str]], + ): + self.motors = motors + + def __len__(self): + return len(self.motors) + + @abc.abstractmethod + def connect(self): + pass + + @abc.abstractmethod + def reconnect(self): + pass + + @abc.abstractmethod + def set_calibration(self, calibration: dict[str, list]): + pass + + @abc.abstractmethod + def apply_calibration(self): + pass + + @abc.abstractmethod + def revert_calibration(self): + pass + + @abc.abstractmethod + def read(self): + pass + + @abc.abstractmethod + def write(self): + pass + + @abc.abstractmethod + def disconnect(self): + pass diff --git a/lerobot/common/motors/utils.py b/lerobot/common/motors/utils.py index 2060679c..1de3bdad 100644 --- a/lerobot/common/motors/utils.py +++ b/lerobot/common/motors/utils.py @@ -1,19 +1,5 @@ -from typing import Protocol - -from lerobot.common.motors.configs import ( - DynamixelMotorsBusConfig, - FeetechMotorsBusConfig, - MotorsBusConfig, -) - - -class MotorsBus(Protocol): - def motor_names(self): ... - def set_calibration(self): ... - def apply_calibration(self): ... - def revert_calibration(self): ... - def read(self): ... - def write(self): ... +from .configs import MotorsBusConfig +from .motors_bus import MotorsBus def make_motors_buses_from_configs(motors_bus_configs: dict[str, MotorsBusConfig]) -> list[MotorsBus]: @@ -21,7 +7,7 @@ def make_motors_buses_from_configs(motors_bus_configs: dict[str, MotorsBusConfig for key, cfg in motors_bus_configs.items(): if cfg.type == "dynamixel": - from lerobot.common.motors.dynamixel.dynamixel import DynamixelMotorsBus + from .dynamixel import DynamixelMotorsBus motors_buses[key] = DynamixelMotorsBus(cfg) @@ -38,13 +24,16 @@ def make_motors_buses_from_configs(motors_bus_configs: dict[str, MotorsBusConfig def make_motors_bus(motor_type: str, **kwargs) -> MotorsBus: if motor_type == "dynamixel": - from lerobot.common.motors.dynamixel.dynamixel import DynamixelMotorsBus + from .configs import DynamixelMotorsBusConfig + from .dynamixel import DynamixelMotorsBus config = DynamixelMotorsBusConfig(**kwargs) return DynamixelMotorsBus(config) elif motor_type == "feetech": - from lerobot.common.motors.feetech.feetech import FeetechMotorsBus + from feetech import FeetechMotorsBus + + from .configs import FeetechMotorsBusConfig config = FeetechMotorsBusConfig(**kwargs) return FeetechMotorsBus(config)