refactor(robots): lekiwi v0.2

lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py

@@ -0,0 +1,37 @@
+from dataclasses import dataclass, field
+
+from lerobot.common.cameras.configs import CameraConfig
+from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig
+from lerobot.common.motors.configs import FeetechMotorsBusConfig, MotorsBusConfig
+from lerobot.common.robots.config import RobotConfig
+
+
+@RobotConfig.register_subclass("daemon_lekiwi")
+@dataclass
+class DaemonLeKiwiRobotConfig(RobotConfig):
+    # `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: int | None = None
+
+    # Network Configuration
+    remote_ip: str = "192.168.0.193"
+    port_zmq_cmd: int = 5555
+    port_zmq_observations: int = 5556
+
+    teleop_keys: dict[str, str] = field(
+        default_factory=lambda: {
+            # Movement
+            "forward": "w",
+            "backward": "s",
+            "left": "a",
+            "right": "d",
+            "rotate_left": "z",
+            "rotate_right": "x",
+            # Speed control
+            "speed_up": "r",
+            "speed_down": "f",
+            # quit teleop
+            "quit": "q",
+        }
+    )

lerobot/common/robots/lekiwi/configuration_lekiwi.py

@@ -2,22 +2,15 @@ from dataclasses import dataclass, field
 
 from lerobot.common.cameras.configs import CameraConfig
 from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig
-from lerobot.common.motors.configs import FeetechMotorsBusConfig, MotorsBusConfig
 from lerobot.common.robots.config import RobotConfig
 
 
 @RobotConfig.register_subclass("lekiwi")
 @dataclass
 class LeKiwiRobotConfig(RobotConfig):
-    # `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: int | None = None
 
