From 3ba05d53b9c90ee1aaac160b55da0e8cea7c4555 Mon Sep 17 00:00:00 2001
From: Remi Cadene <re.cadene@gmail.com>
Date: Sun, 7 Jul 2024 11:01:04 +0200
Subject: [PATCH] Remove async in dynamixel

---
 .../common/robot_devices/cameras/opencv.py    |  1 +
 .../common/robot_devices/motors/dynamixel.py  | 17 ++++++++++
 lerobot/common/robot_devices/robots/koch.py   | 31 +++++--------------
 3 files changed, 26 insertions(+), 23 deletions(-)

diff --git a/lerobot/common/robot_devices/cameras/opencv.py b/lerobot/common/robot_devices/cameras/opencv.py
index ae6ba86c..17e6b38f 100644
--- a/lerobot/common/robot_devices/cameras/opencv.py
+++ b/lerobot/common/robot_devices/cameras/opencv.py
@@ -98,6 +98,7 @@ class OpenCVCamera:
 
     ```python
     camera = OpenCVCamera(2)
+    camera.connect()
     color_image = camera.read()
     ```
     """
diff --git a/lerobot/common/robot_devices/motors/dynamixel.py b/lerobot/common/robot_devices/motors/dynamixel.py
index 440ddde8..90be6b87 100644
--- a/lerobot/common/robot_devices/motors/dynamixel.py
+++ b/lerobot/common/robot_devices/motors/dynamixel.py
@@ -189,6 +189,23 @@ class DriveMode(enum.Enum):
 
 
 class DynamixelMotorsBus:
+    """
+    Example of usage for 1 motor connected to the bus:
+    ```python
+    motor_name = "gripper"
+    motor_index = 6
+    motor_model = "xl330-m077"
+
+    robot = DynamixelMotorsBus(
+        port="/dev/tty.usbmodem575E0031751",
+        motors={motor_name: (motor_index, motor_model)},
+    )
+    robot.connect()
+
+    while True:
+        robot.teleop_step()
+    ```
+    """
     def __init__(
         self,
         port: str,
diff --git a/lerobot/common/robot_devices/robots/koch.py b/lerobot/common/robot_devices/robots/koch.py
index 0461e02b..16e70c41 100644
--- a/lerobot/common/robot_devices/robots/koch.py
+++ b/lerobot/common/robot_devices/robots/koch.py
@@ -226,6 +226,9 @@ class KochRobot:
     Example of usage:
     ```python
     robot = KochRobot()
+    robot.connect()
+    while True:
+        robot.teleop_step()
     ```
     """
 
@@ -247,9 +250,6 @@ class KochRobot:
         self.is_connected = False
         self.logs = {}
 
-        self.async_read = False
-        self.async_write = False
-
     def connect(self):
         if self.is_connected:
             raise ValueError(f"KochRobot is already connected.")
@@ -323,10 +323,7 @@ class KochRobot:
         leader_pos = {}
         for name in self.leader_arms:
             now = time.perf_counter()
-            if self.async_read:
-                leader_pos[name] = self.leader_arms[name].async_read("Present_Position")
-            else:
-                leader_pos[name] = self.leader_arms[name].read("Present_Position")
+            leader_pos[name] = self.leader_arms[name].read("Present_Position")
             self.logs[f"read_leader_{name}_pos_dt_s"] = time.perf_counter() - now
 
         follower_goal_pos = {}
@@ -336,10 +333,7 @@ class KochRobot:
         # Send action
         for name in self.follower_arms:
             now = time.perf_counter()
-            if self.async_write:
-                self.follower_arms[name].async_write("Goal_Position", follower_goal_pos[name])
-            else:
-                self.follower_arms[name].write("Goal_Position", follower_goal_pos[name])
+            self.follower_arms[name].write("Goal_Position", follower_goal_pos[name])
             self.logs[f"write_follower_{name}_goal_pos_dt_s"] = time.perf_counter() - now
 
         # Early exit when recording data is not requested
@@ -350,10 +344,7 @@ class KochRobot:
         follower_pos = {}
         for name in self.follower_arms:
             now = time.perf_counter()
-            if self.async_read:
-                follower_pos[name] = self.follower_arms[name].async_read("Present_Position")
-            else:
-                follower_pos[name] = self.follower_arms[name].read("Present_Position")
+            follower_pos[name] = self.follower_arms[name].read("Present_Position")
             self.logs[f"read_follower_{name}_pos_dt_s"] = time.perf_counter() - now
 
         # Create state by concatenating follower current position
@@ -394,10 +385,7 @@ class KochRobot:
         # Read follower position
         follower_pos = {}
         for name in self.follower_arms:
-            if self.async_read:
-                follower_pos[name] = self.follower_arms[name].async_read("Present_Position")
-            else:
-                follower_pos[name] = self.follower_arms[name].read("Present_Position")
+            follower_pos[name] = self.follower_arms[name].read("Present_Position")
 
         # Create state by concatenating follower current position
         state = []
@@ -432,7 +420,4 @@ class KochRobot:
                 from_idx = to_idx
 
         for name in self.follower_arms:
-            if self.async_write:
-                self.follower_arms[name].async_write("Goal_Position", follower_goal_pos[name].astype(np.int32))
-            else:
-                self.follower_arms[name].write("Goal_Position", follower_goal_pos[name].astype(np.int32))
+            self.follower_arms[name].write("Goal_Position", follower_goal_pos[name].astype(np.int32))