From 67b28e17dc650e7d39a3334623fb99766b9a5b91 Mon Sep 17 00:00:00 2001 From: Remi Cadene Date: Wed, 23 Oct 2024 17:50:40 +0200 Subject: [PATCH] first auto-review --- examples/7_get_started_with_real_robot.md | 7 +++--- lerobot/__init__.py | 3 +++ .../robot_devices/cameras/intelrealsense.py | 3 +-- .../common/robot_devices/cameras/opencv.py | 5 ++--- .../common/robot_devices/motors/dynamixel.py | 22 +++++++++++++------ .../common/robot_devices/motors/feetech.py | 9 ++++---- .../robots/dynamixel_calibration.py | 1 - tests/test_motors.py | 2 +- 8 files changed, 30 insertions(+), 22 deletions(-) diff --git a/examples/7_get_started_with_real_robot.md b/examples/7_get_started_with_real_robot.md index 5933fd560..9db7e2521 100644 --- a/examples/7_get_started_with_real_robot.md +++ b/examples/7_get_started_with_real_robot.md @@ -83,7 +83,7 @@ python lerobot/scripts/find_motors_bus_port.py Example output when identifying the leader arm's port (e.g., `/dev/tty.usbmodem575E0031751` on Mac, or possibly `/dev/ttyACM0` on Linux): ``` -Finding all available ports for the DynamixelMotorsBus. +Finding all available ports for the MotorBus. ['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751'] Remove the usb cable from your DynamixelMotorsBus and press Enter when done. @@ -95,7 +95,7 @@ Reconnect the usb cable. Example output when identifying the follower arm's port (e.g., `/dev/tty.usbmodem575E0032081`, or possibly `/dev/ttyACM1` on Linux): ``` -Finding all available ports for the DynamixelMotorsBus. +Finding all available ports for the MotorBus. ['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751'] Remove the usb cable from your DynamixelMotorsBus and press Enter when done. @@ -544,8 +544,7 @@ To instantiate an [`OpenCVCamera`](../lerobot/common/robot_devices/cameras/openc To find the camera indices, run the following utility script, which will save a few frames from each detected camera: ```bash -python lerobot/scripts/save_images_from_cameras.py \ - --driver opencv \ +python lerobot/common/robot_devices/cameras/opencv.py \ --images-dir outputs/images_from_opencv_cameras ``` diff --git a/lerobot/__init__.py b/lerobot/__init__.py index 5acd8aaad..d462c7337 100644 --- a/lerobot/__init__.py +++ b/lerobot/__init__.py @@ -198,6 +198,8 @@ "koch", "koch_bimanual", "aloha", + "so100", + "moss", ] # lists all available cameras from `lerobot/common/robot_devices/cameras` @@ -209,6 +211,7 @@ # lists all available motors from `lerobot/common/robot_devices/motors` available_motors = [ "dynamixel", + "feetech", ] # keys and values refer to yaml files diff --git a/lerobot/common/robot_devices/cameras/intelrealsense.py b/lerobot/common/robot_devices/cameras/intelrealsense.py index 12336c8bb..e331dee3f 100644 --- a/lerobot/common/robot_devices/cameras/intelrealsense.py +++ b/lerobot/common/robot_devices/cameras/intelrealsense.py @@ -200,8 +200,7 @@ class IntelRealSenseCamera: To find the camera indices of your cameras, you can run our utility script that will save a few frames for each camera: ```bash - python lerobot/scripts/save_images_from_cameras.py \ - --driver intelrealsense \ + python lerobot/common/robot_devices/cameras/intelrealsense.py \ --images-dir outputs/images_from_intelrealsense_cameras ``` diff --git a/lerobot/common/robot_devices/cameras/opencv.py b/lerobot/common/robot_devices/cameras/opencv.py index e89de2f3b..58dd93bd6 100644 --- a/lerobot/common/robot_devices/cameras/opencv.py +++ b/lerobot/common/robot_devices/cameras/opencv.py @@ -216,8 +216,7 @@ class OpenCVCamera: To find the camera indices of your cameras, you can run our utility script that will be save a few frames for each camera: ```bash - python lerobot/scripts/save_images_from_cameras.py \ - --driver opencv \ + python lerobot/common/robot_devices/cameras/opencv.py \ --images-dir outputs/images_from_opencv_cameras ``` @@ -325,7 +324,7 @@ def connect(self): if self.camera_index not in available_cam_ids: raise ValueError( f"`camera_index` is expected to be one of these available cameras {available_cam_ids}, but {self.camera_index} is provided instead. " - "To find the camera index you should use, run `python lerobot/scripts/save_images_from_cameras.py --driver opencv`." + "To find the camera index you should use, run `python lerobot/lerobot/common/robot_devices/cameras/opencv.py`." ) raise OSError(f"Can't access OpenCVCamera({camera_idx}).") diff --git a/lerobot/common/robot_devices/motors/dynamixel.py b/lerobot/common/robot_devices/motors/dynamixel.py index e44f4fe99..1e1396f76 100644 --- a/lerobot/common/robot_devices/motors/dynamixel.py +++ b/lerobot/common/robot_devices/motors/dynamixel.py @@ -261,7 +261,7 @@ class DynamixelMotorsBus: To find the port, you can run our utility script: ```bash python lerobot/scripts/find_motors_bus_port.py - >>> Finding all available ports for the DynamixelMotorsBus. + >>> Finding all available ports for the MotorBus. >>> ['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751'] >>> Remove the usb cable from your DynamixelMotorsBus and press Enter when done. >>> The port of this DynamixelMotorsBus is /dev/tty.usbmodem575E0031751. @@ -371,14 +371,14 @@ def are_motors_configured(self): print(e) return False - def find_motor_indices(self, possible_ids=None): + def find_motor_indices(self, possible_ids=None, num_retry=2): if possible_ids is None: possible_ids = range(MAX_ID_RANGE) indices = [] for idx in tqdm.tqdm(possible_ids): try: - present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID")[0] + present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID", num_retry=num_retry)[0] except ConnectionError: continue @@ -638,7 +638,7 @@ def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | values = np.round(values).astype(np.int32) return values - def read_with_motor_ids(self, motor_models, motor_ids, data_name): + def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY): if self.mock: import tests.mock_dynamixel_sdk as dxl else: @@ -655,7 +655,11 @@ def read_with_motor_ids(self, motor_models, motor_ids, data_name): for idx in motor_ids: group.addParam(idx) - comm = group.txRxPacket() + for _ in range(num_retry): + comm = group.txRxPacket() + if comm == dxl.COMM_SUCCESS: + break + if comm != dxl.COMM_SUCCESS: raise ConnectionError( f"Read failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: " @@ -745,7 +749,7 @@ def read(self, data_name, motor_names: str | list[str] | None = None): return values - def write_with_motor_ids(self, motor_models, motor_ids, data_name, values): + def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY): if self.mock: import tests.mock_dynamixel_sdk as dxl else: @@ -763,7 +767,11 @@ def write_with_motor_ids(self, motor_models, motor_ids, data_name, values): data = convert_to_bytes(value, bytes, self.mock) group.addParam(idx, data) - comm = group.txPacket() + for _ in range(num_retry): + comm = group.txPacket() + if comm == dxl.COMM_SUCCESS: + break + if comm != dxl.COMM_SUCCESS: raise ConnectionError( f"Write failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: " diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index 1ed13d858..030ab399f 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/lerobot/common/robot_devices/motors/feetech.py @@ -94,6 +94,7 @@ "Status": (65, 1), "Moving": (66, 1), "Present_Current": (69, 2), + # Not in the Memory Table "Maximum_Acceleration": (85, 2), } @@ -127,6 +128,7 @@ "sts3215": SCS_SERIES_BAUDRATE_TABLE, } +# High number of retries is needed for feetech compared to dynamixel motors. NUM_READ_RETRY = 20 NUM_WRITE_RETRY = 20 @@ -235,7 +237,6 @@ def __init__(self, message="Joint is out of range"): class FeetechMotorsBus: - # TODO(rcadene): Add a script to find the motor indices without feetechWizzard2 """ The FeetechMotorsBus class allows to efficiently read and write to the attached motors. It relies on the python feetech sdk to communicate with the motors. For more info, see the [feetech SDK Documentation](https://emanual.robotis.com/docs/en/software/feetech/feetech_sdk/sample_code/python_read_write_protocol_2_0/#python-read-write-protocol-20). @@ -243,8 +244,8 @@ class FeetechMotorsBus: A FeetechMotorsBus instance requires a port (e.g. `FeetechMotorsBus(port="/dev/tty.usbmodem575E0031751"`)). To find the port, you can run our utility script: ```bash - python lerobot/common/robot_devices/motors/feetech.py - >>> Finding all available ports for the FeetechMotorsBus. + python lerobot/scripts/find_motors_bus_port.py + >>> Finding all available ports for the MotorsBus. >>> ['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751'] >>> Remove the usb cable from your FeetechMotorsBus and press Enter when done. >>> The port of this FeetechMotorsBus is /dev/tty.usbmodem575E0031751. @@ -255,7 +256,7 @@ class FeetechMotorsBus: ```python motor_name = "gripper" motor_index = 6 - motor_model = "xl330-m288" + motor_model = "sts3215" motors_bus = FeetechMotorsBus( port="/dev/tty.usbmodem575E0031751", diff --git a/lerobot/common/robot_devices/robots/dynamixel_calibration.py b/lerobot/common/robot_devices/robots/dynamixel_calibration.py index 2d582c891..5c4932d2e 100644 --- a/lerobot/common/robot_devices/robots/dynamixel_calibration.py +++ b/lerobot/common/robot_devices/robots/dynamixel_calibration.py @@ -39,7 +39,6 @@ def compute_nearest_rounded_position(position, models): delta_turn = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, models) nearest_pos = np.round(position.astype(float) / delta_turn) * delta_turn return nearest_pos.astype(position.dtype) - return position def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str): diff --git a/tests/test_motors.py b/tests/test_motors.py index 672234071..14cb3b478 100644 --- a/tests/test_motors.py +++ b/tests/test_motors.py @@ -30,8 +30,8 @@ import numpy as np import pytest -from lerobot.common.robot_devices.motors.dynamixel import find_port from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError +from lerobot.scripts.find_motors_bus_port import find_port from tests.utils import TEST_MOTOR_TYPES, make_motors_bus, require_motor