diff --git a/lerobot/common/datasets/v2/batch_convert_dataset_v1_to_v2.py b/lerobot/common/datasets/v2/batch_convert_dataset_v1_to_v2.py index 475096ff..41dd33b6 100644 --- a/lerobot/common/datasets/v2/batch_convert_dataset_v1_to_v2.py +++ b/lerobot/common/datasets/v2/batch_convert_dataset_v1_to_v2.py @@ -27,7 +27,7 @@ from textwrap import dedent from lerobot import available_datasets from lerobot.common.datasets.v2.convert_dataset_v1_to_v2 import convert_dataset -from lerobot.common.robots.configs import AlohaRobotConfig +from lerobot.common.robots.aloha.configuration_aloha import AlohaRobotConfig LOCAL_DIR = Path("data/") diff --git a/lerobot/common/datasets/v2/convert_dataset_v1_to_v2.py b/lerobot/common/datasets/v2/convert_dataset_v1_to_v2.py index 8f8a7898..5e0533af 100644 --- a/lerobot/common/datasets/v2/convert_dataset_v1_to_v2.py +++ b/lerobot/common/datasets/v2/convert_dataset_v1_to_v2.py @@ -141,7 +141,7 @@ from lerobot.common.datasets.video_utils import ( get_image_pixel_channels, get_video_info, ) -from lerobot.common.robots.configs import RobotConfig +from lerobot.common.robots.config_abc import RobotConfig from lerobot.common.robots.utils import make_robot_config V16 = "v1.6" diff --git a/examples/9_use_aloha.md b/lerobot/common/robots/aloha/README.md similarity index 100% rename from examples/9_use_aloha.md rename to lerobot/common/robots/aloha/README.md diff --git a/lerobot/common/robots/aloha/configuration_aloha.py b/lerobot/common/robots/aloha/configuration_aloha.py new file mode 100644 index 00000000..8efdbcc4 --- /dev/null +++ b/lerobot/common/robots/aloha/configuration_aloha.py @@ -0,0 +1,132 @@ +from dataclasses import dataclass, field + +from lerobot.common.cameras.configs import CameraConfig, IntelRealSenseCameraConfig +from lerobot.common.motors.configs import DynamixelMotorsBusConfig, MotorsBusConfig +from lerobot.common.robots.config_abc import ManipulatorRobotConfig, RobotConfig + + +@RobotConfig.register_subclass("aloha") +@dataclass +class AlohaRobotConfig(ManipulatorRobotConfig): + # Specific to Aloha, LeRobot comes with default calibration files. Assuming the motors have been + # properly assembled, no manual calibration step is expected. If you need to run manual calibration, + # simply update this path to ".cache/calibration/aloha" + calibration_dir: str = ".cache/calibration/aloha_default" + + # /!\ FOR SAFETY, READ THIS /!\ + # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. + # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as + # the number of motors in your follower arms. + # For Aloha, for every goal position request, motor rotations are capped at 5 degrees by default. + # When you feel more confident with teleoperation or running the policy, you can extend + # this safety limit and even removing it by setting it to `null`. + # Also, everything is expected to work safely out-of-the-box, but we highly advise to + # first try to teleoperate the grippers only (by commenting out the rest of the motors in this yaml), + # then to gradually add more motors (by uncommenting), until you can teleoperate both arms fully + max_relative_target: int | None = 5 + + leader_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "left": DynamixelMotorsBusConfig( + # window_x + port="/dev/ttyDXL_leader_left", + motors={ + # name: (index, model) + "waist": [1, "xm430-w350"], + "shoulder": [2, "xm430-w350"], + "shoulder_shadow": [3, "xm430-w350"], + "elbow": [4, "xm430-w350"], + "elbow_shadow": [5, "xm430-w350"], + "forearm_roll": [6, "xm430-w350"], + "wrist_angle": [7, "xm430-w350"], + "wrist_rotate": [8, "xl430-w250"], + "gripper": [9, "xc430-w150"], + }, + ), + "right": DynamixelMotorsBusConfig( + # window_x + port="/dev/ttyDXL_leader_right", + motors={ + # name: (index, model) + "waist": [1, "xm430-w350"], + "shoulder": [2, "xm430-w350"], + "shoulder_shadow": [3, "xm430-w350"], + "elbow": [4, "xm430-w350"], + "elbow_shadow": [5, "xm430-w350"], + "forearm_roll": [6, "xm430-w350"], + "wrist_angle": [7, "xm430-w350"], + "wrist_rotate": [8, "xl430-w250"], + "gripper": [9, "xc430-w150"], + }, + ), + } + ) + + follower_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "left": DynamixelMotorsBusConfig( + port="/dev/ttyDXL_follower_left", + motors={ + # name: (index, model) + "waist": [1, "xm540-w270"], + "shoulder": [2, "xm540-w270"], + "shoulder_shadow": [3, "xm540-w270"], + "elbow": [4, "xm540-w270"], + "elbow_shadow": [5, "xm540-w270"], + "forearm_roll": [6, "xm540-w270"], + "wrist_angle": [7, "xm540-w270"], + "wrist_rotate": [8, "xm430-w350"], + "gripper": [9, "xm430-w350"], + }, + ), + "right": DynamixelMotorsBusConfig( + port="/dev/ttyDXL_follower_right", + motors={ + # name: (index, model) + "waist": [1, "xm540-w270"], + "shoulder": [2, "xm540-w270"], + "shoulder_shadow": [3, "xm540-w270"], + "elbow": [4, "xm540-w270"], + "elbow_shadow": [5, "xm540-w270"], + "forearm_roll": [6, "xm540-w270"], + "wrist_angle": [7, "xm540-w270"], + "wrist_rotate": [8, "xm430-w350"], + "gripper": [9, "xm430-w350"], + }, + ), + } + ) + + # Troubleshooting: If one of your IntelRealSense cameras freeze during + # data recording due to bandwidth limit, you might need to plug the camera + # on another USB hub or PCIe card. + cameras: dict[str, CameraConfig] = field( + default_factory=lambda: { + "cam_high": IntelRealSenseCameraConfig( + serial_number=128422271347, + fps=30, + width=640, + height=480, + ), + "cam_low": IntelRealSenseCameraConfig( + serial_number=130322270656, + fps=30, + width=640, + height=480, + ), + "cam_left_wrist": IntelRealSenseCameraConfig( + serial_number=218622272670, + fps=30, + width=640, + height=480, + ), + "cam_right_wrist": IntelRealSenseCameraConfig( + serial_number=130322272300, + fps=30, + width=640, + height=480, + ), + } + ) + + mock: bool = False diff --git a/lerobot/common/robots/config_abc.py b/lerobot/common/robots/config_abc.py new file mode 100644 index 00000000..83216c49 --- /dev/null +++ b/lerobot/common/robots/config_abc.py @@ -0,0 +1,59 @@ +import abc +from dataclasses import dataclass +from typing import Sequence + +import draccus + +from lerobot.common.cameras.configs import CameraConfig +from lerobot.common.motors.configs import MotorsBusConfig, field + + +@dataclass +class RobotConfig(draccus.ChoiceRegistry, abc.ABC): + @property + def type(self) -> str: + return self.get_choice_name(self.__class__) + + +# TODO(rcadene, aliberts): remove ManipulatorRobotConfig abstraction +@dataclass +class ManipulatorRobotConfig(RobotConfig): + leader_arms: dict[str, MotorsBusConfig] = field(default_factory=lambda: {}) + follower_arms: dict[str, MotorsBusConfig] = field(default_factory=lambda: {}) + cameras: dict[str, CameraConfig] = field(default_factory=lambda: {}) + + # Optionally limit the magnitude of the relative positional target vector for safety purposes. + # Set this to a positive scalar to have the same value for all motors, or a list that is the same length + # as the number of motors in your follower arms (assumes all follower arms have the same number of + # motors). + max_relative_target: list[float] | float | None = None + + # Optionally set the leader arm in torque mode with the gripper motor set to this angle. This makes it + # possible to squeeze the gripper and have it spring back to an open position on its own. If None, the + # gripper is not put in torque mode. + gripper_open_degree: float | None = None + + mock: bool = False + + def __post_init__(self): + if self.mock: + for arm in self.leader_arms.values(): + if not arm.mock: + arm.mock = True + for arm in self.follower_arms.values(): + if not arm.mock: + arm.mock = True + for cam in self.cameras.values(): + if not cam.mock: + cam.mock = True + + if self.max_relative_target is not None and isinstance(self.max_relative_target, Sequence): + for name in self.follower_arms: + if len(self.follower_arms[name].motors) != len(self.max_relative_target): + raise ValueError( + f"len(max_relative_target)={len(self.max_relative_target)} but the follower arm with name {name} has " + f"{len(self.follower_arms[name].motors)} motors. Please make sure that the " + f"`max_relative_target` list has as many parameters as there are motors per arm. " + "Note: This feature does not yet work with robots where different follower arms have " + "different numbers of motors." + ) diff --git a/lerobot/common/robots/configs.py b/lerobot/common/robots/configs.py deleted file mode 100644 index 7c02e5b3..00000000 --- a/lerobot/common/robots/configs.py +++ /dev/null @@ -1,599 +0,0 @@ -import abc -from dataclasses import dataclass, field -from typing import Sequence - -import draccus - -from lerobot.common.cameras.configs import ( - CameraConfig, - IntelRealSenseCameraConfig, - OpenCVCameraConfig, -) -from lerobot.common.motors.configs import ( - DynamixelMotorsBusConfig, - FeetechMotorsBusConfig, - MotorsBusConfig, -) - - -@dataclass -class RobotConfig(draccus.ChoiceRegistry, abc.ABC): - @property - def type(self) -> str: - return self.get_choice_name(self.__class__) - - -# TODO(rcadene, aliberts): remove ManipulatorRobotConfig abstraction -@dataclass -class ManipulatorRobotConfig(RobotConfig): - leader_arms: dict[str, MotorsBusConfig] = field(default_factory=lambda: {}) - follower_arms: dict[str, MotorsBusConfig] = field(default_factory=lambda: {}) - cameras: dict[str, CameraConfig] = field(default_factory=lambda: {}) - - # Optionally limit the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a list that is the same length - # as the number of motors in your follower arms (assumes all follower arms have the same number of - # motors). - max_relative_target: list[float] | float | None = None - - # Optionally set the leader arm in torque mode with the gripper motor set to this angle. This makes it - # possible to squeeze the gripper and have it spring back to an open position on its own. If None, the - # gripper is not put in torque mode. - gripper_open_degree: float | None = None - - mock: bool = False - - def __post_init__(self): - if self.mock: - for arm in self.leader_arms.values(): - if not arm.mock: - arm.mock = True - for arm in self.follower_arms.values(): - if not arm.mock: - arm.mock = True - for cam in self.cameras.values(): - if not cam.mock: - cam.mock = True - - if self.max_relative_target is not None and isinstance(self.max_relative_target, Sequence): - for name in self.follower_arms: - if len(self.follower_arms[name].motors) != len(self.max_relative_target): - raise ValueError( - f"len(max_relative_target)={len(self.max_relative_target)} but the follower arm with name {name} has " - f"{len(self.follower_arms[name].motors)} motors. Please make sure that the " - f"`max_relative_target` list has as many parameters as there are motors per arm. " - "Note: This feature does not yet work with robots where different follower arms have " - "different numbers of motors." - ) - - -@RobotConfig.register_subclass("aloha") -@dataclass -class AlohaRobotConfig(ManipulatorRobotConfig): - # Specific to Aloha, LeRobot comes with default calibration files. Assuming the motors have been - # properly assembled, no manual calibration step is expected. If you need to run manual calibration, - # simply update this path to ".cache/calibration/aloha" - calibration_dir: str = ".cache/calibration/aloha_default" - - # /!\ FOR SAFETY, READ THIS /!\ - # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as - # the number of motors in your follower arms. - # For Aloha, for every goal position request, motor rotations are capped at 5 degrees by default. - # When you feel more confident with teleoperation or running the policy, you can extend - # this safety limit and even removing it by setting it to `null`. - # Also, everything is expected to work safely out-of-the-box, but we highly advise to - # first try to teleoperate the grippers only (by commenting out the rest of the motors in this yaml), - # then to gradually add more motors (by uncommenting), until you can teleoperate both arms fully - max_relative_target: int | None = 5 - - leader_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "left": DynamixelMotorsBusConfig( - # window_x - port="/dev/ttyDXL_leader_left", - motors={ - # name: (index, model) - "waist": [1, "xm430-w350"], - "shoulder": [2, "xm430-w350"], - "shoulder_shadow": [3, "xm430-w350"], - "elbow": [4, "xm430-w350"], - "elbow_shadow": [5, "xm430-w350"], - "forearm_roll": [6, "xm430-w350"], - "wrist_angle": [7, "xm430-w350"], - "wrist_rotate": [8, "xl430-w250"], - "gripper": [9, "xc430-w150"], - }, - ), - "right": DynamixelMotorsBusConfig( - # window_x - port="/dev/ttyDXL_leader_right", - motors={ - # name: (index, model) - "waist": [1, "xm430-w350"], - "shoulder": [2, "xm430-w350"], - "shoulder_shadow": [3, "xm430-w350"], - "elbow": [4, "xm430-w350"], - "elbow_shadow": [5, "xm430-w350"], - "forearm_roll": [6, "xm430-w350"], - "wrist_angle": [7, "xm430-w350"], - "wrist_rotate": [8, "xl430-w250"], - "gripper": [9, "xc430-w150"], - }, - ), - } - ) - - follower_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "left": DynamixelMotorsBusConfig( - port="/dev/ttyDXL_follower_left", - motors={ - # name: (index, model) - "waist": [1, "xm540-w270"], - "shoulder": [2, "xm540-w270"], - "shoulder_shadow": [3, "xm540-w270"], - "elbow": [4, "xm540-w270"], - "elbow_shadow": [5, "xm540-w270"], - "forearm_roll": [6, "xm540-w270"], - "wrist_angle": [7, "xm540-w270"], - "wrist_rotate": [8, "xm430-w350"], - "gripper": [9, "xm430-w350"], - }, - ), - "right": DynamixelMotorsBusConfig( - port="/dev/ttyDXL_follower_right", - motors={ - # name: (index, model) - "waist": [1, "xm540-w270"], - "shoulder": [2, "xm540-w270"], - "shoulder_shadow": [3, "xm540-w270"], - "elbow": [4, "xm540-w270"], - "elbow_shadow": [5, "xm540-w270"], - "forearm_roll": [6, "xm540-w270"], - "wrist_angle": [7, "xm540-w270"], - "wrist_rotate": [8, "xm430-w350"], - "gripper": [9, "xm430-w350"], - }, - ), - } - ) - - # Troubleshooting: If one of your IntelRealSense cameras freeze during - # data recording due to bandwidth limit, you might need to plug the camera - # on another USB hub or PCIe card. - cameras: dict[str, CameraConfig] = field( - default_factory=lambda: { - "cam_high": IntelRealSenseCameraConfig( - serial_number=128422271347, - fps=30, - width=640, - height=480, - ), - "cam_low": IntelRealSenseCameraConfig( - serial_number=130322270656, - fps=30, - width=640, - height=480, - ), - "cam_left_wrist": IntelRealSenseCameraConfig( - serial_number=218622272670, - fps=30, - width=640, - height=480, - ), - "cam_right_wrist": IntelRealSenseCameraConfig( - serial_number=130322272300, - fps=30, - width=640, - height=480, - ), - } - ) - - mock: bool = False - - -@RobotConfig.register_subclass("koch") -@dataclass -class KochRobotConfig(ManipulatorRobotConfig): - calibration_dir: str = ".cache/calibration/koch" - # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as - # the number of motors in your follower arms. - max_relative_target: int | None = None - - leader_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "main": DynamixelMotorsBusConfig( - port="/dev/tty.usbmodem585A0085511", - motors={ - # name: (index, model) - "shoulder_pan": [1, "xl330-m077"], - "shoulder_lift": [2, "xl330-m077"], - "elbow_flex": [3, "xl330-m077"], - "wrist_flex": [4, "xl330-m077"], - "wrist_roll": [5, "xl330-m077"], - "gripper": [6, "xl330-m077"], - }, - ), - } - ) - - follower_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "main": DynamixelMotorsBusConfig( - port="/dev/tty.usbmodem585A0076891", - motors={ - # name: (index, model) - "shoulder_pan": [1, "xl430-w250"], - "shoulder_lift": [2, "xl430-w250"], - "elbow_flex": [3, "xl330-m288"], - "wrist_flex": [4, "xl330-m288"], - "wrist_roll": [5, "xl330-m288"], - "gripper": [6, "xl330-m288"], - }, - ), - } - ) - - cameras: dict[str, CameraConfig] = field( - default_factory=lambda: { - "laptop": OpenCVCameraConfig( - camera_index=0, - fps=30, - width=640, - height=480, - ), - "phone": OpenCVCameraConfig( - camera_index=1, - fps=30, - width=640, - height=480, - ), - } - ) - - # ~ Koch specific settings ~ - # Sets the leader arm in torque mode with the gripper motor set to this angle. This makes it possible - # to squeeze the gripper and have it spring back to an open position on its own. - gripper_open_degree: float = 35.156 - - mock: bool = False - - -@RobotConfig.register_subclass("koch_bimanual") -@dataclass -class KochBimanualRobotConfig(ManipulatorRobotConfig): - calibration_dir: str = ".cache/calibration/koch_bimanual" - # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as - # the number of motors in your follower arms. - max_relative_target: int | None = None - - leader_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "left": DynamixelMotorsBusConfig( - port="/dev/tty.usbmodem585A0085511", - motors={ - # name: (index, model) - "shoulder_pan": [1, "xl330-m077"], - "shoulder_lift": [2, "xl330-m077"], - "elbow_flex": [3, "xl330-m077"], - "wrist_flex": [4, "xl330-m077"], - "wrist_roll": [5, "xl330-m077"], - "gripper": [6, "xl330-m077"], - }, - ), - "right": DynamixelMotorsBusConfig( - port="/dev/tty.usbmodem575E0031751", - motors={ - # name: (index, model) - "shoulder_pan": [1, "xl330-m077"], - "shoulder_lift": [2, "xl330-m077"], - "elbow_flex": [3, "xl330-m077"], - "wrist_flex": [4, "xl330-m077"], - "wrist_roll": [5, "xl330-m077"], - "gripper": [6, "xl330-m077"], - }, - ), - } - ) - - follower_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "left": DynamixelMotorsBusConfig( - port="/dev/tty.usbmodem585A0076891", - motors={ - # name: (index, model) - "shoulder_pan": [1, "xl430-w250"], - "shoulder_lift": [2, "xl430-w250"], - "elbow_flex": [3, "xl330-m288"], - "wrist_flex": [4, "xl330-m288"], - "wrist_roll": [5, "xl330-m288"], - "gripper": [6, "xl330-m288"], - }, - ), - "right": DynamixelMotorsBusConfig( - port="/dev/tty.usbmodem575E0032081", - motors={ - # name: (index, model) - "shoulder_pan": [1, "xl430-w250"], - "shoulder_lift": [2, "xl430-w250"], - "elbow_flex": [3, "xl330-m288"], - "wrist_flex": [4, "xl330-m288"], - "wrist_roll": [5, "xl330-m288"], - "gripper": [6, "xl330-m288"], - }, - ), - } - ) - - cameras: dict[str, CameraConfig] = field( - default_factory=lambda: { - "laptop": OpenCVCameraConfig( - camera_index=0, - fps=30, - width=640, - height=480, - ), - "phone": OpenCVCameraConfig( - camera_index=1, - fps=30, - width=640, - height=480, - ), - } - ) - - # ~ Koch specific settings ~ - # Sets the leader arm in torque mode with the gripper motor set to this angle. This makes it possible - # to squeeze the gripper and have it spring back to an open position on its own. - gripper_open_degree: float = 35.156 - - mock: bool = False - - -@RobotConfig.register_subclass("moss") -@dataclass -class MossRobotConfig(ManipulatorRobotConfig): - calibration_dir: str = ".cache/calibration/moss" - # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as - # the number of motors in your follower arms. - max_relative_target: int | None = None - - leader_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "main": FeetechMotorsBusConfig( - port="/dev/tty.usbmodem58760431091", - motors={ - # name: (index, model) - "shoulder_pan": [1, "sts3215"], - "shoulder_lift": [2, "sts3215"], - "elbow_flex": [3, "sts3215"], - "wrist_flex": [4, "sts3215"], - "wrist_roll": [5, "sts3215"], - "gripper": [6, "sts3215"], - }, - ), - } - ) - - follower_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "main": FeetechMotorsBusConfig( - port="/dev/tty.usbmodem585A0076891", - motors={ - # name: (index, model) - "shoulder_pan": [1, "sts3215"], - "shoulder_lift": [2, "sts3215"], - "elbow_flex": [3, "sts3215"], - "wrist_flex": [4, "sts3215"], - "wrist_roll": [5, "sts3215"], - "gripper": [6, "sts3215"], - }, - ), - } - ) - - cameras: dict[str, CameraConfig] = field( - default_factory=lambda: { - "laptop": OpenCVCameraConfig( - camera_index=0, - fps=30, - width=640, - height=480, - ), - "phone": OpenCVCameraConfig( - camera_index=1, - fps=30, - width=640, - height=480, - ), - } - ) - - mock: bool = False - - -@RobotConfig.register_subclass("so100") -@dataclass -class So100RobotConfig(ManipulatorRobotConfig): - calibration_dir: str = ".cache/calibration/so100" - # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as - # the number of motors in your follower arms. - max_relative_target: int | None = None - - leader_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "main": FeetechMotorsBusConfig( - port="/dev/tty.usbmodem58760431091", - motors={ - # name: (index, model) - "shoulder_pan": [1, "sts3215"], - "shoulder_lift": [2, "sts3215"], - "elbow_flex": [3, "sts3215"], - "wrist_flex": [4, "sts3215"], - "wrist_roll": [5, "sts3215"], - "gripper": [6, "sts3215"], - }, - ), - } - ) - - follower_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "main": FeetechMotorsBusConfig( - port="/dev/tty.usbmodem585A0076891", - motors={ - # name: (index, model) - "shoulder_pan": [1, "sts3215"], - "shoulder_lift": [2, "sts3215"], - "elbow_flex": [3, "sts3215"], - "wrist_flex": [4, "sts3215"], - "wrist_roll": [5, "sts3215"], - "gripper": [6, "sts3215"], - }, - ), - } - ) - - cameras: dict[str, CameraConfig] = field( - default_factory=lambda: { - "laptop": OpenCVCameraConfig( - camera_index=0, - fps=30, - width=640, - height=480, - ), - "phone": OpenCVCameraConfig( - camera_index=1, - fps=30, - width=640, - height=480, - ), - } - ) - - mock: bool = False - - -@RobotConfig.register_subclass("stretch") -@dataclass -class StretchRobotConfig(RobotConfig): - # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as - # the number of motors in your follower arms. - max_relative_target: int | None = None - - cameras: dict[str, CameraConfig] = field( - default_factory=lambda: { - "navigation": OpenCVCameraConfig( - camera_index="/dev/hello-nav-head-camera", - fps=10, - width=1280, - height=720, - rotation=-90, - ), - "head": IntelRealSenseCameraConfig( - name="Intel RealSense D435I", - fps=30, - width=640, - height=480, - rotation=90, - ), - "wrist": IntelRealSenseCameraConfig( - name="Intel RealSense D405", - fps=30, - width=640, - height=480, - ), - } - ) - - mock: bool = False - - -@RobotConfig.register_subclass("lekiwi") -@dataclass -class LeKiwiRobotConfig(RobotConfig): - # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as - # the number of motors in your follower arms. - max_relative_target: int | None = None - - # Network Configuration - ip: str = "192.168.0.193" - port: int = 5555 - video_port: int = 5556 - - cameras: dict[str, CameraConfig] = field( - default_factory=lambda: { - "front": OpenCVCameraConfig( - camera_index="/dev/video0", fps=30, width=640, height=480, rotation=90 - ), - "wrist": OpenCVCameraConfig( - camera_index="/dev/video2", fps=30, width=640, height=480, rotation=180 - ), - } - ) - - calibration_dir: str = ".cache/calibration/lekiwi" - - leader_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "main": FeetechMotorsBusConfig( - port="/dev/tty.usbmodem585A0077581", - motors={ - # name: (index, model) - "shoulder_pan": [1, "sts3215"], - "shoulder_lift": [2, "sts3215"], - "elbow_flex": [3, "sts3215"], - "wrist_flex": [4, "sts3215"], - "wrist_roll": [5, "sts3215"], - "gripper": [6, "sts3215"], - }, - ), - } - ) - - follower_arms: dict[str, MotorsBusConfig] = field( - default_factory=lambda: { - "main": FeetechMotorsBusConfig( - port="/dev/ttyACM0", - motors={ - # name: (index, model) - "shoulder_pan": [1, "sts3215"], - "shoulder_lift": [2, "sts3215"], - "elbow_flex": [3, "sts3215"], - "wrist_flex": [4, "sts3215"], - "wrist_roll": [5, "sts3215"], - "gripper": [6, "sts3215"], - "left_wheel": (7, "sts3215"), - "back_wheel": (8, "sts3215"), - "right_wheel": (9, "sts3215"), - }, - ), - } - ) - - teleop_keys: dict[str, str] = field( - default_factory=lambda: { - # Movement - "forward": "w", - "backward": "s", - "left": "a", - "right": "d", - "rotate_left": "z", - "rotate_right": "x", - # Speed control - "speed_up": "r", - "speed_down": "f", - # quit teleop - "quit": "q", - } - ) - - mock: bool = False diff --git a/lerobot/common/robots/koch/configuration_koch.py b/lerobot/common/robots/koch/configuration_koch.py new file mode 100644 index 00000000..a014d51d --- /dev/null +++ b/lerobot/common/robots/koch/configuration_koch.py @@ -0,0 +1,165 @@ +from dataclasses import dataclass, field + +from lerobot.common.cameras.configs import CameraConfig, OpenCVCameraConfig +from lerobot.common.motors.configs import DynamixelMotorsBusConfig, MotorsBusConfig +from lerobot.common.robots.config_abc import ManipulatorRobotConfig, RobotConfig + + +@RobotConfig.register_subclass("koch") +@dataclass +class KochRobotConfig(ManipulatorRobotConfig): + calibration_dir: str = ".cache/calibration/koch" + # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. + # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as + # the number of motors in your follower arms. + max_relative_target: int | None = None + + leader_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "main": DynamixelMotorsBusConfig( + port="/dev/tty.usbmodem585A0085511", + motors={ + # name: (index, model) + "shoulder_pan": [1, "xl330-m077"], + "shoulder_lift": [2, "xl330-m077"], + "elbow_flex": [3, "xl330-m077"], + "wrist_flex": [4, "xl330-m077"], + "wrist_roll": [5, "xl330-m077"], + "gripper": [6, "xl330-m077"], + }, + ), + } + ) + + follower_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "main": DynamixelMotorsBusConfig( + port="/dev/tty.usbmodem585A0076891", + motors={ + # name: (index, model) + "shoulder_pan": [1, "xl430-w250"], + "shoulder_lift": [2, "xl430-w250"], + "elbow_flex": [3, "xl330-m288"], + "wrist_flex": [4, "xl330-m288"], + "wrist_roll": [5, "xl330-m288"], + "gripper": [6, "xl330-m288"], + }, + ), + } + ) + + cameras: dict[str, CameraConfig] = field( + default_factory=lambda: { + "laptop": OpenCVCameraConfig( + camera_index=0, + fps=30, + width=640, + height=480, + ), + "phone": OpenCVCameraConfig( + camera_index=1, + fps=30, + width=640, + height=480, + ), + } + ) + + # ~ Koch specific settings ~ + # Sets the leader arm in torque mode with the gripper motor set to this angle. This makes it possible + # to squeeze the gripper and have it spring back to an open position on its own. + gripper_open_degree: float = 35.156 + + mock: bool = False + + +@RobotConfig.register_subclass("koch_bimanual") +@dataclass +class KochBimanualRobotConfig(ManipulatorRobotConfig): + calibration_dir: str = ".cache/calibration/koch_bimanual" + # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. + # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as + # the number of motors in your follower arms. + max_relative_target: int | None = None + + leader_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "left": DynamixelMotorsBusConfig( + port="/dev/tty.usbmodem585A0085511", + motors={ + # name: (index, model) + "shoulder_pan": [1, "xl330-m077"], + "shoulder_lift": [2, "xl330-m077"], + "elbow_flex": [3, "xl330-m077"], + "wrist_flex": [4, "xl330-m077"], + "wrist_roll": [5, "xl330-m077"], + "gripper": [6, "xl330-m077"], + }, + ), + "right": DynamixelMotorsBusConfig( + port="/dev/tty.usbmodem575E0031751", + motors={ + # name: (index, model) + "shoulder_pan": [1, "xl330-m077"], + "shoulder_lift": [2, "xl330-m077"], + "elbow_flex": [3, "xl330-m077"], + "wrist_flex": [4, "xl330-m077"], + "wrist_roll": [5, "xl330-m077"], + "gripper": [6, "xl330-m077"], + }, + ), + } + ) + + follower_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "left": DynamixelMotorsBusConfig( + port="/dev/tty.usbmodem585A0076891", + motors={ + # name: (index, model) + "shoulder_pan": [1, "xl430-w250"], + "shoulder_lift": [2, "xl430-w250"], + "elbow_flex": [3, "xl330-m288"], + "wrist_flex": [4, "xl330-m288"], + "wrist_roll": [5, "xl330-m288"], + "gripper": [6, "xl330-m288"], + }, + ), + "right": DynamixelMotorsBusConfig( + port="/dev/tty.usbmodem575E0032081", + motors={ + # name: (index, model) + "shoulder_pan": [1, "xl430-w250"], + "shoulder_lift": [2, "xl430-w250"], + "elbow_flex": [3, "xl330-m288"], + "wrist_flex": [4, "xl330-m288"], + "wrist_roll": [5, "xl330-m288"], + "gripper": [6, "xl330-m288"], + }, + ), + } + ) + + cameras: dict[str, CameraConfig] = field( + default_factory=lambda: { + "laptop": OpenCVCameraConfig( + camera_index=0, + fps=30, + width=640, + height=480, + ), + "phone": OpenCVCameraConfig( + camera_index=1, + fps=30, + width=640, + height=480, + ), + } + ) + + # ~ Koch specific settings ~ + # Sets the leader arm in torque mode with the gripper motor set to this angle. This makes it possible + # to squeeze the gripper and have it spring back to an open position on its own. + gripper_open_degree: float = 35.156 + + mock: bool = False diff --git a/examples/11_use_lekiwi.md b/lerobot/common/robots/lekiwi/README.md similarity index 100% rename from examples/11_use_lekiwi.md rename to lerobot/common/robots/lekiwi/README.md diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py new file mode 100644 index 00000000..7878841e --- /dev/null +++ b/lerobot/common/robots/lekiwi/configuration_lekiwi.py @@ -0,0 +1,88 @@ +from dataclasses import dataclass, field + +from lerobot.common.cameras.configs import CameraConfig, OpenCVCameraConfig +from lerobot.common.motors.configs import FeetechMotorsBusConfig, MotorsBusConfig +from lerobot.common.robots.config_abc import RobotConfig + + +@RobotConfig.register_subclass("lekiwi") +@dataclass +class LeKiwiRobotConfig(RobotConfig): + # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. + # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as + # the number of motors in your follower arms. + max_relative_target: int | None = None + + # Network Configuration + ip: str = "192.168.0.193" + port: int = 5555 + video_port: int = 5556 + + cameras: dict[str, CameraConfig] = field( + default_factory=lambda: { + "front": OpenCVCameraConfig( + camera_index="/dev/video0", fps=30, width=640, height=480, rotation=90 + ), + "wrist": OpenCVCameraConfig( + camera_index="/dev/video2", fps=30, width=640, height=480, rotation=180 + ), + } + ) + + calibration_dir: str = ".cache/calibration/lekiwi" + + leader_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "main": FeetechMotorsBusConfig( + port="/dev/tty.usbmodem585A0077581", + motors={ + # name: (index, model) + "shoulder_pan": [1, "sts3215"], + "shoulder_lift": [2, "sts3215"], + "elbow_flex": [3, "sts3215"], + "wrist_flex": [4, "sts3215"], + "wrist_roll": [5, "sts3215"], + "gripper": [6, "sts3215"], + }, + ), + } + ) + + follower_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "main": FeetechMotorsBusConfig( + port="/dev/ttyACM0", + motors={ + # name: (index, model) + "shoulder_pan": [1, "sts3215"], + "shoulder_lift": [2, "sts3215"], + "elbow_flex": [3, "sts3215"], + "wrist_flex": [4, "sts3215"], + "wrist_roll": [5, "sts3215"], + "gripper": [6, "sts3215"], + "left_wheel": (7, "sts3215"), + "back_wheel": (8, "sts3215"), + "right_wheel": (9, "sts3215"), + }, + ), + } + ) + + teleop_keys: dict[str, str] = field( + default_factory=lambda: { + # Movement + "forward": "w", + "backward": "s", + "left": "a", + "right": "d", + "rotate_left": "z", + "rotate_right": "x", + # Speed control + "speed_up": "r", + "speed_down": "f", + # quit teleop + "quit": "q", + } + ) + + mock: bool = False diff --git a/lerobot/common/robots/robot_abstraction.py b/lerobot/common/robots/lekiwi/robot_lekiwi.py similarity index 100% rename from lerobot/common/robots/robot_abstraction.py rename to lerobot/common/robots/lekiwi/robot_lekiwi.py diff --git a/lerobot/common/robots/manipulator.py b/lerobot/common/robots/manipulator.py index 3099c86c..556dc81d 100644 --- a/lerobot/common/robots/manipulator.py +++ b/lerobot/common/robots/manipulator.py @@ -15,7 +15,7 @@ import torch from lerobot.common.cameras.utils import make_cameras_from_configs from lerobot.common.motors.utils import MotorsBus, make_motors_buses_from_configs -from lerobot.common.robots.configs import ManipulatorRobotConfig +from lerobot.common.robots.config_abc import ManipulatorRobotConfig from lerobot.common.robots.utils import get_arm_id from lerobot.common.utils.robot_utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError diff --git a/lerobot/common/robots/mobile_manipulator.py b/lerobot/common/robots/mobile_manipulator.py index 3450b97d..50e68838 100644 --- a/lerobot/common/robots/mobile_manipulator.py +++ b/lerobot/common/robots/mobile_manipulator.py @@ -13,7 +13,7 @@ from lerobot.common.cameras.utils import make_cameras_from_configs from lerobot.common.motors.feetech import TorqueMode from lerobot.common.motors.feetech_calibration import run_arm_manual_calibration from lerobot.common.motors.utils import MotorsBus, make_motors_buses_from_configs -from lerobot.common.robots.configs import LeKiwiRobotConfig +from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig from lerobot.common.robots.utils import get_arm_id from lerobot.common.utils.robot_utils import RobotDeviceNotConnectedError diff --git a/examples/11_use_moss.md b/lerobot/common/robots/moss/README.md similarity index 100% rename from examples/11_use_moss.md rename to lerobot/common/robots/moss/README.md diff --git a/lerobot/common/robots/moss/configuration_moss.py b/lerobot/common/robots/moss/configuration_moss.py new file mode 100644 index 00000000..da37f7a2 --- /dev/null +++ b/lerobot/common/robots/moss/configuration_moss.py @@ -0,0 +1,68 @@ +from dataclasses import dataclass, field + +from lerobot.common.cameras.configs import CameraConfig, OpenCVCameraConfig +from lerobot.common.motors.configs import FeetechMotorsBusConfig, MotorsBusConfig +from lerobot.common.robots.config_abc import ManipulatorRobotConfig, RobotConfig + + +@RobotConfig.register_subclass("moss") +@dataclass +class MossRobotConfig(ManipulatorRobotConfig): + calibration_dir: str = ".cache/calibration/moss" + # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. + # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as + # the number of motors in your follower arms. + max_relative_target: int | None = None + + leader_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "main": FeetechMotorsBusConfig( + port="/dev/tty.usbmodem58760431091", + motors={ + # name: (index, model) + "shoulder_pan": [1, "sts3215"], + "shoulder_lift": [2, "sts3215"], + "elbow_flex": [3, "sts3215"], + "wrist_flex": [4, "sts3215"], + "wrist_roll": [5, "sts3215"], + "gripper": [6, "sts3215"], + }, + ), + } + ) + + follower_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "main": FeetechMotorsBusConfig( + port="/dev/tty.usbmodem585A0076891", + motors={ + # name: (index, model) + "shoulder_pan": [1, "sts3215"], + "shoulder_lift": [2, "sts3215"], + "elbow_flex": [3, "sts3215"], + "wrist_flex": [4, "sts3215"], + "wrist_roll": [5, "sts3215"], + "gripper": [6, "sts3215"], + }, + ), + } + ) + + cameras: dict[str, CameraConfig] = field( + default_factory=lambda: { + "laptop": OpenCVCameraConfig( + camera_index=0, + fps=30, + width=640, + height=480, + ), + "phone": OpenCVCameraConfig( + camera_index=1, + fps=30, + width=640, + height=480, + ), + } + ) + + mock: bool = False diff --git a/lerobot/common/robots/robot_abc.py b/lerobot/common/robots/robot_abc.py new file mode 100644 index 00000000..3b592f0a --- /dev/null +++ b/lerobot/common/robots/robot_abc.py @@ -0,0 +1,24 @@ +import abc + + +class Robot(abc.ABC): + robot_type: str + features: dict + + @abc.abstractmethod + def connect(self): ... + + @abc.abstractmethod + def calibrate(self): ... + + @abc.abstractmethod + def teleop_step(self, record_data=False): ... + + @abc.abstractmethod + def capture_observation(self): ... + + @abc.abstractmethod + def send_action(self, action): ... + + @abc.abstractmethod + def disconnect(self): ... diff --git a/examples/10_use_so100.md b/lerobot/common/robots/so_100/README.md similarity index 100% rename from examples/10_use_so100.md rename to lerobot/common/robots/so_100/README.md diff --git a/lerobot/common/robots/so_100/configuration_so_100.py b/lerobot/common/robots/so_100/configuration_so_100.py new file mode 100644 index 00000000..95ba8bd2 --- /dev/null +++ b/lerobot/common/robots/so_100/configuration_so_100.py @@ -0,0 +1,68 @@ +from dataclasses import dataclass, field + +from lerobot.common.cameras.configs import CameraConfig, OpenCVCameraConfig +from lerobot.common.motors.configs import FeetechMotorsBusConfig, MotorsBusConfig +from lerobot.common.robots.config_abc import ManipulatorRobotConfig, RobotConfig + + +@RobotConfig.register_subclass("so100") +@dataclass +class So100RobotConfig(ManipulatorRobotConfig): + calibration_dir: str = ".cache/calibration/so100" + # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. + # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as + # the number of motors in your follower arms. + max_relative_target: int | None = None + + leader_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "main": FeetechMotorsBusConfig( + port="/dev/tty.usbmodem58760431091", + motors={ + # name: (index, model) + "shoulder_pan": [1, "sts3215"], + "shoulder_lift": [2, "sts3215"], + "elbow_flex": [3, "sts3215"], + "wrist_flex": [4, "sts3215"], + "wrist_roll": [5, "sts3215"], + "gripper": [6, "sts3215"], + }, + ), + } + ) + + follower_arms: dict[str, MotorsBusConfig] = field( + default_factory=lambda: { + "main": FeetechMotorsBusConfig( + port="/dev/tty.usbmodem585A0076891", + motors={ + # name: (index, model) + "shoulder_pan": [1, "sts3215"], + "shoulder_lift": [2, "sts3215"], + "elbow_flex": [3, "sts3215"], + "wrist_flex": [4, "sts3215"], + "wrist_roll": [5, "sts3215"], + "gripper": [6, "sts3215"], + }, + ), + } + ) + + cameras: dict[str, CameraConfig] = field( + default_factory=lambda: { + "laptop": OpenCVCameraConfig( + camera_index=0, + fps=30, + width=640, + height=480, + ), + "phone": OpenCVCameraConfig( + camera_index=1, + fps=30, + width=640, + height=480, + ), + } + ) + + mock: bool = False diff --git a/lerobot/common/robots/so_100/robot_so_100.py b/lerobot/common/robots/so_100/robot_so_100.py new file mode 100644 index 00000000..e69de29b diff --git a/examples/8_use_stretch.md b/lerobot/common/robots/stretch3/README.md similarity index 100% rename from examples/8_use_stretch.md rename to lerobot/common/robots/stretch3/README.md diff --git a/lerobot/common/robots/stretch3/configuration_stretch3.py b/lerobot/common/robots/stretch3/configuration_stretch3.py new file mode 100644 index 00000000..8520c471 --- /dev/null +++ b/lerobot/common/robots/stretch3/configuration_stretch3.py @@ -0,0 +1,40 @@ +from dataclasses import dataclass, field + +from lerobot.common.cameras.configs import CameraConfig, IntelRealSenseCameraConfig, OpenCVCameraConfig +from lerobot.common.robots.config_abc import RobotConfig + + +@RobotConfig.register_subclass("stretch") +@dataclass +class StretchRobotConfig(RobotConfig): + # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. + # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as + # the number of motors in your follower arms. + max_relative_target: int | None = None + + cameras: dict[str, CameraConfig] = field( + default_factory=lambda: { + "navigation": OpenCVCameraConfig( + camera_index="/dev/hello-nav-head-camera", + fps=10, + width=1280, + height=720, + rotation=-90, + ), + "head": IntelRealSenseCameraConfig( + name="Intel RealSense D435I", + fps=30, + width=640, + height=480, + rotation=90, + ), + "wrist": IntelRealSenseCameraConfig( + name="Intel RealSense D405", + fps=30, + width=640, + height=480, + ), + } + ) + + mock: bool = False diff --git a/lerobot/common/robots/stretch3/stretch3_robot.py b/lerobot/common/robots/stretch3/robot_stretch3.py similarity index 99% rename from lerobot/common/robots/stretch3/stretch3_robot.py rename to lerobot/common/robots/stretch3/robot_stretch3.py index c00d1a19..c3be2d9c 100644 --- a/lerobot/common/robots/stretch3/stretch3_robot.py +++ b/lerobot/common/robots/stretch3/robot_stretch3.py @@ -22,7 +22,7 @@ from stretch_body.gamepad_teleop import GamePadTeleop from stretch_body.robot import Robot as StretchAPI from stretch_body.robot_params import RobotParams -from lerobot.common.robots.configs import StretchRobotConfig +from .configuration_stretch3 import StretchRobotConfig class StretchRobot(StretchAPI): diff --git a/lerobot/common/robots/utils.py b/lerobot/common/robots/utils.py index 8a30c496..cb798632 100644 --- a/lerobot/common/robots/utils.py +++ b/lerobot/common/robots/utils.py @@ -1,16 +1,15 @@ from typing import Protocol -from lerobot.common.robots.configs import ( - AlohaRobotConfig, - KochBimanualRobotConfig, - KochRobotConfig, - LeKiwiRobotConfig, +from lerobot.common.robots.aloha.configuration_aloha import AlohaRobotConfig +from lerobot.common.robots.config_abc import ( ManipulatorRobotConfig, - MossRobotConfig, RobotConfig, - So100RobotConfig, - StretchRobotConfig, ) +from lerobot.common.robots.koch.configuration_koch import KochBimanualRobotConfig, KochRobotConfig +from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig +from lerobot.common.robots.moss.configuration_moss import MossRobotConfig +from lerobot.common.robots.so_100.configuration_so_100 import So100RobotConfig +from lerobot.common.robots.stretch3.configuration_stretch3 import StretchRobotConfig def get_arm_id(name, arm_type): @@ -62,7 +61,7 @@ def make_robot_from_config(config: RobotConfig): return MobileManipulator(config) else: - from lerobot.common.robots.stretch3.stretch3_robot import StretchRobot + from lerobot.common.robots.stretch3.robot_stretch3 import StretchRobot return StretchRobot(config) diff --git a/lerobot/configs/control.py b/lerobot/configs/control.py index 6f49b9a4..94922d1f 100644 --- a/lerobot/configs/control.py +++ b/lerobot/configs/control.py @@ -4,7 +4,7 @@ from pathlib import Path import draccus -from lerobot.common.robots.configs import RobotConfig +from lerobot.common.robots.config_abc import RobotConfig from lerobot.common.utils.utils import auto_select_torch_device, is_amp_available, is_torch_device_available from lerobot.configs import parser from lerobot.configs.policies import PreTrainedConfig