fix(lekiwi): HW fixes v0.4

lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py

@@ -13,6 +13,8 @@ class DaemonLeKiwiRobotConfig(RobotConfig):
 
     id = "daemonlekiwi"
 
+    calibration_dir: str = ".cache/calibration/lekiwi"
+
     teleop_keys: dict[str, str] = field(
         default_factory=lambda: {
             # Movement

lerobot/common/robots/lekiwi/configuration_lekiwi.py

@@ -16,10 +16,10 @@ class LeKiwiRobotConfig(RobotConfig):
     cameras: dict[str, CameraConfig] = field(
         default_factory=lambda: {
             "front": OpenCVCameraConfig(
-                camera_index="/dev/video0", fps=30, width=640, height=480, rotation=90
+                camera_index="/dev/video1", fps=30, width=640, height=480, rotation=90
             ),
             "wrist": OpenCVCameraConfig(
-                camera_index="/dev/video2", fps=30, width=640, height=480, rotation=180
+                camera_index="/dev/video4", fps=30, width=640, height=480, rotation=180
             ),
         }
     )

lerobot/common/robots/lekiwi/daemon_lekiwi.py

@@ -62,7 +62,7 @@ class DaemonLeKiwiRobot(Robot):
         self.zmq_observation_socket = None
 
         self.last_frames = {}
-        self.last_present_speed = {}
+        self.last_present_speed = [0, 0, 0]
         self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32)
 
         # Define three speed levels and a current index
@@ -223,13 +223,13 @@ class DaemonLeKiwiRobot(Robot):
 
     # Copied from robot_lekiwi MobileManipulator class
     def wheel_raw_to_body(
-        self, wheel_raw: dict, wheel_radius: float = 0.05, base_radius: float = 0.125
+        self, wheel_raw: np.array, wheel_radius: float = 0.05, base_radius: float = 0.125
     ) -> tuple:
         """
         Convert wheel raw command feedback back into body-frame velocities.
 
         Parameters:
-          wheel_raw   : Dictionary with raw wheel commands (keys: "left_wheel", "back_wheel", "right_wheel").
+          wheel_raw   : Vector with raw wheel commands ("left_wheel", "back_wheel", "right_wheel").
           wheel_radius: Radius of each wheel (meters).
           base_radius : Distance from the robot center to each wheel (meters).
 
@@ -239,15 +239,9 @@ class DaemonLeKiwiRobot(Robot):
              y_cmd      : Linear velocity in y (m/s).
              theta_cmd  : Rotational velocity in deg/s.
         """
-        # Extract the raw values in order.
-        raw_list = [
-            int(wheel_raw.get("left_wheel", 0)),
-            int(wheel_raw.get("back_wheel", 0)),
-            int(wheel_raw.get("right_wheel", 0)),
-        ]
 
         # Convert each raw command back to an angular speed in deg/s.
-        wheel_degps = np.array([DaemonLeKiwiRobot.raw_to_degps(r) for r in raw_list])
+        wheel_degps = np.array([DaemonLeKiwiRobot.raw_to_degps(int(r)) for r in wheel_raw])
         # Convert from deg/s to rad/s.
         wheel_radps = wheel_degps * (np.pi / 180.0)
         # Compute each wheel’s linear speed (m/s) from its angular speed.
@@ -264,6 +258,7 @@ class DaemonLeKiwiRobot(Robot):
         theta_cmd = theta_rad * (180.0 / np.pi)
         return (x_cmd, y_cmd, theta_cmd)
 
+    # TODO(Steven): This is flaky, for example, if we received a state but failed decoding the image, we will not update any value
     def get_data(self):
         # Copied from robot_lekiwi.py
         """Polls the video socket for up to 15 ms. If data arrives, decode only
@@ -271,10 +266,10 @@ class DaemonLeKiwiRobot(Robot):
         nothing arrives for any field, use the last known values."""
 
         frames = {}
-        present_speed = {}
+        present_speed = []
 
         # TODO(Steven): Size is being assumed, is this safe?
-        remote_arm_state_tensor = torch.zeros(6, dtype=torch.float32)
+        remote_arm_state_tensor = torch.empty(6, dtype=torch.float32)
 
         # Poll up to 15 ms
         poller = zmq.Poller()
@@ -303,7 +298,9 @@ class DaemonLeKiwiRobot(Robot):
         # Decode only the final message
         try:
             observation = json.loads(last_msg)
+            observation[OBS_STATE] = np.array(observation[OBS_STATE])
 
+            # TODO(Steven): Consider getting directly the item with observation[OBS_STATE]
             state_observation = {k: v for k, v in observation.items() if k.startswith(OBS_STATE)}
             image_observation = {k: v for k, v in observation.items() if k.startswith(OBS_IMAGES)}
 
@@ -320,10 +317,10 @@ class DaemonLeKiwiRobot(Robot):
             if state_observation is not None and frames is not None:
                 self.last_frames = frames
 
-                remote_arm_state_tensor = torch.tensor(state_observation[:6], dtype=torch.float32)
+                remote_arm_state_tensor = torch.tensor(state_observation[OBS_STATE][:6], dtype=torch.float32)
                 self.last_remote_arm_state = remote_arm_state_tensor
 
-                present_speed = state_observation[6:]
+                present_speed = state_observation[OBS_STATE][6:]
                 self.last_present_speed = present_speed
             else:
                 frames = self.last_frames

lerobot/common/robots/lekiwi/daemon_lekiwi_app.py

@@ -32,7 +32,7 @@ def main():
     logging.info("Starting LeKiwiRobot teleoperation")
     start = time.perf_counter()
     duration = 0
-    while duration < 20:
+    while duration < 100:
         arm_action = leader_arm.get_action()
         base_action = keyboard.get_action()
         action = np.append(arm_action, base_action) if base_action.size > 0 else arm_action

lerobot/common/robots/lekiwi/lekiwi_robot.py

@@ -203,6 +203,7 @@ class LeKiwiRobot(Robot):
 
         self.actuators_bus.set_calibration(calibration)
 
+    # TODO(Steven): Should this be dict[str, Any] ?
     def get_observation(self) -> dict[str, np.ndarray]:
         """The returned observations do not have a batch dimension."""
         if not self.is_connected:
@@ -274,13 +275,17 @@ class LeKiwiRobot(Robot):
     def update_last_observation(self, stop_event):
         while not stop_event.is_set():
             obs = self.get_observation()
+            obs[OBS_STATE] = obs[OBS_STATE].tolist()  # Needed for np.array be serializable
             with self.observation_lock:
                 self.last_observation = obs
             # TODO(Steven): Consider adding a delay to not starve the CPU
+            # TODO(Steven): Check this value
+            time.sleep(0.5)
 
     def stop(self):
         # TODO(Steven): Assumes there's only 3 motors for base
         logging.info("Stopping base")
+        # TODO(Steven): Check if these operations are thread safe!
         self.actuators_bus.write("Goal_Speed", [0, 0, 0], self.base_actuators)
         logging.info("Base motors stopped")
 
@@ -299,23 +304,28 @@ class LeKiwiRobot(Robot):
         logging.info("LeKiwi robot server started. Waiting for commands...")
 
         try:
-            while True:
+            start = time.perf_counter()
+            duration = 0
+            while duration < 100:
                 loop_start_time = time.time()
 
                 try:
                     msg = self.zmq_cmd_socket.recv_string(zmq.NOBLOCK)
                     data = np.array(json.loads(msg))
-                    self.send_action(data)
+                    _action_sent = self.send_action(data)
                     last_cmd_time = time.time()
                 except zmq.Again:
                     logging.warning("ZMQ again")
                 except Exception as e:
                     logging.error("Message fetching failed: %s", e)
 
+                # TODO(Steven): Check this value
                 # Watchdog: stop the robot if no command is received for over 0.5 seconds.
                 now = time.time()
                 if now - last_cmd_time > 0.5:
-                    self.stop()
+                    # TODO(Steven): This doesn't seem to be thread safe!
+                    # self.stop()
+                    pass
 
                 with self.observation_lock:
                     # TODO(Steven): This operation is blocking if no listener is available
@@ -323,9 +333,12 @@ class LeKiwiRobot(Robot):
 
                 # Ensure a short sleep to avoid overloading the CPU.
                 elapsed = time.time() - loop_start_time
+
+                # TODO(Steven): Check this value
                 time.sleep(
                     max(0.033 - elapsed, 0)
                 )  # If robot jitters increase the sleep and monitor cpu load with `top` in cmd
+                duration = time.perf_counter() - start
         except KeyboardInterrupt:
             print("Shutting down LeKiwi server.")
         finally:
@@ -349,7 +362,7 @@ class LeKiwiRobot(Robot):
             cam.disconnect()
         self.zmq_observation_socket.close()
         self.zmq_cmd_socket.close()
-        self.context.term()
+        self.zmq_context.term()
         self.is_connected = False
 
     def __del__(self):