tangger/lerobot
1
0
Fork 1
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

All tests passing except test_control_robot.py

Browse Source
This commit is contained in:
Remi Cadene
2024-07-09 22:53:39 +02:00
parent a0432f1608
commit 798373e7bf
14 changed files with 493 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
lerobot/common/robot_devices/cameras/opencv.py
View File

@@ -1,5 +1,6 @@
import argparse
import math
import threading
import time
from dataclasses import dataclass, replace
from pathlib import Path
@@ -8,9 +9,8 @@ from threading import Thread
import cv2
from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
# Using 1 thread to avoid blocking the main thread.
# Especially useful during data collection when other threads are used
# to save the images.
# Use 1 thread to avoid blocking the main thread. Especially useful during data collection
# when other threads are used to save the images.
cv2.setNumThreads(1)
import numpy as np
@@ -89,6 +89,10 @@ class OpenCVCameraConfig:
height: int | None = None
color: str = "rgb"
def __post_init__(self):
if self.color not in ["rgb", "bgr"]:
raise ValueError(f"Expected color values are 'rgb' or 'bgr', but {self.color} is provided.")
class OpenCVCamera:
# TODO(rcadene): improve dosctring
@@ -122,12 +126,10 @@ class OpenCVCamera:
if not isinstance(self.camera_index, int):
raise ValueError(f"Camera index must be provided as an int, but {self.camera_index} was given instead.")
if self.color not in ["rgb", "bgr"]:
raise ValueError(f"Expected color values are 'rgb' or 'bgr', but {self.color} is provided.")
self.camera = None
self.is_connected = False
self.thread = None
self.stop_event = None
self.color_image = None
self.logs = {}
@@ -159,26 +161,26 @@ class OpenCVCamera:
# needs to be re-created.
self.camera = cv2.VideoCapture(self.camera_index)
if self.fps:
if self.fps is not None:
self.camera.set(cv2.CAP_PROP_FPS, self.fps)
if self.width:
if self.width is not None:
self.camera.set(cv2.CAP_PROP_FRAME_WIDTH, self.width)
if self.height:
if self.height is not None:
self.camera.set(cv2.CAP_PROP_FRAME_HEIGHT, self.height)
actual_fps = self.camera.get(cv2.CAP_PROP_FPS)
actual_width = self.camera.get(cv2.CAP_PROP_FRAME_WIDTH)
actual_height = self.camera.get(cv2.CAP_PROP_FRAME_HEIGHT)
if self.fps and not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
if self.fps is not None and not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
raise OSError(
f"Can't set {self.fps=} for camera {self.camera_index}. Actual value is {actual_fps}."
)
if self.width and self.width != actual_width:
if self.width is not None and self.width != actual_width:
raise OSError(
f"Can't set {self.width=} for camera {self.camera_index}. Actual value is {actual_width}."
)
if self.height and self.height != actual_height:
if self.height is not None and self.height != actual_height:
raise OSError(
f"Can't set {self.height=} for camera {self.camera_index}. Actual value is {actual_height}."
)
@@ -216,6 +218,10 @@ class OpenCVCamera:
if requested_color == "rgb":
color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
h, w, _ = color_image.shape
if h != self.height or w != self.width:
raise OSError(f"Can't capture color image with expected height and width ({self.height} x {self.width}). ({h} x {w}) returned instead.")
# log the number of seconds it took to read the image
self.logs["delta_timestamp_s"] = time.perf_counter() - start_time
@@ -225,7 +231,7 @@ class OpenCVCamera:
return color_image
def read_loop(self):
while True:
while self.stop_event is None or not self.stop_event.is_set():
self.color_image = self.read()
def async_read(self):
@@ -233,6 +239,7 @@ class OpenCVCamera:
raise RobotDeviceNotConnectedError(f"OpenCVCamera({self.camera_index}) is not connected. Try running `camera.connect()` first.")
if self.thread is None:
self.stop_event = threading.Event()
self.thread = Thread(target=self.read_loop, args=())
self.thread.daemon = True
self.thread.start()
@@ -242,27 +249,29 @@ class OpenCVCamera:
num_tries += 1
time.sleep(1/self.fps)
if num_tries > self.fps:
if self.thread.ident is None and not self.thread.is_alive():
if self.thread.ident is None or not self.thread.is_alive():
raise Exception("The thread responsible for `self.async_read()` took too much time to start. There might be an issue. Verify that `self.thread.start()` has been called.")
return self.color_image
def disconnect(self):
if not self.is_connected:
raise RobotDeviceNotConnectedError(f"OpenCVCamera({self.camera_index}) is not connected. Try running `camera.connect()` first.")
self.camera.release()
self.camera = None
if self.thread is not None and self.thread.is_alive():
# wait for the thread to finish
self.stop_event.set()
self.thread.join()
self.thread = None
self.stop_event = None
self.camera.release()
self.camera = None
self.is_connected = False
def __del__(self):
if self.is_connected:
if getattr(self, "is_connected", False):
self.disconnect()