-    # Network Configuration
+    port_zmq_cmd: int = 5555
-    ip: str = "192.168.0.193"
+    port_zmq_observations: int = 5556
-    port: int = 5555
-    video_port: int = 5556
 
     cameras: dict[str, CameraConfig] = field(
         default_factory=lambda: {
@@ -32,58 +25,15 @@ class LeKiwiRobotConfig(RobotConfig):
 
     calibration_dir: str = ".cache/calibration/lekiwi"
     
-    leader_arms: dict[str, MotorsBusConfig] = field(
+    port_motor_bus = "/dev/ttyACM0"
-        default_factory=lambda: {
-            "main": FeetechMotorsBusConfig(
-                port="/dev/tty.usbmodem585A0077581",
-                motors={
-                    # name: (index, model)
-                    "shoulder_pan": [1, "sts3215"],
-                    "shoulder_lift": [2, "sts3215"],
-                    "elbow_flex": [3, "sts3215"],
-                    "wrist_flex": [4, "sts3215"],
-                    "wrist_roll": [5, "sts3215"],
-                    "gripper": [6, "sts3215"],
-                },
-            ),
-        }
-    )
 
-    follower_arms: dict[str, MotorsBusConfig] = field(
+    # TODO(Steven): consider split this into arm and base
-        default_factory=lambda: {
+    shoulder_pan: tuple = (1, "sts3215")
-            "main": FeetechMotorsBusConfig(
+    shoulder_lift: tuple = (2, "sts3215")
-                port="/dev/ttyACM0",
+    elbow_flex: tuple=(3, "sts3215")
-                motors={
+    wrist_flex: tuple=(4, "sts3215")
-                    # name: (index, model)
+    wrist_roll: tuple=(5, "sts3215")
-                    "shoulder_pan": [1, "sts3215"],
+    gripper: tuple =(6, "sts3215")
-                    "shoulder_lift": [2, "sts3215"],
+    left_wheel: tuple= (7, "sts3215")
-                    "elbow_flex": [3, "sts3215"],
+    back_wheel: tuple =  (8, "sts3215")
-                    "wrist_flex": [4, "sts3215"],
+    right_wheel: tuple = (9, "sts3215")
-                    "wrist_roll": [5, "sts3215"],
-                    "gripper": [6, "sts3215"],
-                    "left_wheel": (7, "sts3215"),
-                    "back_wheel": (8, "sts3215"),
-                    "right_wheel": (9, "sts3215"),
-                },
-            ),
-        }
-    )
-
-    teleop_keys: dict[str, str] = field(
-        default_factory=lambda: {
-            # Movement
-            "forward": "w",
-            "backward": "s",
-            "left": "a",
-            "right": "d",
-            "rotate_left": "z",
-            "rotate_right": "x",
-            # Speed control
-            "speed_up": "r",
-            "speed_down": "f",
-            # quit teleop
-            "quit": "q",
-        }
-    )
-
-    mock: bool = False

lerobot/common/robots/lekiwi/daemon_lekiwi.py

@@ -0,0 +1,437 @@
+#!/usr/bin/env python
+
+# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import json
+import logging
+import time
+import threading
+import numpy as np
+import time
+# import torch
+import base64
+import cv2
+import torch
+
+from lerobot.common.cameras.utils import make_cameras_from_configs
+from lerobot.common.constants import OBS_IMAGES, OBS_STATE
+from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
+from ..robot import Robot
+from ..utils import ensure_safe_goal_position
+from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig
+import zmq
+
+# TODO(Steven): This doesn't need to inherit from Robot
+# But we do it for now to offer a familiar API
+# TODO(Steven): This doesn't need to take care of the
+# mapping from teleop to motor commands, but given that
+# we already have a middle-man (this class) we add it here
+class DaemonLeKiwiRobot(Robot):
+
+    config_class = DaemonLeKiwiRobotConfig
+    name = "daemonlekiwi"
+
+    def __init__(self, config: DaemonLeKiwiRobotConfig):
+        super().__init__(config)
+        self.config = config
+        self.id = config.id
+        self.robot_type = config.type
+
+        self.max_relative_target = config.max_relative_target
+
+        self.remote_ip = config.remote_ip
+        self.port_zmq_cmd = config.port_zmq_cmd
+        self.port_zmq_observations = config.port_zmq_observations
+
+        self.teleop_keys = config.teleop_keys
+
+        self.zmq_context = None
+        self.zmq_cmd_socket = None
+        self.zmq_observation_socket = None
+
+        self.last_frames = {}
+        self.last_present_speed = {}
+        self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32)
+
+        # Define three speed levels and a current index
+        self.speed_levels = [
+            {"xy": 0.1, "theta": 30},  # slow
+            {"xy": 0.2, "theta": 60},  # medium
+            {"xy": 0.3, "theta": 90},  # fast
+        ]
+        self.speed_index = 0  # Start at slow
+
+        # Keyboard state for base teleoperation.
+        # self.running = True
+        # self.pressed_keys = {
+        #     "forward": False,
+        #     "backward": False,
+        #     "left": False,
+        #     "right": False,
+        #     "rotate_left": False,
+        #     "rotate_right": False,
+        # }
+
+        self.is_connected = False
+        self.logs = {}
+
+    @property
+    def state_feature(self) -> dict:
+        # TODO(Steven): Get this from the data fetched?
+        # return {
+        #     "dtype": "float32",
+        #     "shape": (len(self.actuators),),
+        #     "names": {"motors": list(self.actuators.motors)},
+        # }
+        pass
+
+    @property
+    def action_feature(self) -> dict:
+        return self.state_feature
+
+    @property
+    def camera_features(self) -> dict[str, dict]:
+        # TODO(Steven): Fetch this info or set it static?
+        # cam_ft = {}
+        # for cam_key, cam in self.cameras.items():
+        #     cam_ft[cam_key] = {
+        #         "shape": (cam.height, cam.width, cam.channels),
+        #         "names": ["height", "width", "channels"],
+        #         "info": None,
+        #     }
+        # return cam_ft
+        pass
+    
+    def connect(self) -> None:
+        if self.is_connected:
+            raise DeviceAlreadyConnectedError(
+                "LeKiwi Daemon is already connected. Do not run `robot.connect()` twice."
+            )
+
+        self.zmq_context = zmq.Context()
+        self.zmq_cmd_socket = self.zmq_context.socket(zmq.PUSH)
+        zmq_cmd_locator = f"tcp://{self.remote_ip}:{self.port_zmq_cmd}"
+        self.zmq_cmd_socket.connect(zmq_cmd_locator)
+        self.zmq_cmd_socket.setsockopt(zmq.CONFLATE, 1)
+
+        self.zmq_observation_socket = self.zmq_context.socket(zmq.PULL)
+        zmq_observations_locator = f"tcp://{self.remote_ip}:{self.port_zmq_observations}"
+        self.zmq_observation_socket.connect(zmq_observations_locator)
+        self.zmq_observation_socket.setsockopt(zmq.CONFLATE,1)
+
+        self.is_connected = True
+
+    def calibrate(self) -> None:
+        # TODO(Steven): Nothing to calibrate
+        pass
+    
+    # Consider moving these static functions out of the class
+    # Copied from robot_lekiwi MobileManipulator class
+    @staticmethod
+    def degps_to_raw(degps: float) -> int:
+        steps_per_deg = 4096.0 / 360.0
+        speed_in_steps = abs(degps) * steps_per_deg
+        speed_int = int(round(speed_in_steps))
+        if speed_int > 0x7FFF:
+            speed_int = 0x7FFF
+        if degps < 0:
+            return speed_int | 0x8000
+        else:
+            return speed_int & 0x7FFF
+    
+    # Copied from robot_lekiwi MobileManipulator class
+    @staticmethod
+    def raw_to_degps(raw_speed: int) -> float:
+        steps_per_deg = 4096.0 / 360.0
+        magnitude = raw_speed & 0x7FFF
+        degps = magnitude / steps_per_deg
+        if raw_speed & 0x8000:
+            degps = -degps
+        return degps
+    
+    # Copied from robot_lekiwi MobileManipulator class
+    def body_to_wheel_raw(
+        self,
+        x_cmd: float,
+        y_cmd: float,
+        theta_cmd: float,
+        wheel_radius: float = 0.05,
+        base_radius: float = 0.125,
+        max_raw: int = 3000,
+    ) -> dict:
+        """
+        Convert desired body-frame velocities into wheel raw commands.
+
+        Parameters:
+          x_cmd      : Linear velocity in x (m/s).
+          y_cmd      : Linear velocity in y (m/s).
+          theta_cmd  : Rotational velocity (deg/s).
+          wheel_radius: Radius of each wheel (meters).
+          base_radius : Distance from the center of rotation to each wheel (meters).
+          max_raw    : Maximum allowed raw command (ticks) per wheel.
+
+        Returns:
+          A dictionary with wheel raw commands:
+             {"left_wheel": value, "back_wheel": value, "right_wheel": value}.
+
+        Notes:
+          - Internally, the method converts theta_cmd to rad/s for the kinematics.
+          - The raw command is computed from the wheels angular speed in deg/s
+            using degps_to_raw(). If any command exceeds max_raw, all commands
+            are scaled down proportionally.
+        """
+        # Convert rotational velocity from deg/s to rad/s.
+        theta_rad = theta_cmd * (np.pi / 180.0)
+        # Create the body velocity vector [x, y, theta_rad].
+        velocity_vector = np.array([x_cmd, y_cmd, theta_rad])
+
+        # Define the wheel mounting angles with a -90° offset.
+        angles = np.radians(np.array([240, 120, 0]) - 90)
+        # Build the kinematic matrix: each row maps body velocities to a wheel’s linear speed.
+        # The third column (base_radius) accounts for the effect of rotation.
+        m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
+
+        # Compute each wheel’s linear speed (m/s) and then its angular speed (rad/s).
+        wheel_linear_speeds = m.dot(velocity_vector)
+        wheel_angular_speeds = wheel_linear_speeds / wheel_radius
+
+        # Convert wheel angular speeds from rad/s to deg/s.
+        wheel_degps = wheel_angular_speeds * (180.0 / np.pi)
+
+        # Scaling
+        steps_per_deg = 4096.0 / 360.0
+        raw_floats = [abs(degps) * steps_per_deg for degps in wheel_degps]
+        max_raw_computed = max(raw_floats)
+        if max_raw_computed > max_raw:
+            scale = max_raw / max_raw_computed
+            wheel_degps = wheel_degps * scale
+
+        # Convert each wheel’s angular speed (deg/s) to a raw integer.
+        wheel_raw = [DaemonLeKiwiRobot.degps_to_raw(deg) for deg in wheel_degps]
+
+        return {"left_wheel": wheel_raw[0], "back_wheel": wheel_raw[1], "right_wheel": wheel_raw[2]}
+    
+    # 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
+    ) -> 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_radius: Radius of each wheel (meters).
+          base_radius : Distance from the robot center to each wheel (meters).
+
+        Returns:
+          A tuple (x_cmd, y_cmd, theta_cmd) where:
+             x_cmd      : Linear velocity in x (m/s).
+             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])
+        # 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.
+        wheel_linear_speeds = wheel_radps * wheel_radius
+
+        # Define the wheel mounting angles with a -90° offset.
+        angles = np.radians(np.array([240, 120, 0]) - 90)
+        m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
+
+        # Solve the inverse kinematics: body_velocity = M⁻¹ · wheel_linear_speeds.
+        m_inv = np.linalg.inv(m)
+        velocity_vector = m_inv.dot(wheel_linear_speeds)
+        x_cmd, y_cmd, theta_rad = velocity_vector
+        theta_cmd = theta_rad * (180.0 / np.pi)
+        return (x_cmd, y_cmd, theta_cmd)
+    
+    def get_data(self):
+        """Polls the video socket for up to 15 ms. If data arrives, decode only
+        the *latest* message, returning frames, speed, and arm state. If
+        nothing arrives for any field, use the last known values."""
+
+        frames = {}
+        present_speed = {}
+        remote_arm_state_tensor = torch.zeros(6, dtype=torch.float32)
+
+        # Poll up to 15 ms
+        poller = zmq.Poller()
+        poller.register(self.zmq_observation_socket, zmq.POLLIN)
+        socks = dict(poller.poll(15))
+        if self.zmq_observation_socket not in socks or socks[self.zmq_observation_socket] != zmq.POLLIN:
+            # No new data arrived → reuse ALL old data
+            return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
+
+        # Drain all messages, keep only the last
+        last_msg = None
+        while True:
+            try:
+                obs_string = self.zmq_observation_socket.recv_string(zmq.NOBLOCK)
+                last_msg = obs_string
+            except zmq.Again:
+                break
+
+        if not last_msg:
+            # No new message → also reuse old
+            return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
+
+        # Decode only the final message
+        try:
+            observation = json.loads(last_msg)
+
+            #TODO(Steven): Check this
+            images_dict = observation.get("images", {})
+            new_speed = observation.get("present_speed", {})
+            new_arm_state = observation.get("follower_arm_state", None)
+
+            # Convert images
+            for cam_name, image_b64 in images_dict.items():
+                if image_b64:
+                    jpg_data = base64.b64decode(image_b64)
+                    np_arr = np.frombuffer(jpg_data, dtype=np.uint8)
+                    frame_candidate = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+                    if frame_candidate is not None:
+                        frames[cam_name] = frame_candidate
+
+            # If remote_arm_state is None and frames is None there is no message then use the previous message
+            if new_arm_state is not None and frames is not None:
+                self.last_frames = frames
+
+                remote_arm_state_tensor = torch.tensor(new_arm_state, dtype=torch.float32)
+                self.last_remote_arm_state = remote_arm_state_tensor
+
+                present_speed = new_speed
+                self.last_present_speed = new_speed
+            else:
+                frames = self.last_frames
+
+                remote_arm_state_tensor = self.last_remote_arm_state
+
+                present_speed = self.last_present_speed
+
+        except Exception as e:
+            print(f"[DEBUG] Error decoding video message: {e}")
+            # If decode fails, fall back to old data
+            return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
+        return frames, present_speed, remote_arm_state_tensor
+    
+    # TODO(Steven): The returned space is different from the get_observation of LeKiwiRobot
+    # This returns body-frames velocities instead of wheel pos/speeds
+    def get_observation(self) -> dict[str, np.ndarray]:
+        """
+        Capture observations from the remote robot: current follower arm positions,
+        present wheel speeds (converted to body-frame velocities: x, y, theta),
+        and a camera frame.
+        """
+        if not self.is_connected:
+            raise DeviceNotConnectedError(
+                "DaemonLeKiwiRobot is not connected. You need to run `robot.connect()`."
+            )
+
+        obs_dict = {}
+
+        # TODO(Steven): Check this
+        frames, present_speed, remote_arm_state_tensor = self.get_data()
+        body_state = self.wheel_raw_to_body(present_speed)
+        body_state_mm = (body_state[0] * 1000.0, body_state[1] * 1000.0, body_state[2])  # Convert x,y to mm/s
+        wheel_state_tensor = torch.tensor(body_state_mm, dtype=torch.float32)
+        combined_state_tensor = torch.cat((remote_arm_state_tensor, wheel_state_tensor), dim=0)
+        
+        obs_dict = {OBS_STATE: combined_state_tensor}
+
+        # Loop over each configured camera
+        for cam_name, cam in self.cameras.items():
+            frame = frames.get(cam_name, None)
+            if frame is None:
+                # Create a black image using the camera's configured width, height, and channels
+                frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8)
+            obs_dict[f"{OBS_IMAGES}.{cam_name}"] = torch.from_numpy(frame)
+
+        return obs_dict
+
+    def from_teleop_action_to_motor_action(self, action):
+        # # Speed control
+        #     elif key.char == self.teleop_keys["speed_up"]:
+        #         self.speed_index = min(self.speed_index + 1, 2)
+        #         print(f"Speed index increased to {self.speed_index}")
+        #     elif key.char == self.teleop_keys["speed_down"]:
+        #         self.speed_index = max(self.speed_index - 1, 0)
+        #         print(f"Speed index decreased to {self.speed_index}")
+        pass
+
+    # TODO(Steven)
+    def send_action(self, action: np.ndarray) -> np.ndarray:
+        # Copied from S100 robot
+        """Command lekiwi to move to a target joint configuration.
+
+        The relative action magnitude may be clipped depending on the configuration parameter
+        `max_relative_target`. In this case, the action sent differs from original action.
+        Thus, this function always returns the action actually sent.
+
+        Args:
+            action (np.ndarray): array containing the goal positions for the motors.
+
+        Raises:
+            RobotDeviceNotConnectedError: if robot is not connected.
+
+        Returns:
+            np.ndarray: the action sent to the motors, potentially clipped.
+        """
+        if not self.is_connected:
+            raise DeviceNotConnectedError(
+                "ManipulatorRobot is not connected. You need to run `robot.connect()`."
+            )
+
+        goal_pos = action
+
+        # Cap goal position when too far away from present position.
+        # /!\ Slower fps expected due to reading from the follower.
+        if self.config.max_relative_target is not None:
+            present_pos = self.actuators.read("Present_Position")
+            goal_pos = ensure_safe_goal_position(goal_pos, present_pos, self.config.max_relative_target)
+
+        # Send goal position to the actuators
+        # TODO(Steven): Base motors should set a vel instead
+        self.actuators.write("Goal_Position", goal_pos.astype(np.int32))
+
+        return goal_pos
+    
+    def print_logs(self):
+        # TODO(Steven): Refactor logger
+        pass
+
+    def disconnect(self):
+        if not self.is_connected:
+            raise DeviceNotConnectedError(
+                "LeKiwi is not connected. You need to run `robot.connect()` before disconnecting."
+            )
+        # TODO(Steven): Consider sending a stop to the remote mobile robot
+        self.zmq_observation_socket.close()
+        self.zmq_cmd_socket.close()
+        self.zmq_context.term()
+        self.is_connected = False
+    
+    def __del__(self):
+        if getattr(self, "is_connected", False):
+            self.disconnect()

