From 6afc113a91f8f1447704e0176857eb2805cdde5f Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 12 Mar 2025 19:12:27 +0100 Subject: [PATCH 01/19] refactor(robots): lewiki v0.1 --- lerobot/common/robots/lekiwi/lekiwi_api.py | 35 + ...{lekiwi_remote.py => old_lekiwi_remote.py} | 0 .../old_robot_lekiwi.py} | 38 +- lerobot/common/robots/lekiwi/robot_lekiwi.py | 871 ++++++------------ .../teleoperators/keyboard/teleop_keyboard.py | 7 + lerobot/scripts/control_robot.py | 2 +- 6 files changed, 315 insertions(+), 638 deletions(-) create mode 100644 lerobot/common/robots/lekiwi/lekiwi_api.py rename lerobot/common/robots/lekiwi/{lekiwi_remote.py => old_lekiwi_remote.py} (100%) rename lerobot/common/robots/{mobile_manipulator.py => lekiwi/old_robot_lekiwi.py} (96%) diff --git a/lerobot/common/robots/lekiwi/lekiwi_api.py b/lerobot/common/robots/lekiwi/lekiwi_api.py new file mode 100644 index 00000000..da812c53 --- /dev/null +++ b/lerobot/common/robots/lekiwi/lekiwi_api.py @@ -0,0 +1,35 @@ +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_remote.py b/lerobot/common/robots/lekiwi/old_lekiwi_remote.py similarity index 100% rename from lerobot/common/robots/lekiwi/lekiwi_remote.py rename to lerobot/common/robots/lekiwi/old_lekiwi_remote.py diff --git a/lerobot/common/robots/mobile_manipulator.py b/lerobot/common/robots/lekiwi/old_robot_lekiwi.py similarity index 96% rename from lerobot/common/robots/mobile_manipulator.py rename to lerobot/common/robots/lekiwi/old_robot_lekiwi.py index 33027791..8f2f9037 100644 --- a/lerobot/common/robots/mobile_manipulator.py +++ b/lerobot/common/robots/lekiwi/old_robot_lekiwi.py @@ -1,17 +1,3 @@ -# 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 base64 import json import os @@ -407,19 +393,21 @@ class MobileManipulator: for name in self.leader_arms: pos = self.leader_arms[name].read("Present_Position") pos_tensor = torch.from_numpy(pos).float() + # Instead of pos_tensor.item(), use tolist() to convert the entire tensor to a list arm_positions.extend(pos_tensor.tolist()) - y_cmd = 0.0 # m/s forward/backward - x_cmd = 0.0 # m/s lateral + # (The rest of your code for generating wheel commands remains unchanged) + x_cmd = 0.0 # m/s forward/backward + y_cmd = 0.0 # m/s lateral theta_cmd = 0.0 # deg/s rotation if self.pressed_keys["forward"]: - y_cmd += xy_speed - if self.pressed_keys["backward"]: - y_cmd -= xy_speed - if self.pressed_keys["left"]: x_cmd += xy_speed - if self.pressed_keys["right"]: + if self.pressed_keys["backward"]: x_cmd -= xy_speed + if self.pressed_keys["left"]: + y_cmd += xy_speed + if self.pressed_keys["right"]: + y_cmd -= xy_speed if self.pressed_keys["rotate_left"]: theta_cmd += theta_speed if self.pressed_keys["rotate_right"]: @@ -597,8 +585,8 @@ class MobileManipulator: # Create the body velocity vector [x, y, theta_rad]. velocity_vector = np.array([x_cmd, y_cmd, theta_rad]) - # Define the wheel mounting angles (defined from y axis cw) - angles = np.radians(np.array([300, 180, 60])) + # 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]) @@ -654,8 +642,8 @@ class MobileManipulator: # Compute each wheel’s linear speed (m/s) from its angular speed. wheel_linear_speeds = wheel_radps * wheel_radius - # Define the wheel mounting angles (defined from y axis cw) - angles = np.radians(np.array([300, 180, 60])) + # 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. diff --git a/lerobot/common/robots/lekiwi/robot_lekiwi.py b/lerobot/common/robots/lekiwi/robot_lekiwi.py index 8f2f9037..aa89a9af 100644 --- a/lerobot/common/robots/lekiwi/robot_lekiwi.py +++ b/lerobot/common/robots/lekiwi/robot_lekiwi.py @@ -1,692 +1,339 @@ -import base64 -import json -import os -import sys -from pathlib import Path +#!/usr/bin/env python -import cv2 +# 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 torch -import zmq +import time +# import torch from lerobot.common.cameras.utils import make_cameras_from_configs -from lerobot.common.errors import DeviceNotConnectedError -from lerobot.common.motors.feetech.feetech import TorqueMode -from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration -from lerobot.common.motors.motors_bus import MotorsBus -from lerobot.common.motors.utils import make_motors_buses_from_configs -from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig -from lerobot.common.robots.utils import get_arm_id +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_lekiwi import LeKiwiRobotConfig +from lerobot.common.motors.feetech import ( + FeetechMotorsBus, + TorqueMode, + run_arm_manual_calibration, +) +import zmq -PYNPUT_AVAILABLE = True -try: - # Only import if there's a valid X server or if we're not on a Pi - if ("DISPLAY" not in os.environ) and ("linux" in sys.platform): - print("No DISPLAY set. Skipping pynput import.") - raise ImportError("pynput blocked intentionally due to no display.") - - from pynput import keyboard -except ImportError: - keyboard = None - PYNPUT_AVAILABLE = False -except Exception as e: - keyboard = None - PYNPUT_AVAILABLE = False - print(f"Could not import pynput: {e}") - - -class MobileManipulator: +class LeKiwiRobot(Robot): """ - MobileManipulator is a class for connecting to and controlling a remote mobile manipulator robot. The robot includes a three omniwheel mobile base and a remote follower arm. The leader arm is connected locally (on the laptop) and its joint positions are recorded and then forwarded to the remote follower arm (after applying a safety clamp). In parallel, keyboard teleoperation is used to generate raw velocity commands for the wheels. """ + config_class = LeKiwiRobotConfig + name = "lekiwi" + def __init__(self, config: LeKiwiRobotConfig): - """ - Expected keys in config: - - ip, port, video_port for the remote connection. - - calibration_dir, leader_arms, follower_arms, max_relative_target, etc. - """ - self.robot_type = config.type + 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.calibration_dir = Path(self.config.calibration_dir) - self.logs = {} + # self.logs = {} + # TODO(Steven): This should go to teleop + # self.teleop_keys = self.config.teleop_keys - 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 + self.actuators = FeetechMotorsBus( + port=self.config.port, + motors={ + "shoulder_pan": config.shoulder_pan, + "shoulder_lift": config.shoulder_lift, + "elbow_flex": config.elbow_flex, + "wrist_flex": config.wrist_flex, + "wrist_roll": config.wrist_roll, + "gripper": config.gripper, + "left_wheel": config.left_wheel, + "right_wheel": config.right_wheel, + "back_wheel": config.back_wheel, + }, + ) - # For teleoperation, the leader arm (local) is used to record the desired arm pose. - self.leader_arms = make_motors_buses_from_configs(self.config.leader_arms) - self.follower_arms = make_motors_buses_from_configs(self.config.follower_arms) - self.cameras = make_cameras_from_configs(self.config.cameras) + #TODO(Steven): Consider removing cameras from configs + self.cameras = make_cameras_from_configs(config.cameras) self.is_connected = False + self.logs = {} - 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 + 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.video_socket = None + self.observation_socket = None - # 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, - } - - if PYNPUT_AVAILABLE: - print("pynput is available - enabling local keyboard listener.") - self.listener = keyboard.Listener( - on_press=self.on_press, - on_release=self.on_release, - ) - self.listener.start() - else: - print("pynput not available - skipping local keyboard listener.") - self.listener = None - - 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] + @property - def camera_features(self) -> dict: + def state_feature(self) -> dict: + return { + "dtype": "float32", + "shape": (len(self.actuators),), + "names": {"motors": list(self.actuators.motors)}, + } + + @property + def action_feature(self) -> dict: + return self.state_feature + + @property + def camera_features(self) -> dict[str, dict]: cam_ft = {} for cam_key, cam in self.cameras.items(): - key = f"observation.images.{cam_key}" - cam_ft[key] = { + cam_ft[cam_key] = { "shape": (cam.height, cam.width, cam.channels), "names": ["height", "width", "channels"], "info": None, } return cam_ft - @property - def motor_features(self) -> dict: - follower_arm_names = [ - "shoulder_pan", - "shoulder_lift", - "elbow_flex", - "wrist_flex", - "wrist_roll", - "gripper", - ] - observations = ["x_mm", "y_mm", "theta"] - combined_names = follower_arm_names + observations - return { - "action": { - "dtype": "float32", - "shape": (len(combined_names),), - "names": combined_names, - }, - "observation.state": { - "dtype": "float32", - "shape": (len(combined_names),), - "names": combined_names, - }, - } + def setup_zmq_sockets(self, config): + context = zmq.Context() + cmd_socket = context.socket(zmq.PULL) + cmd_socket.setsockopt(zmq.CONFLATE, 1) + cmd_socket.bind(f"tcp://*:{config.port}") - @property - def features(self): - return {**self.motor_features, **self.camera_features} + observation_socket = context.socket(zmq.PUSH) + observation_socket.setsockopt(zmq.CONFLATE, 1) + observation_socket.bind(f"tcp://*:{config.video_port}") - @property - def has_camera(self): - return len(self.cameras) > 0 + return context, cmd_socket, observation_socket + + def setup_actuators(self): + # We assume that at connection time, arm is in a rest position, + # and torque can be safely disabled to run calibration. + self.actuators.write("Torque_Enable", TorqueMode.DISABLED.value) + self.calibrate() - @property - def num_cameras(self): - return len(self.cameras) + # Mode=0 for Position Control + # TODO(Steven): Base robots should actually be in vel mode + self.actuators.write("Mode", 0) + # Set P_Coefficient to lower value to avoid shakiness (Default is 32) + self.actuators.write("P_Coefficient", 16) + # Set I_Coefficient and D_Coefficient to default value 0 and 32 + self.actuators.write("I_Coefficient", 0) + self.actuators.write("D_Coefficient", 32) + # Close the write lock so that Maximum_Acceleration gets written to EPROM address, + # which is mandatory for Maximum_Acceleration to take effect after rebooting. + self.actuators.write("Lock", 0) + # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of + # the motors. Note: this configuration is not in the official STS3215 Memory Table + self.actuators.write("Maximum_Acceleration", 254) + self.actuators.write("Acceleration", 254) - @property - def available_arms(self): - available = [] - for name in self.leader_arms: - available.append(get_arm_id(name, "leader")) - for name in self.follower_arms: - available.append(get_arm_id(name, "follower")) - return available + logging.info("Activating torque.") + self.actuators.write("Torque_Enable", TorqueMode.ENABLED.value) - def on_press(self, key): - try: - # Movement - if key.char == self.teleop_keys["forward"]: - self.pressed_keys["forward"] = True - elif key.char == self.teleop_keys["backward"]: - self.pressed_keys["backward"] = True - elif key.char == self.teleop_keys["left"]: - self.pressed_keys["left"] = True - elif key.char == self.teleop_keys["right"]: - self.pressed_keys["right"] = True - elif key.char == self.teleop_keys["rotate_left"]: - self.pressed_keys["rotate_left"] = True - elif key.char == self.teleop_keys["rotate_right"]: - self.pressed_keys["rotate_right"] = True + # Check arm can be read + self.actuators.read("Present_Position") - # Quit teleoperation - elif key.char == self.teleop_keys["quit"]: - self.running = False - return False + def connect(self) -> None: + if self.is_connected: + raise DeviceAlreadyConnectedError( + "LeKiwi Robot is already connected. Do not run `robot.connect()` twice." + ) - # 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}") + logging.info("Connecting actuators.") + self.actuators.connect() + self.setup_actuators() - except AttributeError: - # e.g., if key is special like Key.esc - if key == keyboard.Key.esc: - self.running = False - return False + logging.info("Connecting cameras.") + for cam in self.cameras.values(): + cam.connect() - def on_release(self, key): - try: - if hasattr(key, "char"): - if key.char == self.teleop_keys["forward"]: - self.pressed_keys["forward"] = False - elif key.char == self.teleop_keys["backward"]: - self.pressed_keys["backward"] = False - elif key.char == self.teleop_keys["left"]: - self.pressed_keys["left"] = False - elif key.char == self.teleop_keys["right"]: - self.pressed_keys["right"] = False - elif key.char == self.teleop_keys["rotate_left"]: - self.pressed_keys["rotate_left"] = False - elif key.char == self.teleop_keys["rotate_right"]: - self.pressed_keys["rotate_right"] = False - except AttributeError: - pass + logging.info("Connecting ZMQ sockets.") + self.context, self.cmd_socket, self.observation_socket = self.setup_zmq_sockets(self.config) - def connect(self): - if not self.leader_arms: - raise ValueError("MobileManipulator has no leader arm to connect.") - for name in self.leader_arms: - print(f"Connecting {name} leader arm.") - self.calibrate_leader() - - # Set up ZeroMQ sockets to communicate with the remote mobile robot. - self.context = zmq.Context() - self.cmd_socket = self.context.socket(zmq.PUSH) - connection_string = f"tcp://{self.remote_ip}:{self.remote_port}" - self.cmd_socket.connect(connection_string) - self.cmd_socket.setsockopt(zmq.CONFLATE, 1) - self.video_socket = self.context.socket(zmq.PULL) - video_connection = f"tcp://{self.remote_ip}:{self.remote_port_video}" - self.video_socket.connect(video_connection) - self.video_socket.setsockopt(zmq.CONFLATE, 1) - print( - f"[INFO] Connected to remote robot at {connection_string} and video stream at {video_connection}." - ) self.is_connected = True - def load_or_run_calibration_(self, name, arm, arm_type): - arm_id = get_arm_id(name, arm_type) - arm_calib_path = self.calibration_dir / f"{arm_id}.json" + def calibrate(self) -> None: + # Copied from S100 robot + """After calibration all motors function in human interpretable ranges. + Rotations are expressed in degrees in nominal range of [-180, 180], + and linear motions (like gripper of Aloha) in nominal range of [0, 100]. + """ + actuators_calib_path = self.calibration_dir / f"{self.config.id}.json" - if arm_calib_path.exists(): - with open(arm_calib_path) as f: + if actuators_calib_path.exists(): + with open(actuators_calib_path) as f: calibration = json.load(f) else: - print(f"Missing calibration file '{arm_calib_path}'") - calibration = run_full_arm_calibration(arm, self.robot_type, name, arm_type) - print(f"Calibration is done! Saving calibration file '{arm_calib_path}'") - arm_calib_path.parent.mkdir(parents=True, exist_ok=True) - with open(arm_calib_path, "w") as f: + # 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") + + logging.info(f"Calibration is done! Saving calibration file '{actuators_calib_path}'") + actuators_calib_path.parent.mkdir(parents=True, exist_ok=True) + with open(actuators_calib_path, "w") as f: json.dump(calibration, f) - return calibration + self.actuators.set_calibration(calibration) - def calibrate_leader(self): - for name, arm in self.leader_arms.items(): - # Connect the bus - arm.connect() - - # Disable torque on all motors - for motor_id in arm.motors: - arm.write("Torque_Enable", TorqueMode.DISABLED.value, motor_id) - - # Now run calibration - calibration = self.load_or_run_calibration_(name, arm, "leader") - arm.set_calibration(calibration) - - def calibrate_follower(self): - for name, bus in self.follower_arms.items(): - bus.connect() - - # Disable torque on all motors - for motor_id in bus.motors: - bus.write("Torque_Enable", 0, motor_id) - - # Then filter out wheels - arm_only_dict = {k: v for k, v in bus.motors.items() if not k.startswith("wheel_")} - if not arm_only_dict: - continue - - original_motors = bus.motors - bus.motors = arm_only_dict - - calibration = self.load_or_run_calibration_(name, bus, "follower") - bus.set_calibration(calibration) - - bus.motors = original_motors - - 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.video_socket, zmq.POLLIN) - socks = dict(poller.poll(15)) - if self.video_socket not in socks or socks[self.video_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.video_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) - - 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 - - def _process_present_speed(self, present_speed: dict) -> torch.Tensor: - state_tensor = torch.zeros(3, dtype=torch.int32) - if present_speed: - decoded = {key: MobileManipulator.raw_to_degps(value) for key, value in present_speed.items()} - if "1" in decoded: - state_tensor[0] = decoded["1"] - if "2" in decoded: - state_tensor[1] = decoded["2"] - if "3" in decoded: - state_tensor[2] = decoded["3"] - return state_tensor - - def teleop_step( - self, record_data: bool = False - ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]: + def get_observation(self) -> dict[str, np.ndarray]: + """The returned observations do not have a batch dimension.""" if not self.is_connected: - raise DeviceNotConnectedError("MobileManipulator is not connected. Run `connect()` first.") + raise DeviceNotConnectedError( + "ManipulatorRobot is not connected. You need to run `robot.connect()`." + ) - speed_setting = self.speed_levels[self.speed_index] - xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4 - theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90 + obs_dict = {} - # Prepare to assign the position of the leader to the follower - arm_positions = [] - for name in self.leader_arms: - pos = self.leader_arms[name].read("Present_Position") - pos_tensor = torch.from_numpy(pos).float() - # Instead of pos_tensor.item(), use tolist() to convert the entire tensor to a list - arm_positions.extend(pos_tensor.tolist()) + # Read actuators position + # TODO(Steven): Base motors should return a vel instead of a pos + before_read_t = time.perf_counter() + obs_dict[OBS_STATE] = self.actuators.read("Present_Position") + self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t - # (The rest of your code for generating wheel commands remains unchanged) - x_cmd = 0.0 # m/s forward/backward - y_cmd = 0.0 # m/s lateral - theta_cmd = 0.0 # deg/s rotation - if self.pressed_keys["forward"]: - x_cmd += xy_speed - if self.pressed_keys["backward"]: - x_cmd -= xy_speed - if self.pressed_keys["left"]: - y_cmd += xy_speed - if self.pressed_keys["right"]: - y_cmd -= xy_speed - if self.pressed_keys["rotate_left"]: - theta_cmd += theta_speed - if self.pressed_keys["rotate_right"]: - theta_cmd -= theta_speed - - wheel_commands = self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) - - message = {"raw_velocity": wheel_commands, "arm_positions": arm_positions} - self.cmd_socket.send_string(json.dumps(message)) - - if not record_data: - return - - obs_dict = self.capture_observation() - - arm_state_tensor = torch.tensor(arm_positions, dtype=torch.float32) - - wheel_velocity_tuple = self.wheel_raw_to_body(wheel_commands) - wheel_velocity_mm = ( - wheel_velocity_tuple[0] * 1000.0, - wheel_velocity_tuple[1] * 1000.0, - wheel_velocity_tuple[2], - ) - wheel_tensor = torch.tensor(wheel_velocity_mm, dtype=torch.float32) - action_tensor = torch.cat([arm_state_tensor, wheel_tensor]) - action_dict = {"action": action_tensor} - - return obs_dict, action_dict - - def capture_observation(self) -> dict: - """ - 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("Not connected. Run `connect()` first.") - - 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 = {"observation.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"observation.images.{cam_name}"] = torch.from_numpy(frame) + # 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() + 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 return obs_dict - def send_action(self, action: torch.Tensor) -> torch.Tensor: + 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("Not connected. Run `connect()` first.") + raise DeviceNotConnectedError( + "ManipulatorRobot is not connected. You need to run `robot.connect()`." + ) - # Ensure the action tensor has at least 9 elements: - # - First 6: arm positions. - # - Last 3: base commands. - if action.numel() < 9: - # Pad with zeros if there are not enough elements. - padded = torch.zeros(9, dtype=action.dtype) - padded[: action.numel()] = action - action = padded + goal_pos = action - # Extract arm and base actions. - arm_actions = action[:6].flatten() - base_actions = action[6:].flatten() + # 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) - x_cmd_mm = base_actions[0].item() # mm/s - y_cmd_mm = base_actions[1].item() # mm/s - theta_cmd = base_actions[2].item() # deg/s + # 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)) - # Convert mm/s to m/s for the kinematics calculations. - x_cmd = x_cmd_mm / 1000.0 # m/s - y_cmd = y_cmd_mm / 1000.0 # m/s + return goal_pos - # Compute wheel commands from body commands. - wheel_commands = self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) + def update_last_observation(self, stop_event): + while not stop_event.is_set(): + with self.observation_lock: + self.last_observation = self.get_observation() + # TODO(Steven): Consider adding a delay to not starve the CPU - arm_positions_list = arm_actions.tolist() + def run(self): + # Copied from run_lekiwi in lekiwi_remote.py + # TODO(Steven): Csnsider with, finally + if not self.is_connected: + self.connect() + + stop_event = threading.Event() + observation_thread = threading.Thread( + target=self.update_last_observation, args=(stop_event), daemon=True + ) + observation_thread.start() - message = {"raw_velocity": wheel_commands, "arm_positions": arm_positions_list} - self.cmd_socket.send_string(json.dumps(message)) + last_cmd_time = time.time() + logging.info("LeKiwi robot server started. Waiting for commands...") - return action + try: + while True: + loop_start_time = time.time() + try: + msg = self.cmd_socket.recv_string(zmq.NOBLOCK) + data = json.loads(msg) + # TODO(Steven): Process data correctly + self.send_action(data) + last_cmd_time = time.time() + # except zmq.Again: + # logging.warning("ZMQ again") + except Exception as e: + logging.warning(f"[ERROR] Message fetching failed: {e}") + + # 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() + pass + + with self.observation_lock: + self.observation_socket.send_string(json.dumps(self.last_observation)) + + # Ensure a short sleep to avoid overloading the CPU. + elapsed = time.time() - loop_start_time + time.sleep( + max(0.033 - elapsed, 0) + ) # If robot jitters increase the sleep and monitor cpu load with `top` in cmd + except KeyboardInterrupt: + print("Shutting down LeKiwi server.") + finally: + #TODO(Steven): Implement finally + pass + def print_logs(self): + # TODO(aliberts): move robot-specific logs logic here pass def disconnect(self): if not self.is_connected: - raise DeviceNotConnectedError("Not connected.") - if self.cmd_socket: - stop_cmd = { - "raw_velocity": {"left_wheel": 0, "back_wheel": 0, "right_wheel": 0}, - "arm_positions": {}, - } - self.cmd_socket.send_string(json.dumps(stop_cmd)) - self.cmd_socket.close() - if self.video_socket: - self.video_socket.close() - if self.context: - self.context.term() - if PYNPUT_AVAILABLE: - self.listener.stop() + 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.actuators.disconnect() + for cam in self.cameras.values(): + cam.disconnect() + self.is_connected = False - print("[INFO] Disconnected from remote robot.") - - def __del__(self): - if getattr(self, "is_connected", False): - self.disconnect() - if PYNPUT_AVAILABLE: - self.listener.stop() - - @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 - - @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 - - 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 = [MobileManipulator.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]} - - 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([MobileManipulator.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) - - -class LeKiwi: - def __init__(self, motor_bus): - """ - Initializes the LeKiwi with Feetech motors bus. - """ - self.motor_bus = motor_bus - self.motor_ids = ["left_wheel", "back_wheel", "right_wheel"] - - # Initialize motors in velocity mode. - self.motor_bus.write("Lock", 0) - self.motor_bus.write("Mode", [1, 1, 1], self.motor_ids) - self.motor_bus.write("Lock", 1) - print("Motors set to velocity mode.") - - def read_velocity(self): - """ - Reads the raw speeds for all wheels. Returns a dictionary with motor names: - """ - raw_speeds = self.motor_bus.read("Present_Speed", self.motor_ids) - return { - "left_wheel": int(raw_speeds[0]), - "back_wheel": int(raw_speeds[1]), - "right_wheel": int(raw_speeds[2]), - } - - def set_velocity(self, command_speeds): - """ - Sends raw velocity commands (16-bit encoded values) directly to the motor bus. - The order of speeds must correspond to self.motor_ids. - """ - self.motor_bus.write("Goal_Speed", command_speeds, self.motor_ids) - - def stop(self): - """Stops the robot by setting all motor speeds to zero.""" - self.motor_bus.write("Goal_Speed", [0, 0, 0], self.motor_ids) - print("Motors stopped.") diff --git a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py index 65f1dff7..1a679b18 100644 --- a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py +++ b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py @@ -64,6 +64,7 @@ class KeyboardTeleop(Teleoperator): @property def action_feature(self) -> dict: + #TODO(Steven): Verify this is correct return { "dtype": "float32", "shape": (len(self.arm),), @@ -75,6 +76,12 @@ class KeyboardTeleop(Teleoperator): return {} def connect(self) -> None: + #TODO(Steven): Consider instead of raising a warning and then returning the status + # if self.is_connected: + # logging.warning( + # "ManipulatorRobot is already connected. Do not run `robot.connect()` twice." + # ) + # return self.is_connected if self.is_connected: raise DeviceAlreadyConnectedError( "ManipulatorRobot is already connected. Do not run `robot.connect()` twice." diff --git a/lerobot/scripts/control_robot.py b/lerobot/scripts/control_robot.py index ce8f0948..a8f6986c 100644 --- a/lerobot/scripts/control_robot.py +++ b/lerobot/scripts/control_robot.py @@ -379,7 +379,7 @@ def control_robot(cfg: ControlPipelineConfig): elif isinstance(cfg.control, ReplayControlConfig): replay(robot, cfg.control) elif isinstance(cfg.control, RemoteRobotConfig): - from lerobot.common.robots.lekiwi.lekiwi_remote import run_lekiwi + from lerobot.common.robots.lekiwi.old_lekiwi_remote import run_lekiwi run_lekiwi(cfg.robot) From baff7bb11610e3ed1a2c9a1ed3653c5785bc5d90 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 13 Mar 2025 12:54:27 +0100 Subject: [PATCH 02/19] refactor(robots): lekiwi v0.2 --- .../lekiwi/configuration_daemon_lekiwi.py | 37 ++ .../robots/lekiwi/configuration_lekiwi.py | 78 +--- lerobot/common/robots/lekiwi/daemon_lekiwi.py | 437 ++++++++++++++++++ .../common/robots/lekiwi/daemon_lekiwi_app.py | 52 +++ lerobot/common/robots/lekiwi/lekiwi_api.py | 35 -- lerobot/common/robots/lekiwi/lekiwi_app.py | 22 + .../{robot_lekiwi.py => lekiwi_robot.py} | 100 ++-- 7 files changed, 618 insertions(+), 143 deletions(-) create mode 100644 lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py create mode 100644 lerobot/common/robots/lekiwi/daemon_lekiwi.py create mode 100644 lerobot/common/robots/lekiwi/daemon_lekiwi_app.py delete mode 100644 lerobot/common/robots/lekiwi/lekiwi_api.py create mode 100644 lerobot/common/robots/lekiwi/lekiwi_app.py rename lerobot/common/robots/lekiwi/{robot_lekiwi.py => lekiwi_robot.py} (83%) 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() From 7efea7d21a5296c0cbfa35c3fb75cb8482273b7b Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 13 Mar 2025 23:59:39 +0100 Subject: [PATCH 03/19] refactor(robots): lewiki v0.3 --- lerobot/common/errors.py | 2 + lerobot/common/robots/lekiwi/daemon_lekiwi.py | 70 +++++++++++++------ .../common/robots/lekiwi/daemon_lekiwi_app.py | 19 +++-- .../keyboard/configuration_keyboard.py | 1 + 4 files changed, 66 insertions(+), 26 deletions(-) diff --git a/lerobot/common/errors.py b/lerobot/common/errors.py index 4f506b61..f37ac3e3 100644 --- a/lerobot/common/errors.py +++ b/lerobot/common/errors.py @@ -15,3 +15,5 @@ class DeviceAlreadyConnectedError(ConnectionError): ): self.message = message super().__init__(self.message) + +# TODO(Steven): Consider adding an InvalidActionError \ No newline at end of file diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index 4fb4eab3..7d26c880 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi.py @@ -370,19 +370,39 @@ class DaemonLeKiwiRobot(Robot): 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 + def from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray): + # Speed control + # TODO(Steven): Handle the right action + if self.teleop_keys["speed_up"] in pressed_keys: + self.speed_index = min(self.speed_index + 1, 2) + if self.teleop_keys["speed_down"] in pressed_keys: + self.speed_index = max(self.speed_index - 1, 0) + speed_setting = self.speed_levels[self.speed_index] + xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4 + theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90 + + # (The rest of your code for generating wheel commands remains unchanged) + x_cmd = 0.0 # m/s forward/backward + y_cmd = 0.0 # m/s lateral + theta_cmd = 0.0 # deg/s rotation + + # TODO(Steven): Handle action properly + if self.teleop_keys["forward"] in pressed_keys: + x_cmd += xy_speed + if self.teleop_keys["backward"] in pressed_keys: + x_cmd -= xy_speed + if self.teleop_keys["left"] in pressed_keys: + y_cmd += xy_speed + if self.teleop_keys["right"] in pressed_keys: + y_cmd -= xy_speed + if self.teleop_keys["rotate_left"] in pressed_keys: + theta_cmd += theta_speed + if self.teleop_keys["rotate_right"] in pressed_keys: + theta_cmd -= theta_speed + + return self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) - # 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 @@ -402,18 +422,24 @@ class DaemonLeKiwiRobot(Robot): raise DeviceNotConnectedError( "ManipulatorRobot is not connected. You need to run `robot.connect()`." ) + if self.mode is TELEOP: + # do conversion keys to motor + else: + # convert policy output + + # TODO(Steven): This won't work if this is called by a policy with body vels outputs + goal_pos: np.array = np.empty(9) + if action.size <6: + # TODO(Steven): Handle this properly + raise Exception + + # TODO(Steven): Assumes size and order is respected + # TODO(Steven): This assumes this call is always called from a keyboard teleop command + wheel_actions = [v for _,v in self.from_keyboard_to_wheel_action(action[6:])] + goal_pos[:6]=action[:6] + goal_pos[6:]=wheel_actions - 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)) + self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) return goal_pos diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index 94d2e039..2f907093 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py @@ -30,14 +30,25 @@ def main(): duration = 0 while duration < 20: - arm_action = leader_arm.get_action() - base_action = keyboard.get_action() + arm_action = leader_arm.get_action() # 6 motors + base_action = keyboard.get_action() # n keys pressed 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 + robot.set_mode(TELEOP) + action_sent = robot.send_action(action) # Translates to motor space + sends over ZMQ + obs = robot.get_observation() # Receives over ZMQ, translate to body-frame vel + + dataset.save(action_sent, obs) + + # TODO(Steven) + robot.set_mode(AUTO) + policy_action = policy.get_action() # This might be in body frame or in key space + robot.send_action(policy_action) # This has no way to know + + teleop_step() # teleop + send_action() #policy duration = time.perf_counter() - start diff --git a/lerobot/common/teleoperators/keyboard/configuration_keyboard.py b/lerobot/common/teleoperators/keyboard/configuration_keyboard.py index 91b596bf..18db787c 100644 --- a/lerobot/common/teleoperators/keyboard/configuration_keyboard.py +++ b/lerobot/common/teleoperators/keyboard/configuration_keyboard.py @@ -22,4 +22,5 @@ from ..config import TeleoperatorConfig @TeleoperatorConfig.register_subclass("keyboard") @dataclass class KeyboardTeleopConfig(TeleoperatorConfig): + # TODO(Steven): Maybe set in here the keys that we want to capture mock: bool = False From 9e69f0965c37ea994785e1aa59705763d0072fc4 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 14 Mar 2025 14:30:55 +0100 Subject: [PATCH 04/19] refactor(robots): lekiwi v0.4 --- lerobot/common/errors.py | 9 +- .../lekiwi/configuration_daemon_lekiwi.py | 8 -- .../robots/lekiwi/configuration_lekiwi.py | 7 + lerobot/common/robots/lekiwi/daemon_lekiwi.py | 126 +++++++++--------- .../common/robots/lekiwi/daemon_lekiwi_app.py | 25 ++-- lerobot/common/robots/lekiwi/lekiwi_robot.py | 104 ++++++++------- lerobot/common/robots/robot.py | 7 + .../keyboard/configuration_keyboard.py | 2 +- 8 files changed, 154 insertions(+), 134 deletions(-) diff --git a/lerobot/common/errors.py b/lerobot/common/errors.py index f37ac3e3..b064a295 100644 --- a/lerobot/common/errors.py +++ b/lerobot/common/errors.py @@ -16,4 +16,11 @@ class DeviceAlreadyConnectedError(ConnectionError): self.message = message super().__init__(self.message) -# TODO(Steven): Consider adding an InvalidActionError \ No newline at end of file +class InvalidActionError(ConnectionError): + """Exception raised when an action is already invalid.""" + def __init__( + self, + message="The action is invalid. Check the value follows what it is expected from the action space.", + ): + self.message = message + super().__init__(self.message) \ No newline at end of file diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index 6bd4f0d8..fe7e175b 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py @@ -1,18 +1,10 @@ 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" diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py index 37a88ded..3fd34d11 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_lekiwi.py @@ -28,6 +28,8 @@ class LeKiwiRobotConfig(RobotConfig): port_motor_bus = "/dev/ttyACM0" # TODO(Steven): consider split this into arm and base + # TODO(Steven): Consider also removing this entirely as we can say that + # LeKiwiRobot will always have (and needs) such shoulder_pan: tuple = (1, "sts3215") shoulder_lift: tuple = (2, "sts3215") elbow_flex: tuple=(3, "sts3215") @@ -37,3 +39,8 @@ class LeKiwiRobotConfig(RobotConfig): left_wheel: tuple= (7, "sts3215") back_wheel: tuple = (8, "sts3215") right_wheel: tuple = (9, "sts3215") + + # `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 diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index 7d26c880..94c04d2f 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi.py @@ -16,20 +16,14 @@ 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 lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError, InvalidActionError +from ..robot import Robot, RobotMode from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig import zmq @@ -38,6 +32,12 @@ import zmq # 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 +# Other options include: +# 1. Adding it to the Telop implementation for lekiwi +# (meaning each robot will need a teleop imple) or +# 2. Adding it into the robot implementation +# (meaning the policy might be needed to be train +# over the teleop action space) class DaemonLeKiwiRobot(Robot): config_class = DaemonLeKiwiRobotConfig @@ -49,8 +49,6 @@ class DaemonLeKiwiRobot(Robot): 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 @@ -65,6 +63,7 @@ class DaemonLeKiwiRobot(Robot): 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 @@ -73,23 +72,14 @@ class DaemonLeKiwiRobot(Robot): ] 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? + # TODO(Steven): Motor names are unknown for the Daemon + # Or assume its size/metadata? # return { # "dtype": "float32", # "shape": (len(self.actuators),), @@ -103,7 +93,9 @@ class DaemonLeKiwiRobot(Robot): @property def camera_features(self) -> dict[str, dict]: - # TODO(Steven): Fetch this info or set it static? + # TODO(Steven): Get this from the data fetched? + # TODO(Steven): Motor names are unknown for the Daemon + # Or assume its size/metadata? # cam_ft = {} # for cam_key, cam in self.cameras.items(): # cam_ft[cam_key] = { @@ -134,8 +126,11 @@ class DaemonLeKiwiRobot(Robot): self.is_connected = True def calibrate(self) -> None: - # TODO(Steven): Nothing to calibrate - pass + # TODO(Steven): Nothing to calibrate. + # Consider triggering calibrate() on the remote mobile robot? + # Althought this would require a more complex comms schema + logging.warning("DaemonLeKiwiRobot has nothing to calibrate.") + return # Consider moving these static functions out of the class # Copied from robot_lekiwi MobileManipulator class @@ -267,12 +262,15 @@ class DaemonLeKiwiRobot(Robot): return (x_cmd, y_cmd, theta_cmd) def get_data(self): + # Copied from robot_lekiwi.py """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 = {} + + # TODO(Steven): Size is being assumed, is this safe? remote_arm_state_tensor = torch.zeros(6, dtype=torch.float32) # Poll up to 15 ms @@ -281,10 +279,13 @@ class DaemonLeKiwiRobot(Robot): 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 + # TODO(Steven): This might return empty variables at init return (self.last_frames, self.last_present_speed, self.last_remote_arm_state) # Drain all messages, keep only the last last_msg = None + # TODO(Steven): There's probably a way to do this without while True + # TODO(Steven): Even consider changing to PUB/SUB while True: try: obs_string = self.zmq_observation_socket.recv_string(zmq.NOBLOCK) @@ -300,13 +301,11 @@ class DaemonLeKiwiRobot(Robot): 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) + 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)} # Convert images - for cam_name, image_b64 in images_dict.items(): + for cam_name, image_b64 in image_observation.items(): if image_b64: jpg_data = base64.b64decode(image_b64) np_arr = np.frombuffer(jpg_data, dtype=np.uint8) @@ -315,19 +314,17 @@ class DaemonLeKiwiRobot(Robot): 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: + if state_observation is not None and frames is not None: self.last_frames = frames - remote_arm_state_tensor = torch.tensor(new_arm_state, dtype=torch.float32) + remote_arm_state_tensor = torch.tensor(state_observation[:6], dtype=torch.float32) self.last_remote_arm_state = remote_arm_state_tensor - present_speed = new_speed - self.last_present_speed = new_speed + present_speed = state_observation[6:] + self.last_present_speed = present_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: @@ -351,7 +348,6 @@ class DaemonLeKiwiRobot(Robot): 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 @@ -361,18 +357,18 @@ class DaemonLeKiwiRobot(Robot): 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) + for cam_name, frame in frames.items(): 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) + # TODO(Steven): Daemon doesn't know camera dimensions + logging.warning("Frame is None") + #frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8) + obs_dict[cam_name] = torch.from_numpy(frame) return obs_dict def from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray): + # Speed control - # TODO(Steven): Handle the right action if self.teleop_keys["speed_up"] in pressed_keys: self.speed_index = min(self.speed_index + 1, 2) if self.teleop_keys["speed_down"] in pressed_keys: @@ -381,12 +377,10 @@ class DaemonLeKiwiRobot(Robot): xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4 theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90 - # (The rest of your code for generating wheel commands remains unchanged) x_cmd = 0.0 # m/s forward/backward y_cmd = 0.0 # m/s lateral theta_cmd = 0.0 # deg/s rotation - # TODO(Steven): Handle action properly if self.teleop_keys["forward"] in pressed_keys: x_cmd += xy_speed if self.teleop_keys["backward"] in pressed_keys: @@ -402,13 +396,21 @@ class DaemonLeKiwiRobot(Robot): return self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) + # TODO(Steven): This assumes this call is always called from a keyboard teleop command + # TODO(Steven): Doing this mapping in here adds latecy between send_action and movement from the user perspective. + # t0: get teleop_cmd + # t1: send_action(teleop_cmd) + # t2: mapping teleop_cmd -> motor_cmd + # t3: execute motor_md + # This mapping for other robots/teleop devices might be slower. Doing this in the teleop will make this explicit + # t0': get teleop_cmd + # t1': mapping teleop_cmd -> motor_cmd + # t2': send_action(motor_cmd) + # t3': execute motor_cmd + # t3'-t2' << t3-t1 def send_action(self, action: np.ndarray) -> np.ndarray: """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. @@ -422,24 +424,24 @@ class DaemonLeKiwiRobot(Robot): raise DeviceNotConnectedError( "ManipulatorRobot is not connected. You need to run `robot.connect()`." ) - if self.mode is TELEOP: - # do conversion keys to motor - else: - # convert policy output - # TODO(Steven): This won't work if this is called by a policy with body vels outputs goal_pos: np.array = np.empty(9) - if action.size <6: - # TODO(Steven): Handle this properly + + if self.robot_mode is RobotMode.AUTO: + # TODO(Steven): Not yet implemented. The policy outputs might need a different conversion raise Exception - # TODO(Steven): Assumes size and order is respected - # TODO(Steven): This assumes this call is always called from a keyboard teleop command - wheel_actions = [v for _,v in self.from_keyboard_to_wheel_action(action[6:])] - goal_pos[:6]=action[:6] - goal_pos[6:]=wheel_actions - - self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) + # TODO(Steven): This assumes teleop mode is always used with keyboard + if self.robot_mode is RobotMode.TELEOP: + if action.size <6: + logging.error("Action should include at least the 6 states of the leader arm") + raise InvalidActionError + + # TODO(Steven): Assumes size and order is respected + wheel_actions = [v for _,v in self.from_keyboard_to_wheel_action(action[6:])] + goal_pos[:6]=action[:6] + goal_pos[6:]=wheel_actions + self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) #action is in motor space return goal_pos diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index 2f907093..79c89f6b 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py @@ -1,7 +1,7 @@ from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig -from .daemon_lekiwi import DaemonLeKiwiRobot +from .daemon_lekiwi import DaemonLeKiwiRobot, RobotMode import time import logging @@ -24,31 +24,28 @@ def main(): logging.info("Connecting remote LeKiwiRobot") robot.connect() # Establishes ZMQ sockets with the remote mobile robot + robot.robot_mode = RobotMode.TELEOP logging.info("Starting LeKiwiRobot teleoperation") start = time.perf_counter() duration = 0 while duration < 20: - arm_action = leader_arm.get_action() # 6 motors - base_action = keyboard.get_action() # n keys pressed + arm_action = leader_arm.get_action() + base_action = keyboard.get_action() action = { **arm_action, **base_action } - robot.set_mode(TELEOP) - action_sent = robot.send_action(action) # Translates to motor space + sends over ZMQ - obs = robot.get_observation() # Receives over ZMQ, translate to body-frame vel + _action_sent = robot.send_action(action) # Translates to motor space + sends over ZMQ + _observation = robot.get_observation() # Receives over ZMQ, translate to body-frame vel - dataset.save(action_sent, obs) + # dataset.save(action_sent, obs) - # TODO(Steven) - robot.set_mode(AUTO) - policy_action = policy.get_action() # This might be in body frame or in key space - robot.send_action(policy_action) # This has no way to know - - teleop_step() # teleop - send_action() #policy + # TODO(Steven): Deal with policy action space + # robot.set_mode(RobotMode.AUTO) + # policy_action = policy.get_action() # This might be in body frame, key space or smt else + # robot.send_action(policy_action) duration = time.perf_counter() - start diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index d70196e9..b28629f1 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -20,7 +20,6 @@ import time import threading import numpy as np import time -# import torch import base64 import cv2 @@ -59,24 +58,29 @@ class LeKiwiRobot(Robot): # 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(Steven): We will need to have a key for arm and base for calibration - self.actuators = FeetechMotorsBus( + # TODO(Steven): Order and dimension are generaly assumed to be 6 first for arm, 3 last for base + self.actuators_bus = FeetechMotorsBus( port=self.config.port_motor_bus, motors={ - "shoulder_pan": config.shoulder_pan, - "shoulder_lift": config.shoulder_lift, - "elbow_flex": config.elbow_flex, - "wrist_flex": config.wrist_flex, - "wrist_roll": config.wrist_roll, - "gripper": config.gripper, - "left_wheel": config.left_wheel, - "right_wheel": config.right_wheel, - "back_wheel": config.back_wheel, + "arm_shoulder_pan": config.shoulder_pan, + "arm_shoulder_lift": config.shoulder_lift, + "arm_elbow_flex": config.elbow_flex, + "arm_wrist_flex": config.wrist_flex, + "arm_wrist_roll": config.wrist_roll, + "arm_gripper": config.gripper, + "base_left_wheel": config.left_wheel, + "base_right_wheel": config.right_wheel, + "base_back_wheel": config.back_wheel, }, ) + self.arm_actuators = [m for m in self.actuators_bus.motor_names if m.startswith("arm")] + self.base_actuators = [m for m in self.actuators_bus.motor_names if m.startswith("base")] + + self.max_relative_target = config.max_relative_target + #TODO(Steven): Consider removing cameras from configs - self.cameras = make_cameras_from_configs(config.cameras) + self.cameras = make_cameras_from_configs(config.cameras) self.observation_lock = threading.Lock() self.last_observation = None @@ -94,8 +98,8 @@ class LeKiwiRobot(Robot): def state_feature(self) -> dict: return { "dtype": "float32", - "shape": (len(self.actuators),), - "names": {"motors": list(self.actuators.motors)}, + "shape": (len(self.actuators_bus),), + "names": {"motors": list(self.actuators_bus.motors)}, } @property @@ -126,32 +130,39 @@ class LeKiwiRobot(Robot): return context, cmd_socket, observation_socket def setup_actuators(self): + + # Set-up arm actuators (position mode) # We assume that at connection time, arm is in a rest position, # and torque can be safely disabled to run calibration. - self.actuators.write("Torque_Enable", TorqueMode.DISABLED.value) - self.calibrate() + self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value,self.arm_actuators) + self.calibrate() # TODO(Steven): This should be only for the arm # Mode=0 for Position Control - # TODO(Steven): Base robots should actually be in vel mode - self.actuators.write("Mode", 0) + self.actuators_bus.write("Mode", 0,self.arm_actuators) # Set P_Coefficient to lower value to avoid shakiness (Default is 32) - self.actuators.write("P_Coefficient", 16) + self.actuators_bus.write("P_Coefficient", 16,self.arm_actuators) # Set I_Coefficient and D_Coefficient to default value 0 and 32 - self.actuators.write("I_Coefficient", 0) - self.actuators.write("D_Coefficient", 32) + self.actuators_bus.write("I_Coefficient", 0, self.arm_actuators) + self.actuators_bus.write("D_Coefficient", 32,self.arm_actuators) # Close the write lock so that Maximum_Acceleration gets written to EPROM address, # which is mandatory for Maximum_Acceleration to take effect after rebooting. - self.actuators.write("Lock", 0) + self.actuators_bus.write("Lock", 0,self.arm_actuators) # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of # the motors. Note: this configuration is not in the official STS3215 Memory Table - self.actuators.write("Maximum_Acceleration", 254) - self.actuators.write("Acceleration", 254) + self.actuators_bus.write("Maximum_Acceleration", 254,self.arm_actuators) + self.actuators_bus.write("Acceleration", 254, self.arm_actuators) logging.info("Activating torque.") - self.actuators.write("Torque_Enable", TorqueMode.ENABLED.value) + self.actuators_bus.write("Torque_Enable", TorqueMode.ENABLED.value,self.arm_actuators) # Check arm can be read - self.actuators.read("Present_Position") + self.actuators_bus.read("Present_Position",self.arm_actuators) + + # Set-up base actuators (velocity mode) + self.actuators_bus.write("Lock",0,self.base_actuators) + self.actuators_bus.write("Mode",[1,1,1],self.base_actuators) + self.actuators_bus.write("Lock",1,self.base_actuators) + def connect(self) -> None: if self.is_connected: @@ -160,7 +171,7 @@ class LeKiwiRobot(Robot): ) logging.info("Connecting actuators.") - self.actuators.connect() + self.actuators_bus.connect() self.setup_actuators() logging.info("Connecting cameras.") @@ -168,14 +179,10 @@ class LeKiwiRobot(Robot): cam.connect() logging.info("Connecting ZMQ sockets.") - self.zmq_context, self.zmq_cmd_socket, self.zmq_observation_socket = self.setup_zmq_sockets(self.config) + self.zmq_context, self.zmq_cmd_socket, self.zmq_observation_socket = self.setup_zmq_sockets() 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. @@ -189,14 +196,14 @@ class LeKiwiRobot(Robot): calibration = json.load(f) else: logging.info(f"Missing calibration file '{actuators_calib_path}'") - calibration = run_arm_manual_calibration(self.actuators, self.robot_type, self.name, "follower") + calibration = run_arm_manual_calibration(self.actuators_bus, self.robot_type, self.name, "follower") logging.info(f"Calibration is done! Saving calibration file '{actuators_calib_path}'") actuators_calib_path.parent.mkdir(parents=True, exist_ok=True) with open(actuators_calib_path, "w") as f: json.dump(calibration, f) - self.actuators.set_calibration(calibration) + self.actuators_bus.set_calibration(calibration) def get_observation(self) -> dict[str, np.ndarray]: """The returned observations do not have a batch dimension.""" @@ -207,10 +214,9 @@ class LeKiwiRobot(Robot): obs_dict = {} - # Read actuators position - # TODO(Steven): Base motors should return a vel instead of a pos + # Read actuators position for arm and vel for base before_read_t = time.perf_counter() - obs_dict[OBS_STATE] = self.actuators.read("Present_Position") + obs_dict[OBS_STATE] = self.actuators_bus.read("Present_Position",self.arm_actuators) + self.actuators_bus.read("Present_Speed", self.base_actuators) self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t # Capture images from cameras @@ -249,17 +255,18 @@ class LeKiwiRobot(Robot): "LeKiwiRobot is not connected. You need to run `robot.connect()`." ) - goal_pos = action - + # Input action is in motor space + 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) + present_pos = self.actuators_bus.read("Present_Position",self.arm_actuators) + goal_pos[:6] = ensure_safe_goal_position(goal_pos[:6], 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)) + # TODO(Steven): This happens synchronously + self.actuators_bus.write("Goal_Position", goal_pos[:6].astype(np.int32),self.arm_actuators) + self.actuators_bus.write("Goal_Speed",goal_pos[6:].astype(np.int32),self.base_actuators) return goal_pos @@ -271,8 +278,10 @@ class LeKiwiRobot(Robot): # 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 + # TODO(Steven): Assumes there's only 3 motors for base + logging.info("Stopping base") + self.actuators_bus.write("Goal_Speed",[0,0,0],self.base_actuators) + logging.info("Base motors stopped") def run(self): # Copied logic from run_lekiwi in lekiwi_remote.py @@ -297,7 +306,6 @@ class LeKiwiRobot(Robot): try: msg = self.cmd_socket.recv_string(zmq.NOBLOCK) data = json.loads(msg) - # TODO(Steven): Process data correctly self.send_action(data) last_cmd_time = time.time() # except zmq.Again: @@ -338,7 +346,7 @@ class LeKiwiRobot(Robot): ) self.stop() - self.actuators.disconnect() + self.actuators_bus.disconnect() for cam in self.cameras.values(): cam.disconnect() self.observation_socket.close() diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py index a7ec4eda..de908426 100644 --- a/lerobot/common/robots/robot.py +++ b/lerobot/common/robots/robot.py @@ -9,6 +9,12 @@ from lerobot.common.motors import MotorCalibration from .config import RobotConfig +import enum + +class RobotMode(enum.Enum): + TELEOP = 0 + AUTO = 1 + # TODO(aliberts): action/obs typing such as Generic[ObsType, ActType] similar to gym.Env ? # https://github.com/Farama-Foundation/Gymnasium/blob/3287c869f9a48d99454306b0d4b4ec537f0f35e3/gymnasium/core.py#L23 @@ -21,6 +27,7 @@ class Robot(abc.ABC): def __init__(self, config: RobotConfig): self.robot_type = self.name + self.robot_mode: RobotMode | None = None self.id = config.id self.calibration_dir = ( config.calibration_dir if config.calibration_dir else HF_LEROBOT_CALIBRATION / ROBOTS / self.name diff --git a/lerobot/common/teleoperators/keyboard/configuration_keyboard.py b/lerobot/common/teleoperators/keyboard/configuration_keyboard.py index 18db787c..a1cfbbe7 100644 --- a/lerobot/common/teleoperators/keyboard/configuration_keyboard.py +++ b/lerobot/common/teleoperators/keyboard/configuration_keyboard.py @@ -22,5 +22,5 @@ from ..config import TeleoperatorConfig @TeleoperatorConfig.register_subclass("keyboard") @dataclass class KeyboardTeleopConfig(TeleoperatorConfig): - # TODO(Steven): Maybe set in here the keys that we want to capture + # TODO(Steven): Maybe set in here the keys that we want to capture/listen mock: bool = False From ca1a1423cf38cab13f474064900c681a97cee097 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 14 Mar 2025 14:38:06 +0100 Subject: [PATCH 05/19] refactor(robots): lekiwi v0.5 --- lerobot/common/errors.py | 4 +- ...lekiwi_remote.py => _old_lekiwi_remote.py} | 0 ...d_robot_lekiwi.py => _old_robot_lekiwi.py} | 0 .../lekiwi/configuration_daemon_lekiwi.py | 4 +- .../robots/lekiwi/configuration_lekiwi.py | 15 ++- lerobot/common/robots/lekiwi/daemon_lekiwi.py | 64 +++++----- .../common/robots/lekiwi/daemon_lekiwi_app.py | 30 ++--- lerobot/common/robots/lekiwi/lekiwi_app.py | 7 +- lerobot/common/robots/lekiwi/lekiwi_robot.py | 114 +++++++++--------- lerobot/common/robots/robot.py | 2 +- .../teleoperators/keyboard/teleop_keyboard.py | 4 +- 11 files changed, 121 insertions(+), 123 deletions(-) rename lerobot/common/robots/lekiwi/{old_lekiwi_remote.py => _old_lekiwi_remote.py} (100%) rename lerobot/common/robots/lekiwi/{old_robot_lekiwi.py => _old_robot_lekiwi.py} (100%) diff --git a/lerobot/common/errors.py b/lerobot/common/errors.py index b064a295..6398bc6f 100644 --- a/lerobot/common/errors.py +++ b/lerobot/common/errors.py @@ -16,11 +16,13 @@ class DeviceAlreadyConnectedError(ConnectionError): self.message = message super().__init__(self.message) + class InvalidActionError(ConnectionError): """Exception raised when an action is already invalid.""" + def __init__( self, message="The action is invalid. Check the value follows what it is expected from the action space.", ): self.message = message - super().__init__(self.message) \ No newline at end of file + super().__init__(self.message) diff --git a/lerobot/common/robots/lekiwi/old_lekiwi_remote.py b/lerobot/common/robots/lekiwi/_old_lekiwi_remote.py similarity index 100% rename from lerobot/common/robots/lekiwi/old_lekiwi_remote.py rename to lerobot/common/robots/lekiwi/_old_lekiwi_remote.py diff --git a/lerobot/common/robots/lekiwi/old_robot_lekiwi.py b/lerobot/common/robots/lekiwi/_old_robot_lekiwi.py similarity index 100% rename from lerobot/common/robots/lekiwi/old_robot_lekiwi.py rename to lerobot/common/robots/lekiwi/_old_robot_lekiwi.py diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index fe7e175b..f95dc8d4 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py @@ -2,10 +2,10 @@ from dataclasses import dataclass, field from lerobot.common.robots.config import RobotConfig + @RobotConfig.register_subclass("daemon_lekiwi") @dataclass class DaemonLeKiwiRobotConfig(RobotConfig): - # Network Configuration remote_ip: str = "192.168.0.193" port_zmq_cmd: int = 5555 @@ -26,4 +26,4 @@ class DaemonLeKiwiRobotConfig(RobotConfig): # 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 3fd34d11..c72b427d 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_lekiwi.py @@ -8,7 +8,6 @@ from lerobot.common.robots.config import RobotConfig @RobotConfig.register_subclass("lekiwi") @dataclass class LeKiwiRobotConfig(RobotConfig): - port_zmq_cmd: int = 5555 port_zmq_observations: int = 5556 @@ -24,7 +23,7 @@ class LeKiwiRobotConfig(RobotConfig): ) calibration_dir: str = ".cache/calibration/lekiwi" - + port_motor_bus = "/dev/ttyACM0" # TODO(Steven): consider split this into arm and base @@ -32,12 +31,12 @@ class LeKiwiRobotConfig(RobotConfig): # LeKiwiRobot will always have (and needs) such 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") + 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") # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index 94c04d2f..3878e08e 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi.py @@ -14,32 +14,34 @@ # See the License for the specific language governing permissions and # limitations under the License. +import base64 import json import logging -import numpy as np -import base64 + import cv2 +import numpy as np import torch +import zmq from lerobot.common.constants import OBS_IMAGES, OBS_STATE from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError, InvalidActionError + from ..robot import Robot, RobotMode 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 -# Other options include: +# Other options include: # 1. Adding it to the Telop implementation for lekiwi # (meaning each robot will need a teleop imple) or -# 2. Adding it into the robot implementation -# (meaning the policy might be needed to be train +# 2. Adding it into the robot implementation +# (meaning the policy might be needed to be train # over the teleop action space) class DaemonLeKiwiRobot(Robot): - config_class = DaemonLeKiwiRobotConfig name = "daemonlekiwi" @@ -63,7 +65,6 @@ class DaemonLeKiwiRobot(Robot): 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 @@ -105,7 +106,7 @@ class DaemonLeKiwiRobot(Robot): # } # return cam_ft pass - + def connect(self) -> None: if self.is_connected: raise DeviceAlreadyConnectedError( @@ -121,17 +122,17 @@ class DaemonLeKiwiRobot(Robot): 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.zmq_observation_socket.setsockopt(zmq.CONFLATE, 1) self.is_connected = True def calibrate(self) -> None: - # TODO(Steven): Nothing to calibrate. + # TODO(Steven): Nothing to calibrate. # Consider triggering calibrate() on the remote mobile robot? - # Althought this would require a more complex comms schema + # Although this would require a more complex comms schema logging.warning("DaemonLeKiwiRobot has nothing to calibrate.") return - + # Consider moving these static functions out of the class # Copied from robot_lekiwi MobileManipulator class @staticmethod @@ -145,7 +146,7 @@ class DaemonLeKiwiRobot(Robot): return speed_int | 0x8000 else: return speed_int & 0x7FFF - + # Copied from robot_lekiwi MobileManipulator class @staticmethod def raw_to_degps(raw_speed: int) -> float: @@ -155,7 +156,7 @@ class DaemonLeKiwiRobot(Robot): if raw_speed & 0x8000: degps = -degps return degps - + # Copied from robot_lekiwi MobileManipulator class def body_to_wheel_raw( self, @@ -217,7 +218,7 @@ class DaemonLeKiwiRobot(Robot): 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 @@ -260,7 +261,7 @@ class DaemonLeKiwiRobot(Robot): 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): # Copied from robot_lekiwi.py """Polls the video socket for up to 15 ms. If data arrives, decode only @@ -332,7 +333,7 @@ class DaemonLeKiwiRobot(Robot): # 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]: @@ -353,7 +354,7 @@ class DaemonLeKiwiRobot(Robot): 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 @@ -361,13 +362,12 @@ class DaemonLeKiwiRobot(Robot): if frame is None: # TODO(Steven): Daemon doesn't know camera dimensions logging.warning("Frame is None") - #frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8) + # frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8) obs_dict[cam_name] = torch.from_numpy(frame) return obs_dict def from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray): - # Speed control if self.teleop_keys["speed_up"] in pressed_keys: self.speed_index = min(self.speed_index + 1, 2) @@ -376,7 +376,7 @@ class DaemonLeKiwiRobot(Robot): speed_setting = self.speed_levels[self.speed_index] xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4 theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90 - + x_cmd = 0.0 # m/s forward/backward y_cmd = 0.0 # m/s lateral theta_cmd = 0.0 # deg/s rotation @@ -424,27 +424,27 @@ class DaemonLeKiwiRobot(Robot): raise DeviceNotConnectedError( "ManipulatorRobot is not connected. You need to run `robot.connect()`." ) - + goal_pos: np.array = np.empty(9) if self.robot_mode is RobotMode.AUTO: # TODO(Steven): Not yet implemented. The policy outputs might need a different conversion raise Exception - + # TODO(Steven): This assumes teleop mode is always used with keyboard if self.robot_mode is RobotMode.TELEOP: - if action.size <6: + if action.size < 6: logging.error("Action should include at least the 6 states of the leader arm") raise InvalidActionError - + # TODO(Steven): Assumes size and order is respected - wheel_actions = [v for _,v in self.from_keyboard_to_wheel_action(action[6:])] - goal_pos[:6]=action[:6] - goal_pos[6:]=wheel_actions - self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) #action is in motor space + wheel_actions = [v for _, v in self.from_keyboard_to_wheel_action(action[6:])] + goal_pos[:6] = action[:6] + goal_pos[6:] = wheel_actions + self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) # action is in motor space return goal_pos - + def print_logs(self): # TODO(Steven): Refactor logger pass @@ -459,7 +459,7 @@ class DaemonLeKiwiRobot(Robot): 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 index 79c89f6b..52da223f 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py @@ -1,12 +1,15 @@ -from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig +import logging +import time + +import numpy as np + from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig +from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig from .daemon_lekiwi import DaemonLeKiwiRobot, RobotMode -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) @@ -21,24 +24,20 @@ def main(): 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 + robot.connect() # Establishes ZMQ sockets with the remote mobile robot robot.robot_mode = RobotMode.TELEOP 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 - } - _action_sent = robot.send_action(action) # Translates to motor space + sends over ZMQ - _observation = robot.get_observation() # Receives over ZMQ, translate to body-frame vel + action = np.concatenate((arm_action, base_action)) + _action_sent = robot.send_action(action) # Translates to motor space + sends over ZMQ + _observation = robot.get_observation() # Receives over ZMQ, translate to body-frame vel # dataset.save(action_sent, obs) @@ -50,11 +49,12 @@ def main(): duration = time.perf_counter() - start logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon") - robot.disconnect() # Cleans ZMQ comms + 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 + main() diff --git a/lerobot/common/robots/lekiwi/lekiwi_app.py b/lerobot/common/robots/lekiwi/lekiwi_app.py index 349c952b..f8638651 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/lekiwi_app.py @@ -1,9 +1,10 @@ import logging + from .configuration_lekiwi import LeKiwiRobotConfig from .lekiwi_robot import LeKiwiRobot -def main(): +def main(): logging.info("Configuring LeKiwiRobot") robot_config = LeKiwiRobotConfig() robot = LeKiwiRobot(robot_config) @@ -14,9 +15,9 @@ def main(): # Remotely teleoperated logging.info("Starting LeKiwiRobot teleoperation") robot.run() - + logging.info("Finished LeKiwiRobot cleanly") if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index b28629f1..9096abde 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -14,27 +14,29 @@ # See the License for the specific language governing permissions and # limitations under the License. +import base64 import json import logging -import time import threading -import numpy as np import time -import base64 + import cv2 +import numpy as np +import zmq 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_lekiwi import LeKiwiRobotConfig from lerobot.common.motors.feetech import ( FeetechMotorsBus, TorqueMode, - run_arm_manual_calibration, + run_full_arm_calibration, ) -import zmq + +from ..robot import Robot +from ..utils import ensure_safe_goal_position +from .configuration_lekiwi import LeKiwiRobotConfig + class LeKiwiRobot(Robot): """ @@ -58,7 +60,7 @@ class LeKiwiRobot(Robot): # 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(Steven): Order and dimension are generaly assumed to be 6 first for arm, 3 last for base + # TODO(Steven): Order and dimension are generally assumed to be 6 first for arm, 3 last for base self.actuators_bus = FeetechMotorsBus( port=self.config.port_motor_bus, motors={ @@ -79,7 +81,7 @@ class LeKiwiRobot(Robot): self.max_relative_target = config.max_relative_target - #TODO(Steven): Consider removing cameras from configs + # TODO(Steven): Consider removing cameras from configs self.cameras = make_cameras_from_configs(config.cameras) self.observation_lock = threading.Lock() @@ -92,8 +94,6 @@ class LeKiwiRobot(Robot): self.is_connected = False self.logs = {} - - @property def state_feature(self) -> dict: return { @@ -128,41 +128,39 @@ class LeKiwiRobot(Robot): observation_socket.bind(f"tcp://*:{self.port_zmq_observations}") return context, cmd_socket, observation_socket - - def setup_actuators(self): + def setup_actuators(self): # Set-up arm actuators (position mode) # We assume that at connection time, arm is in a rest position, # and torque can be safely disabled to run calibration. - self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value,self.arm_actuators) - self.calibrate() # TODO(Steven): This should be only for the arm + self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value, self.arm_actuators) + self.calibrate() # TODO(Steven): This should be only for the arm # Mode=0 for Position Control - self.actuators_bus.write("Mode", 0,self.arm_actuators) + self.actuators_bus.write("Mode", 0, self.arm_actuators) # Set P_Coefficient to lower value to avoid shakiness (Default is 32) - self.actuators_bus.write("P_Coefficient", 16,self.arm_actuators) + self.actuators_bus.write("P_Coefficient", 16, self.arm_actuators) # Set I_Coefficient and D_Coefficient to default value 0 and 32 self.actuators_bus.write("I_Coefficient", 0, self.arm_actuators) - self.actuators_bus.write("D_Coefficient", 32,self.arm_actuators) + self.actuators_bus.write("D_Coefficient", 32, self.arm_actuators) # Close the write lock so that Maximum_Acceleration gets written to EPROM address, # which is mandatory for Maximum_Acceleration to take effect after rebooting. - self.actuators_bus.write("Lock", 0,self.arm_actuators) + self.actuators_bus.write("Lock", 0, self.arm_actuators) # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of # the motors. Note: this configuration is not in the official STS3215 Memory Table - self.actuators_bus.write("Maximum_Acceleration", 254,self.arm_actuators) + self.actuators_bus.write("Maximum_Acceleration", 254, self.arm_actuators) self.actuators_bus.write("Acceleration", 254, self.arm_actuators) logging.info("Activating torque.") - self.actuators_bus.write("Torque_Enable", TorqueMode.ENABLED.value,self.arm_actuators) + self.actuators_bus.write("Torque_Enable", TorqueMode.ENABLED.value, self.arm_actuators) # Check arm can be read - self.actuators_bus.read("Present_Position",self.arm_actuators) + self.actuators_bus.read("Present_Position", self.arm_actuators) # Set-up base actuators (velocity mode) - self.actuators_bus.write("Lock",0,self.base_actuators) - self.actuators_bus.write("Mode",[1,1,1],self.base_actuators) - self.actuators_bus.write("Lock",1,self.base_actuators) - + self.actuators_bus.write("Lock", 0, self.base_actuators) + self.actuators_bus.write("Mode", [1, 1, 1], self.base_actuators) + self.actuators_bus.write("Lock", 1, self.base_actuators) def connect(self) -> None: if self.is_connected: @@ -192,15 +190,17 @@ class LeKiwiRobot(Robot): actuators_calib_path = self.calibration_dir / f"{self.config.id}.json" if actuators_calib_path.exists(): - with open(actuators_calib_path) as f: + with open(actuators_calib_path,encoding="utf-8") as f: calibration = json.load(f) else: - logging.info(f"Missing calibration file '{actuators_calib_path}'") - calibration = run_arm_manual_calibration(self.actuators_bus, self.robot_type, self.name, "follower") + logging.info("Missing calibration file '%s'",actuators_calib_path) + calibration = run_full_arm_calibration( + self.actuators_bus, self.robot_type, self.name, "follower" + ) - logging.info(f"Calibration is done! Saving calibration file '{actuators_calib_path}'") + logging.info("Calibration is done! Saving calibration file '%s'",actuators_calib_path) actuators_calib_path.parent.mkdir(parents=True, exist_ok=True) - with open(actuators_calib_path, "w") as f: + with open(actuators_calib_path, "w",encoding="utf-8") as f: json.dump(calibration, f) self.actuators_bus.set_calibration(calibration) @@ -208,15 +208,15 @@ class LeKiwiRobot(Robot): def get_observation(self) -> dict[str, np.ndarray]: """The returned observations do not have a batch dimension.""" if not self.is_connected: - raise DeviceNotConnectedError( - "LeKiwiRobot is not connected. You need to run `robot.connect()`." - ) + raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.") obs_dict = {} # Read actuators position for arm and vel for base before_read_t = time.perf_counter() - obs_dict[OBS_STATE] = self.actuators_bus.read("Present_Position",self.arm_actuators) + self.actuators_bus.read("Present_Speed", self.base_actuators) + obs_dict[OBS_STATE] = self.actuators_bus.read( + "Present_Position", self.arm_actuators + ) + self.actuators_bus.read("Present_Speed", self.base_actuators) self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t # Capture images from cameras @@ -251,22 +251,22 @@ class LeKiwiRobot(Robot): np.ndarray: the action sent to the motors, potentially clipped. """ if not self.is_connected: - raise DeviceNotConnectedError( - "LeKiwiRobot is not connected. You need to run `robot.connect()`." - ) + raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.") # Input action is in motor space - goal_pos=action + 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_bus.read("Present_Position",self.arm_actuators) - goal_pos[:6] = ensure_safe_goal_position(goal_pos[:6], present_pos, self.config.max_relative_target) + present_pos = self.actuators_bus.read("Present_Position", self.arm_actuators) + goal_pos[:6] = ensure_safe_goal_position( + goal_pos[:6], present_pos, self.config.max_relative_target + ) # Send goal position to the actuators # TODO(Steven): This happens synchronously - self.actuators_bus.write("Goal_Position", goal_pos[:6].astype(np.int32),self.arm_actuators) - self.actuators_bus.write("Goal_Speed",goal_pos[6:].astype(np.int32),self.base_actuators) + self.actuators_bus.write("Goal_Position", goal_pos[:6].astype(np.int32), self.arm_actuators) + self.actuators_bus.write("Goal_Speed", goal_pos[6:].astype(np.int32), self.base_actuators) return goal_pos @@ -280,16 +280,14 @@ class LeKiwiRobot(Robot): def stop(self): # TODO(Steven): Assumes there's only 3 motors for base logging.info("Stopping base") - self.actuators_bus.write("Goal_Speed",[0,0,0],self.base_actuators) + self.actuators_bus.write("Goal_Speed", [0, 0, 0], self.base_actuators) logging.info("Base motors stopped") def run(self): # Copied logic from run_lekiwi in lekiwi_remote.py if not self.is_connected: - raise DeviceNotConnectedError( - "LeKiwiRobot is not connected. You need to run `robot.connect()`." - ) - + raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.") + stop_event = threading.Event() observation_thread = threading.Thread( target=self.update_last_observation, args=(stop_event), daemon=True @@ -311,17 +309,16 @@ class LeKiwiRobot(Robot): # except zmq.Again: # logging.warning("ZMQ again") except Exception as e: - logging.warning(f"[ERROR] Message fetching failed: {e}") - + logging.error("Message fetching failed: %s",e) + # 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() - pass - + self.stop() + with self.observation_lock: 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 time.sleep( @@ -333,8 +330,7 @@ class LeKiwiRobot(Robot): stop_event.set() observation_thread.join() self.disconnect() - pass - + def print_logs(self): # TODO(Steven): Refactor logger pass @@ -344,7 +340,7 @@ class LeKiwiRobot(Robot): raise DeviceNotConnectedError( "LeKiwi is not connected. You need to run `robot.connect()` before disconnecting." ) - + self.stop() self.actuators_bus.disconnect() for cam in self.cameras.values(): @@ -353,7 +349,7 @@ class LeKiwiRobot(Robot): self.cmd_socket.close() self.context.term() self.is_connected = False - + def __del__(self): if getattr(self, "is_connected", False): self.disconnect() diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py index de908426..e3cca3de 100644 --- a/lerobot/common/robots/robot.py +++ b/lerobot/common/robots/robot.py @@ -1,5 +1,6 @@ import abc from pathlib import Path +import enum from typing import Any import draccus @@ -9,7 +10,6 @@ from lerobot.common.motors import MotorCalibration from .config import RobotConfig -import enum class RobotMode(enum.Enum): TELEOP = 0 diff --git a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py index 1a679b18..1a8e0e96 100644 --- a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py +++ b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py @@ -64,7 +64,7 @@ class KeyboardTeleop(Teleoperator): @property def action_feature(self) -> dict: - #TODO(Steven): Verify this is correct + # TODO(Steven): Verify this is correct return { "dtype": "float32", "shape": (len(self.arm),), @@ -76,7 +76,7 @@ class KeyboardTeleop(Teleoperator): return {} def connect(self) -> None: - #TODO(Steven): Consider instead of raising a warning and then returning the status + # TODO(Steven): Consider instead of raising a warning and then returning the status # if self.is_connected: # logging.warning( # "ManipulatorRobot is already connected. Do not run `robot.connect()` twice." From 465608db2dafa72e9d38666d3dcdb670d6924ba4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 14 Mar 2025 14:01:07 +0000 Subject: [PATCH 06/19] fix(lekiwi): HW fixes v0.1 --- .../lekiwi/configuration_daemon_lekiwi.py | 4 ++- .../robots/lekiwi/configuration_lekiwi.py | 2 ++ .../common/robots/lekiwi/daemon_lekiwi_app.py | 9 +++-- lerobot/common/robots/lekiwi/lekiwi_app.py | 4 +-- lerobot/common/robots/lekiwi/lekiwi_robot.py | 36 ++++++++++--------- lerobot/common/robots/robot.py | 5 ++- 6 files changed, 34 insertions(+), 26 deletions(-) diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index f95dc8d4..b9c8e549 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py @@ -7,10 +7,12 @@ from lerobot.common.robots.config import RobotConfig @dataclass class DaemonLeKiwiRobotConfig(RobotConfig): # Network Configuration - remote_ip: str = "192.168.0.193" + remote_ip: str = "172.18.133.90" port_zmq_cmd: int = 5555 port_zmq_observations: int = 5556 + id = "daemonlekiwi" + teleop_keys: dict[str, str] = field( default_factory=lambda: { # Movement diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py index c72b427d..160c17d0 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_lekiwi.py @@ -11,6 +11,8 @@ class LeKiwiRobotConfig(RobotConfig): port_zmq_cmd: int = 5555 port_zmq_observations: int = 5556 + id = "lekiwi" + cameras: dict[str, CameraConfig] = field( default_factory=lambda: { "front": OpenCVCameraConfig( diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index 52da223f..b459f21c 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py @@ -3,10 +3,10 @@ import time import numpy as np -from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig -from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig -from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig -from .daemon_lekiwi import DaemonLeKiwiRobot, RobotMode +from lerobot.common.robots.lekiwi.configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig +from lerobot.common.robots.lekiwi.daemon_lekiwi import DaemonLeKiwiRobot, RobotMode +from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig +from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig def main(): @@ -45,7 +45,6 @@ def main(): # robot.set_mode(RobotMode.AUTO) # policy_action = policy.get_action() # This might be in body frame, key space or smt else # robot.send_action(policy_action) - duration = time.perf_counter() - start logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon") diff --git a/lerobot/common/robots/lekiwi/lekiwi_app.py b/lerobot/common/robots/lekiwi/lekiwi_app.py index f8638651..6df462fe 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/lekiwi_app.py @@ -1,7 +1,7 @@ import logging -from .configuration_lekiwi import LeKiwiRobotConfig -from .lekiwi_robot import LeKiwiRobot +from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig +from lerobot.common.robots.lekiwi.lekiwi_robot import LeKiwiRobot def main(): diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index 9096abde..f5204ffd 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -134,7 +134,7 @@ class LeKiwiRobot(Robot): # We assume that at connection time, arm is in a rest position, # and torque can be safely disabled to run calibration. self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value, self.arm_actuators) - self.calibrate() # TODO(Steven): This should be only for the arm + # self.calibrate() # TODO(Steven): This should be only for the arm # Mode=0 for Position Control self.actuators_bus.write("Mode", 0, self.arm_actuators) @@ -190,17 +190,15 @@ class LeKiwiRobot(Robot): actuators_calib_path = self.calibration_dir / f"{self.config.id}.json" if actuators_calib_path.exists(): - with open(actuators_calib_path,encoding="utf-8") as f: + with open(actuators_calib_path, encoding="utf-8") as f: calibration = json.load(f) else: - logging.info("Missing calibration file '%s'",actuators_calib_path) - calibration = run_full_arm_calibration( - self.actuators_bus, self.robot_type, self.name, "follower" - ) + logging.info("Missing calibration file '%s'", actuators_calib_path) + calibration = run_full_arm_calibration(self.actuators_bus, self.robot_type, self.name, "follower") - logging.info("Calibration is done! Saving calibration file '%s'",actuators_calib_path) + logging.info("Calibration is done! Saving calibration file '%s'", actuators_calib_path) actuators_calib_path.parent.mkdir(parents=True, exist_ok=True) - with open(actuators_calib_path, "w",encoding="utf-8") as f: + with open(actuators_calib_path, "w", encoding="utf-8") as f: json.dump(calibration, f) self.actuators_bus.set_calibration(calibration) @@ -214,9 +212,12 @@ class LeKiwiRobot(Robot): # Read actuators position for arm and vel for base before_read_t = time.perf_counter() - obs_dict[OBS_STATE] = self.actuators_bus.read( - "Present_Position", self.arm_actuators - ) + self.actuators_bus.read("Present_Speed", self.base_actuators) + obs_dict[OBS_STATE] = np.concatenate( + ( + self.actuators_bus.read("Present_Position", self.arm_actuators), + self.actuators_bus.read("Present_Speed", self.base_actuators), + ) + ) self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t # Capture images from cameras @@ -290,7 +291,7 @@ class LeKiwiRobot(Robot): stop_event = threading.Event() observation_thread = threading.Thread( - target=self.update_last_observation, args=(stop_event), daemon=True + target=self.update_last_observation, args=[stop_event], daemon=True ) observation_thread.start() @@ -302,14 +303,14 @@ class LeKiwiRobot(Robot): loop_start_time = time.time() try: - msg = self.cmd_socket.recv_string(zmq.NOBLOCK) + msg = self.zmq_cmd_socket.recv_string(zmq.NOBLOCK) data = json.loads(msg) self.send_action(data) last_cmd_time = time.time() - # except zmq.Again: - # logging.warning("ZMQ again") + except zmq.Again: + logging.warning("ZMQ again") except Exception as e: - logging.error("Message fetching failed: %s",e) + logging.error("Message fetching failed: %s", e) # Watchdog: stop the robot if no command is received for over 0.5 seconds. now = time.time() @@ -317,6 +318,7 @@ class LeKiwiRobot(Robot): self.stop() with self.observation_lock: + # TODO(Steven): This operation is blocking if no listener is available self.zmq_observation_socket.send_string(json.dumps(self.last_observation)) # Ensure a short sleep to avoid overloading the CPU. @@ -346,7 +348,7 @@ class LeKiwiRobot(Robot): for cam in self.cameras.values(): cam.disconnect() self.observation_socket.close() - self.cmd_socket.close() + self.zmq_cmd_socket.close() self.context.term() self.is_connected = False diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py index e3cca3de..57ff41a4 100644 --- a/lerobot/common/robots/robot.py +++ b/lerobot/common/robots/robot.py @@ -1,6 +1,7 @@ import abc from pathlib import Path import enum +from pathlib import Path from typing import Any import draccus @@ -30,7 +31,9 @@ class Robot(abc.ABC): self.robot_mode: RobotMode | None = None self.id = config.id self.calibration_dir = ( - config.calibration_dir if config.calibration_dir else HF_LEROBOT_CALIBRATION / ROBOTS / self.name + Path(config.calibration_dir) + if config.calibration_dir + else HF_LEROBOT_CALIBRATION / ROBOTS / self.name ) self.calibration_dir.mkdir(parents=True, exist_ok=True) self.calibration_fpath = self.calibration_dir / f"{self.id}.json" From 9a25c41f91ed6310a15c40d17fd47ad0e7d20bb7 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Mon, 17 Mar 2025 17:38:08 +0100 Subject: [PATCH 07/19] fix(lekiwi): HW fixes v0.2 --- lerobot/common/robots/lekiwi/daemon_lekiwi.py | 20 ++++++++++++------- .../common/robots/lekiwi/daemon_lekiwi_app.py | 12 +++++------ lerobot/common/robots/lekiwi/lekiwi_robot.py | 2 +- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index 3878e08e..3a76f809 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi.py @@ -108,6 +108,8 @@ class DaemonLeKiwiRobot(Robot): pass def connect(self) -> None: + """Establishes ZMQ sockets with the remote mobile robot""" + if self.is_connected: raise DeviceAlreadyConnectedError( "LeKiwi Daemon is already connected. Do not run `robot.connect()` twice." @@ -340,7 +342,7 @@ class DaemonLeKiwiRobot(Robot): """ 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. + and a camera frame. Receives over ZMQ, translate to body-frame vel """ if not self.is_connected: raise DeviceNotConnectedError( @@ -409,7 +411,7 @@ class DaemonLeKiwiRobot(Robot): # t3': execute motor_cmd # t3'-t2' << t3-t1 def send_action(self, action: np.ndarray) -> np.ndarray: - """Command lekiwi to move to a target joint configuration. + """Command lekiwi to move to a target joint configuration. Translates to motor space + sends over ZMQ Args: action (np.ndarray): array containing the goal positions for the motors. @@ -425,7 +427,7 @@ class DaemonLeKiwiRobot(Robot): "ManipulatorRobot is not connected. You need to run `robot.connect()`." ) - goal_pos: np.array = np.empty(9) + goal_pos: np.array = np.zeros(9) if self.robot_mode is RobotMode.AUTO: # TODO(Steven): Not yet implemented. The policy outputs might need a different conversion @@ -437,11 +439,13 @@ class DaemonLeKiwiRobot(Robot): logging.error("Action should include at least the 6 states of the leader arm") raise InvalidActionError - # TODO(Steven): Assumes size and order is respected - wheel_actions = [v for _, v in self.from_keyboard_to_wheel_action(action[6:])] goal_pos[:6] = action[:6] - goal_pos[6:] = wheel_actions - self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) # action is in motor space + if action.size > 6: + # TODO(Steven): Assumes size and order is respected + wheel_actions = [v for _, v in self.from_keyboard_to_wheel_action(action[6:]).items()] + goal_pos[6:] = wheel_actions + + self.zmq_cmd_socket.send_string(json.dumps(goal_pos.tolist())) # action is in motor space return goal_pos @@ -450,6 +454,8 @@ class DaemonLeKiwiRobot(Robot): pass def disconnect(self): + """Cleans ZMQ comms""" + if not self.is_connected: raise DeviceNotConnectedError( "LeKiwi is not connected. You need to run `robot.connect()` before disconnecting." diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index b459f21c..a52f534b 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py @@ -11,7 +11,7 @@ from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig def main(): logging.info("Configuring Teleop Devices") - leader_arm_config = SO100TeleopConfig(port="/dev/tty.usbmodem585A0085511") + leader_arm_config = SO100TeleopConfig(port="/dev/tty.usbmodem58760429271") leader_arm = SO100Teleop(leader_arm_config) keyboard_config = KeyboardTeleopConfig() @@ -26,7 +26,7 @@ def main(): robot = DaemonLeKiwiRobot(robot_config) logging.info("Connecting remote LeKiwiRobot") - robot.connect() # Establishes ZMQ sockets with the remote mobile robot + robot.connect() robot.robot_mode = RobotMode.TELEOP logging.info("Starting LeKiwiRobot teleoperation") @@ -35,9 +35,9 @@ def main(): while duration < 20: arm_action = leader_arm.get_action() base_action = keyboard.get_action() - action = np.concatenate((arm_action, base_action)) - _action_sent = robot.send_action(action) # Translates to motor space + sends over ZMQ - _observation = robot.get_observation() # Receives over ZMQ, translate to body-frame vel + action = np.append(arm_action, base_action) if base_action.size > 0 else arm_action + _action_sent = robot.send_action(action) + _observation = robot.get_observation() # dataset.save(action_sent, obs) @@ -48,7 +48,7 @@ def main(): duration = time.perf_counter() - start logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon") - robot.disconnect() # Cleans ZMQ comms + robot.disconnect() leader_arm.disconnect() keyboard.disconnect() diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index f5204ffd..ed0e7af4 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -304,7 +304,7 @@ class LeKiwiRobot(Robot): try: msg = self.zmq_cmd_socket.recv_string(zmq.NOBLOCK) - data = json.loads(msg) + data = np.array(json.loads(msg)) self.send_action(data) last_cmd_time = time.time() except zmq.Again: From c0f2dba2510b95c46a786b7efa4afbea6ac2d16c Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Mon, 17 Mar 2025 18:03:32 +0100 Subject: [PATCH 08/19] fix(lekiwi): HW fixes v0.3 --- lerobot/common/robots/lekiwi/lekiwi_robot.py | 4 ++-- lerobot/common/teleoperators/so100/config_so100_leader.py | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index ed0e7af4..79388a12 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -134,7 +134,7 @@ class LeKiwiRobot(Robot): # We assume that at connection time, arm is in a rest position, # and torque can be safely disabled to run calibration. self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value, self.arm_actuators) - # self.calibrate() # TODO(Steven): This should be only for the arm + self.calibrate() # TODO(Steven): This should be only for the arm # Mode=0 for Position Control self.actuators_bus.write("Mode", 0, self.arm_actuators) @@ -347,7 +347,7 @@ class LeKiwiRobot(Robot): self.actuators_bus.disconnect() for cam in self.cameras.values(): cam.disconnect() - self.observation_socket.close() + self.zmq_observation_socket.close() self.zmq_cmd_socket.close() self.context.term() self.is_connected = False diff --git a/lerobot/common/teleoperators/so100/config_so100_leader.py b/lerobot/common/teleoperators/so100/config_so100_leader.py index a97949b7..aa2747b0 100644 --- a/lerobot/common/teleoperators/so100/config_so100_leader.py +++ b/lerobot/common/teleoperators/so100/config_so100_leader.py @@ -24,3 +24,4 @@ from ..config import TeleoperatorConfig class SO100LeaderConfig(TeleoperatorConfig): # Port to connect to the arm port: str + id = "so100" From 93082ec22c80703ff9e6703d27212d700a573028 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Mon, 17 Mar 2025 18:49:05 +0100 Subject: [PATCH 09/19] fix(lekiwi): HW fixes v0.4 --- .../lekiwi/configuration_daemon_lekiwi.py | 2 ++ .../robots/lekiwi/configuration_lekiwi.py | 4 +-- lerobot/common/robots/lekiwi/daemon_lekiwi.py | 25 ++++++++----------- .../common/robots/lekiwi/daemon_lekiwi_app.py | 2 +- lerobot/common/robots/lekiwi/lekiwi_robot.py | 21 +++++++++++++--- 5 files changed, 33 insertions(+), 21 deletions(-) diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index b9c8e549..b526bce9 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/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 diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py index 160c17d0..80c25d20 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/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 ), } ) diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index 3a76f809..b096ec02 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/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 diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index a52f534b..3c45377d 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/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 diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index 79388a12..22153b5f 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/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): From dd6a4b7b8c7d85d07aa976ed43be6df17a201115 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 18 Mar 2025 17:48:58 +0100 Subject: [PATCH 10/19] fix(lekiwi): fix calibration issue --- lerobot/common/robots/lekiwi/lekiwi_robot.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index 22153b5f..1eb279fe 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -30,6 +30,7 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte from lerobot.common.motors.feetech import ( FeetechMotorsBus, TorqueMode, + apply_feetech_offsets_from_calibration, run_full_arm_calibration, ) @@ -202,6 +203,7 @@ class LeKiwiRobot(Robot): json.dump(calibration, f) self.actuators_bus.set_calibration(calibration) + apply_feetech_offsets_from_calibration(self.actuators_bus, calibration) # TODO(Steven): Should this be dict[str, Any] ? def get_observation(self) -> dict[str, np.ndarray]: From 8691d72757680ac1106b36a95941a580c26b187b Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 19 Mar 2025 10:23:47 +0100 Subject: [PATCH 11/19] feat(lekiwi): de-couple classes + make it single-threaded --- .../robots/lekiwi/_old_lekiwi_remote.py | 224 ------ .../common/robots/lekiwi/_old_robot_lekiwi.py | 692 ------------------ .../robots/lekiwi/configuration_lekiwi.py | 3 - lerobot/common/robots/lekiwi/daemon_lekiwi.py | 26 +- lerobot/common/robots/lekiwi/lekiwi_app.py | 75 +- lerobot/common/robots/lekiwi/lekiwi_robot.py | 101 +-- 6 files changed, 87 insertions(+), 1034 deletions(-) delete mode 100644 lerobot/common/robots/lekiwi/_old_lekiwi_remote.py delete mode 100644 lerobot/common/robots/lekiwi/_old_robot_lekiwi.py diff --git a/lerobot/common/robots/lekiwi/_old_lekiwi_remote.py b/lerobot/common/robots/lekiwi/_old_lekiwi_remote.py deleted file mode 100644 index 1a3af5cf..00000000 --- a/lerobot/common/robots/lekiwi/_old_lekiwi_remote.py +++ /dev/null @@ -1,224 +0,0 @@ -# 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 base64 -import json -import threading -import time -from pathlib import Path - -import cv2 -import zmq - -from lerobot.common.robots.mobile_manipulator import LeKiwi - - -def setup_zmq_sockets(config): - context = zmq.Context() - cmd_socket = context.socket(zmq.PULL) - cmd_socket.setsockopt(zmq.CONFLATE, 1) - cmd_socket.bind(f"tcp://*:{config.port}") - - video_socket = context.socket(zmq.PUSH) - video_socket.setsockopt(zmq.CONFLATE, 1) - video_socket.bind(f"tcp://*:{config.video_port}") - - return context, cmd_socket, video_socket - - -def run_camera_capture(cameras, images_lock, latest_images_dict, stop_event): - while not stop_event.is_set(): - local_dict = {} - for name, cam in cameras.items(): - frame = cam.async_read() - ret, buffer = cv2.imencode(".jpg", frame, [int(cv2.IMWRITE_JPEG_QUALITY), 90]) - if ret: - local_dict[name] = base64.b64encode(buffer).decode("utf-8") - else: - local_dict[name] = "" - with images_lock: - latest_images_dict.update(local_dict) - time.sleep(0.01) - - -def calibrate_follower_arm(motors_bus, calib_dir_str): - """ - Calibrates the follower arm. Attempts to load an existing calibration file; - if not found, runs manual calibration and saves the result. - """ - calib_dir = Path(calib_dir_str) - calib_dir.mkdir(parents=True, exist_ok=True) - calib_file = calib_dir / "main_follower.json" - try: - from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration - except ImportError: - print("[WARNING] Calibration function not available. Skipping calibration.") - return - - if calib_file.exists(): - with open(calib_file) as f: - calibration = json.load(f) - print(f"[INFO] Loaded calibration from {calib_file}") - else: - print("[INFO] Calibration file not found. Running manual calibration...") - calibration = run_full_arm_calibration(motors_bus, "lekiwi", "follower_arm", "follower") - print(f"[INFO] Calibration complete. Saving to {calib_file}") - with open(calib_file, "w") as f: - json.dump(calibration, f) - try: - motors_bus.set_calibration(calibration) - print("[INFO] Applied calibration for follower arm.") - except Exception as e: - print(f"[WARNING] Could not apply calibration: {e}") - - -def run_lekiwi(robot_config): - """ - Runs the LeKiwi robot: - - Sets up cameras and connects them. - - Initializes the follower arm motors. - - Calibrates the follower arm if necessary. - - Creates ZeroMQ sockets for receiving commands and streaming observations. - - Processes incoming commands (arm and wheel commands) and sends back sensor and camera data. - """ - # Import helper functions and classes - from lerobot.common.cameras.utils import make_cameras_from_configs - from lerobot.common.motors.feetech.feetech import FeetechMotorsBus, TorqueMode - - # Initialize cameras from the robot configuration. - cameras = make_cameras_from_configs(robot_config.cameras) - for cam in cameras.values(): - cam.connect() - - # Initialize the motors bus using the follower arm configuration. - motor_config = robot_config.follower_arms.get("main") - if motor_config is None: - print("[ERROR] Follower arm 'main' configuration not found.") - return - motors_bus = FeetechMotorsBus(motor_config) - motors_bus.connect() - - # Calibrate the follower arm. - calibrate_follower_arm(motors_bus, robot_config.calibration_dir) - - # Create the LeKiwi robot instance. - robot = LeKiwi(motors_bus) - - # Define the expected arm motor IDs. - arm_motor_ids = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"] - - # Disable torque for each arm motor. - for motor in arm_motor_ids: - motors_bus.write("Torque_Enable", TorqueMode.DISABLED.value, motor) - - # Set up ZeroMQ sockets. - context, cmd_socket, video_socket = setup_zmq_sockets(robot_config) - - # Start the camera capture thread. - latest_images_dict = {} - images_lock = threading.Lock() - stop_event = threading.Event() - cam_thread = threading.Thread( - target=run_camera_capture, args=(cameras, images_lock, latest_images_dict, stop_event), daemon=True - ) - cam_thread.start() - - last_cmd_time = time.time() - print("LeKiwi robot server started. Waiting for commands...") - - try: - while True: - loop_start_time = time.time() - - # Process incoming commands (non-blocking). - while True: - try: - msg = cmd_socket.recv_string(zmq.NOBLOCK) - except zmq.Again: - break - try: - data = json.loads(msg) - # Process arm position commands. - if "arm_positions" in data: - arm_positions = data["arm_positions"] - if not isinstance(arm_positions, list): - print(f"[ERROR] Invalid arm_positions: {arm_positions}") - elif len(arm_positions) < len(arm_motor_ids): - print( - f"[WARNING] Received {len(arm_positions)} arm positions, expected {len(arm_motor_ids)}" - ) - else: - for motor, pos in zip(arm_motor_ids, arm_positions, strict=False): - motors_bus.write("Goal_Position", pos, motor) - # Process wheel (base) commands. - if "raw_velocity" in data: - raw_command = data["raw_velocity"] - # Expect keys: "left_wheel", "back_wheel", "right_wheel". - command_speeds = [ - int(raw_command.get("left_wheel", 0)), - int(raw_command.get("back_wheel", 0)), - int(raw_command.get("right_wheel", 0)), - ] - robot.set_velocity(command_speeds) - last_cmd_time = time.time() - except Exception as e: - print(f"[ERROR] Parsing message failed: {e}") - - # Watchdog: stop the robot if no command is received for over 0.5 seconds. - now = time.time() - if now - last_cmd_time > 0.5: - robot.stop() - last_cmd_time = now - - # Read current wheel speeds from the robot. - current_velocity = robot.read_velocity() - - # Read the follower arm state from the motors bus. - follower_arm_state = [] - for motor in arm_motor_ids: - try: - pos = motors_bus.read("Present_Position", motor) - # Convert the position to a float (or use as is if already numeric). - follower_arm_state.append(float(pos) if not isinstance(pos, (int, float)) else pos) - except Exception as e: - print(f"[ERROR] Reading motor {motor} failed: {e}") - - # Get the latest camera images. - with images_lock: - images_dict_copy = dict(latest_images_dict) - - # Build the observation dictionary. - observation = { - "images": images_dict_copy, - "present_speed": current_velocity, - "follower_arm_state": follower_arm_state, - } - # Send the observation over the video socket. - video_socket.send_string(json.dumps(observation)) - - # Ensure a short sleep to avoid overloading the CPU. - elapsed = time.time() - loop_start_time - time.sleep( - max(0.033 - elapsed, 0) - ) # If robot jitters increase the sleep and monitor cpu load with `top` in cmd - except KeyboardInterrupt: - print("Shutting down LeKiwi server.") - finally: - stop_event.set() - cam_thread.join() - robot.stop() - motors_bus.disconnect() - cmd_socket.close() - video_socket.close() - context.term() diff --git a/lerobot/common/robots/lekiwi/_old_robot_lekiwi.py b/lerobot/common/robots/lekiwi/_old_robot_lekiwi.py deleted file mode 100644 index 8f2f9037..00000000 --- a/lerobot/common/robots/lekiwi/_old_robot_lekiwi.py +++ /dev/null @@ -1,692 +0,0 @@ -import base64 -import json -import os -import sys -from pathlib import Path - -import cv2 -import numpy as np -import torch -import zmq - -from lerobot.common.cameras.utils import make_cameras_from_configs -from lerobot.common.errors import DeviceNotConnectedError -from lerobot.common.motors.feetech.feetech import TorqueMode -from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration -from lerobot.common.motors.motors_bus import MotorsBus -from lerobot.common.motors.utils import make_motors_buses_from_configs -from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig -from lerobot.common.robots.utils import get_arm_id - -PYNPUT_AVAILABLE = True -try: - # Only import if there's a valid X server or if we're not on a Pi - if ("DISPLAY" not in os.environ) and ("linux" in sys.platform): - print("No DISPLAY set. Skipping pynput import.") - raise ImportError("pynput blocked intentionally due to no display.") - - from pynput import keyboard -except ImportError: - keyboard = None - PYNPUT_AVAILABLE = False -except Exception as e: - keyboard = None - PYNPUT_AVAILABLE = False - print(f"Could not import pynput: {e}") - - -class MobileManipulator: - """ - MobileManipulator is a class for connecting to and controlling a remote mobile manipulator robot. - The robot includes a three omniwheel mobile base and a remote follower arm. - The leader arm is connected locally (on the laptop) and its joint positions are recorded and then - forwarded to the remote follower arm (after applying a safety clamp). - In parallel, keyboard teleoperation is used to generate raw velocity commands for the wheels. - """ - - def __init__(self, config: LeKiwiRobotConfig): - """ - Expected keys in config: - - ip, port, video_port for the remote connection. - - calibration_dir, leader_arms, follower_arms, max_relative_target, etc. - """ - self.robot_type = config.type - self.config = config - self.remote_ip = config.ip - self.remote_port = config.port - self.remote_port_video = config.video_port - self.calibration_dir = Path(self.config.calibration_dir) - self.logs = {} - - self.teleop_keys = self.config.teleop_keys - - # For teleoperation, the leader arm (local) is used to record the desired arm pose. - self.leader_arms = make_motors_buses_from_configs(self.config.leader_arms) - - self.follower_arms = make_motors_buses_from_configs(self.config.follower_arms) - - self.cameras = make_cameras_from_configs(self.config.cameras) - - self.is_connected = False - - 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 - - # ZeroMQ context and sockets. - self.context = None - self.cmd_socket = None - self.video_socket = None - - # 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, - } - - if PYNPUT_AVAILABLE: - print("pynput is available - enabling local keyboard listener.") - self.listener = keyboard.Listener( - on_press=self.on_press, - on_release=self.on_release, - ) - self.listener.start() - else: - print("pynput not available - skipping local keyboard listener.") - self.listener = None - - 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] - - @property - def camera_features(self) -> dict: - cam_ft = {} - for cam_key, cam in self.cameras.items(): - key = f"observation.images.{cam_key}" - cam_ft[key] = { - "shape": (cam.height, cam.width, cam.channels), - "names": ["height", "width", "channels"], - "info": None, - } - return cam_ft - - @property - def motor_features(self) -> dict: - follower_arm_names = [ - "shoulder_pan", - "shoulder_lift", - "elbow_flex", - "wrist_flex", - "wrist_roll", - "gripper", - ] - observations = ["x_mm", "y_mm", "theta"] - combined_names = follower_arm_names + observations - return { - "action": { - "dtype": "float32", - "shape": (len(combined_names),), - "names": combined_names, - }, - "observation.state": { - "dtype": "float32", - "shape": (len(combined_names),), - "names": combined_names, - }, - } - - @property - def features(self): - return {**self.motor_features, **self.camera_features} - - @property - def has_camera(self): - return len(self.cameras) > 0 - - @property - def num_cameras(self): - return len(self.cameras) - - @property - def available_arms(self): - available = [] - for name in self.leader_arms: - available.append(get_arm_id(name, "leader")) - for name in self.follower_arms: - available.append(get_arm_id(name, "follower")) - return available - - def on_press(self, key): - try: - # Movement - if key.char == self.teleop_keys["forward"]: - self.pressed_keys["forward"] = True - elif key.char == self.teleop_keys["backward"]: - self.pressed_keys["backward"] = True - elif key.char == self.teleop_keys["left"]: - self.pressed_keys["left"] = True - elif key.char == self.teleop_keys["right"]: - self.pressed_keys["right"] = True - elif key.char == self.teleop_keys["rotate_left"]: - self.pressed_keys["rotate_left"] = True - elif key.char == self.teleop_keys["rotate_right"]: - self.pressed_keys["rotate_right"] = True - - # Quit teleoperation - elif key.char == self.teleop_keys["quit"]: - self.running = False - return False - - # 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}") - - except AttributeError: - # e.g., if key is special like Key.esc - if key == keyboard.Key.esc: - self.running = False - return False - - def on_release(self, key): - try: - if hasattr(key, "char"): - if key.char == self.teleop_keys["forward"]: - self.pressed_keys["forward"] = False - elif key.char == self.teleop_keys["backward"]: - self.pressed_keys["backward"] = False - elif key.char == self.teleop_keys["left"]: - self.pressed_keys["left"] = False - elif key.char == self.teleop_keys["right"]: - self.pressed_keys["right"] = False - elif key.char == self.teleop_keys["rotate_left"]: - self.pressed_keys["rotate_left"] = False - elif key.char == self.teleop_keys["rotate_right"]: - self.pressed_keys["rotate_right"] = False - except AttributeError: - pass - - def connect(self): - if not self.leader_arms: - raise ValueError("MobileManipulator has no leader arm to connect.") - for name in self.leader_arms: - print(f"Connecting {name} leader arm.") - self.calibrate_leader() - - # Set up ZeroMQ sockets to communicate with the remote mobile robot. - self.context = zmq.Context() - self.cmd_socket = self.context.socket(zmq.PUSH) - connection_string = f"tcp://{self.remote_ip}:{self.remote_port}" - self.cmd_socket.connect(connection_string) - self.cmd_socket.setsockopt(zmq.CONFLATE, 1) - self.video_socket = self.context.socket(zmq.PULL) - video_connection = f"tcp://{self.remote_ip}:{self.remote_port_video}" - self.video_socket.connect(video_connection) - self.video_socket.setsockopt(zmq.CONFLATE, 1) - print( - f"[INFO] Connected to remote robot at {connection_string} and video stream at {video_connection}." - ) - self.is_connected = True - - def load_or_run_calibration_(self, name, arm, arm_type): - arm_id = get_arm_id(name, arm_type) - arm_calib_path = self.calibration_dir / f"{arm_id}.json" - - if arm_calib_path.exists(): - with open(arm_calib_path) as f: - calibration = json.load(f) - else: - print(f"Missing calibration file '{arm_calib_path}'") - calibration = run_full_arm_calibration(arm, self.robot_type, name, arm_type) - print(f"Calibration is done! Saving calibration file '{arm_calib_path}'") - arm_calib_path.parent.mkdir(parents=True, exist_ok=True) - with open(arm_calib_path, "w") as f: - json.dump(calibration, f) - - return calibration - - def calibrate_leader(self): - for name, arm in self.leader_arms.items(): - # Connect the bus - arm.connect() - - # Disable torque on all motors - for motor_id in arm.motors: - arm.write("Torque_Enable", TorqueMode.DISABLED.value, motor_id) - - # Now run calibration - calibration = self.load_or_run_calibration_(name, arm, "leader") - arm.set_calibration(calibration) - - def calibrate_follower(self): - for name, bus in self.follower_arms.items(): - bus.connect() - - # Disable torque on all motors - for motor_id in bus.motors: - bus.write("Torque_Enable", 0, motor_id) - - # Then filter out wheels - arm_only_dict = {k: v for k, v in bus.motors.items() if not k.startswith("wheel_")} - if not arm_only_dict: - continue - - original_motors = bus.motors - bus.motors = arm_only_dict - - calibration = self.load_or_run_calibration_(name, bus, "follower") - bus.set_calibration(calibration) - - bus.motors = original_motors - - 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.video_socket, zmq.POLLIN) - socks = dict(poller.poll(15)) - if self.video_socket not in socks or socks[self.video_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.video_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) - - 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 - - def _process_present_speed(self, present_speed: dict) -> torch.Tensor: - state_tensor = torch.zeros(3, dtype=torch.int32) - if present_speed: - decoded = {key: MobileManipulator.raw_to_degps(value) for key, value in present_speed.items()} - if "1" in decoded: - state_tensor[0] = decoded["1"] - if "2" in decoded: - state_tensor[1] = decoded["2"] - if "3" in decoded: - state_tensor[2] = decoded["3"] - return state_tensor - - def teleop_step( - self, record_data: bool = False - ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]: - if not self.is_connected: - raise DeviceNotConnectedError("MobileManipulator is not connected. Run `connect()` first.") - - speed_setting = self.speed_levels[self.speed_index] - xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4 - theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90 - - # Prepare to assign the position of the leader to the follower - arm_positions = [] - for name in self.leader_arms: - pos = self.leader_arms[name].read("Present_Position") - pos_tensor = torch.from_numpy(pos).float() - # Instead of pos_tensor.item(), use tolist() to convert the entire tensor to a list - arm_positions.extend(pos_tensor.tolist()) - - # (The rest of your code for generating wheel commands remains unchanged) - x_cmd = 0.0 # m/s forward/backward - y_cmd = 0.0 # m/s lateral - theta_cmd = 0.0 # deg/s rotation - if self.pressed_keys["forward"]: - x_cmd += xy_speed - if self.pressed_keys["backward"]: - x_cmd -= xy_speed - if self.pressed_keys["left"]: - y_cmd += xy_speed - if self.pressed_keys["right"]: - y_cmd -= xy_speed - if self.pressed_keys["rotate_left"]: - theta_cmd += theta_speed - if self.pressed_keys["rotate_right"]: - theta_cmd -= theta_speed - - wheel_commands = self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) - - message = {"raw_velocity": wheel_commands, "arm_positions": arm_positions} - self.cmd_socket.send_string(json.dumps(message)) - - if not record_data: - return - - obs_dict = self.capture_observation() - - arm_state_tensor = torch.tensor(arm_positions, dtype=torch.float32) - - wheel_velocity_tuple = self.wheel_raw_to_body(wheel_commands) - wheel_velocity_mm = ( - wheel_velocity_tuple[0] * 1000.0, - wheel_velocity_tuple[1] * 1000.0, - wheel_velocity_tuple[2], - ) - wheel_tensor = torch.tensor(wheel_velocity_mm, dtype=torch.float32) - action_tensor = torch.cat([arm_state_tensor, wheel_tensor]) - action_dict = {"action": action_tensor} - - return obs_dict, action_dict - - def capture_observation(self) -> dict: - """ - 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("Not connected. Run `connect()` first.") - - 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 = {"observation.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"observation.images.{cam_name}"] = torch.from_numpy(frame) - - return obs_dict - - def send_action(self, action: torch.Tensor) -> torch.Tensor: - if not self.is_connected: - raise DeviceNotConnectedError("Not connected. Run `connect()` first.") - - # Ensure the action tensor has at least 9 elements: - # - First 6: arm positions. - # - Last 3: base commands. - if action.numel() < 9: - # Pad with zeros if there are not enough elements. - padded = torch.zeros(9, dtype=action.dtype) - padded[: action.numel()] = action - action = padded - - # Extract arm and base actions. - arm_actions = action[:6].flatten() - base_actions = action[6:].flatten() - - x_cmd_mm = base_actions[0].item() # mm/s - y_cmd_mm = base_actions[1].item() # mm/s - theta_cmd = base_actions[2].item() # deg/s - - # Convert mm/s to m/s for the kinematics calculations. - x_cmd = x_cmd_mm / 1000.0 # m/s - y_cmd = y_cmd_mm / 1000.0 # m/s - - # Compute wheel commands from body commands. - wheel_commands = self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) - - arm_positions_list = arm_actions.tolist() - - message = {"raw_velocity": wheel_commands, "arm_positions": arm_positions_list} - self.cmd_socket.send_string(json.dumps(message)) - - return action - - def print_logs(self): - pass - - def disconnect(self): - if not self.is_connected: - raise DeviceNotConnectedError("Not connected.") - if self.cmd_socket: - stop_cmd = { - "raw_velocity": {"left_wheel": 0, "back_wheel": 0, "right_wheel": 0}, - "arm_positions": {}, - } - self.cmd_socket.send_string(json.dumps(stop_cmd)) - self.cmd_socket.close() - if self.video_socket: - self.video_socket.close() - if self.context: - self.context.term() - if PYNPUT_AVAILABLE: - self.listener.stop() - self.is_connected = False - print("[INFO] Disconnected from remote robot.") - - def __del__(self): - if getattr(self, "is_connected", False): - self.disconnect() - if PYNPUT_AVAILABLE: - self.listener.stop() - - @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 - - @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 - - 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 = [MobileManipulator.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]} - - 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([MobileManipulator.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) - - -class LeKiwi: - def __init__(self, motor_bus): - """ - Initializes the LeKiwi with Feetech motors bus. - """ - self.motor_bus = motor_bus - self.motor_ids = ["left_wheel", "back_wheel", "right_wheel"] - - # Initialize motors in velocity mode. - self.motor_bus.write("Lock", 0) - self.motor_bus.write("Mode", [1, 1, 1], self.motor_ids) - self.motor_bus.write("Lock", 1) - print("Motors set to velocity mode.") - - def read_velocity(self): - """ - Reads the raw speeds for all wheels. Returns a dictionary with motor names: - """ - raw_speeds = self.motor_bus.read("Present_Speed", self.motor_ids) - return { - "left_wheel": int(raw_speeds[0]), - "back_wheel": int(raw_speeds[1]), - "right_wheel": int(raw_speeds[2]), - } - - def set_velocity(self, command_speeds): - """ - Sends raw velocity commands (16-bit encoded values) directly to the motor bus. - The order of speeds must correspond to self.motor_ids. - """ - self.motor_bus.write("Goal_Speed", command_speeds, self.motor_ids) - - def stop(self): - """Stops the robot by setting all motor speeds to zero.""" - self.motor_bus.write("Goal_Speed", [0, 0, 0], self.motor_ids) - print("Motors stopped.") diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py index 80c25d20..3b8fe5d4 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_lekiwi.py @@ -8,9 +8,6 @@ from lerobot.common.robots.config import RobotConfig @RobotConfig.register_subclass("lekiwi") @dataclass class LeKiwiRobotConfig(RobotConfig): - port_zmq_cmd: int = 5555 - port_zmq_observations: int = 5556 - id = "lekiwi" cameras: dict[str, CameraConfig] = field( diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index b096ec02..e73ce3cc 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi.py @@ -138,7 +138,7 @@ class DaemonLeKiwiRobot(Robot): # Consider moving these static functions out of the class # Copied from robot_lekiwi MobileManipulator class @staticmethod - def degps_to_raw(degps: float) -> int: + 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)) @@ -151,7 +151,7 @@ class DaemonLeKiwiRobot(Robot): # Copied from robot_lekiwi MobileManipulator class @staticmethod - def raw_to_degps(raw_speed: int) -> float: + def _raw_to_degps(raw_speed: int) -> float: steps_per_deg = 4096.0 / 360.0 magnitude = raw_speed & 0x7FFF degps = magnitude / steps_per_deg @@ -160,7 +160,7 @@ class DaemonLeKiwiRobot(Robot): return degps # Copied from robot_lekiwi MobileManipulator class - def body_to_wheel_raw( + def _body_to_wheel_raw( self, x_cmd: float, y_cmd: float, @@ -187,7 +187,7 @@ class DaemonLeKiwiRobot(Robot): 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 + 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. @@ -217,12 +217,12 @@ class DaemonLeKiwiRobot(Robot): 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] + 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( + def _wheel_raw_to_body( self, wheel_raw: np.array, wheel_radius: float = 0.05, base_radius: float = 0.125 ) -> tuple: """ @@ -241,7 +241,7 @@ class DaemonLeKiwiRobot(Robot): """ # Convert each raw command back to an angular speed in deg/s. - wheel_degps = np.array([DaemonLeKiwiRobot.raw_to_degps(int(r)) for r in wheel_raw]) + 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. @@ -259,7 +259,7 @@ class DaemonLeKiwiRobot(Robot): 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): + def _get_data(self): # Copied from robot_lekiwi.py """Polls the video socket for up to 15 ms. If data arrives, decode only the *latest* message, returning frames, speed, and arm state. If @@ -348,8 +348,8 @@ class DaemonLeKiwiRobot(Robot): obs_dict = {} - frames, present_speed, remote_arm_state_tensor = self.get_data() - body_state = self.wheel_raw_to_body(present_speed) + 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) @@ -366,7 +366,7 @@ class DaemonLeKiwiRobot(Robot): return obs_dict - def from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray): + def _from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray): # Speed control if self.teleop_keys["speed_up"] in pressed_keys: self.speed_index = min(self.speed_index + 1, 2) @@ -393,7 +393,7 @@ class DaemonLeKiwiRobot(Robot): if self.teleop_keys["rotate_right"] in pressed_keys: theta_cmd -= theta_speed - return self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) + return self._body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) # TODO(Steven): This assumes this call is always called from a keyboard teleop command # TODO(Steven): Doing this mapping in here adds latecy between send_action and movement from the user perspective. @@ -439,7 +439,7 @@ class DaemonLeKiwiRobot(Robot): goal_pos[:6] = action[:6] if action.size > 6: # TODO(Steven): Assumes size and order is respected - wheel_actions = [v for _, v in self.from_keyboard_to_wheel_action(action[6:]).items()] + wheel_actions = [v for _, v in self._from_keyboard_to_wheel_action(action[6:]).items()] goal_pos[6:] = wheel_actions self.zmq_cmd_socket.send_string(json.dumps(goal_pos.tolist())) # action is in motor space diff --git a/lerobot/common/robots/lekiwi/lekiwi_app.py b/lerobot/common/robots/lekiwi/lekiwi_app.py index 6df462fe..cec214ac 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/lekiwi_app.py @@ -1,8 +1,35 @@ +import json import logging +import time +import numpy as np +import zmq + +from lerobot.common.constants import OBS_STATE from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig from lerobot.common.robots.lekiwi.lekiwi_robot import LeKiwiRobot +# Network Configuration +PORT_ZMQ_CMD: int = 5555 +PORT_ZMQ_OBSERVATIONS: int = 5556 + + +class RemoteAgent: + def __init__(self): + self.zmq_context = zmq.Context() + self.zmq_cmd_socket = self.zmq_context.socket(zmq.PULL) + self.zmq_cmd_socket.setsockopt(zmq.CONFLATE, 1) + self.zmq_cmd_socket.bind(f"tcp://*:{PORT_ZMQ_CMD}") + + self.zmq_observation_socket = self.zmq_context.socket(zmq.PUSH) + self.zmq_observation_socket.setsockopt(zmq.CONFLATE, 1) + self.zmq_observation_socket.bind(f"tcp://*:{PORT_ZMQ_OBSERVATIONS}") + + def disconnect(self): + self.zmq_observation_socket.close() + self.zmq_cmd_socket.close() + self.zmq_context.term() + def main(): logging.info("Configuring LeKiwiRobot") @@ -12,9 +39,51 @@ def main(): logging.info("Connecting LeKiwiRobot") robot.connect() - # Remotely teleoperated - logging.info("Starting LeKiwiRobot teleoperation") - robot.run() + logging.info("Starting RemoteAgent") + remote_agent = RemoteAgent() + + last_cmd_time = time.time() + logging.info("Waiting for commands...") + try: + # Business logic + start = time.perf_counter() + duration = 0 + while duration < 100: + loop_start_time = time.time() + try: + msg = remote_agent.zmq_cmd_socket.recv_string(zmq.NOBLOCK) + data = np.array(json.loads(msg)) + _action_sent = robot.send_action(data) + last_cmd_time = time.time() + except zmq.Again: + logging.warning("No command available") + 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: + robot.stop_base() + + last_observation = robot.get_observation() + last_observation[OBS_STATE] = last_observation[OBS_STATE].tolist() + remote_agent.zmq_observation_socket.send_string(json.dumps(last_observation)) + + # 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: + robot.disconnect() + remote_agent.disconnect() logging.info("Finished LeKiwiRobot cleanly") diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index 1eb279fe..09234845 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -17,12 +17,10 @@ import base64 import json import logging -import threading import time import cv2 import numpy as np -import zmq from lerobot.common.cameras.utils import make_cameras_from_configs from lerobot.common.constants import OBS_IMAGES, OBS_STATE @@ -55,9 +53,6 @@ class LeKiwiRobot(Robot): self.config = config 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 @@ -85,13 +80,6 @@ class LeKiwiRobot(Robot): # TODO(Steven): Consider removing cameras from configs self.cameras = make_cameras_from_configs(config.cameras) - self.observation_lock = threading.Lock() - self.last_observation = None - - self.zmq_context = None - self.zmq_cmd_socket = None - self.zmq_observation_socket = None - self.is_connected = False self.logs = {} @@ -118,18 +106,6 @@ class LeKiwiRobot(Robot): } return cam_ft - 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://*:{self.port_zmq_cmd}") - - observation_socket = context.socket(zmq.PUSH) - observation_socket.setsockopt(zmq.CONFLATE, 1) - observation_socket.bind(f"tcp://*:{self.port_zmq_observations}") - - return context, cmd_socket, observation_socket - def setup_actuators(self): # Set-up arm actuators (position mode) # We assume that at connection time, arm is in a rest position, @@ -177,9 +153,6 @@ class LeKiwiRobot(Robot): for cam in self.cameras.values(): cam.connect() - logging.info("Connecting ZMQ sockets.") - self.zmq_context, self.zmq_cmd_socket, self.zmq_observation_socket = self.setup_zmq_sockets() - self.is_connected = True def calibrate(self) -> None: @@ -274,80 +247,13 @@ class LeKiwiRobot(Robot): return goal_pos - 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): + def stop_base(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") - def run(self): - # Copied logic from run_lekiwi in lekiwi_remote.py - if not self.is_connected: - raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.") - - stop_event = threading.Event() - observation_thread = threading.Thread( - target=self.update_last_observation, args=[stop_event], daemon=True - ) - observation_thread.start() - - last_cmd_time = time.time() - logging.info("LeKiwi robot server started. Waiting for commands...") - - try: - 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)) - _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: - # 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 - 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 - - # 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: - stop_event.set() - observation_thread.join() - self.disconnect() - def print_logs(self): # TODO(Steven): Refactor logger pass @@ -358,13 +264,10 @@ class LeKiwiRobot(Robot): "LeKiwi is not connected. You need to run `robot.connect()` before disconnecting." ) - self.stop() + self.stop_base() self.actuators_bus.disconnect() for cam in self.cameras.values(): cam.disconnect() - self.zmq_observation_socket.close() - self.zmq_cmd_socket.close() - self.zmq_context.term() self.is_connected = False def __del__(self): From 46cf20db825067cedec4d3e2405de1d8ad01d7bd Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 19 Mar 2025 10:54:58 +0100 Subject: [PATCH 12/19] feat(lekiwi): Make dataset recording work --- lerobot/common/robots/config.py | 7 ++ .../lekiwi/configuration_daemon_lekiwi.py | 4 - lerobot/common/robots/lekiwi/daemon_lekiwi.py | 70 ++++++++---- .../common/robots/lekiwi/daemon_lekiwi_app.py | 105 +++++++++++++++--- lerobot/common/robots/robot.py | 9 +- 5 files changed, 144 insertions(+), 51 deletions(-) diff --git a/lerobot/common/robots/config.py b/lerobot/common/robots/config.py index 83a13ca9..1ab0b004 100644 --- a/lerobot/common/robots/config.py +++ b/lerobot/common/robots/config.py @@ -1,16 +1,23 @@ import abc +import enum from dataclasses import dataclass from pathlib import Path import draccus +class RobotMode(enum.Enum): + TELEOP = 0 + AUTO = 1 + + @dataclass(kw_only=True) class RobotConfig(draccus.ChoiceRegistry, abc.ABC): # Allows to distinguish between different robots of the same type id: str | None = None # Directory to store calibration file calibration_dir: Path | None = None + robot_mode: RobotMode | None = None @property def type(self) -> str: diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index b526bce9..42d77cd3 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py @@ -11,10 +11,6 @@ class DaemonLeKiwiRobotConfig(RobotConfig): port_zmq_cmd: int = 5555 port_zmq_observations: int = 5556 - id = "daemonlekiwi" - - calibration_dir: str = ".cache/calibration/lekiwi" - teleop_keys: dict[str, str] = field( default_factory=lambda: { # Movement diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index e73ce3cc..e89a41e6 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi.py @@ -25,8 +25,9 @@ import zmq from lerobot.common.constants import OBS_IMAGES, OBS_STATE from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError, InvalidActionError +from lerobot.common.robots.config import RobotMode -from ..robot import Robot, RobotMode +from ..robot import Robot from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig @@ -50,6 +51,7 @@ class DaemonLeKiwiRobot(Robot): self.config = config self.id = config.id self.robot_type = config.type + self.robot_mode = config.robot_mode self.remote_ip = config.remote_ip self.port_zmq_cmd = config.port_zmq_cmd @@ -63,7 +65,9 @@ class DaemonLeKiwiRobot(Robot): self.last_frames = {} self.last_present_speed = [0, 0, 0] - self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32) + + # TODO(Steven): Consider 32 instead + self.last_remote_arm_state = torch.zeros(6, dtype=torch.float64) # Define three speed levels and a current index self.speed_levels = [ @@ -81,12 +85,23 @@ class DaemonLeKiwiRobot(Robot): # TODO(Steven): Get this from the data fetched? # TODO(Steven): Motor names are unknown for the Daemon # Or assume its size/metadata? - # return { - # "dtype": "float32", - # "shape": (len(self.actuators),), - # "names": {"motors": list(self.actuators.motors)}, - # } - pass + return { + "dtype": "float64", + "shape": (9,), + "names": { + "motors": [ + "arm_shoulder_pan", + "arm_shoulder_lift", + "arm_elbow_flex", + "arm_wrist_flex", + "arm_wrist_roll", + "arm_gripper", + "base_left_wheel", + "base_right_wheel", + "base_back_wheel", + ] + }, + } @property def action_feature(self) -> dict: @@ -97,15 +112,19 @@ class DaemonLeKiwiRobot(Robot): # TODO(Steven): Get this from the data fetched? # TODO(Steven): Motor names are unknown for the Daemon # Or assume its size/metadata? - # 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 + cam_ft = { + "front": { + "shape": (480, 640, 3), + "names": ["height", "width", "channels"], + "info": None, + }, + "wrist": { + "shape": (480, 640, 3), + "names": ["height", "width", "channels"], + "info": None, + }, + } + return cam_ft def connect(self) -> None: """Establishes ZMQ sockets with the remote mobile robot""" @@ -259,6 +278,7 @@ class DaemonLeKiwiRobot(Robot): 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 + # TODO(Steven): All this function needs to be refactored def _get_data(self): # Copied from robot_lekiwi.py """Polls the video socket for up to 15 ms. If data arrives, decode only @@ -269,7 +289,7 @@ class DaemonLeKiwiRobot(Robot): present_speed = [] # TODO(Steven): Size is being assumed, is this safe? - remote_arm_state_tensor = torch.empty(6, dtype=torch.float32) + remote_arm_state_tensor = torch.empty(6, dtype=torch.float64) # Poll up to 15 ms poller = zmq.Poller() @@ -317,7 +337,7 @@ 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[OBS_STATE][:6], dtype=torch.float32) + remote_arm_state_tensor = torch.tensor(state_observation[OBS_STATE][:6], dtype=torch.float64) self.last_remote_arm_state = remote_arm_state_tensor present_speed = state_observation[OBS_STATE][6:] @@ -351,7 +371,7 @@ class DaemonLeKiwiRobot(Robot): 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) + wheel_state_tensor = torch.tensor(body_state_mm, dtype=torch.float64) combined_state_tensor = torch.cat((remote_arm_state_tensor, wheel_state_tensor), dim=0) obs_dict = {OBS_STATE: combined_state_tensor} @@ -361,9 +381,15 @@ class DaemonLeKiwiRobot(Robot): if frame is None: # TODO(Steven): Daemon doesn't know camera dimensions logging.warning("Frame is None") - # frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8) + frame = np.zeros((480, 640, 3), dtype=np.uint8) obs_dict[cam_name] = torch.from_numpy(frame) + # TODO(Steven): Refactor this ugly thing + if OBS_IMAGES + ".wrist" not in obs_dict: + obs_dict[OBS_IMAGES + ".wrist"] = np.zeros(shape=(480, 640, 3)) + if OBS_IMAGES + ".front" not in obs_dict: + obs_dict[OBS_IMAGES + ".front"] = np.zeros(shape=(640, 480, 3)) + return obs_dict def _from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray): @@ -425,7 +451,6 @@ class DaemonLeKiwiRobot(Robot): ) goal_pos: np.array = np.zeros(9) - if self.robot_mode is RobotMode.AUTO: # TODO(Steven): Not yet implemented. The policy outputs might need a different conversion raise Exception @@ -441,7 +466,6 @@ class DaemonLeKiwiRobot(Robot): # TODO(Steven): Assumes size and order is respected wheel_actions = [v for _, v in self._from_keyboard_to_wheel_action(action[6:]).items()] goal_pos[6:] = wheel_actions - self.zmq_cmd_socket.send_string(json.dumps(goal_pos.tolist())) # action is in motor space return goal_pos diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index 3c45377d..241f5b66 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py @@ -1,13 +1,69 @@ import logging -import time import numpy as np +from lerobot.common.datasets.lerobot_dataset import LeRobotDataset +from lerobot.common.robots.config import RobotMode from lerobot.common.robots.lekiwi.configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig -from lerobot.common.robots.lekiwi.daemon_lekiwi import DaemonLeKiwiRobot, RobotMode +from lerobot.common.robots.lekiwi.daemon_lekiwi import DaemonLeKiwiRobot from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig +DUMMY_FEATURES = { + "observation.state": { + "dtype": "float64", + "shape": (9,), + "names": { + "motors": [ + "arm_shoulder_pan", + "arm_shoulder_lift", + "arm_elbow_flex", + "arm_wrist_flex", + "arm_wrist_roll", + "arm_gripper", + "base_left_wheel", + "base_right_wheel", + "base_back_wheel", + ] + }, + }, + "action": { + "dtype": "float64", + "shape": (9,), + "names": { + "motors": [ + "arm_shoulder_pan", + "arm_shoulder_lift", + "arm_elbow_flex", + "arm_wrist_flex", + "arm_wrist_roll", + "arm_gripper", + "base_left_wheel", + "base_right_wheel", + "base_back_wheel", + ] + }, + }, + "observation.images.front": { + "dtype": "image", + "shape": (640, 480, 3), + "names": [ + "width", + "height", + "channels", + ], + }, + "observation.images.wrist": { + "dtype": "image", + "shape": (480, 640, 3), + "names": [ + "width", + "height", + "channels", + ], + }, +} + def main(): logging.info("Configuring Teleop Devices") @@ -17,41 +73,58 @@ def main(): keyboard_config = KeyboardTeleopConfig() keyboard = KeyboardTeleop(keyboard_config) + logging.info("Configuring LeKiwiRobot Daemon") + robot_config = DaemonLeKiwiRobotConfig( + id="daemonlekiwi", calibration_dir=".cache/calibration/lekiwi", robot_mode=RobotMode.TELEOP + ) + robot = DaemonLeKiwiRobot(robot_config) + + logging.info("Creating LeRobot Dataset") + + # TODO(Steven): Check this creation + dataset = LeRobotDataset.create( + repo_id="user/lekiwi", + fps=10, + features=DUMMY_FEATURES, + ) + 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() - robot.robot_mode = RobotMode.TELEOP logging.info("Starting LeKiwiRobot teleoperation") - start = time.perf_counter() - duration = 0 - while duration < 100: + i = 0 + while i < 1000: 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 - _action_sent = robot.send_action(action) - _observation = robot.get_observation() - - # dataset.save(action_sent, obs) # TODO(Steven): Deal with policy action space # robot.set_mode(RobotMode.AUTO) # policy_action = policy.get_action() # This might be in body frame, key space or smt else # robot.send_action(policy_action) - duration = time.perf_counter() - start + + action_sent = robot.send_action(action) + observation = robot.get_observation() + + frame = {"action": action_sent} + frame.update(observation) + frame.update({"task": "Dummy Task Dataset"}) + + logging.info("Saved a frame into the dataset") + dataset.add_frame(frame) + i += 1 + + dataset.save_episode() + # dataset.push_to_hub() logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon") robot.disconnect() leader_arm.disconnect() keyboard.disconnect() - logging.info("Finished LeKiwiRobot cleanly") diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py index 57ff41a4..d63026fc 100644 --- a/lerobot/common/robots/robot.py +++ b/lerobot/common/robots/robot.py @@ -1,7 +1,5 @@ import abc from pathlib import Path -import enum -from pathlib import Path from typing import Any import draccus @@ -12,11 +10,6 @@ from lerobot.common.motors import MotorCalibration from .config import RobotConfig -class RobotMode(enum.Enum): - TELEOP = 0 - AUTO = 1 - - # TODO(aliberts): action/obs typing such as Generic[ObsType, ActType] similar to gym.Env ? # https://github.com/Farama-Foundation/Gymnasium/blob/3287c869f9a48d99454306b0d4b4ec537f0f35e3/gymnasium/core.py#L23 class Robot(abc.ABC): @@ -28,8 +21,8 @@ class Robot(abc.ABC): def __init__(self, config: RobotConfig): self.robot_type = self.name - self.robot_mode: RobotMode | None = None self.id = config.id + self.robot_mode = config.robot_mode self.calibration_dir = ( Path(config.calibration_dir) if config.calibration_dir From e31e1da39b1ec9cb363d44cd1b8941f242a1d53d Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 20 Mar 2025 09:48:55 +0100 Subject: [PATCH 13/19] chore(doc): update todos + license --- lerobot/common/robots/lekiwi/README.md | 2 ++ .../robots/lekiwi/configuration_daemon_lekiwi.py | 14 ++++++++++++++ .../common/robots/lekiwi/configuration_lekiwi.py | 14 ++++++++++++++ lerobot/common/robots/lekiwi/daemon_lekiwi.py | 11 +++++------ .../common/robots/lekiwi/daemon_lekiwi_app.py | 14 ++++++++++++++ lerobot/common/robots/lekiwi/lekiwi_app.py | 16 ++++++++++++++++ lerobot/common/robots/lekiwi/lekiwi_robot.py | 5 ++++- 7 files changed, 69 insertions(+), 7 deletions(-) diff --git a/lerobot/common/robots/lekiwi/README.md b/lerobot/common/robots/lekiwi/README.md index 215419e1..0077d2d8 100644 --- a/lerobot/common/robots/lekiwi/README.md +++ b/lerobot/common/robots/lekiwi/README.md @@ -1,3 +1,5 @@ +# TODO(Steven): Update README + # Using the [LeKiwi](https://github.com/SIGRobotics-UIUC/LeKiwi) Robot with LeRobot ## Table of Contents diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index 42d77cd3..c25d8b35 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py @@ -1,3 +1,17 @@ +# 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. + from dataclasses import dataclass, field from lerobot.common.robots.config import RobotConfig diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py index 3b8fe5d4..a112ae8b 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_lekiwi.py @@ -1,3 +1,17 @@ +# 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. + from dataclasses import dataclass, field from lerobot.common.cameras.configs import CameraConfig diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index e89a41e6..5962018e 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python - # Copyright 2024 The HuggingFace Inc. team. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -66,7 +64,7 @@ class DaemonLeKiwiRobot(Robot): self.last_frames = {} self.last_present_speed = [0, 0, 0] - # TODO(Steven): Consider 32 instead + # TODO(Steven): Move everything to 32 instead self.last_remote_arm_state = torch.zeros(6, dtype=torch.float64) # Define three speed levels and a current index @@ -112,6 +110,7 @@ class DaemonLeKiwiRobot(Robot): # TODO(Steven): Get this from the data fetched? # TODO(Steven): Motor names are unknown for the Daemon # Or assume its size/metadata? + # TODO(Steven): Check consistency of image sizes cam_ft = { "front": { "shape": (480, 640, 3), @@ -384,7 +383,7 @@ class DaemonLeKiwiRobot(Robot): frame = np.zeros((480, 640, 3), dtype=np.uint8) obs_dict[cam_name] = torch.from_numpy(frame) - # TODO(Steven): Refactor this ugly thing + # TODO(Steven): Refactor this ugly thing (needed for when there are not comms at init) if OBS_IMAGES + ".wrist" not in obs_dict: obs_dict[OBS_IMAGES + ".wrist"] = np.zeros(shape=(480, 640, 3)) if OBS_IMAGES + ".front" not in obs_dict: @@ -455,7 +454,7 @@ class DaemonLeKiwiRobot(Robot): # TODO(Steven): Not yet implemented. The policy outputs might need a different conversion raise Exception - # TODO(Steven): This assumes teleop mode is always used with keyboard + # TODO(Steven): This assumes teleop mode is always used with keyboard. Tomorrow we could teleop with another device ... ? if self.robot_mode is RobotMode.TELEOP: if action.size < 6: logging.error("Action should include at least the 6 states of the leader arm") @@ -481,7 +480,7 @@ class DaemonLeKiwiRobot(Robot): 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 + # TODO(Steven): Consider sending a stop to the remote mobile robot. Although this would need a moore complex comms schema self.zmq_observation_socket.close() self.zmq_cmd_socket.close() self.zmq_context.term() diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index 241f5b66..47eb2e10 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py @@ -1,3 +1,17 @@ +# 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 logging import numpy as np diff --git a/lerobot/common/robots/lekiwi/lekiwi_app.py b/lerobot/common/robots/lekiwi/lekiwi_app.py index cec214ac..bc720494 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/lekiwi_app.py @@ -1,3 +1,19 @@ +#!/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 diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index 09234845..211cefc7 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -37,6 +37,9 @@ from ..utils import ensure_safe_goal_position from .configuration_lekiwi import LeKiwiRobotConfig +# TODO(Steven): Everything in here is pretty much single-threaded and synchronous. The assumption is that we can run the loop at a +# high enough frequency to not need to worry about threading. This is a good assumption for now, but we should consider +# making this more robust in the future. class LeKiwiRobot(Robot): """ The robot includes a three omniwheel mobile base and a remote follower arm. @@ -111,7 +114,7 @@ class LeKiwiRobot(Robot): # We assume that at connection time, arm is in a rest position, # and torque can be safely disabled to run calibration. self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value, self.arm_actuators) - self.calibrate() # TODO(Steven): This should be only for the arm + self.calibrate() # TODO(Steven): This should be only for the arm, but apparently it doesn't harm the base # Mode=0 for Position Control self.actuators_bus.write("Mode", 0, self.arm_actuators) From d206b94d7440a985df30bf329d5837a617d60b6d Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 21 Mar 2025 10:46:51 +0100 Subject: [PATCH 14/19] refactor(kiwi): update to latest motor API --- lerobot/common/robots/lekiwi/lekiwi_robot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index 211cefc7..139478be 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -25,9 +25,9 @@ import numpy as np 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 lerobot.common.motors import TorqueMode from lerobot.common.motors.feetech import ( FeetechMotorsBus, - TorqueMode, apply_feetech_offsets_from_calibration, run_full_arm_calibration, ) From 2cfb897d5f1f5cbb2671ae2268d8b0214326f8ef Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Fri, 4 Apr 2025 11:26:11 +0200 Subject: [PATCH 15/19] Update Lekiwi with new MotorsBus --- .../robots/lekiwi/configuration_lekiwi.py | 31 +-- lerobot/common/robots/lekiwi/lekiwi_robot.py | 256 ++++++++---------- 2 files changed, 123 insertions(+), 164 deletions(-) diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py index a112ae8b..df163109 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_lekiwi.py @@ -22,7 +22,14 @@ from lerobot.common.robots.config import RobotConfig @RobotConfig.register_subclass("lekiwi") @dataclass class LeKiwiRobotConfig(RobotConfig): - id = "lekiwi" + port = "/dev/ttyACM0" # port to connect to the bus + + disable_torque_on_disconnect: bool = True + + # `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 cameras: dict[str, CameraConfig] = field( default_factory=lambda: { @@ -34,25 +41,3 @@ class LeKiwiRobotConfig(RobotConfig): ), } ) - - calibration_dir: str = ".cache/calibration/lekiwi" - - port_motor_bus = "/dev/ttyACM0" - - # TODO(Steven): consider split this into arm and base - # TODO(Steven): Consider also removing this entirely as we can say that - # LeKiwiRobot will always have (and needs) such - 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") - - # `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 diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index 139478be..512f3d21 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -15,31 +15,28 @@ # limitations under the License. import base64 -import json import logging import time +from typing import Any import cv2 -import numpy as np 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 lerobot.common.motors import TorqueMode +from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode from lerobot.common.motors.feetech import ( FeetechMotorsBus, - apply_feetech_offsets_from_calibration, - run_full_arm_calibration, + OperatingMode, ) from ..robot import Robot from ..utils import ensure_safe_goal_position from .configuration_lekiwi import LeKiwiRobotConfig +logger = logging.getLogger(__name__) + -# TODO(Steven): Everything in here is pretty much single-threaded and synchronous. The assumption is that we can run the loop at a -# high enough frequency to not need to worry about threading. This is a good assumption for now, but we should consider -# making this more robust in the future. class LeKiwiRobot(Robot): """ The robot includes a three omniwheel mobile base and a remote follower arm. @@ -55,43 +52,33 @@ class LeKiwiRobot(Robot): super().__init__(config) self.config = config self.id = config.id - - # 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(Steven): Order and dimension are generally assumed to be 6 first for arm, 3 last for base - self.actuators_bus = FeetechMotorsBus( - port=self.config.port_motor_bus, + self.bus = FeetechMotorsBus( + port=self.config.port, motors={ - "arm_shoulder_pan": config.shoulder_pan, - "arm_shoulder_lift": config.shoulder_lift, - "arm_elbow_flex": config.elbow_flex, - "arm_wrist_flex": config.wrist_flex, - "arm_wrist_roll": config.wrist_roll, - "arm_gripper": config.gripper, - "base_left_wheel": config.left_wheel, - "base_right_wheel": config.right_wheel, - "base_back_wheel": config.back_wheel, + # arm + "arm_shoulder_pan": Motor(1, "sts3215", MotorNormMode.RANGE_M100_100), + "arm_shoulder_lift": Motor(2, "sts3215", MotorNormMode.RANGE_M100_100), + "arm_elbow_flex": Motor(3, "sts3215", MotorNormMode.RANGE_M100_100), + "arm_wrist_flex": Motor(4, "sts3215", MotorNormMode.RANGE_M100_100), + "arm_wrist_roll": Motor(5, "sts3215", MotorNormMode.RANGE_M100_100), + "arm_gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100), + # base + "base_left_wheel": Motor(7, "sts3215", MotorNormMode.RANGE_M100_100), + "base_back_wheel": Motor(8, "sts3215", MotorNormMode.RANGE_M100_100), + "base_right_wheel": Motor(9, "sts3215", MotorNormMode.RANGE_M100_100), }, + calibration=self.calibration, ) - - self.arm_actuators = [m for m in self.actuators_bus.motor_names if m.startswith("arm")] - self.base_actuators = [m for m in self.actuators_bus.motor_names if m.startswith("base")] - - self.max_relative_target = config.max_relative_target - - # TODO(Steven): Consider removing cameras from configs + self.arm_motors = [m for m in self.bus.names if m.startswith("arm")] + self.base_motors = [m for m in self.bus.names if m.startswith("base")] self.cameras = make_cameras_from_configs(config.cameras) - self.is_connected = False - self.logs = {} - @property def state_feature(self) -> dict: return { "dtype": "float32", - "shape": (len(self.actuators_bus),), - "names": {"motors": list(self.actuators_bus.motors)}, + "shape": (len(self.bus),), + "names": {"motors": list(self.bus.motors)}, } @property @@ -109,111 +96,114 @@ class LeKiwiRobot(Robot): } return cam_ft - def setup_actuators(self): - # Set-up arm actuators (position mode) - # We assume that at connection time, arm is in a rest position, - # and torque can be safely disabled to run calibration. - self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value, self.arm_actuators) - self.calibrate() # TODO(Steven): This should be only for the arm, but apparently it doesn't harm the base - - # Mode=0 for Position Control - self.actuators_bus.write("Mode", 0, self.arm_actuators) - # Set P_Coefficient to lower value to avoid shakiness (Default is 32) - self.actuators_bus.write("P_Coefficient", 16, self.arm_actuators) - # Set I_Coefficient and D_Coefficient to default value 0 and 32 - self.actuators_bus.write("I_Coefficient", 0, self.arm_actuators) - self.actuators_bus.write("D_Coefficient", 32, self.arm_actuators) - # Close the write lock so that Maximum_Acceleration gets written to EPROM address, - # which is mandatory for Maximum_Acceleration to take effect after rebooting. - self.actuators_bus.write("Lock", 0, self.arm_actuators) - # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of - # the motors. Note: this configuration is not in the official STS3215 Memory Table - self.actuators_bus.write("Maximum_Acceleration", 254, self.arm_actuators) - self.actuators_bus.write("Acceleration", 254, self.arm_actuators) - - logging.info("Activating torque.") - self.actuators_bus.write("Torque_Enable", TorqueMode.ENABLED.value, self.arm_actuators) - - # Check arm can be read - self.actuators_bus.read("Present_Position", self.arm_actuators) - - # Set-up base actuators (velocity mode) - self.actuators_bus.write("Lock", 0, self.base_actuators) - self.actuators_bus.write("Mode", [1, 1, 1], self.base_actuators) - self.actuators_bus.write("Lock", 1, self.base_actuators) + @property + def is_connected(self) -> bool: + # TODO(aliberts): add cam.is_connected for cam in self.cameras + return self.bus.is_connected def connect(self) -> None: if self.is_connected: - raise DeviceAlreadyConnectedError( - "LeKiwi Robot is already connected. Do not run `robot.connect()` twice." - ) + raise DeviceAlreadyConnectedError(f"{self} already connected") - logging.info("Connecting actuators.") - self.actuators_bus.connect() - self.setup_actuators() + self.bus.connect() + if not self.is_calibrated: + self.calibrate() - logging.info("Connecting cameras.") for cam in self.cameras.values(): cam.connect() - self.is_connected = True + self.configure() + logger.info(f"{self} connected.") + + @property + def is_calibrated(self) -> bool: + return self.bus.is_calibrated def calibrate(self) -> None: - # Copied from S100 robot - """After calibration all motors function in human interpretable ranges. - Rotations are expressed in degrees in nominal range of [-180, 180], - and linear motions (like gripper of Aloha) in nominal range of [0, 100]. - """ - actuators_calib_path = self.calibration_dir / f"{self.config.id}.json" + logger.info(f"\nRunning calibration of {self}") + self.bus.disable_torque(self.arm_motors) + for name in self.arm_motors: + self.bus.write("Operating_Mode", name, OperatingMode.POSITION.value) - if actuators_calib_path.exists(): - with open(actuators_calib_path, encoding="utf-8") as f: - calibration = json.load(f) - else: - logging.info("Missing calibration file '%s'", actuators_calib_path) - calibration = run_full_arm_calibration(self.actuators_bus, self.robot_type, self.name, "follower") + input("Move robot to the middle of its range of motion and press ENTER....") + homing_offsets = self.bus.set_half_turn_homings(self.arm_motors) - logging.info("Calibration is done! Saving calibration file '%s'", actuators_calib_path) - actuators_calib_path.parent.mkdir(parents=True, exist_ok=True) - with open(actuators_calib_path, "w", encoding="utf-8") as f: - json.dump(calibration, f) + full_turn_motor = "arm_wrist_roll" + unknown_range_motors = [name for name in self.arm_motors if name != full_turn_motor] + logger.info( + f"Move all arm joints except '{full_turn_motor}' sequentially through their " + "entire ranges of motion.\nRecording positions. Press ENTER to stop..." + ) + range_mins, range_maxes = self.bus.record_ranges_of_motion(unknown_range_motors) + for name in [*self.base_motors, full_turn_motor]: + range_mins[name] = 0 + range_maxes[name] = 4095 - self.actuators_bus.set_calibration(calibration) - apply_feetech_offsets_from_calibration(self.actuators_bus, calibration) + self.calibration = {} + for name, motor in self.bus.motors.items(): + self.calibration[name] = MotorCalibration( + id=motor.id, + drive_mode=0, + homing_offset=homing_offsets[name], + range_min=range_mins[name], + range_max=range_maxes[name], + ) - # 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.""" + self.bus.write_calibration(self.calibration) + self._save_calibration() + print("Calibration saved to", self.calibration_fpath) + + def configure(self): + # Set-up arm actuators (position mode) + # We assume that at connection time, arm is in a rest position, + # and torque can be safely disabled to run calibration. + self.bus.disable_torque(self.arm_motors) + for name in self.arm_motors: + self.bus.write("Mode", name, OperatingMode.POSITION.value) + # Set P_Coefficient to lower value to avoid shakiness (Default is 32) + self.bus.write("P_Coefficient", name, 16) + # Set I_Coefficient and D_Coefficient to default value 0 and 32 + self.bus.write("I_Coefficient", name, 0) + self.bus.write("D_Coefficient", name, 32) + # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of + # the motors. Note: this configuration is not in the official STS3215 Memory Table + self.bus.write("Maximum_Acceleration", name, 254) + self.bus.write("Acceleration", name, 254) + + for name in self.base_motors: + self.bus.write("Mode", name, OperatingMode.VELOCITY.value) + + self.bus.enable_torque() + + def get_observation(self) -> dict[str, Any]: if not self.is_connected: - raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.") + raise DeviceNotConnectedError(f"{self} is not connected.") obs_dict = {} # Read actuators position for arm and vel for base - before_read_t = time.perf_counter() - obs_dict[OBS_STATE] = np.concatenate( - ( - self.actuators_bus.read("Present_Position", self.arm_actuators), - self.actuators_bus.read("Present_Speed", self.base_actuators), - ) - ) - self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t + start = time.perf_counter() + arm_pos = self.bus.sync_read("Present_Position", self.arm_motors) + base_vel = self.bus.sync_read("Present_Speed", self.base_motors) + obs_dict[OBS_STATE] = {**arm_pos, **base_vel} + dt_ms = (time.perf_counter() - start) * 1e3 + logger.debug(f"{self} read state: {dt_ms:.1f}ms") # Capture images from cameras for cam_key, cam in self.cameras.items(): - before_camread_t = time.perf_counter() + start = time.perf_counter() 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 + dt_ms = (time.perf_counter() - start) * 1e3 + logger.debug(f"{self} read {cam_key}: {dt_ms:.1f}ms") return obs_dict - def send_action(self, action: np.ndarray) -> np.ndarray: + def send_action(self, action: dict[str, Any]) -> dict[str, Any]: # Copied from S100 robot """Command lekiwi to move to a target joint configuration. @@ -221,9 +211,6 @@ class LeKiwiRobot(Robot): `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. @@ -231,48 +218,35 @@ class LeKiwiRobot(Robot): np.ndarray: the action sent to the motors, potentially clipped. """ if not self.is_connected: - raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.") + raise DeviceNotConnectedError(f"{self} is not connected.") + + arm_goal_pos = {k: v for k, v in action.items() if k in self.arm_motors} + base_goal_vel = {k: v for k, v in action.items() if k in self.base_motors} - # Input action is in motor space - 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_bus.read("Present_Position", self.arm_actuators) - goal_pos[:6] = ensure_safe_goal_position( - goal_pos[:6], present_pos, self.config.max_relative_target - ) + present_pos = self.bus.sync_read("Present_Position", self.arm_motors) + goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in arm_goal_pos.items()} + arm_safe_goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) # Send goal position to the actuators - # TODO(Steven): This happens synchronously - self.actuators_bus.write("Goal_Position", goal_pos[:6].astype(np.int32), self.arm_actuators) - self.actuators_bus.write("Goal_Speed", goal_pos[6:].astype(np.int32), self.base_actuators) + self.bus.sync_write("Goal_Position", arm_safe_goal_pos) + self.bus.sync_write("Goal_Speed", base_goal_vel) - return goal_pos + return {**arm_safe_goal_pos, **base_goal_vel} def stop_base(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") - - def print_logs(self): - # TODO(Steven): Refactor logger - pass + self.bus.sync_write("Goal_Speed", {name: 0 for name in self.base_motors}, num_retry=5) + logger.info("Base motors stopped") def disconnect(self): if not self.is_connected: - raise DeviceNotConnectedError( - "LeKiwi is not connected. You need to run `robot.connect()` before disconnecting." - ) + raise DeviceNotConnectedError(f"{self} is not connected.") self.stop_base() - self.actuators_bus.disconnect() + self.bus.disconnect(self.config.disable_torque_on_disconnect) for cam in self.cameras.values(): cam.disconnect() - self.is_connected = False - def __del__(self): - if getattr(self, "is_connected", False): - self.disconnect() + logger.info(f"{self} disconnected.") From fe474e09009ce326e13d9de025f6bfe9ed7a89f5 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Fri, 4 Apr 2025 11:44:35 +0200 Subject: [PATCH 16/19] Rename Lekiwi files & classes --- lerobot/common/robots/lekiwi/README.md | 8 +++---- ...nfiguration_lekiwi.py => config_lekiwi.py} | 2 +- .../lekiwi/configuration_daemon_lekiwi.py | 4 ++-- .../lekiwi/{lekiwi_robot.py => lekiwi.py} | 8 +++---- .../{daemon_lekiwi.py => lekiwi_client.py} | 22 +++++++++---------- ...mon_lekiwi_app.py => lekiwi_client_app.py} | 18 +++++++-------- .../lekiwi/{lekiwi_app.py => lekiwi_host.py} | 20 ++++++++--------- lerobot/common/robots/utils.py | 8 +++---- .../keyboard/teleop_keyboard_app.py | 2 +- 9 files changed, 45 insertions(+), 47 deletions(-) rename lerobot/common/robots/lekiwi/{configuration_lekiwi.py => config_lekiwi.py} (97%) rename lerobot/common/robots/lekiwi/{lekiwi_robot.py => lekiwi.py} (98%) rename lerobot/common/robots/lekiwi/{daemon_lekiwi.py => lekiwi_client.py} (96%) rename lerobot/common/robots/lekiwi/{daemon_lekiwi_app.py => lekiwi_client_app.py} (89%) rename lerobot/common/robots/lekiwi/{lekiwi_app.py => lekiwi_host.py} (87%) diff --git a/lerobot/common/robots/lekiwi/README.md b/lerobot/common/robots/lekiwi/README.md index 0077d2d8..302b42f1 100644 --- a/lerobot/common/robots/lekiwi/README.md +++ b/lerobot/common/robots/lekiwi/README.md @@ -184,11 +184,11 @@ sudo chmod 666 /dev/ttyACM1 #### d. Update config file -IMPORTANTLY: Now that you have your ports of leader and follower arm and ip address of the mobile-so100, update the **ip** in Network configuration, **port** in leader_arms and **port** in lekiwi. In the [`LeKiwiRobotConfig`](../lerobot/common/robot_devices/robots/configs.py) file. Where you will find something like: +IMPORTANTLY: Now that you have your ports of leader and follower arm and ip address of the mobile-so100, update the **ip** in Network configuration, **port** in leader_arms and **port** in lekiwi. In the [`LeKiwiConfig`](../lerobot/common/robot_devices/robots/configs.py) file. Where you will find something like: ```python @RobotConfig.register_subclass("lekiwi") @dataclass -class LeKiwiRobotConfig(RobotConfig): +class LeKiwiConfig(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. @@ -271,7 +271,7 @@ For the wired LeKiwi version your configured IP address should refer to your own ```python @RobotConfig.register_subclass("lekiwi") @dataclass -class LeKiwiRobotConfig(RobotConfig): +class LeKiwiConfig(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. @@ -434,7 +434,7 @@ You should see on your laptop something like this: ```[INFO] Connected to remote | F | Decrease speed | > [!TIP] -> If you use a different keyboard you can change the keys for each command in the [`LeKiwiRobotConfig`](../lerobot/common/robot_devices/robots/configs.py). +> If you use a different keyboard you can change the keys for each command in the [`LeKiwiConfig`](../lerobot/common/robot_devices/robots/configs.py). ### Wired version If you have the **wired** LeKiwi version please run all commands including both these teleoperation commands on your laptop. diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/config_lekiwi.py similarity index 97% rename from lerobot/common/robots/lekiwi/configuration_lekiwi.py rename to lerobot/common/robots/lekiwi/config_lekiwi.py index df163109..331dd9ff 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/lerobot/common/robots/lekiwi/config_lekiwi.py @@ -21,7 +21,7 @@ from lerobot.common.robots.config import RobotConfig @RobotConfig.register_subclass("lekiwi") @dataclass -class LeKiwiRobotConfig(RobotConfig): +class LeKiwiConfig(RobotConfig): port = "/dev/ttyACM0" # port to connect to the bus disable_torque_on_disconnect: bool = True diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index c25d8b35..31027478 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py @@ -17,9 +17,9 @@ from dataclasses import dataclass, field from lerobot.common.robots.config import RobotConfig -@RobotConfig.register_subclass("daemon_lekiwi") +@RobotConfig.register_subclass("lekiwi_client") @dataclass -class DaemonLeKiwiRobotConfig(RobotConfig): +class LeKiwiClientConfig(RobotConfig): # Network Configuration remote_ip: str = "172.18.133.90" port_zmq_cmd: int = 5555 diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi.py similarity index 98% rename from lerobot/common/robots/lekiwi/lekiwi_robot.py rename to lerobot/common/robots/lekiwi/lekiwi.py index 512f3d21..9c9b686d 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi.py @@ -32,12 +32,12 @@ from lerobot.common.motors.feetech import ( from ..robot import Robot from ..utils import ensure_safe_goal_position -from .configuration_lekiwi import LeKiwiRobotConfig +from .config_lekiwi import LeKiwiConfig logger = logging.getLogger(__name__) -class LeKiwiRobot(Robot): +class LeKiwi(Robot): """ The robot includes a three omniwheel mobile base and a remote follower arm. The leader arm is connected locally (on the laptop) and its joint positions are recorded and then @@ -45,10 +45,10 @@ class LeKiwiRobot(Robot): In parallel, keyboard teleoperation is used to generate raw velocity commands for the wheels. """ - config_class = LeKiwiRobotConfig + config_class = LeKiwiConfig name = "lekiwi" - def __init__(self, config: LeKiwiRobotConfig): + def __init__(self, config: LeKiwiConfig): super().__init__(config) self.config = config self.id = config.id diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/lekiwi_client.py similarity index 96% rename from lerobot/common/robots/lekiwi/daemon_lekiwi.py rename to lerobot/common/robots/lekiwi/lekiwi_client.py index 5962018e..5968c21d 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/lekiwi_client.py @@ -26,7 +26,7 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte from lerobot.common.robots.config import RobotMode from ..robot import Robot -from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig +from .configuration_daemon_lekiwi import LeKiwiClientConfig # TODO(Steven): This doesn't need to inherit from Robot @@ -40,11 +40,11 @@ from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig # 2. Adding it into the robot implementation # (meaning the policy might be needed to be train # over the teleop action space) -class DaemonLeKiwiRobot(Robot): - config_class = DaemonLeKiwiRobotConfig - name = "daemonlekiwi" +class LeKiwiClient(Robot): + config_class = LeKiwiClientConfig + name = "lekiwi_client" - def __init__(self, config: DaemonLeKiwiRobotConfig): + def __init__(self, config: LeKiwiClientConfig): super().__init__(config) self.config = config self.id = config.id @@ -150,7 +150,7 @@ class DaemonLeKiwiRobot(Robot): # TODO(Steven): Nothing to calibrate. # Consider triggering calibrate() on the remote mobile robot? # Although this would require a more complex comms schema - logging.warning("DaemonLeKiwiRobot has nothing to calibrate.") + logging.warning("LeKiwiClient has nothing to calibrate.") return # Consider moving these static functions out of the class @@ -235,7 +235,7 @@ class DaemonLeKiwiRobot(Robot): 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] + wheel_raw = [LeKiwiClient._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]} @@ -259,7 +259,7 @@ class DaemonLeKiwiRobot(Robot): """ # Convert each raw command back to an angular speed in deg/s. - wheel_degps = np.array([DaemonLeKiwiRobot._raw_to_degps(int(r)) for r in wheel_raw]) + wheel_degps = np.array([LeKiwiClient._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. @@ -352,7 +352,7 @@ class DaemonLeKiwiRobot(Robot): 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 + # TODO(Steven): The returned space is different from the get_observation of LeKiwi # This returns body-frames velocities instead of wheel pos/speeds def get_observation(self) -> dict[str, np.ndarray]: """ @@ -361,9 +361,7 @@ class DaemonLeKiwiRobot(Robot): and a camera frame. Receives over ZMQ, translate to body-frame vel """ if not self.is_connected: - raise DeviceNotConnectedError( - "DaemonLeKiwiRobot is not connected. You need to run `robot.connect()`." - ) + raise DeviceNotConnectedError("LeKiwiClient is not connected. You need to run `robot.connect()`.") obs_dict = {} diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/lekiwi_client_app.py similarity index 89% rename from lerobot/common/robots/lekiwi/daemon_lekiwi_app.py rename to lerobot/common/robots/lekiwi/lekiwi_client_app.py index 47eb2e10..2d10fe09 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/lekiwi_client_app.py @@ -18,8 +18,8 @@ import numpy as np from lerobot.common.datasets.lerobot_dataset import LeRobotDataset from lerobot.common.robots.config import RobotMode -from lerobot.common.robots.lekiwi.configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig -from lerobot.common.robots.lekiwi.daemon_lekiwi import DaemonLeKiwiRobot +from lerobot.common.robots.lekiwi.configuration_daemon_lekiwi import LeKiwiClientConfig +from lerobot.common.robots.lekiwi.lekiwi_client import LeKiwiClient from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig @@ -87,11 +87,11 @@ def main(): keyboard_config = KeyboardTeleopConfig() keyboard = KeyboardTeleop(keyboard_config) - logging.info("Configuring LeKiwiRobot Daemon") - robot_config = DaemonLeKiwiRobotConfig( + logging.info("Configuring LeKiwi Client") + robot_config = LeKiwiClientConfig( id="daemonlekiwi", calibration_dir=".cache/calibration/lekiwi", robot_mode=RobotMode.TELEOP ) - robot = DaemonLeKiwiRobot(robot_config) + robot = LeKiwiClient(robot_config) logging.info("Creating LeRobot Dataset") @@ -106,10 +106,10 @@ def main(): leader_arm.connect() keyboard.connect() - logging.info("Connecting remote LeKiwiRobot") + logging.info("Connecting remote LeKiwi") robot.connect() - logging.info("Starting LeKiwiRobot teleoperation") + logging.info("Starting LeKiwi teleoperation") i = 0 while i < 1000: arm_action = leader_arm.get_action() @@ -135,11 +135,11 @@ def main(): dataset.save_episode() # dataset.push_to_hub() - logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon") + logging.info("Disconnecting Teleop Devices and LeKiwi Client") robot.disconnect() leader_arm.disconnect() keyboard.disconnect() - logging.info("Finished LeKiwiRobot cleanly") + logging.info("Finished LeKiwi cleanly") if __name__ == "__main__": diff --git a/lerobot/common/robots/lekiwi/lekiwi_app.py b/lerobot/common/robots/lekiwi/lekiwi_host.py similarity index 87% rename from lerobot/common/robots/lekiwi/lekiwi_app.py rename to lerobot/common/robots/lekiwi/lekiwi_host.py index bc720494..6e4d76d3 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/lekiwi_host.py @@ -22,15 +22,15 @@ import numpy as np import zmq from lerobot.common.constants import OBS_STATE -from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig -from lerobot.common.robots.lekiwi.lekiwi_robot import LeKiwiRobot +from lerobot.common.robots.lekiwi.config_lekiwi import LeKiwiConfig +from lerobot.common.robots.lekiwi.lekiwi import LeKiwi # Network Configuration PORT_ZMQ_CMD: int = 5555 PORT_ZMQ_OBSERVATIONS: int = 5556 -class RemoteAgent: +class HostAgent: def __init__(self): self.zmq_context = zmq.Context() self.zmq_cmd_socket = self.zmq_context.socket(zmq.PULL) @@ -48,15 +48,15 @@ class RemoteAgent: def main(): - logging.info("Configuring LeKiwiRobot") - robot_config = LeKiwiRobotConfig() - robot = LeKiwiRobot(robot_config) + logging.info("Configuring LeKiwi") + robot_config = LeKiwiConfig() + robot = LeKiwi(robot_config) - logging.info("Connecting LeKiwiRobot") + logging.info("Connecting LeKiwi") robot.connect() - logging.info("Starting RemoteAgent") - remote_agent = RemoteAgent() + logging.info("Starting HostAgent") + remote_agent = HostAgent() last_cmd_time = time.time() logging.info("Waiting for commands...") @@ -101,7 +101,7 @@ def main(): robot.disconnect() remote_agent.disconnect() - logging.info("Finished LeKiwiRobot cleanly") + logging.info("Finished LeKiwi cleanly") if __name__ == "__main__": diff --git a/lerobot/common/robots/utils.py b/lerobot/common/robots/utils.py index 19b7c1b9..86f67882 100644 --- a/lerobot/common/robots/utils.py +++ b/lerobot/common/robots/utils.py @@ -49,22 +49,22 @@ def make_robot_config(robot_type: str, **kwargs) -> RobotConfig: return Stretch3RobotConfig(**kwargs) elif robot_type == "lekiwi": - from .lekiwi.configuration_lekiwi import LeKiwiRobotConfig + from .lekiwi.config_lekiwi import LeKiwiConfig - return LeKiwiRobotConfig(**kwargs) + return LeKiwiConfig(**kwargs) else: raise ValueError(f"Robot type '{robot_type}' is not available.") def make_robot_from_config(config: RobotConfig): - from .lekiwi.configuration_lekiwi import LeKiwiRobotConfig + from .lekiwi.config_lekiwi import LeKiwiConfig from .manipulator import ManipulatorRobotConfig if isinstance(config, ManipulatorRobotConfig): from lerobot.common.robots.manipulator import ManipulatorRobot return ManipulatorRobot(config) - elif isinstance(config, LeKiwiRobotConfig): + elif isinstance(config, LeKiwiConfig): from lerobot.common.robots.mobile_manipulator import MobileManipulator return MobileManipulator(config) diff --git a/lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py b/lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py index 4f463814..6ecd78bd 100755 --- a/lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py +++ b/lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py @@ -21,7 +21,7 @@ def main(): i += 1 keyboard.disconnect() - logging.info("Finished LeKiwiRobot cleanly") + logging.info("Finished LeKiwi cleanly") if __name__ == "__main__": From c7dd4f0197dfe1611b6b2dc71bdb043b677aeffd Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Fri, 4 Apr 2025 11:48:41 +0200 Subject: [PATCH 17/19] Cleanup imports --- lerobot/common/robots/lekiwi/__init__.py | 4 ++++ lerobot/common/robots/lekiwi/config_lekiwi.py | 3 ++- .../robots/lekiwi/configuration_daemon_lekiwi.py | 2 +- lerobot/common/robots/lekiwi/lekiwi_client_app.py | 11 ++++++----- lerobot/common/robots/lekiwi/lekiwi_host.py | 5 +++-- 5 files changed, 16 insertions(+), 9 deletions(-) create mode 100644 lerobot/common/robots/lekiwi/__init__.py diff --git a/lerobot/common/robots/lekiwi/__init__.py b/lerobot/common/robots/lekiwi/__init__.py new file mode 100644 index 00000000..0567006d --- /dev/null +++ b/lerobot/common/robots/lekiwi/__init__.py @@ -0,0 +1,4 @@ +from .config_lekiwi import LeKiwiConfig +from .configuration_daemon_lekiwi import LeKiwiClientConfig +from .lekiwi import LeKiwi +from .lekiwi_client import LeKiwiClient diff --git a/lerobot/common/robots/lekiwi/config_lekiwi.py b/lerobot/common/robots/lekiwi/config_lekiwi.py index 331dd9ff..7cb0190c 100644 --- a/lerobot/common/robots/lekiwi/config_lekiwi.py +++ b/lerobot/common/robots/lekiwi/config_lekiwi.py @@ -16,7 +16,8 @@ from dataclasses import dataclass, field from lerobot.common.cameras.configs import CameraConfig from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig -from lerobot.common.robots.config import RobotConfig + +from ..config import RobotConfig @RobotConfig.register_subclass("lekiwi") diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index 31027478..ad0c89f8 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py @@ -14,7 +14,7 @@ from dataclasses import dataclass, field -from lerobot.common.robots.config import RobotConfig +from ..config import RobotConfig @RobotConfig.register_subclass("lekiwi_client") diff --git a/lerobot/common/robots/lekiwi/lekiwi_client_app.py b/lerobot/common/robots/lekiwi/lekiwi_client_app.py index 2d10fe09..ff55421a 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_client_app.py +++ b/lerobot/common/robots/lekiwi/lekiwi_client_app.py @@ -18,10 +18,11 @@ import numpy as np from lerobot.common.datasets.lerobot_dataset import LeRobotDataset from lerobot.common.robots.config import RobotMode -from lerobot.common.robots.lekiwi.configuration_daemon_lekiwi import LeKiwiClientConfig -from lerobot.common.robots.lekiwi.lekiwi_client import LeKiwiClient from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig -from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig +from lerobot.common.teleoperators.so100 import SO100Leader, SO100LeaderConfig + +from .configuration_daemon_lekiwi import LeKiwiClientConfig +from .lekiwi_client import LeKiwiClient DUMMY_FEATURES = { "observation.state": { @@ -81,8 +82,8 @@ DUMMY_FEATURES = { def main(): logging.info("Configuring Teleop Devices") - leader_arm_config = SO100TeleopConfig(port="/dev/tty.usbmodem58760429271") - leader_arm = SO100Teleop(leader_arm_config) + leader_arm_config = SO100LeaderConfig(port="/dev/tty.usbmodem58760429271") + leader_arm = SO100Leader(leader_arm_config) keyboard_config = KeyboardTeleopConfig() keyboard = KeyboardTeleop(keyboard_config) diff --git a/lerobot/common/robots/lekiwi/lekiwi_host.py b/lerobot/common/robots/lekiwi/lekiwi_host.py index 6e4d76d3..fdbe3079 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_host.py +++ b/lerobot/common/robots/lekiwi/lekiwi_host.py @@ -22,8 +22,9 @@ import numpy as np import zmq from lerobot.common.constants import OBS_STATE -from lerobot.common.robots.lekiwi.config_lekiwi import LeKiwiConfig -from lerobot.common.robots.lekiwi.lekiwi import LeKiwi + +from .config_lekiwi import LeKiwiConfig +from .lekiwi import LeKiwi # Network Configuration PORT_ZMQ_CMD: int = 5555 From 9d4d2cc8cfc608a8300e78e35210e84d46132089 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Fri, 4 Apr 2025 11:51:37 +0200 Subject: [PATCH 18/19] Group config files --- lerobot/common/robots/lekiwi/__init__.py | 3 +- lerobot/common/robots/lekiwi/config_lekiwi.py | 26 +++++++++++ .../lekiwi/configuration_daemon_lekiwi.py | 43 ------------------- lerobot/common/robots/lekiwi/lekiwi_client.py | 2 +- .../common/robots/lekiwi/lekiwi_client_app.py | 2 +- 5 files changed, 29 insertions(+), 47 deletions(-) delete mode 100644 lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py diff --git a/lerobot/common/robots/lekiwi/__init__.py b/lerobot/common/robots/lekiwi/__init__.py index 0567006d..e3d10c5c 100644 --- a/lerobot/common/robots/lekiwi/__init__.py +++ b/lerobot/common/robots/lekiwi/__init__.py @@ -1,4 +1,3 @@ -from .config_lekiwi import LeKiwiConfig -from .configuration_daemon_lekiwi import LeKiwiClientConfig +from .config_lekiwi import LeKiwiClientConfig, LeKiwiConfig from .lekiwi import LeKiwi from .lekiwi_client import LeKiwiClient diff --git a/lerobot/common/robots/lekiwi/config_lekiwi.py b/lerobot/common/robots/lekiwi/config_lekiwi.py index 7cb0190c..c74e683d 100644 --- a/lerobot/common/robots/lekiwi/config_lekiwi.py +++ b/lerobot/common/robots/lekiwi/config_lekiwi.py @@ -42,3 +42,29 @@ class LeKiwiConfig(RobotConfig): ), } ) + + +@RobotConfig.register_subclass("lekiwi_client") +@dataclass +class LeKiwiClientConfig(RobotConfig): + # Network Configuration + remote_ip: str = "172.18.133.90" + 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", + } + ) diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py deleted file mode 100644 index ad0c89f8..00000000 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ /dev/null @@ -1,43 +0,0 @@ -# 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. - -from dataclasses import dataclass, field - -from ..config import RobotConfig - - -@RobotConfig.register_subclass("lekiwi_client") -@dataclass -class LeKiwiClientConfig(RobotConfig): - # Network Configuration - remote_ip: str = "172.18.133.90" - 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", - } - ) diff --git a/lerobot/common/robots/lekiwi/lekiwi_client.py b/lerobot/common/robots/lekiwi/lekiwi_client.py index 5968c21d..0fb12456 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_client.py +++ b/lerobot/common/robots/lekiwi/lekiwi_client.py @@ -26,7 +26,7 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte from lerobot.common.robots.config import RobotMode from ..robot import Robot -from .configuration_daemon_lekiwi import LeKiwiClientConfig +from .config_lekiwi import LeKiwiClientConfig # TODO(Steven): This doesn't need to inherit from Robot diff --git a/lerobot/common/robots/lekiwi/lekiwi_client_app.py b/lerobot/common/robots/lekiwi/lekiwi_client_app.py index ff55421a..2489261f 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_client_app.py +++ b/lerobot/common/robots/lekiwi/lekiwi_client_app.py @@ -21,7 +21,7 @@ from lerobot.common.robots.config import RobotMode from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig from lerobot.common.teleoperators.so100 import SO100Leader, SO100LeaderConfig -from .configuration_daemon_lekiwi import LeKiwiClientConfig +from .config_lekiwi import LeKiwiClientConfig from .lekiwi_client import LeKiwiClient DUMMY_FEATURES = { From c6548caf5dfef385c9caaefe078a1c880e8aec84 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 4 Apr 2025 14:26:46 +0200 Subject: [PATCH 19/19] refactor(robots): update lekiwi for the latest motor bus api chore(teleop): Add missing abstract methods to keyboard implementation refactor(robots): update lekiwi client and host code for the new api chore(config): update host lekiwi ip in client config chore(examples): move application scripts to the examples directory fix(motors): missing type check condition in set_half_turn_homings fix(robots): fix assumption in calibrate() for robots with more than just an arm fix(robot): change Mode to Operating_Mode in configure write for lekiwi fix(robots): make sure message is display in calibrate() method lekiwi fix(robots): no need for .tolist() in lekiwi host app fix(teleop): fix is_connected in teleoperator keyboard fix(teleop): always display calibration message in so100 fix(robots): fix send_action in lekiwi_client debug(examples): configuration for lekiwi client app fix(robots): fix send_action in lekiwi client part 2 refactor(robots): use dicts in lekiwi for get_obs and send_action dbg(robots): check sent action wheels lekiwi debug(robots): fix overflow base commands debug(robots): fix how we deal with negative values lekiwi debug(robots): lekiwi sign degrees fix fix(robots): right motors id in lekiwi host chore(doc): update todos --- .../robots}/lekiwi_client_app.py | 40 +++-- .../teleoperators}/teleop_keyboard_app.py | 0 lerobot/common/constants.py | 2 +- lerobot/common/motors/motors_bus.py | 3 +- lerobot/common/robots/lekiwi/config_lekiwi.py | 2 +- lerobot/common/robots/lekiwi/lekiwi.py | 52 ++++--- lerobot/common/robots/lekiwi/lekiwi_client.py | 141 ++++++++++-------- lerobot/common/robots/lekiwi/lekiwi_host.py | 20 ++- .../teleoperators/keyboard/teleop_keyboard.py | 44 +++--- .../teleoperators/so100/so100_leader.py | 2 +- lerobot/common/teleoperators/teleoperator.py | 3 + 11 files changed, 176 insertions(+), 133 deletions(-) rename {lerobot/common/robots/lekiwi => examples/robots}/lekiwi_client_app.py (82%) mode change 100644 => 100755 rename {lerobot/common/teleoperators/keyboard => examples/teleoperators}/teleop_keyboard_app.py (100%) diff --git a/lerobot/common/robots/lekiwi/lekiwi_client_app.py b/examples/robots/lekiwi_client_app.py old mode 100644 new mode 100755 similarity index 82% rename from lerobot/common/robots/lekiwi/lekiwi_client_app.py rename to examples/robots/lekiwi_client_app.py index 2489261f..4daca962 --- a/lerobot/common/robots/lekiwi/lekiwi_client_app.py +++ b/examples/robots/lekiwi_client_app.py @@ -14,16 +14,12 @@ import logging -import numpy as np - -from lerobot.common.datasets.lerobot_dataset import LeRobotDataset from lerobot.common.robots.config import RobotMode +from lerobot.common.robots.lekiwi.config_lekiwi import LeKiwiClientConfig +from lerobot.common.robots.lekiwi.lekiwi_client import LeKiwiClient from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig from lerobot.common.teleoperators.so100 import SO100Leader, SO100LeaderConfig -from .config_lekiwi import LeKiwiClientConfig -from .lekiwi_client import LeKiwiClient - DUMMY_FEATURES = { "observation.state": { "dtype": "float64", @@ -82,26 +78,24 @@ DUMMY_FEATURES = { def main(): logging.info("Configuring Teleop Devices") - leader_arm_config = SO100LeaderConfig(port="/dev/tty.usbmodem58760429271") + leader_arm_config = SO100LeaderConfig(port="/dev/tty.usbmodem58760434171") leader_arm = SO100Leader(leader_arm_config) keyboard_config = KeyboardTeleopConfig() keyboard = KeyboardTeleop(keyboard_config) logging.info("Configuring LeKiwi Client") - robot_config = LeKiwiClientConfig( - id="daemonlekiwi", calibration_dir=".cache/calibration/lekiwi", robot_mode=RobotMode.TELEOP - ) + robot_config = LeKiwiClientConfig(id="lekiwi", robot_mode=RobotMode.TELEOP) robot = LeKiwiClient(robot_config) logging.info("Creating LeRobot Dataset") - # TODO(Steven): Check this creation - dataset = LeRobotDataset.create( - repo_id="user/lekiwi", - fps=10, - features=DUMMY_FEATURES, - ) + # # TODO(Steven): Check this creation + # dataset = LeRobotDataset.create( + # repo_id="user/lekiwi2", + # fps=10, + # features=DUMMY_FEATURES, + # ) logging.info("Connecting Teleop Devices") leader_arm.connect() @@ -110,30 +104,32 @@ def main(): logging.info("Connecting remote LeKiwi") robot.connect() + if not robot.is_connected or not leader_arm.is_connected or not keyboard.is_connected: + logging.error("Failed to connect to all devices") + return + logging.info("Starting LeKiwi teleoperation") i = 0 while i < 1000: 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 + action = {**arm_action, **base_action} if len(base_action) > 0 else arm_action # TODO(Steven): Deal with policy action space # robot.set_mode(RobotMode.AUTO) # policy_action = policy.get_action() # This might be in body frame, key space or smt else # robot.send_action(policy_action) - action_sent = robot.send_action(action) observation = robot.get_observation() - frame = {"action": action_sent} - frame.update(observation) + frame = {**action_sent, **observation} frame.update({"task": "Dummy Task Dataset"}) logging.info("Saved a frame into the dataset") - dataset.add_frame(frame) + # dataset.add_frame(frame) i += 1 - dataset.save_episode() + # dataset.save_episode() # dataset.push_to_hub() logging.info("Disconnecting Teleop Devices and LeKiwi Client") diff --git a/lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py b/examples/teleoperators/teleop_keyboard_app.py similarity index 100% rename from lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py rename to examples/teleoperators/teleop_keyboard_app.py diff --git a/lerobot/common/constants.py b/lerobot/common/constants.py index 22d7568e..e78e748b 100644 --- a/lerobot/common/constants.py +++ b/lerobot/common/constants.py @@ -48,5 +48,5 @@ default_cache_path = Path(HF_HOME) / "lerobot" HF_LEROBOT_HOME = Path(os.getenv("HF_LEROBOT_HOME", default_cache_path)).expanduser() # calibration dir -default_calibration_path = HF_LEROBOT_HOME / ".calibration" +default_calibration_path = HF_LEROBOT_HOME / "calibration" HF_LEROBOT_CALIBRATION = Path(os.getenv("HF_LEROBOT_CALIBRATION", default_calibration_path)).expanduser() diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index 3c64be7b..14144d29 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -544,7 +544,7 @@ class MotorsBus(abc.ABC): motors = self.names elif isinstance(motors, (str, int)): motors = [motors] - else: + elif not isinstance(motors, list): raise TypeError(motors) self.reset_calibration(motors) @@ -606,6 +606,7 @@ class MotorsBus(abc.ABC): min_ = self.calibration[name].range_min max_ = self.calibration[name].range_max bounded_val = min(max_, max(min_, val)) + # TODO(Steven): normalization can go boom if max_ == min_, we should add a check probably in record_ranges_of_motions (which probably indicates the user forgot to move a motor) if self.motors[name].norm_mode is MotorNormMode.RANGE_M100_100: normalized_values[id_] = (((bounded_val - min_) / (max_ - min_)) * 200) - 100 elif self.motors[name].norm_mode is MotorNormMode.RANGE_0_100: diff --git a/lerobot/common/robots/lekiwi/config_lekiwi.py b/lerobot/common/robots/lekiwi/config_lekiwi.py index c74e683d..2d6c0493 100644 --- a/lerobot/common/robots/lekiwi/config_lekiwi.py +++ b/lerobot/common/robots/lekiwi/config_lekiwi.py @@ -48,7 +48,7 @@ class LeKiwiConfig(RobotConfig): @dataclass class LeKiwiClientConfig(RobotConfig): # Network Configuration - remote_ip: str = "172.18.133.90" + remote_ip: str = "172.18.129.208" port_zmq_cmd: int = 5555 port_zmq_observations: int = 5556 diff --git a/lerobot/common/robots/lekiwi/lekiwi.py b/lerobot/common/robots/lekiwi/lekiwi.py index 9c9b686d..7ca0fc07 100644 --- a/lerobot/common/robots/lekiwi/lekiwi.py +++ b/lerobot/common/robots/lekiwi/lekiwi.py @@ -14,13 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -import base64 import logging import time from typing import Any -import cv2 - 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 @@ -64,8 +61,8 @@ class LeKiwi(Robot): "arm_gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100), # base "base_left_wheel": Motor(7, "sts3215", MotorNormMode.RANGE_M100_100), - "base_back_wheel": Motor(8, "sts3215", MotorNormMode.RANGE_M100_100), - "base_right_wheel": Motor(9, "sts3215", MotorNormMode.RANGE_M100_100), + "base_right_wheel": Motor(8, "sts3215", MotorNormMode.RANGE_M100_100), + "base_back_wheel": Motor(9, "sts3215", MotorNormMode.RANGE_M100_100), }, calibration=self.calibration, ) @@ -119,23 +116,33 @@ class LeKiwi(Robot): def is_calibrated(self) -> bool: return self.bus.is_calibrated + # TODO(Steven): I think we should extend this to give the user the option of re-calibrate + # calibrate(recalibrate: bool = False) -> None: + # If true, then we overwrite the previous calibration file with new values def calibrate(self) -> None: logger.info(f"\nRunning calibration of {self}") + + motors = self.arm_motors + self.base_motors + self.bus.disable_torque(self.arm_motors) for name in self.arm_motors: self.bus.write("Operating_Mode", name, OperatingMode.POSITION.value) input("Move robot to the middle of its range of motion and press ENTER....") - homing_offsets = self.bus.set_half_turn_homings(self.arm_motors) + homing_offsets = self.bus.set_half_turn_homings(motors) - full_turn_motor = "arm_wrist_roll" - unknown_range_motors = [name for name in self.arm_motors if name != full_turn_motor] - logger.info( + # TODO(Steven): Might be worth to do this also in other robots but it should be added in the docs + full_turn_motor = [ + motor for motor in motors if any(keyword in motor for keyword in ["wheel", "gripper"]) + ] + unknown_range_motors = [motor for motor in motors if motor not in full_turn_motor] + + print( f"Move all arm joints except '{full_turn_motor}' sequentially through their " "entire ranges of motion.\nRecording positions. Press ENTER to stop..." ) range_mins, range_maxes = self.bus.record_ranges_of_motion(unknown_range_motors) - for name in [*self.base_motors, full_turn_motor]: + for name in full_turn_motor: range_mins[name] = 0 range_maxes[name] = 4095 @@ -159,7 +166,7 @@ class LeKiwi(Robot): # and torque can be safely disabled to run calibration. self.bus.disable_torque(self.arm_motors) for name in self.arm_motors: - self.bus.write("Mode", name, OperatingMode.POSITION.value) + self.bus.write("Operating_Mode", name, OperatingMode.POSITION.value) # Set P_Coefficient to lower value to avoid shakiness (Default is 32) self.bus.write("P_Coefficient", name, 16) # Set I_Coefficient and D_Coefficient to default value 0 and 32 @@ -171,15 +178,15 @@ class LeKiwi(Robot): self.bus.write("Acceleration", name, 254) for name in self.base_motors: - self.bus.write("Mode", name, OperatingMode.VELOCITY.value) + self.bus.write("Operating_Mode", name, OperatingMode.VELOCITY.value) - self.bus.enable_torque() + self.bus.enable_torque() # TODO(Steven): Operation has failed with: ConnectionError: Failed to write 'Lock' on id_=6 with '1' after 1 tries. [TxRxResult] Incorrect status packet! def get_observation(self) -> dict[str, Any]: if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") - obs_dict = {} + obs_dict = {OBS_IMAGES: {}} # Read actuators position for arm and vel for base start = time.perf_counter() @@ -192,12 +199,7 @@ class LeKiwi(Robot): # Capture images from cameras for cam_key, cam in self.cameras.items(): start = time.perf_counter() - 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}"] = "" + obs_dict[OBS_IMAGES][cam_key] = cam.async_read() dt_ms = (time.perf_counter() - start) * 1e3 logger.debug(f"{self} read {cam_key}: {dt_ms:.1f}ms") @@ -229,15 +231,19 @@ class LeKiwi(Robot): present_pos = self.bus.sync_read("Present_Position", self.arm_motors) goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in arm_goal_pos.items()} arm_safe_goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) + arm_goal_pos = arm_safe_goal_pos + + # TODO(Steven): Message fetching failed: Magnitude 34072 exceeds 32767 (max for sign_bit_index=15) + # TODO(Steven): IMO, this should be a check in the motor bus # Send goal position to the actuators - self.bus.sync_write("Goal_Position", arm_safe_goal_pos) + self.bus.sync_write("Goal_Position", arm_goal_pos) self.bus.sync_write("Goal_Speed", base_goal_vel) - return {**arm_safe_goal_pos, **base_goal_vel} + return {**arm_goal_pos, **base_goal_vel} def stop_base(self): - self.bus.sync_write("Goal_Speed", {name: 0 for name in self.base_motors}, num_retry=5) + self.bus.sync_write("Goal_Speed", dict.fromkeys(self.base_motors, 0), num_retry=5) logger.info("Base motors stopped") def disconnect(self): diff --git a/lerobot/common/robots/lekiwi/lekiwi_client.py b/lerobot/common/robots/lekiwi/lekiwi_client.py index 0fb12456..b9e3a5cd 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_client.py +++ b/lerobot/common/robots/lekiwi/lekiwi_client.py @@ -15,6 +15,7 @@ import base64 import json import logging +from typing import Any import cv2 import numpy as np @@ -40,6 +41,7 @@ from .config_lekiwi import LeKiwiClientConfig # 2. Adding it into the robot implementation # (meaning the policy might be needed to be train # over the teleop action space) +# TODO(Steven): Check if we can move everything to 32 instead class LeKiwiClient(Robot): config_class = LeKiwiClientConfig name = "lekiwi_client" @@ -62,10 +64,9 @@ class LeKiwiClient(Robot): self.zmq_observation_socket = None self.last_frames = {} - self.last_present_speed = [0, 0, 0] + self.last_present_speed = {"x_cmd": 0.0, "y_cmd": 0.0, "theta_cmd": 0.0} - # TODO(Steven): Move everything to 32 instead - self.last_remote_arm_state = torch.zeros(6, dtype=torch.float64) + self.last_remote_arm_state = {} # Define three speed levels and a current index self.speed_levels = [ @@ -75,7 +76,7 @@ class LeKiwiClient(Robot): ] self.speed_index = 0 # Start at slow - self.is_connected = False + self._is_connected = False self.logs = {} @property @@ -108,7 +109,7 @@ class LeKiwiClient(Robot): @property def camera_features(self) -> dict[str, dict]: # TODO(Steven): Get this from the data fetched? - # TODO(Steven): Motor names are unknown for the Daemon + # TODO(Steven): camera names are unknown for the Daemon # Or assume its size/metadata? # TODO(Steven): Check consistency of image sizes cam_ft = { @@ -125,10 +126,18 @@ class LeKiwiClient(Robot): } return cam_ft + @property + def is_connected(self) -> bool: + return self._is_connected + + @property + def is_calibrated(self) -> bool: + pass + def connect(self) -> None: """Establishes ZMQ sockets with the remote mobile robot""" - if self.is_connected: + if self._is_connected: raise DeviceAlreadyConnectedError( "LeKiwi Daemon is already connected. Do not run `robot.connect()` twice." ) @@ -144,37 +153,32 @@ class LeKiwiClient(Robot): self.zmq_observation_socket.connect(zmq_observations_locator) self.zmq_observation_socket.setsockopt(zmq.CONFLATE, 1) - self.is_connected = True + self._is_connected = True def calibrate(self) -> None: - # TODO(Steven): Nothing to calibrate. - # Consider triggering calibrate() on the remote mobile robot? - # Although this would require a more complex comms schema logging.warning("LeKiwiClient has nothing to calibrate.") return # Consider moving these static functions out of the class - # Copied from robot_lekiwi MobileManipulator class + # Copied from robot_lekiwi MobileManipulator class* (before the refactor) @staticmethod def _degps_to_raw(degps: float) -> int: steps_per_deg = 4096.0 / 360.0 - speed_in_steps = abs(degps) * steps_per_deg + speed_in_steps = degps * steps_per_deg speed_int = int(round(speed_in_steps)) + # Cap the value to fit within signed 16-bit range (-32768 to 32767) if speed_int > 0x7FFF: - speed_int = 0x7FFF - if degps < 0: - return speed_int | 0x8000 - else: - return speed_int & 0x7FFF + speed_int = 0x7FFF # 32767 -> maximum positive value + elif speed_int < -0x8000: + speed_int = -0x8000 # -32768 -> minimum negative value + return speed_int # 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 + magnitude = raw_speed degps = magnitude / steps_per_deg - if raw_speed & 0x8000: - degps = -degps return degps # Copied from robot_lekiwi MobileManipulator class @@ -237,12 +241,13 @@ class LeKiwiClient(Robot): # Convert each wheel’s angular speed (deg/s) to a raw integer. wheel_raw = [LeKiwiClient._degps_to_raw(deg) for deg in wheel_degps] + # TODO(Steven): remove hard-coded names 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: np.array, wheel_radius: float = 0.05, base_radius: float = 0.125 - ) -> tuple: + self, wheel_raw: dict[str, Any], wheel_radius: float = 0.05, base_radius: float = 0.125 + ) -> dict[str, Any]: """ Convert wheel raw command feedback back into body-frame velocities. @@ -258,8 +263,9 @@ class LeKiwiClient(Robot): theta_cmd : Rotational velocity in deg/s. """ + # TODO(Steven): No check is done for dict keys # Convert each raw command back to an angular speed in deg/s. - wheel_degps = np.array([LeKiwiClient._raw_to_degps(int(r)) for r in wheel_raw]) + wheel_degps = np.array([LeKiwiClient._raw_to_degps(int(v)) for _, v in wheel_raw.items()]) # 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. @@ -274,7 +280,7 @@ class LeKiwiClient(Robot): 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) + return {"x_cmd": x_cmd, "y_cmd": y_cmd, "theta_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 # TODO(Steven): All this function needs to be refactored @@ -285,10 +291,9 @@ class LeKiwiClient(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.empty(6, dtype=torch.float64) + remote_arm_state_tensor = {} # Poll up to 15 ms poller = zmq.Poller() @@ -317,11 +322,9 @@ class LeKiwiClient(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)} + state_observation = observation[OBS_STATE] + image_observation = observation[OBS_IMAGES] # Convert images for cam_name, image_b64 in image_observation.items(): @@ -332,14 +335,16 @@ class LeKiwiClient(Robot): if frame_candidate is not None: frames[cam_name] = frame_candidate + # TODO(Steven): Should we really ignore the arm state if the image is None? # If remote_arm_state is None and frames is None there is no message then use the previous message if state_observation is not None and frames is not None: self.last_frames = frames - remote_arm_state_tensor = torch.tensor(state_observation[OBS_STATE][:6], dtype=torch.float64) + # TODO(Steven): hard-coded name of expected keys, not good + remote_arm_state_tensor = {k: v for k, v in state_observation.items() if k.startswith("arm")} self.last_remote_arm_state = remote_arm_state_tensor - present_speed = state_observation[OBS_STATE][6:] + present_speed = {k: v for k, v in state_observation.items() if k.startswith("base")} self.last_present_speed = present_speed else: frames = self.last_frames @@ -354,38 +359,35 @@ class LeKiwiClient(Robot): # TODO(Steven): The returned space is different from the get_observation of LeKiwi # This returns body-frames velocities instead of wheel pos/speeds - def get_observation(self) -> dict[str, np.ndarray]: + def get_observation(self) -> dict[str, Any]: """ 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. Receives over ZMQ, translate to body-frame vel """ - if not self.is_connected: + if not self._is_connected: raise DeviceNotConnectedError("LeKiwiClient is not connected. You need to run `robot.connect()`.") - obs_dict = {} + # TODO(Steven): remove hard-coded cam name + # This is needed at init for when there's no comms + obs_dict = { + OBS_IMAGES: {"wrist": np.zeros(shape=(480, 640, 3)), "front": np.zeros(shape=(640, 480, 3))} + } 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.float64) - combined_state_tensor = torch.cat((remote_arm_state_tensor, wheel_state_tensor), dim=0) + # TODO(Steven): output isdict[str,Any] and we multiply by 1000.0. This should be more explicit and specify the expected type instead of Any + body_state_mm = {k: v * 1000.0 for k, v in body_state.items()} # Convert x,y to mm/s - obs_dict = {OBS_STATE: combined_state_tensor} + obs_dict[OBS_STATE] = {**remote_arm_state_tensor, **body_state_mm} # Loop over each configured camera for cam_name, frame in frames.items(): if frame is None: - # TODO(Steven): Daemon doesn't know camera dimensions + # TODO(Steven): Daemon doesn't know camera dimensions (hard-coded for now), consider at least getting them from state features logging.warning("Frame is None") frame = np.zeros((480, 640, 3), dtype=np.uint8) - obs_dict[cam_name] = torch.from_numpy(frame) - - # TODO(Steven): Refactor this ugly thing (needed for when there are not comms at init) - if OBS_IMAGES + ".wrist" not in obs_dict: - obs_dict[OBS_IMAGES + ".wrist"] = np.zeros(shape=(480, 640, 3)) - if OBS_IMAGES + ".front" not in obs_dict: - obs_dict[OBS_IMAGES + ".front"] = np.zeros(shape=(640, 480, 3)) + obs_dict[OBS_IMAGES][cam_name] = torch.from_numpy(frame) return obs_dict @@ -415,9 +417,11 @@ class LeKiwiClient(Robot): theta_cmd += theta_speed if self.teleop_keys["rotate_right"] in pressed_keys: theta_cmd -= theta_speed - return self._body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) + def configure(self): + pass + # TODO(Steven): This assumes this call is always called from a keyboard teleop command # TODO(Steven): Doing this mapping in here adds latecy between send_action and movement from the user perspective. # t0: get teleop_cmd @@ -430,7 +434,7 @@ class LeKiwiClient(Robot): # t2': send_action(motor_cmd) # t3': execute motor_cmd # t3'-t2' << t3-t1 - def send_action(self, action: np.ndarray) -> np.ndarray: + def send_action(self, action: dict[str, Any]) -> dict[str, Any]: """Command lekiwi to move to a target joint configuration. Translates to motor space + sends over ZMQ Args: @@ -442,28 +446,40 @@ class LeKiwiClient(Robot): Returns: np.ndarray: the action sent to the motors, potentially clipped. """ - if not self.is_connected: + if not self._is_connected: raise DeviceNotConnectedError( "ManipulatorRobot is not connected. You need to run `robot.connect()`." ) - goal_pos: np.array = np.zeros(9) if self.robot_mode is RobotMode.AUTO: # TODO(Steven): Not yet implemented. The policy outputs might need a different conversion raise Exception + goal_pos = {} # TODO(Steven): This assumes teleop mode is always used with keyboard. Tomorrow we could teleop with another device ... ? if self.robot_mode is RobotMode.TELEOP: - if action.size < 6: - logging.error("Action should include at least the 6 states of the leader arm") + motors_name = self.state_feature.get("names").get("motors") + + common_keys = [ + key for key in action if key in (motor.replace("arm_", "") for motor in motors_name) + ] + + # TODO(Steven): I don't like this + if len(common_keys) < 6: + logging.error("Action should include at least the states of the leader arm") raise InvalidActionError - goal_pos[:6] = action[:6] - if action.size > 6: - # TODO(Steven): Assumes size and order is respected - wheel_actions = [v for _, v in self._from_keyboard_to_wheel_action(action[6:]).items()] - goal_pos[6:] = wheel_actions - self.zmq_cmd_socket.send_string(json.dumps(goal_pos.tolist())) # action is in motor space + arm_actions = {"arm_" + arm_motor: action[arm_motor] for arm_motor in common_keys} + goal_pos = arm_actions + + if len(action) > 6: + keyboard_keys = np.array(list(set(action.keys()) - set(common_keys))) + wheel_actions = { + "base_" + k: v for k, v in self._from_keyboard_to_wheel_action(keyboard_keys).items() + } + goal_pos = {**arm_actions, **wheel_actions} + + self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) # action is in motor space return goal_pos @@ -474,15 +490,14 @@ class LeKiwiClient(Robot): def disconnect(self): """Cleans ZMQ comms""" - if not self.is_connected: + 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. Although this would need a moore complex comms schema self.zmq_observation_socket.close() self.zmq_cmd_socket.close() self.zmq_context.term() - self.is_connected = False + self._is_connected = False def __del__(self): if getattr(self, "is_connected", False): diff --git a/lerobot/common/robots/lekiwi/lekiwi_host.py b/lerobot/common/robots/lekiwi/lekiwi_host.py index fdbe3079..56228ac9 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_host.py +++ b/lerobot/common/robots/lekiwi/lekiwi_host.py @@ -14,14 +14,15 @@ # See the License for the specific language governing permissions and # limitations under the License. +import base64 import json import logging import time -import numpy as np +import cv2 import zmq -from lerobot.common.constants import OBS_STATE +from lerobot.common.constants import OBS_IMAGES from .config_lekiwi import LeKiwiConfig from .lekiwi import LeKiwi @@ -69,7 +70,7 @@ def main(): loop_start_time = time.time() try: msg = remote_agent.zmq_cmd_socket.recv_string(zmq.NOBLOCK) - data = np.array(json.loads(msg)) + data = dict(json.loads(msg)) _action_sent = robot.send_action(data) last_cmd_time = time.time() except zmq.Again: @@ -84,7 +85,18 @@ def main(): robot.stop_base() last_observation = robot.get_observation() - last_observation[OBS_STATE] = last_observation[OBS_STATE].tolist() + + # Encode ndarrays to base64 strings + for cam_key, _ in robot.cameras.items(): + ret, buffer = cv2.imencode( + ".jpg", last_observation[OBS_IMAGES][cam_key], [int(cv2.IMWRITE_JPEG_QUALITY), 90] + ) + if ret: + last_observation[OBS_IMAGES][cam_key] = base64.b64encode(buffer).decode("utf-8") + else: + last_observation[OBS_IMAGES][cam_key] = "" + + # Send the observation to the remote agent remote_agent.zmq_observation_socket.send_string(json.dumps(last_observation)) # Ensure a short sleep to avoid overloading the CPU. diff --git a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py index 1a8e0e96..ccb00252 100644 --- a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py +++ b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py @@ -19,8 +19,7 @@ import os import sys import time from queue import Queue - -import numpy as np +from typing import Any from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError @@ -59,7 +58,7 @@ class KeyboardTeleop(Teleoperator): self.event_queue = Queue() self.current_pressed = {} self.listener = None - self.is_connected = False + self._is_connected = False self.logs = {} @property @@ -75,14 +74,22 @@ class KeyboardTeleop(Teleoperator): def feedback_feature(self) -> dict: return {} + @property + def is_connected(self) -> bool: + return self._is_connected + + @property + def is_calibrated(self) -> bool: + pass + def connect(self) -> None: # TODO(Steven): Consider instead of raising a warning and then returning the status - # if self.is_connected: + # if self._is_connected: # logging.warning( # "ManipulatorRobot is already connected. Do not run `robot.connect()` twice." # ) - # return self.is_connected - if self.is_connected: + # return self._is_connected + if self._is_connected: raise DeviceAlreadyConnectedError( "ManipulatorRobot is already connected. Do not run `robot.connect()` twice." ) @@ -90,24 +97,24 @@ class KeyboardTeleop(Teleoperator): if PYNPUT_AVAILABLE: logging.info("pynput is available - enabling local keyboard listener.") self.listener = keyboard.Listener( - on_press=self.on_press, - on_release=self.on_release, + on_press=self._on_press, + on_release=self._on_release, ) self.listener.start() else: logging.info("pynput not available - skipping local keyboard listener.") self.listener = None - self.is_connected = True + self._is_connected = True def calibrate(self) -> None: pass - def on_press(self, key): + def _on_press(self, key): if hasattr(key, "char"): self.event_queue.put((key.char, True)) - def on_release(self, key): + def _on_release(self, key): if hasattr(key, "char"): self.event_queue.put((key.char, False)) if key == keyboard.Key.esc: @@ -119,10 +126,13 @@ class KeyboardTeleop(Teleoperator): key_char, is_pressed = self.event_queue.get_nowait() self.current_pressed[key_char] = is_pressed - def get_action(self) -> np.ndarray: + def configure(self): + pass + + def get_action(self) -> dict[str, Any]: before_read_t = time.perf_counter() - if not self.is_connected: + if not self._is_connected: raise DeviceNotConnectedError( "KeyboardTeleop is not connected. You need to run `connect()` before `get_action()`." ) @@ -133,17 +143,17 @@ class KeyboardTeleop(Teleoperator): action = {key for key, val in self.current_pressed.items() if val} self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t - return np.array(list(action)) + return dict.fromkeys(action, None) - def send_feedback(self, feedback: np.ndarray) -> None: + def send_feedback(self, feedback: dict[str, Any]) -> None: pass def disconnect(self) -> None: - if not self.is_connected: + if not self._is_connected: raise DeviceNotConnectedError( "KeyboardTeleop is not connected. You need to run `robot.connect()` before `disconnect()`." ) if self.listener is not None: self.listener.stop() - self.is_connected = False + self._is_connected = False diff --git a/lerobot/common/teleoperators/so100/so100_leader.py b/lerobot/common/teleoperators/so100/so100_leader.py index f8f7239e..cdd16300 100644 --- a/lerobot/common/teleoperators/so100/so100_leader.py +++ b/lerobot/common/teleoperators/so100/so100_leader.py @@ -95,7 +95,7 @@ class SO100Leader(Teleoperator): full_turn_motor = "wrist_roll" unknown_range_motors = [name for name in self.arm.names if name != full_turn_motor] - logger.info( + print( f"Move all joints except '{full_turn_motor}' sequentially through their " "entire ranges of motion.\nRecording positions. Press ENTER to stop..." ) diff --git a/lerobot/common/teleoperators/teleoperator.py b/lerobot/common/teleoperators/teleoperator.py index d6285f5c..5da1aeb5 100644 --- a/lerobot/common/teleoperators/teleoperator.py +++ b/lerobot/common/teleoperators/teleoperator.py @@ -45,6 +45,9 @@ class Teleoperator(abc.ABC): def is_connected(self) -> bool: pass + # TODO(Steven): I think connect() should return a bool, such that the client/application code can check if the device connected successfully + # if not device.connect(): + # raise DeviceNotConnectedError(f"{device} failed to connect") @abc.abstractmethod def connect(self) -> None: """Connects to the teleoperator."""