diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py new file mode 100644 index 00000000..6bd4f0d8 --- /dev/null +++ b/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", + } + ) \ No newline at end of file diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py index 076dbbc2..37a88ded 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/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 - ip: str = "192.168.0.193" - port: int = 5555 - video_port: int = 5556 + port_zmq_cmd: int = 5555 + port_zmq_observations: int = 5556 cameras: dict[str, CameraConfig] = field( default_factory=lambda: { @@ -31,59 +24,16 @@ class LeKiwiRobotConfig(RobotConfig): ) calibration_dir: str = ".cache/calibration/lekiwi" + + port_motor_bus = "/dev/ttyACM0" - leader_arms: dict[str, MotorsBusConfig] = field( - 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( - default_factory=lambda: { - "main": FeetechMotorsBusConfig( - port="/dev/ttyACM0", - 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"], - "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 + # TODO(Steven): consider split this into arm and base + shoulder_pan: tuple = (1, "sts3215") + shoulder_lift: tuple = (2, "sts3215") + elbow_flex: tuple=(3, "sts3215") + wrist_flex: tuple=(4, "sts3215") + wrist_roll: tuple=(5, "sts3215") + gripper: tuple =(6, "sts3215") + left_wheel: tuple= (7, "sts3215") + back_wheel: tuple = (8, "sts3215") + right_wheel: tuple = (9, "sts3215") diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py new file mode 100644 index 00000000..4fb4eab3 --- /dev/null +++ b/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() diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py new file mode 100644 index 00000000..94d2e039 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/lerobot/common/robots/lekiwi/lekiwi_api.py b/lerobot/common/robots/lekiwi/lekiwi_api.py deleted file mode 100644 index da812c53..00000000 --- a/lerobot/common/robots/lekiwi/lekiwi_api.py +++ /dev/null @@ -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() \ No newline at end of file diff --git a/lerobot/common/robots/lekiwi/lekiwi_app.py b/lerobot/common/robots/lekiwi/lekiwi_app.py new file mode 100644 index 00000000..349c952b --- /dev/null +++ b/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() \ No newline at end of file diff --git a/lerobot/common/robots/lekiwi/robot_lekiwi.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py similarity index 83% rename from lerobot/common/robots/lekiwi/robot_lekiwi.py rename to lerobot/common/robots/lekiwi/lekiwi_robot.py index aa89a9af..d70196e9 100644 --- a/lerobot/common/robots/lekiwi/robot_lekiwi.py +++ b/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.remote_ip = config.ip - self.remote_port = config.port - self.remote_port_video = config.video_port - # self.logs = {} - # TODO(Steven): This should go to teleop - # self.teleop_keys = self.config.teleop_keys + self.id = config.id + + self.port_zmq_cmd = config.port_zmq_cmd + self.port_zmq_observations = config.port_zmq_observations # 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.cameras = make_cameras_from_configs(config.cameras) 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.context = None - self.cmd_socket = None - self.observation_socket = None + self.zmq_context = None + self.zmq_cmd_socket = None + self.zmq_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 - # TODO(Steven): Csnsider with, finally + # Copied logic from run_lekiwi in lekiwi_remote.py 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 - # TODO(Steven): Stop main loop threads + + self.stop() 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()