lerobot/common/robots/lekiwi/daemon_lekiwi_app.py

@@ -0,0 +1,52 @@
+from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig
+from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig
+from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig
+from .daemon_lekiwi import DaemonLeKiwiRobot
+import time
+import logging
+
+def main():
+
+    logging.info("Configuring Teleop Devices")
+    leader_arm_config = SO100TeleopConfig(port="/dev/tty.usbmodem585A0085511")
+    leader_arm = SO100Teleop(leader_arm_config)
+
+    keyboard_config = KeyboardTeleopConfig()
+    keyboard = KeyboardTeleop(keyboard_config)
+
+    logging.info("Connecting Teleop Devices")
+    leader_arm.connect()
+    keyboard.connect()
+
+    logging.info("Configuring LeKiwiRobot Daemon")
+    robot_config = DaemonLeKiwiRobotConfig()
+    robot = DaemonLeKiwiRobot(robot_config)
+    
+    logging.info("Connecting remote LeKiwiRobot")
+    robot.connect() # Establishes ZMQ sockets with the remote mobile robot
+
+    logging.info("Starting LeKiwiRobot teleoperation")
+    start = time.perf_counter()
+    duration = 0
+    while duration < 20:
+        
+        arm_action = leader_arm.get_action()
+        base_action = keyboard.get_action()
+        action = {
+            **arm_action,
+            **base_action
+        }
+        robot.send_action(action) # Translates to motor space + sends over ZMQ
+        robot.get_observation() # Receives over ZMQ, translate to body-frame vel
+
+        duration = time.perf_counter() - start
+
+    logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon")
+    robot.disconnect() # Cleans ZMQ comms
+    leader_arm.disconnect()
+    keyboard.disconnect()
+
+    logging.info("Finished LeKiwiRobot cleanly")
+
+if __name__ == "__main__":
+    main()