50
lerobot/common/robot_devices/motors/dynamixel.py
View File

@@ -101,6 +101,7 @@ MODEL_CONTROL_TABLE = {
}
# TODO(rcadene): find better namming for these functions
def uint32_to_int32(values: np.ndarray):
"""
Convert an unsigned 32-bit integer array to a signed 32-bit integer array.
@@ -120,35 +121,18 @@ def int32_to_uint32(values: np.ndarray):
values[i] = values[i] + 4294967296
return values
def motor_position_to_angle(position: np.ndarray) -> np.ndarray:
"""
Convert from motor position in [-2048, 2048] to radian in [-pi, pi]
"""
return (position / 2048) * 3.14
def motor_angle_to_position(angle: np.ndarray) -> np.ndarray:
"""
Convert from radian in [-pi, pi] to motor position in [-2048, 2048]
"""
return ((angle / 3.14) * 2048).astype(np.int64)
# def pwm2vel(pwm: np.ndarray) -> np.ndarray:
# def motor_position_to_angle(position: np.ndarray) -> np.ndarray:
# """
# :param pwm: numpy array of pwm/s joint velocities
# :return: numpy array of rad/s joint velocities
# Convert from motor position in [-2048, 2048] to radian in [-pi, pi]
# """
# return pwm * 3.14 / 2048
# return (position / 2048) * 3.14
# def vel2pwm(vel: np.ndarray) -> np.ndarray:
# def motor_angle_to_position(angle: np.ndarray) -> np.ndarray:
# """
# :param vel: numpy array of rad/s joint velocities
# :return: numpy array of pwm/s joint velocities
# Convert from radian in [-pi, pi] to motor position in [-2048, 2048]
# """
# return (vel * 2048 / 3.14).astype(np.int64)
# return ((angle / 3.14) * 2048).astype(np.int64)
def get_group_sync_key(data_name, motor_names):
@@ -285,15 +269,18 @@ class DynamixelMotorsBus:
return values
def read(self, data_name, motor_names: list[str] | None = None):
def read(self, data_name, motor_names: str | list[str] | None = None):
if not self.is_connected:
raise ValueError(f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`.")
raise RobotDeviceNotConnectedError(f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`.")
start_time = time.perf_counter()
if motor_names is None:
motor_names = self.motor_names
if isinstance(motor_names, str):
motor_names = [motor_names]
motor_ids = []
models = []
for name in motor_names:
@@ -352,7 +339,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 ValueError(f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`.")
raise RobotDeviceNotConnectedError(f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`.")
start_time = time.perf_counter()
@@ -444,10 +431,17 @@ class DynamixelMotorsBus:
if not self.is_connected:
raise RobotDeviceNotConnectedError(f"DynamixelMotorsBus({self.port}) is not connected. Try running `motors_bus.connect()` first.")
closePort
if self.port_handler is not None:
self.port_handler.closePort()
self.port_handler = None
self.packet_handler = None
self.group_readers = {}
self.group_writers = {}
self.is_connected = False
def __del__(self):
if self.is_connected:
if getattr(self, "is_connected", False):
self.disconnect()

123
lerobot/common/robot_devices/robots/koch.py
View File

@@ -14,15 +14,21 @@ from lerobot.common.robot_devices.motors.dynamixel import (
TorqueMode,
)
from lerobot.common.robot_devices.motors.utils import MotorsBus
from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
URL_HORIZONTAL_POSITION = {
"follower": "https://raw.githubusercontent.com/huggingface/lerobot/main/media/koch/follower_horizontal.png",
"leader": "https://raw.githubusercontent.com/huggingface/lerobot/main/media/koch/leader_horizontal.png",
}
URL_90_DEGREE_POSITION = {
"follower": "https://raw.githubusercontent.com/huggingface/lerobot/main/media/koch/follower_90_degree.png",
"leader": "https://raw.githubusercontent.com/huggingface/lerobot/main/media/koch/leader_90_degree.png",
}
########################################################################
# Calibration logic
########################################################################
# TARGET_HORIZONTAL_POSITION = motor_position_to_angle(np.array([0, -1024, 1024, 0, -1024, 0]))
# TARGET_90_DEGREE_POSITION = motor_position_to_angle(np.array([1024, 0, 0, 1024, 0, -1024]))
# GRIPPER_OPEN = motor_position_to_angle(np.array([-400]))
TARGET_HORIZONTAL_POSITION = np.array([0, -1024, 1024, 0, -1024, 0])
TARGET_90_DEGREE_POSITION = np.array([1024, 0, 0, 1024, 0, -1024])
GRIPPER_OPEN = np.array([-400])
@@ -137,11 +143,16 @@ def reset_arm(arm: MotorsBus):
arm.write("Drive_Mode", DriveMode.NON_INVERTED.value)
def run_arm_calibration(arm: MotorsBus, name: str):
def run_arm_calibration(arm: MotorsBus, name: str, arm_type: str):
""" Example of usage:
```python
run_arm_calibration(arm, "left", "follower")
```
"""
reset_arm(arm)
# TODO(rcadene): document what position 1 mean
print(f"Please move the '{name}' arm to the horizontal position (gripper fully closed)")
print(f"Please move the '{name} {arm_type}' arm to the horizontal position (gripper fully closed, see {URL_HORIZONTAL_POSITION[arm_type]})")
input("Press Enter to continue...")
horizontal_homing_offset = compute_homing_offset(
@@ -149,7 +160,7 @@ def run_arm_calibration(arm: MotorsBus, name: str):
)
# TODO(rcadene): document what position 2 mean
print(f"Please move the '{name}' arm to the 90 degree position (gripper fully open)")
print(f"Please move the '{name} {arm_type}' arm to the 90 degree position (gripper fully open, see {URL_90_DEGREE_POSITION[arm_type]})")
input("Press Enter to continue...")
drive_mode = compute_drive_mode(arm, horizontal_homing_offset)
@@ -184,42 +195,15 @@ class KochRobotConfig:
```
"""
# Define all the components of the robot
# Define all components of the robot
leader_arms: dict[str, MotorsBus] = field(
default_factory=lambda: {
"main": DynamixelMotorsBus(
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"),
},
),
}
default_factory=lambda: {}
)
follower_arms: dict[str, MotorsBus] = field(
default_factory=lambda: {
"main": DynamixelMotorsBus(
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"),
},
),
}
default_factory=lambda: {}
)
cameras: dict[str, Camera] = field(default_factory=lambda: {})
class KochRobot:
"""Tau Robotics: https://tau-robotics.com
@@ -306,6 +290,11 @@ class KochRobot:
# Orders the robot to move
robot.send_action(action)
```
Example of disconnecting which is not mandatory since we disconnect when the object is deleted:
```python
robot.disconnect()
```
"""
def __init__(
@@ -328,59 +317,68 @@ class KochRobot:
def connect(self):
if self.is_connected:
raise ValueError(f"KochRobot is already connected.")
raise RobotDeviceAlreadyConnectedError(f"KochRobot is already connected. Do not run `robot.connect()` twice.")
if not self.leader_arms and not self.follower_arms and not self.cameras:
raise ValueError("KochRobot doesn't have any device to connect. See example of usage in docstring of the class.")
# Connect the arms
for name in self.follower_arms:
self.follower_arms[name].connect()
self.leader_arms[name].connect()
# Reset the arms and load or run calibration
if self.calibration_path.exists():
# Reset all arms before setting calibration
for name in self.follower_arms:
reset_arm(self.follower_arms[name])
for name in self.leader_arms:
reset_arm(self.leader_arms[name])
with open(self.calibration_path, "rb") as f:
calibration = pickle.load(f)
else:
# Run calibration process which begins by reseting all arms
calibration = self.run_calibration()
self.calibration_path.parent.mkdir(parents=True, exist_ok=True)
with open(self.calibration_path, "wb") as f:
pickle.dump(calibration, f)
# Set calibration
for name in self.follower_arms:
self.follower_arms[name].set_calibration(calibration[f"follower_{name}"])
self.follower_arms[name].write("Torque_Enable", 1)
for name in self.leader_arms:
self.leader_arms[name].set_calibration(calibration[f"leader_{name}"])
# TODO(rcadene): add comments
self.leader_arms[name].write("Goal_Position", GRIPPER_OPEN, "gripper")
self.leader_arms[name].write("Torque_Enable", 1, "gripper")
# Enable torque on all motors of the follower arms
for name in self.follower_arms:
self.follower_arms[name].write("Torque_Enable", 1)
# Enable torque on the gripper of the leader arms, and move it to 45 degrees,
# so that we can use it as a trigger to close the gripper of the follower arms.
for name in self.leader_arms:
self.leader_arms[name].write("Torque_Enable", 1, "gripper")
self.leader_arms[name].write("Goal_Position", GRIPPER_OPEN, "gripper")
# Connect the cameras
for name in self.cameras:
self.cameras[name].connect()
self.is_connected = True
def run_calibration(self):
if not self.is_connected:
raise ValueError(f"KochRobot is not connected. You need to run `robot.connect()`.")
calibration = {}
for name in self.follower_arms:
homing_offset, drive_mode = run_arm_calibration(self.follower_arms[name], f"{name} follower")
homing_offset, drive_mode = run_arm_calibration(self.follower_arms[name], name, "follower")
calibration[f"follower_{name}"] = {}
for idx, motor_name in enumerate(self.follower_arms[name].motor_names):
calibration[f"follower_{name}"][motor_name] = (homing_offset[idx], drive_mode[idx])
for name in self.leader_arms:
homing_offset, drive_mode = run_arm_calibration(self.leader_arms[name], f"{name} leader")
homing_offset, drive_mode = run_arm_calibration(self.leader_arms[name], name, "leader")
calibration[f"leader_{name}"] = {}
for idx, motor_name in enumerate(self.leader_arms[name].motor_names):
@@ -392,7 +390,7 @@ class KochRobot:
self, record_data=False
) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
if not self.is_connected:
raise ValueError(f"KochRobot is not connected. You need to run `robot.connect()`.")
raise RobotDeviceNotConnectedError(f"KochRobot is not connected. You need to run `robot.connect()`.")
# Prepare to assign the positions of the leader to the follower
leader_pos = {}
@@ -455,7 +453,7 @@ class KochRobot:
def capture_observation(self):
if not self.is_connected:
raise ValueError(f"KochRobot is not connected. You need to run `robot.connect()`.")
raise RobotDeviceNotConnectedError(f"KochRobot is not connected. You need to run `robot.connect()`.")
# Read follower position
follower_pos = {}
@@ -481,9 +479,9 @@ class KochRobot:
obs_dict[f"observation.images.{name}"] = torch.from_numpy(images[name])
return obs_dict
def send_action(self, action):
def send_action(self, action: torch.Tensor):
if not self.is_connected:
raise ValueError(f"KochRobot is not connected. You need to run `robot.connect()`.")
raise RobotDeviceNotConnectedError(f"KochRobot is not connected. You need to run `robot.connect()`.")
from_idx = 0
to_idx = 0
@@ -496,3 +494,22 @@ class KochRobot:
for name in self.follower_arms:
self.follower_arms[name].write("Goal_Position", follower_goal_pos[name].astype(np.int32))
def disconnect(self):
if not self.is_connected:
raise RobotDeviceNotConnectedError(f"KochRobot is not connected. You need to run `robot.connect()` before disconnecting.")
for name in self.follower_arms:
self.follower_arms[name].disconnect()
for name in self.leader_arms:
self.leader_arms[name].disconnect()
for name in self.cameras:
self.cameras[name].disconnect()
self.is_connected = False
def __del__(self):
if getattr(self, "is_connected", False):
self.disconnect()