Add gello capabities to lerobot

Include an example called cooper.yaml which exemplifies the use
of Gello with a bimanual setup

Signed-off-by: vmayoral <[email protected]>

.gitignore

@@ -7,6 +7,7 @@ wandb
 data
 data*
 outputs
+src
 
 # Apple
 .DS_Store

README.md

@@ -417,3 +417,22 @@ Additionally, if you are using any of the particular policy architecture, pretra
   year={2024}
 }
 ```
+
+## Gello
+
+Gello integration can be added by installing first the package using the corresponding fork of the `gello` package:
+
+```bash
+pip install -e "git+https://gitlab.com/accelerationrobotics/customers/standardcooper/teleop@teleop#egg=gello"
+```
+
+See `examples/u850/3_teleop_lerobot_gello.py` for an example of how to use the Gello robot.
+
+### NOTE on permissions
+
+Often, it's needed to assign permissions to the corresponding serial port.
+
+On Linux, you can do this by running:
+```bash
+sudo chmod 666 /dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT9BTDLW-if00-port0 
+```

examples/u850/3_teleop_lerobot_gello.py

@@ -0,0 +1,134 @@
+import time
+import numpy as np
+
+from wasabi import color
+
+from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera
+from lerobot.common.robot_devices.motors.ufactory import XArmWrapper
+from lerobot.common.robot_devices.robots.manipulator import ManipulatorRobot
+from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus
+from lerobot.common.robot_devices.motors.gello import GelloDynamixelWrapper
+# from lerobot.common.robot_devices.motors.gello_aux import GelloDynamixelWrapper
+
+
+# Defines how to communicate with the motors of the leader and follower arms
+leader_arms = {
+    "left": GelloDynamixelWrapper(
+        port="/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT9BTDLW-if00-port0",
+        motors={
+            # name: (index, model)
+            "joint1": (1, "xl330-m288"),
+            "joint2": (2, "xl330-m288"), 
+            "joint3": (3, "xl330-m288"),
+            "joint4": (4, "xl330-m288"),
+            "joint5": (5, "xl330-m288"),
+            "joint6": (6, "xl330-m288"),
+            "gripper": (7, "xl330-m077"),
+        },
+    ),
+    "right": GelloDynamixelWrapper(
+        port="/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT9BTEGF-if00-port0",
+        motors={
+            # name: (index, model)
+            "joint1": (1, "xl330-m288"),
+            "joint2": (2, "xl330-m288"), 
+            "joint3": (3, "xl330-m288"),
+            "joint4": (4, "xl330-m288"),
+            "joint5": (5, "xl330-m288"),
+            "joint6": (6, "xl330-m288"),
+            "gripper": (7, "xl330-m077"),
+        },
+    ),    
+}
+follower_arms = {
+    "left": XArmWrapper(
+        port="192.168.1.236",
+        motors={
+            # name: (index, model)
+            "joint1": (1, "ufactory-850"),
+            "joint2": (2, "ufactory-850"),
+            "joint3": (3, "ufactory-850"),
+            "joint4": (4, "ufactory-850"),
+            "joint5": (5, "ufactory-850"),
+            "joint6": (6, "ufactory-850"),
+            "gripper": (7, "ufactory-850"),
+        },
+    ),
+    "right": XArmWrapper(
+        port="192.168.1.218",
+        motors={
+            # name: (index, model)
+            "joint1": (1, "ufactory-850"),
+            "joint2": (2, "ufactory-850"),
+            "joint3": (3, "ufactory-850"),
+            "joint4": (4, "ufactory-850"),
+            "joint5": (5, "ufactory-850"),
+            "joint6": (6, "ufactory-850"),
+            "gripper": (7, "ufactory-850"),
+        },
+    ),    
+}
+robot = ManipulatorRobot(
+    robot_type="u850",
+    calibration_dir=".cache/calibration/u850",
+    leader_arms=leader_arms,
+    follower_arms=follower_arms,
+    # cameras={
+    #     "top": OpenCVCamera(4, fps=30, width=640, height=480),
+    #     "wrist": OpenCVCamera(10, fps=30, width=640, height=480),
+    # },
+)
+
+# Connect motors buses and cameras if any (Required)
+robot.connect()
+
+# print(robot.leader_arms["main"].get_position())
+# print("---")
+# print(robot.follower_arms["main"].get_position())
+
+try:
+    import time
+
+    start_time = time.time()
+    iteration_count = 0
+
+    while True:
+        robot.teleop_step()
+        # time.sleep(0.033)  # 30 Hz -> barely smooth
+        # time.sleep(0.004)  # 250 Hz -> very smooth
+
+        iteration_count += 1
+        elapsed_time = time.time() - start_time
+
+        if elapsed_time >= 1.0:  # Print frequency every second
+            frequency = iteration_count / elapsed_time
+            print(f"Current teleoperation frequency: {frequency:.2f} Hz")
+            start_time = time.time()
+            iteration_count = 0
+
+        # # Recording data, only joints
+        # leader_pos = robot.leader_arms["main"].get_position()
+        # follower_pos = robot.follower_arms["main"].get_position()
+        # observation, action = robot.teleop_step(record_data=True)
+
+        # print(follower_pos)
+        # print(observation)
+        # print(leader_pos)
+        # print(action)
+        # print("---")
+
+        # Recording data with cameras
+        # observation, action = robot.teleop_step(record_data=True)
+        # print(observation["observation.images.top"].shape)
+        # print(observation["observation.images.wrist"].shape)
+        # print(observation["observation.images.top"].min().item())
+        # print(observation["observation.images.top"].max().item())
+        # print("---")
+
+
+except KeyboardInterrupt:
+    print("Operation interrupted by user.")
+
+
+print(color("Disconnecting...", "red"))
+robot.disconnect()

lerobot/common/robot_devices/motors/gello.py

@@ -0,0 +1,126 @@
+import enum
+import numpy as np
+from dynamixel_sdk import *
+from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
+
+from gello.agents.gello_agent import GelloAgent
+
+class TorqueMode(enum.Enum):
+    ENABLED = 1
+    DISABLED = 0
+
+class GelloDynamixelWrapper:
+    """
+    Wrapper for Dynamixel motors used in Gello robot
+    
+    NOTE: Gello builds upon inter-process communication using ZeroMQ. In particular, the communication happens through the RobotEnv class instanciated within the run_env.py script which connects to the GelloAgent, which abstracts away the hardware and the actuation.
+
+    This wrapper is used to interface with the Gello robot's hardware part only. For that purpose, only the GelloAgent is used without the ZeroMQ part and/or its communication overhead.
+    """
+    GRIPPER_OPEN = 800
+    GRIPPER_CLOSE = 0
+
+    def __init__(
+        self,
+        port: str,
+        motors: dict[str, tuple[int, str]],
+        mock: bool = False,
+    ):
+        """Initialize the Gello Dynamixel wrapper
+        
+        Args:
+            port: Serial port for Dynamixel communication
+            motors: Dictionary mapping motor names to (ID, model) tuples
+            joint_ids: IDs of joint motors in order
+            joint_offsets: Joint offset angles in radians
+            joint_signs: Signs (+1/-1) for joint directions
+            gripper_config: Tuple of (gripper_id, open_pos, closed_pos)
+            mock: If True, run in simulation mode
+            baudrate: Communication baudrate
+        """
+        self.port = port
+        self.motors = motors
+        self.mock = mock
+
+        self.is_connected = False
+        self.gello = None
+
+    @property
+    def motor_names(self) -> list[str]:
+        return list(self.motors.keys())
+
+    @property 
+    def motor_models(self) -> list[str]:
+        return [model for _, model in self.motors.values()]
+
+    @property
+    def motor_indices(self) -> list[int]:
+        return [idx for idx, _ in self.motors.values()]
+
+    def write(self, data_name, values: int | float | np.ndarray, motor_names: str | list[str] | None = None):
+        pass  # TODO (@vmayoral): implement if of interest
+
+    def read(self, data_name, motor_names: str | list[str] | None = None):
+        pass  # TODO (@vmayoral): implement if of interest
+
+    def enable(self, follower: bool = False):
+        pass  # TODO (@vmayoral): implement if of interest
+
+    def connect(self):
+        """Connect to the Dynamixel motors"""
+        if self.is_connected:
+            raise RobotDeviceAlreadyConnectedError(
+                f"GelloDynamixelWrapper({self.port}) is already connected."
+            )
+
+        if self.mock:
+            return
+
+        self.gello = agent = GelloAgent(port=self.port, start_joints=None)
+
+        self.is_connected = True
+
+    def disconnect(self):
+        """Disconnect from the Dynamixel motors"""
+        if not self.is_connected:
+            raise RobotDeviceNotConnectedError(
+                f"GelloDynamixelWrapper({self.port}) is not connected."
+            )
+
+        self.is_connected = False
+
+    def get_position(self) -> list[float]:
+        """Get current positions of all joints and gripper
+        
+        Returns:
+            List of positions [joint1, joint2, ..., gripper]
+        """
+        if self.mock:
+            return [0.0] * (len(self.joint_ids) + 1)
+
+        rads_pos = self.gello.act(None).tolist()
+        degrees_pos = np.rad2deg(rads_pos[:-1])  # Convert all but last to degrees
+
+        # NOTE: heuristic obtained from the code of the XArmRobot in the `gello` package
+        #
+        gripper_value = self.GRIPPER_OPEN + rads_pos[-1] * (self.GRIPPER_CLOSE - self.GRIPPER_OPEN)
+        degrees_pos = degrees_pos.tolist()  # Convert to list for appending
+        degrees_pos.append(gripper_value)
+
+        return degrees_pos
+
+    def set_position(self, position: np.ndarray):
+        """Set positions for all joints and gripper
+        
+        Args:
+            position: Array of positions [joint1, joint2, ..., gripper]
+        """
+        if self.mock:
+            return
+        pass
+
+    def robot_reset(self):
+        """Reset the robot to a safe state"""
+        if not self.mock:
+            # Open gripper to safe position
+            pass

lerobot/configs/robot/cooper.yaml

@@ -0,0 +1,83 @@
+# See user manual at https://www.ufactory.cc/wp-content/uploads/2023/07/UFactory-850-User-Manual-V2.1.0.pdf
+#
+_target_: lerobot.common.robot_devices.robots.manipulator.ManipulatorRobot
+robot_type: u850
+calibration_dir: .cache/calibration/u850
+
+# `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: null
+
+leader_arms:  # L
+  left:
+    _target_: lerobot.common.robot_devices.motors.gello.GelloDynamixelWrapper
+    port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT9BTDLW-if00-port0"
+    motors:
+      # name: (index, model)
+      joint1: [1, "xl330-m288"]
+      joint2: [2, "xl330-m288"]
+      joint3: [3, "xl330-m288"]
+      joint4: [4, "xl330-m288"]
+      joint5: [5, "xl330-m288"]
+      joint6: [6, "xl330-m288"]
+      gripper: [7, "xl330-m077"]
+  right:
+    _target_: lerobot.common.robot_devices.motors.gello.GelloDynamixelWrapper
+    port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT9BTEGF-if00-port0"
+    motors:
+      # name: (index, model)
+      joint1: [1, "xl330-m288"]
+      joint2: [2, "xl330-m288"]
+      joint3: [3, "xl330-m288"]
+      joint4: [4, "xl330-m288"]
+      joint5: [5, "xl330-m288"]
+      joint6: [6, "xl330-m288"]
+      gripper: [7, "xl330-m077"]
+
+follower_arms:
+  left:
+    _target_: lerobot.common.robot_devices.motors.ufactory.XArmWrapper
+    port: 192.168.1.236
+    motors:
+      # name: (index, model)
+      joint1: [1, "ufactory-850"]  # +/- 360 degrees
+      joint2: [2, "ufactory-850"]  # +/- 132 degrees
+      joint3: [3, "ufactory-850"]  # -242 to 3.5 degrees
+      joint4: [4, "ufactory-850"]  # +/- 360 degrees
+      joint5: [5, "ufactory-850"]  # +/- 124 degrees
+      joint6: [6, "ufactory-850"]  # +/- 360 degrees
+      # joint7: [7, "ufactory-850"]  # NOT PRESENT in 850
+      gripper: [7, "ufactory-850"]  # gripper
+  right:
+    _target_: lerobot.common.robot_devices.motors.ufactory.XArmWrapper
+    port: 192.168.1.218
+    motors:
+      # name: (index, model)
+      joint1: [1, "ufactory-850"]  # +/- 360 degrees
+      joint2: [2, "ufactory-850"]  # +/- 132 degrees
+      joint3: [3, "ufactory-850"]  # -242 to 3.5 degrees
+      joint4: [4, "ufactory-850"]  # +/- 360 degrees
+      joint5: [5, "ufactory-850"]  # +/- 124 degrees
+      joint6: [6, "ufactory-850"]  # +/- 360 degrees
+      # joint7: [7, "ufactory-850"]  # NOT PRESENT in 850
+      gripper: [7, "ufactory-850"]  # gripper
+
+# cameras:
+#   top:
+#     _target_: lerobot.common.robot_devices.cameras.opencv.OpenCVCamera
+#     camera_index: 4
+#     fps: 30
+#     width: 640
+#     height: 480
+#   wrist:
+#     _target_: lerobot.common.robot_devices.cameras.opencv.OpenCVCamera
+#     camera_index: 10
+#     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: 35.156