lerobot/common/robots/lekiwi/lekiwi_api.py

@@ -1,35 +0,0 @@
-def main():
-    teleop_arm_config = SO100TeleopConfig(port="/dev/tty.usbmodem585A0085511")
-    teleop_arm = SO100Teleop(teleop_arm_config)
-
-    keyboard_config = KeyboardTeleopConfig()
-    keyboard = KeyboardTeleop(keyboard_config)
-
-    robot_config = kiwiconfig(port="/dev/tty.usbmodem575E0032081")
-    robot = KiwiRobotDaemon(robot_config)
-
-    teleop_arm.connect()
-    keyboard.connect()
-    robot.connect() # Establish ZMQ sockets with the mobile robot
-
-    start = time.perf_counter()
-    duration = 0
-    while duration < 20:
-        
-        arm_action = teleop_arm.get_action()
-        base_action = keyboard.get_action()
-        action = {
-            **arm_action,
-            # **base_action ??
-        }
-        robot.send_action(action) # sends over ZMQ
-        # robot.get_observation() # receives over ZMQ
-
-        print(action)
-        duration = time.perf_counter() - start
-
-    robot.disconnect() # cleans ZMQ comms
-    teleop.disconnect()
-
-if __name__ == "__main__":
-    main()

lerobot/common/robots/lekiwi/lekiwi_app.py

