From 9ff829a3a133f6578c79ac9db9f2c420bb566e7a Mon Sep 17 00:00:00 2001
From: Remi <remi.cadene@huggingface.co>
Date: Fri, 6 Sep 2024 21:07:52 +0200
Subject: [PATCH] Add comments for Aloha (#417)

Co-authored-by: Simon Alibert <75076266+aliberts@users.noreply.github.com>
---
 examples/7_get_started_with_real_robot.md      |  7 +++++++
 .../common/robot_devices/robots/manipulator.py | 11 +++++++++++
 lerobot/common/robot_devices/utils.py          | 18 ++++++++++++------
 lerobot/configs/robot/aloha.yaml               |  8 ++++++++
 4 files changed, 38 insertions(+), 6 deletions(-)

diff --git a/examples/7_get_started_with_real_robot.md b/examples/7_get_started_with_real_robot.md
index a15c5c5a..50a2c645 100644
--- a/examples/7_get_started_with_real_robot.md
+++ b/examples/7_get_started_with_real_robot.md
@@ -41,6 +41,13 @@ Or using `poetry`:
 poetry install --sync --extras "dynamixel"
 ```
 
+/!\ For Linux only, ffmpeg and opencv requires conda install for now. Run this exact sequence of commands:
+```bash
+conda install -c conda-forge ffmpeg
+pip uninstall opencv-python
+conda install -c conda-forge opencv>=4.10.0
+```
+
 You are now ready to plug the 5V power supply to the motor bus of the leader arm (the smaller one) since all its motors only require 5V.
 
 Then plug the 12V power supply to the motor bus of the follower arm. It has two motors that need 12V, and the rest will be powered with 5V through the voltage convertor.
diff --git a/lerobot/common/robot_devices/robots/manipulator.py b/lerobot/common/robot_devices/robots/manipulator.py
index c4b2c431..33751976 100644
--- a/lerobot/common/robot_devices/robots/manipulator.py
+++ b/lerobot/common/robot_devices/robots/manipulator.py
@@ -496,6 +496,17 @@ class ManipulatorRobot:
             # Set a velocity limit of 131 as advised by Trossen Robotics
             self.follower_arms[name].write("Velocity_Limit", 131)
 
+            # 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 self.follower_arms[name].motor_names if name != "gripper"
+            ]
+            if len(all_motors_except_gripper) > 0:
+                # 4 corresponds to Extended Position on Aloha motors
+                self.follower_arms[name].write("Operating_Mode", 4, all_motors_except_gripper)
+
             # Use 'position control current based' for follower gripper to be limited by the limit of the current.
             # 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).
diff --git a/lerobot/common/robot_devices/utils.py b/lerobot/common/robot_devices/utils.py
index 79724af9..bcbeb8e0 100644
--- a/lerobot/common/robot_devices/utils.py
+++ b/lerobot/common/robot_devices/utils.py
@@ -1,13 +1,19 @@
+import platform
 import time
 
 
 def busy_wait(seconds):
-    # Significantly more accurate than `time.sleep`, and mandatory for our use case,
-    # but it consumes CPU cycles.
-    # TODO(rcadene): find an alternative: from python 11, time.sleep is precise
-    end_time = time.perf_counter() + seconds
-    while time.perf_counter() < end_time:
-        pass
+    if platform.system() == "Darwin":
+        # On Mac, `time.sleep` is not accurate and we need to use this while loop trick,
+        # but it consumes CPU cycles.
+        # TODO(rcadene): find an alternative: from python 11, time.sleep is precise
+        end_time = time.perf_counter() + seconds
+        while time.perf_counter() < end_time:
+            pass
+    else:
+        # On Linux time.sleep is accurate
+        if seconds > 0:
+            time.sleep(seconds)
 
 
 class RobotDeviceNotConnectedError(Exception):
diff --git a/lerobot/configs/robot/aloha.yaml b/lerobot/configs/robot/aloha.yaml
index 2380cfeb..938fa2e3 100644
--- a/lerobot/configs/robot/aloha.yaml
+++ b/lerobot/configs/robot/aloha.yaml
@@ -1,3 +1,11 @@
+# Aloha: A Low-Cost Hardware for Bimanual Teleoperation
+# https://aloha-2.github.io
+# https://www.trossenrobotics.com/aloha-stationary
+
+# Requires installing extras packages
+# With pip: `pip install -e ".[dynamixel intelrealsense]"`
+# With poetry: `poetry install --sync --extras "dynamixel intelrealsense"`
+
 _target_: lerobot.common.robot_devices.robots.manipulator.ManipulatorRobot
 robot_type: aloha
 # Specific to Aloha, LeRobot comes with default calibration files. Assuming the motors have been