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)