@@ -0,0 +1,22 @@
+import logging
+from .configuration_lekiwi import LeKiwiRobotConfig
+from .lekiwi_robot import LeKiwiRobot
+
+def main():
+
+    logging.info("Configuring LeKiwiRobot")
+    robot_config = LeKiwiRobotConfig()
+    robot = LeKiwiRobot(robot_config)
+
+    logging.info("Connecting LeKiwiRobot")
+    robot.connect()
+
+    # Remotely teleoperated
+    logging.info("Starting LeKiwiRobot teleoperation")
+    robot.run()
+    
+    logging.info("Finished LeKiwiRobot cleanly")
+
+
+if __name__ == "__main__":
+    main()

lerobot/common/robots/lekiwi/lekiwi_robot.py

@@ -21,6 +21,8 @@ import threading
 import numpy as np
 import time
 # import torch
+import base64
+import cv2
 
 from lerobot.common.cameras.utils import make_cameras_from_configs
 from lerobot.common.constants import OBS_IMAGES, OBS_STATE
@@ -49,21 +51,17 @@ class LeKiwiRobot(Robot):
     def __init__(self, config: LeKiwiRobotConfig):
         super().__init__(config)
         self.config = config
-        # self.robot_type = config.type
+        self.id = config.id
-        # self.id = config.id
+
-        self.remote_ip = config.ip
+        self.port_zmq_cmd = config.port_zmq_cmd
-        self.remote_port = config.port
+        self.port_zmq_observations = config.port_zmq_observations
-        self.remote_port_video = config.video_port
-        # self.logs = {}
-        # TODO(Steven): This should go to teleop
-        # self.teleop_keys = self.config.teleop_keys
 
         # TODO(Steven): Consider in the future using S100 robot class
         # TODO(Steven): Another option is to use the motorbus factory, but in this case we assume that
         # what we consider 'lekiwi robot' always uses the FeetechMotorsBus
-        # TODO(Steve): We will need to have a key for arm and base for calibration
+        # TODO(Steven): We will need to have a key for arm and base for calibration
         self.actuators = FeetechMotorsBus(
-            port=self.config.port,
+            port=self.config.port_motor_bus,
             motors={
                 "shoulder_pan": config.shoulder_pan,
                 "shoulder_lift": config.shoulder_lift,
@@ -77,24 +75,18 @@ class LeKiwiRobot(Robot):
             },
         )
 
-
-
         #TODO(Steven): Consider removing cameras from configs
         self.cameras = make_cameras_from_configs(config.cameras)        
 
-        self.is_connected = False
-        self.logs = {}
-
         self.observation_lock = threading.Lock()
         self.last_observation = None
-        # self.last_frames = {}
-        # self.last_present_speed = {}
-        # self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32)
 
-        # ZeroMQ context and sockets.
+        self.zmq_context = None
-        self.context = None
+        self.zmq_cmd_socket = None
-        self.cmd_socket = None
+        self.zmq_observation_socket = None
-        self.observation_socket = None
+
+        self.is_connected = False
+        self.logs = {}
 
         
 
@@ -121,15 +113,15 @@ class LeKiwiRobot(Robot):
             }
         return cam_ft
 
-    def setup_zmq_sockets(self, config):
+    def setup_zmq_sockets(self):
         context = zmq.Context()
         cmd_socket = context.socket(zmq.PULL)
         cmd_socket.setsockopt(zmq.CONFLATE, 1)
-        cmd_socket.bind(f"tcp://*:{config.port}")
+        cmd_socket.bind(f"tcp://*:{self.port_zmq_cmd}")
 
         observation_socket = context.socket(zmq.PUSH)
         observation_socket.setsockopt(zmq.CONFLATE, 1)
-        observation_socket.bind(f"tcp://*:{config.video_port}")
+        observation_socket.bind(f"tcp://*:{self.port_zmq_observations}")
 
         return context, cmd_socket, observation_socket
     
@@ -176,10 +168,14 @@ class LeKiwiRobot(Robot):
             cam.connect()
 
         logging.info("Connecting ZMQ sockets.")
-        self.context, self.cmd_socket, self.observation_socket = self.setup_zmq_sockets(self.config)
+        self.zmq_context, self.zmq_cmd_socket, self.zmq_observation_socket = self.setup_zmq_sockets(self.config)
 
         self.is_connected = True
 
+    # TODO(Steven): Consider using this
+    # def get_motor_names(self, arms: dict[str, MotorsBus]) -> list:
+    #     return [f"{arm}_{motor}" for arm, bus in arms.items() for motor in bus.motors]
+
     def calibrate(self) -> None:
         # Copied from S100 robot
         """After calibration all motors function in human interpretable ranges.
@@ -192,7 +188,6 @@ class LeKiwiRobot(Robot):
             with open(actuators_calib_path) as f:
                 calibration = json.load(f)
         else:
-            # TODO(rcadene): display a warning in __init__ if calibration file not available
             logging.info(f"Missing calibration file '{actuators_calib_path}'")
             calibration = run_arm_manual_calibration(self.actuators, self.robot_type, self.name, "follower")
 
@@ -207,7 +202,7 @@ class LeKiwiRobot(Robot):
         """The returned observations do not have a batch dimension."""
         if not self.is_connected:
             raise DeviceNotConnectedError(
-                "ManipulatorRobot is not connected. You need to run `robot.connect()`."
+                "LeKiwiRobot is not connected. You need to run `robot.connect()`."
             )
 
         obs_dict = {}
@@ -221,7 +216,12 @@ class LeKiwiRobot(Robot):
         # Capture images from cameras
         for cam_key, cam in self.cameras.items():
             before_camread_t = time.perf_counter()
-            obs_dict[f"{OBS_IMAGES}.{cam_key}"] = cam.async_read()
+            frame = cam.async_read()
+            ret, buffer = cv2.imencode(".jpg", frame, [int(cv2.IMWRITE_JPEG_QUALITY), 90])
+            if ret:
+                obs_dict[f"{OBS_IMAGES}.{cam_key}"] = base64.b64encode(buffer).decode("utf-8")
+            else:
+                obs_dict[f"{OBS_IMAGES}.{cam_key}"] = ""
             self.logs[f"read_camera_{cam_key}_dt_s"] = cam.logs["delta_timestamp_s"]
             self.logs[f"async_read_camera_{cam_key}_dt_s"] = time.perf_counter() - before_camread_t
 
@@ -246,7 +246,7 @@ class LeKiwiRobot(Robot):
         """
         if not self.is_connected:
             raise DeviceNotConnectedError(
-                "ManipulatorRobot is not connected. You need to run `robot.connect()`."
+                "LeKiwiRobot is not connected. You need to run `robot.connect()`."
             )
 
         goal_pos = action
@@ -265,15 +265,21 @@ class LeKiwiRobot(Robot):
 
     def update_last_observation(self, stop_event):
         while not stop_event.is_set():
+            obs = self.get_observation()
             with self.observation_lock:
-                self.last_observation = self.get_observation()
+                self.last_observation = obs
             # TODO(Steven): Consider adding a delay to not starve the CPU
 
+    def stop(self):
+        # TODO(Steven): Base motors speed should be set to 0
+        pass
+
     def run(self):
-        # Copied from run_lekiwi in lekiwi_remote.py
+        # Copied logic from run_lekiwi in lekiwi_remote.py
-        # TODO(Steven): Csnsider with, finally
         if not self.is_connected:
-            self.connect()
+            raise DeviceNotConnectedError(
+                "LeKiwiRobot is not connected. You need to run `robot.connect()`."
+            )
         
         stop_event = threading.Event()
         observation_thread = threading.Thread(
@@ -302,12 +308,11 @@ class LeKiwiRobot(Robot):
                 # Watchdog: stop the robot if no command is received for over 0.5 seconds.
                 now = time.time()
                 if now - last_cmd_time > 0.5:
-                    # TODO(Steven): Implement stop()
+                    self.stop() 
-                    #self.stop() 
                     pass
                 
                 with self.observation_lock:
-                    self.observation_socket.send_string(json.dumps(self.last_observation))
+                    self.zmq_observation_socket.send_string(json.dumps(self.last_observation))
                 
                 # Ensure a short sleep to avoid overloading the CPU.
                 elapsed = time.time() - loop_start_time
@@ -317,11 +322,13 @@ class LeKiwiRobot(Robot):
         except KeyboardInterrupt:
             print("Shutting down LeKiwi server.")
         finally:
-            #TODO(Steven): Implement finally
+            stop_event.set()
+            observation_thread.join()
+            self.disconnect()
             pass
     
     def print_logs(self):
-        # TODO(aliberts): move robot-specific logs logic here
+        # TODO(Steven): Refactor logger
         pass
 
     def disconnect(self):
@@ -329,11 +336,16 @@ class LeKiwiRobot(Robot):
             raise DeviceNotConnectedError(
                 "LeKiwi is not connected. You need to run `robot.connect()` before disconnecting."
             )
-        # TODO(Steven): Base motors speed should be set to 0
+        
-        # TODO(Steven): Close ZMQ sockets
+        self.stop()
-        # TODO(Steven): Stop main loop threads
         self.actuators.disconnect()
         for cam in self.cameras.values():
             cam.disconnect()
-
+        self.observation_socket.close()
+        self.cmd_socket.close()
+        self.context.term()
         self.is_connected = False
+    
+    def __del__(self):
+        if getattr(self, "is_connected", False):
+            self.disconnect()