diff --git a/src/lerobot/robots/xlerobot/__init__.py b/src/lerobot/robots/xlerobot/__init__.py new file mode 100644 index 0000000000..e81c43e248 --- /dev/null +++ b/src/lerobot/robots/xlerobot/__init__.py @@ -0,0 +1,4 @@ +from .xlerobot_config import XLerobotConfig +from .xlerobot import XLerobot +from .xlerobot_client import XLerobotClient +from .xlerobot_host import XLerobotHost, XLerobotHostConfig diff --git a/src/lerobot/robots/xlerobot/xlerobot.py b/src/lerobot/robots/xlerobot/xlerobot.py new file mode 100644 index 0000000000..333f6e8c2c --- /dev/null +++ b/src/lerobot/robots/xlerobot/xlerobot.py @@ -0,0 +1,624 @@ +#!/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 logging +import time +from functools import cached_property +from itertools import chain +from typing import Any + +import numpy as np + +from lerobot.cameras.utils import make_cameras_from_configs +from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError +from lerobot.motors import Motor, MotorCalibration, MotorNormMode +from lerobot.motors.feetech import ( + FeetechMotorsBus, + OperatingMode, +) + +from ..robot import Robot +from ..utils import ensure_safe_goal_position +from .xlerobot_config import XLerobotConfig +from .xlerobot_base_keyboard import BaseKeyboardController +from .xlerobot_form_factor import ROBOT_PARTS, BUS_MAPPINGS, DEFAULT_IDS_BY_LAYOUT + +logger = logging.getLogger(__name__) + + +class XLerobot(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 = XLerobotConfig + name = "xlerobot" + + def __init__(self, config: XLerobotConfig): + super().__init__(config) + self.config = config + self.teleop_keys = config.teleop_keys + self.base_keyboard = BaseKeyboardController(self.teleop_keys) + + # Get bus mapping for the selected layout + bus_mapping = BUS_MAPPINGS[config.motor_layout] + + # Build buses dynamically from robot parts + bus mapping + self.buses: dict[str, FeetechMotorsBus] = {} + self.motor_to_bus: dict[str, str] = {} # motor_name -> bus_name + self.role_to_motors: dict[str, list[str]] = {} # role -> list of motor names + + motor_ids = dict(self.config.motor_ids) + layout_defaults = DEFAULT_IDS_BY_LAYOUT.get(self.config.motor_layout, {}) + calibration_ids = {name: calib.id for name, calib in self.calibration.items()} + + for bus_name, bus_config in bus_mapping.items(): + port = config.ports.get(bus_config.port_key) + if not port: + continue + + # Find motors that match this bus's patterns + bus_motors = {} + calibration_subset = {} + + for motor_name, motor_def in ROBOT_PARTS.items(): + # Check if this motor matches any pattern for this bus + matches = False + for pattern in bus_config.motor_patterns: + if pattern.endswith("*"): + prefix = pattern[:-1] + if motor_name.startswith(prefix): + matches = True + break + elif motor_name == pattern: + matches = True + break + + if not matches: + continue + + # Create Motor object + norm_mode = motor_def.norm_mode if config.use_degrees else MotorNormMode.RANGE_M100_100 + configured_id = motor_ids.get(motor_name) + if configured_id is not None: + motor_id = configured_id + else: + motor_id = calibration_ids.get( + motor_name, layout_defaults.get(motor_name, motor_def.id) + ) + bus_motors[motor_name] = Motor(motor_id, motor_def.model, norm_mode) + + # Track mappings + self.motor_to_bus[motor_name] = bus_name + if motor_def.role not in self.role_to_motors: + self.role_to_motors[motor_def.role] = [] + self.role_to_motors[motor_def.role].append(motor_name) + + # Extract calibration + if self.calibration and motor_name in self.calibration: + calibration_subset[motor_name] = self.calibration[motor_name] + + # Create bus + self.buses[bus_name] = FeetechMotorsBus( + port=port, + motors=bus_motors, + calibration=calibration_subset, + ) + + self.cameras = make_cameras_from_configs(self.config.cameras) + + @property + def _state_ft(self) -> dict[str, type]: + keys = [] + + # Add position keys for arms and head + for role in ["left_arm", "right_arm", "head"]: + for motor_name in self.role_to_motors.get(role, []): + keys.append(f"{motor_name}.pos") + + # Add velocity keys for base + if self.role_to_motors.get("base"): + keys.extend(["x.vel", "y.vel", "theta.vel"]) + + return dict.fromkeys(tuple(keys), float) + + @property + def _cameras_ft(self) -> dict[str, tuple]: + return { + cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras + } + + @cached_property + def observation_features(self) -> dict[str, type | tuple]: + return {**self._state_ft, **self._cameras_ft} + + @cached_property + def action_features(self) -> dict[str, type]: + return self._state_ft + + @property + def is_connected(self) -> bool: + return (all(bus.is_connected for bus in self.buses.values()) and all(cam.is_connected for cam in self.cameras.values())) + + def connect(self, calibrate: bool = True) -> None: + if self.is_connected: + raise DeviceAlreadyConnectedError(f"{self} already connected") + + handshake = self.config.verify_motors_on_connect + for bus in self.buses.values(): + bus.connect(handshake=handshake) + + if not self.is_calibrated and calibrate: + self.calibrate() + + for cam in self.cameras.values(): + cam.connect() + + self.configure() + logger.info(f"{self} connected.") + + @property + def is_calibrated(self) -> bool: + return all(bus.is_calibrated for bus in self.buses.values()) + + def calibrate(self) -> None: + logger.info(f"\nRunning calibration of {self}") + + all_calibrations = {} + + # Calibrate each role separately + for role in ["left_arm", "right_arm", "head", "base"]: + role_motors = self.role_to_motors.get(role, []) + if not role_motors: + continue + + # Get the bus for the first motor of this role + first_motor = role_motors[0] + bus_name = self.motor_to_bus[first_motor] + bus = self.buses[bus_name] + + if role == "base": + # Base wheels: simple calibration (full rotation) + calibration_role = {} + for motor_name in role_motors: + motor = bus.motors[motor_name] + calibration_role[motor_name] = MotorCalibration( + id=motor.id, + drive_mode=0, + homing_offset=0, + range_min=0, + range_max=4095, + ) + bus.write_calibration(calibration_role) + all_calibrations.update(calibration_role) + else: + # Arms and head: full calibration process + bus.disable_torque() + for motor_name in role_motors: + bus.write("Operating_Mode", motor_name, OperatingMode.POSITION.value) + + input(f"Move {role} motors to the middle of their range of motion and press ENTER....") + homing_offsets = bus.set_half_turn_homings(role_motors) + + print(f"Move all {role} joints sequentially through their " + "entire ranges of motion.\nRecording positions. Press ENTER to stop...") + range_mins, range_maxes = bus.record_ranges_of_motion(role_motors) + + calibration_role = {} + for motor_name in role_motors: + motor = bus.motors[motor_name] + calibration_role[motor_name] = MotorCalibration( + id=motor.id, + drive_mode=0, + homing_offset=homing_offsets[motor_name], + range_min=range_mins[motor_name], + range_max=range_maxes[motor_name], + ) + bus.write_calibration(calibration_role) + all_calibrations.update(calibration_role) + + # Combine all calibrations and save + self.calibration = all_calibrations + self._save_calibration() + print("Calibration saved to", self.calibration_fpath) + + + def configure(self): + # Set-up all actuators + # We assume that at connection time, robot is in a rest position, + # and torque can be safely disabled to run configuration + for bus in self.buses.values(): + bus.disable_torque() + bus.configure_motors() + + # Configure each motor based on its role and definition + for motor_name, motor_def in ROBOT_PARTS.items(): + if motor_name not in self.motor_to_bus: + continue + + bus_name = self.motor_to_bus[motor_name] + bus = self.buses[bus_name] + + # Set operating mode based on motor definition + if motor_def.operating_mode == "position": + bus.write("Operating_Mode", motor_name, OperatingMode.POSITION.value) + # Set PID for position mode + bus.write("P_Coefficient", motor_name, motor_def.pid_p) + bus.write("I_Coefficient", motor_name, motor_def.pid_i) + bus.write("D_Coefficient", motor_name, motor_def.pid_d) + elif motor_def.operating_mode == "velocity": + bus.write("Operating_Mode", motor_name, OperatingMode.VELOCITY.value) + + # Enable torque on all buses + for bus in self.buses.values(): + bus.enable_torque() + + + def setup_motors(self) -> None: + # Setup motors grouped by bus + for bus_name, bus in self.buses.items(): + print(f"\n=== Setting up {bus_name} ===") + bus_motors = list(bus.motors.keys()) + for motor in reversed(bus_motors): + input(f"Connect the controller board to the '{motor}' motor only and press enter.") + bus.setup_motor(motor) + print(f"'{motor}' motor id set to {bus.motors[motor].id}") + + + @staticmethod + def _degps_to_raw(degps: float) -> int: + steps_per_deg = 4096.0 / 360.0 + 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 # 32767 -> maximum positive value + elif speed_int < -0x8000: + speed_int = -0x8000 # -32768 -> minimum negative value + return speed_int + + @staticmethod + def _raw_to_degps(raw_speed: int) -> float: + steps_per_deg = 4096.0 / 360.0 + magnitude = raw_speed + degps = magnitude / steps_per_deg + return degps + + def _body_to_wheel_raw( + self, + x: float, + y: float, + theta: 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: + {"base_left_wheel": value, "base_back_wheel": value, "base_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 * (np.pi / 180.0) + # Create the body velocity vector [x, y, theta_rad]. + velocity_vector = np.array([x, y, theta_rad]) + + # Define the wheel mounting angles with a -90° offset. + angles = np.radians(np.array([240, 0, 120]) - 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 = [self._degps_to_raw(deg) for deg in wheel_degps] + + return { + "base_left_wheel": wheel_raw[0], + "base_back_wheel": wheel_raw[1], + "base_right_wheel": wheel_raw[2], + } + + def _wheel_raw_to_body( + self, + left_wheel_speed, + back_wheel_speed, + right_wheel_speed, + wheel_radius: float = 0.05, + base_radius: float = 0.125, + ) -> dict[str, Any]: + """ + Convert wheel raw command feedback back into body-frame velocities. + + Parameters: + wheel_raw : Vector with raw wheel commands ("base_left_wheel", "base_back_wheel", "base_right_wheel"). + wheel_radius: Radius of each wheel (meters). + base_radius : Distance from the robot center to each wheel (meters). + + Returns: + A dict (x.vel, y.vel, theta.vel) all in m/s + """ + + # Convert each raw command back to an angular speed in deg/s. + wheel_degps = np.array( + [ + self._raw_to_degps(left_wheel_speed), + self._raw_to_degps(back_wheel_speed), + self._raw_to_degps(right_wheel_speed), + ] + ) + + # 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, 0, 120]) - 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, y, theta_rad = velocity_vector + theta = theta_rad * (180.0 / np.pi) + return { + "x.vel": x, + "y.vel": y, + "theta.vel": theta, + } # m/s and deg/s + + def _from_keyboard_to_base_action(self, pressed_keys: np.ndarray): + return self.base_keyboard.compute_action(pressed_keys) + + def get_observation(self) -> dict[str, Any]: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + # Read actuators position for arms and head, velocity for base + start = time.perf_counter() + + # Group motors by bus for efficient reading + bus_reads = {} + for role in ["left_arm", "right_arm", "head", "base"]: + role_motors = self.role_to_motors.get(role, []) + if not role_motors: + continue + + # Group motors by bus + for motor_name in role_motors: + bus_name = self.motor_to_bus[motor_name] + if bus_name not in bus_reads: + bus_reads[bus_name] = {"position": [], "velocity": []} + + if role == "base": + bus_reads[bus_name]["velocity"].append(motor_name) + else: + bus_reads[bus_name]["position"].append(motor_name) + + # Read from each bus + all_positions = {} + all_velocities = {} + for bus_name, motor_lists in bus_reads.items(): + bus = self.buses[bus_name] + if motor_lists["position"]: + positions = bus.sync_read("Present_Position", motor_lists["position"]) + all_positions.update(positions) + if motor_lists["velocity"]: + velocities = bus.sync_read("Present_Velocity", motor_lists["velocity"]) + all_velocities.update(velocities) + + # Convert base wheel velocities to body frame + base_vel = {} + base_motors = self.role_to_motors.get("base", []) + if base_motors and all_velocities: + base_vel = self._wheel_raw_to_body( + all_velocities.get("base_left_wheel", 0), + all_velocities.get("base_back_wheel", 0), + all_velocities.get("base_right_wheel", 0), + ) + + # Build observation dict + obs_dict = {} + for motor_name, pos in all_positions.items(): + obs_dict[f"{motor_name}.pos"] = pos + obs_dict.update(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(): + start = time.perf_counter() + obs_dict[cam_key] = cam.async_read() + 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: dict[str, Any]) -> dict[str, Any]: + """Command xlerobot 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. + + 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(f"{self} is not connected.") + + action = self.base_keyboard.augment_action(action) + + # Parse action by role + role_actions = {"left_arm": {}, "right_arm": {}, "head": {}, "base": {}} + base_goal_vel = {} + + for key, value in action.items(): + if key.endswith(".pos"): + motor_name = key.replace(".pos", "") + for role in ["left_arm", "right_arm", "head"]: + if motor_name in self.role_to_motors.get(role, []): + role_actions[role][key] = value + break + elif key.endswith(".vel"): + base_goal_vel[key] = value + + # Convert base velocities to wheel commands + base_wheel_goal_vel = {} + if base_goal_vel: + base_wheel_goal_vel = self._body_to_wheel_raw( + base_goal_vel.get("x.vel", 0.0), + base_goal_vel.get("y.vel", 0.0), + base_goal_vel.get("theta.vel", 0.0), + ) + + # Safety clamping if configured + if self.config.max_relative_target is not None: + # Read present positions for all position-controlled motors + present_pos = {} + for role in ["left_arm", "right_arm", "head"]: + role_motors = self.role_to_motors.get(role, []) + if not role_motors: + continue + + # Get bus for first motor of this role + first_motor = role_motors[0] + bus_name = self.motor_to_bus[first_motor] + bus = self.buses[bus_name] + + # Read positions for all motors of this role + positions = bus.sync_read("Present_Position", role_motors) + present_pos.update(positions) + + # Apply safety clamping + goal_present_pos = {} + for role in ["left_arm", "right_arm", "head"]: + for key, value in role_actions[role].items(): + motor_name = key.replace(".pos", "") + if motor_name in present_pos: + goal_present_pos[key] = (value, present_pos[motor_name]) + + if goal_present_pos: + safe_goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) + # Update role actions with safe positions + for key, safe_value in safe_goal_pos.items(): + motor_name = key.replace(".pos", "") + for role in ["left_arm", "right_arm", "head"]: + if motor_name in self.role_to_motors.get(role, []): + role_actions[role][key] = safe_value + break + + # Group actions by bus for efficient writing + bus_writes = {} + for role in ["left_arm", "right_arm", "head"]: + role_motors = self.role_to_motors.get(role, []) + if not role_motors: + continue + + # Group by bus + for key, value in role_actions[role].items(): + motor_name = key.replace(".pos", "") + bus_name = self.motor_to_bus[motor_name] + if bus_name not in bus_writes: + bus_writes[bus_name] = {} + bus_writes[bus_name][motor_name] = value + + # Add base wheel commands + if base_wheel_goal_vel: + base_motors = self.role_to_motors.get("base", []) + for motor_name in base_motors: + bus_name = self.motor_to_bus[motor_name] + if bus_name not in bus_writes: + bus_writes[bus_name] = {} + bus_writes[bus_name][motor_name] = base_wheel_goal_vel.get(motor_name, 0) + + # Write to buses + base_motors = self.role_to_motors.get("base", []) + for bus_name, commands in bus_writes.items(): + bus = self.buses[bus_name] + if any(motor in base_motors for motor in commands.keys()): + # Base motors use velocity commands + bus.sync_write("Goal_Velocity", commands) + else: + # Other motors use position commands + bus.sync_write("Goal_Position", commands) + + # Return the action that was sent + result = {} + for role_actions_dict in role_actions.values(): + result.update(role_actions_dict) + result.update(base_goal_vel) + return result + + def stop_base(self): + base_motors = self.role_to_motors.get("base", []) + if base_motors: + # Find the bus for base motors + first_base_motor = base_motors[0] + bus_name = self.motor_to_bus[first_base_motor] + bus = self.buses[bus_name] + bus.sync_write("Goal_Velocity", dict.fromkeys(base_motors, 0), num_retry=5) + logger.info("Base motors stopped") + + def disconnect(self): + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + self.stop_base() + for bus in self.buses.values(): + bus.disconnect(self.config.disable_torque_on_disconnect) + for cam in self.cameras.values(): + cam.disconnect() + + logger.info(f"{self} disconnected.") diff --git a/src/lerobot/robots/xlerobot/xlerobot_base_keyboard.py b/src/lerobot/robots/xlerobot/xlerobot_base_keyboard.py new file mode 100644 index 0000000000..742aec431b --- /dev/null +++ b/src/lerobot/robots/xlerobot/xlerobot_base_keyboard.py @@ -0,0 +1,103 @@ +#!/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. + +from __future__ import annotations + +from collections.abc import Iterable, Mapping +from typing import Any + + +class BaseKeyboardController: + """Shared helper to convert pressed keys into XLerobot base actions.""" + + def __init__( + self, + teleop_keys: Mapping[str, str], + speed_levels: list[dict[str, float]] | None = None, + ) -> None: + self.teleop_keys = teleop_keys + self.speed_levels = speed_levels or [ + {"xy": 0.1, "theta": 30}, # slow + {"xy": 0.2, "theta": 60}, # medium + {"xy": 0.3, "theta": 90}, # fast + ] + self.speed_index = 0 + + def compute_action(self, pressed_keys: Iterable[str]) -> dict[str, float]: + """Return {'x.vel','y.vel','theta.vel'} based on the pressed key set.""" + keys = {str(key) for key in pressed_keys} + + if self.teleop_keys["speed_up"] in keys: + self.speed_index = min(self.speed_index + 1, len(self.speed_levels) - 1) + if self.teleop_keys["speed_down"] in keys: + self.speed_index = max(self.speed_index - 1, 0) + + speed = self.speed_levels[self.speed_index] + xy_speed = speed["xy"] + theta_speed = speed["theta"] + + x_cmd = 0.0 + y_cmd = 0.0 + theta_cmd = 0.0 + + if self.teleop_keys["forward"] in keys: + x_cmd += xy_speed + if self.teleop_keys["backward"] in keys: + x_cmd -= xy_speed + if self.teleop_keys["left"] in keys: + y_cmd += xy_speed + if self.teleop_keys["right"] in keys: + y_cmd -= xy_speed + if self.teleop_keys["rotate_left"] in keys: + theta_cmd += theta_speed + if self.teleop_keys["rotate_right"] in keys: + theta_cmd -= theta_speed + + return { + "x.vel": x_cmd, + "y.vel": y_cmd, + "theta.vel": theta_cmd, + } + + def augment_action(self, action: Mapping[str, Any] | None) -> dict[str, Any]: + """Merge keyboard presses from ``action`` into base velocity commands.""" + if action is None: + return {} + + if not isinstance(action, Mapping): + return action + + action_dict = dict(action.items()) + + teleop_values = set(self.teleop_keys.values()) + pressed_keys = { + key + for key, value in action_dict.items() + if isinstance(key, str) and key in teleop_values and value is None + } + + if not pressed_keys: + return action_dict + + for key in pressed_keys: + action_dict.pop(key, None) + + base_velocities = self.compute_action(pressed_keys) + + for key, value in base_velocities.items(): + action_dict.setdefault(key, value) + + return action_dict diff --git a/src/lerobot/robots/xlerobot/xlerobot_client.py b/src/lerobot/robots/xlerobot/xlerobot_client.py new file mode 100644 index 0000000000..cdeb38b3b2 --- /dev/null +++ b/src/lerobot/robots/xlerobot/xlerobot_client.py @@ -0,0 +1,318 @@ +# 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. + +# TODO(aliberts, Steven, Pepijn): use gRPC calls instead of zmq? + +import base64 +import json +import logging +from functools import cached_property +from typing import Any, Dict, Optional, Tuple + +import cv2 +import numpy as np +import zmq + +from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError + +from ..robot import Robot +from .xlerobot_base_keyboard import BaseKeyboardController +from .xlerobot_config import XLerobotConfig, XLerobotClientConfig + + +class XLerobotClient(Robot): + config_class = XLerobotClientConfig + name = "xlerobot_client" + + def __init__(self, config: XLerobotClientConfig): + super().__init__(config) + self.config = config + self.id = config.id + self.robot_type = config.type + + 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.base_keyboard = BaseKeyboardController(self.teleop_keys) + + self.polling_timeout_ms = config.polling_timeout_ms + self.connect_timeout_s = config.connect_timeout_s + + self.zmq_context = None + self.zmq_cmd_socket = None + self.zmq_observation_socket = None + + self.last_frames = {} + + self.last_remote_state = {} + + self._is_connected = False + self.logs = {} + + @cached_property + def _state_ft(self) -> dict[str, type]: + return dict.fromkeys( + ( + "left_arm_shoulder_pan.pos", + "left_arm_shoulder_lift.pos", + "left_arm_elbow_flex.pos", + "left_arm_wrist_flex.pos", + "left_arm_wrist_roll.pos", + "left_arm_gripper.pos", + "right_arm_shoulder_pan.pos", + "right_arm_shoulder_lift.pos", + "right_arm_elbow_flex.pos", + "right_arm_wrist_flex.pos", + "right_arm_wrist_roll.pos", + "right_arm_gripper.pos", + "head_motor_1.pos", + "head_motor_2.pos", + "x.vel", + "y.vel", + "theta.vel", + ), + float, + ) + + @cached_property + def _state_order(self) -> tuple[str, ...]: + return tuple(self._state_ft.keys()) + + @cached_property + def _cameras_ft(self) -> dict[str, tuple[int | None, int | None, int]]: + return {name: (cfg.height, cfg.width, 3) for name, cfg in self.config.cameras.items()} + + @cached_property + def observation_features(self) -> dict[str, type | tuple]: + return {**self._state_ft, **self._cameras_ft} + + @cached_property + def action_features(self) -> dict[str, type]: + return self._state_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: + raise DeviceAlreadyConnectedError( + "XLerobot client 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) + + poller = zmq.Poller() + poller.register(self.zmq_observation_socket, zmq.POLLIN) + socks = dict(poller.poll(self.connect_timeout_s * 1000)) + if self.zmq_observation_socket not in socks or socks[self.zmq_observation_socket] != zmq.POLLIN: + raise DeviceNotConnectedError("Timeout waiting for XLerobot Host to connect expired.") + + self._is_connected = True + + def calibrate(self) -> None: + pass + + def _poll_and_get_latest_message(self) -> Optional[str]: + """Polls the ZMQ socket for a limited time and returns the latest message string.""" + poller = zmq.Poller() + poller.register(self.zmq_observation_socket, zmq.POLLIN) + + try: + socks = dict(poller.poll(self.polling_timeout_ms)) + except zmq.ZMQError as e: + logging.error(f"ZMQ polling error: {e}") + return None + + if self.zmq_observation_socket not in socks: + logging.info("No new data available within timeout.") + return None + + last_msg = None + while True: + try: + msg = self.zmq_observation_socket.recv_string(zmq.NOBLOCK) + last_msg = msg + except zmq.Again: + break + + if last_msg is None: + logging.warning("Poller indicated data, but failed to retrieve message.") + + return last_msg + + def _parse_observation_json(self, obs_string: str) -> Optional[Dict[str, Any]]: + """Parses the JSON observation string.""" + try: + return json.loads(obs_string) + except json.JSONDecodeError as e: + logging.error(f"Error decoding JSON observation: {e}") + return None + + def _decode_image_from_b64(self, image_b64: str) -> Optional[np.ndarray]: + """Decodes a base64 encoded image string to an OpenCV image.""" + if not image_b64: + return None + try: + jpg_data = base64.b64decode(image_b64) + np_arr = np.frombuffer(jpg_data, dtype=np.uint8) + frame = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + if frame is None: + logging.warning("cv2.imdecode returned None for an image.") + return frame + except (TypeError, ValueError) as e: + logging.error(f"Error decoding base64 image data: {e}") + return None + + def _remote_state_from_obs( + self, observation: Dict[str, Any] + ) -> Tuple[Dict[str, np.ndarray], Dict[str, Any]]: + """Extracts frames, and state from the parsed observation.""" + + flat_state = {key: observation.get(key, 0.0) for key in self._state_order} + + state_vec = np.array([flat_state[key] for key in self._state_order], dtype=np.float32) + + obs_dict: Dict[str, Any] = {**flat_state, "observation.state": state_vec} + + # Decode images + current_frames: Dict[str, np.ndarray] = {} + for cam_name, image_b64 in observation.items(): + if cam_name not in self._cameras_ft: + continue + frame = self._decode_image_from_b64(image_b64) + if frame is not None: + current_frames[cam_name] = frame + + return current_frames, obs_dict + + def _get_data(self) -> Tuple[Dict[str, np.ndarray], Dict[str, Any], Dict[str, Any]]: + """ + Polls the video socket for the latest observation data. + + Attempts to retrieve and decode the latest message within a short timeout. + If successful, updates and returns the new frames, speed, and arm state. + If no new data arrives or decoding fails, returns the last known values. + """ + + # 1. Get the latest message string from the socket + latest_message_str = self._poll_and_get_latest_message() + + # 2. If no message, return cached data + if latest_message_str is None: + return self.last_frames, self.last_remote_state + + # 3. Parse the JSON message + observation = self._parse_observation_json(latest_message_str) + + # 4. If JSON parsing failed, return cached data + if observation is None: + return self.last_frames, self.last_remote_state + + # 5. Process the valid observation data + try: + new_frames, new_state = self._remote_state_from_obs(observation) + except Exception as e: + logging.error(f"Error processing observation data, serving last observation: {e}") + return self.last_frames, self.last_remote_state + + self.last_frames = new_frames + self.last_remote_state = new_state + + return new_frames, new_state + + 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: + raise DeviceNotConnectedError("XLerobotClient is not connected. You need to run `robot.connect()`.") + + frames, obs_dict = self._get_data() + + # Loop over each configured camera + for cam_name, frame in frames.items(): + if frame is None: + logging.warning("Frame is None") + frame = np.zeros((640, 480, 3), dtype=np.uint8) + obs_dict[cam_name] = frame + + return obs_dict + + def _from_keyboard_to_base_action(self, pressed_keys: np.ndarray): + return self.base_keyboard.compute_action(pressed_keys) + + def configure(self): + pass + + def send_action(self, action: dict[str, Any]) -> dict[str, Any]: + """Command xlerobot 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. + + 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()`" + ) + + action = self.base_keyboard.augment_action(action) + + logging.debug("XLerobotClient.send_action keys: %s", list(action.keys())) + + self.zmq_cmd_socket.send_string(json.dumps(action)) # action is in motor space + + # TODO(Steven): Remove the np conversion when it is possible to record a non-numpy array value + actions = np.array([action.get(k, 0.0) for k in self._state_order], dtype=np.float32) + + action_sent = {key: actions[i] for i, key in enumerate(self._state_order)} + action_sent["action"] = actions + return action_sent + + def disconnect(self): + """Cleans ZMQ comms""" + + if not self._is_connected: + raise DeviceNotConnectedError( + "XLerobot client is not connected. You need to run `robot.connect()` before disconnecting." + ) + self.zmq_observation_socket.close() + self.zmq_cmd_socket.close() + self.zmq_context.term() + self._is_connected = False diff --git a/src/lerobot/robots/xlerobot/xlerobot_config.py b/src/lerobot/robots/xlerobot/xlerobot_config.py new file mode 100644 index 0000000000..cb5d570d09 --- /dev/null +++ b/src/lerobot/robots/xlerobot/xlerobot_config.py @@ -0,0 +1,156 @@ +# 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.cameras.configs import CameraConfig, Cv2Rotation, ColorMode +from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig +from lerobot.cameras.realsense import RealSenseCamera, RealSenseCameraConfig + +from ..config import RobotConfig +from .xlerobot_form_factor import ROBOT_PARTS, BUS_MAPPINGS + + +def xlerobot_cameras_config() -> dict[str, CameraConfig]: + return { + "left_wrist": OpenCVCameraConfig( + index_or_path="/dev/video6", + fps=30, + width=1024, + height=768, + rotation=Cv2Rotation.NO_ROTATION, + fourcc="MJPG", + ), + + "right_wrist": OpenCVCameraConfig( + index_or_path="/dev/video8", + fps=30, + width=1024, + height=768, + rotation=Cv2Rotation.NO_ROTATION, + fourcc="MJPG", + ), + # Pick either the OpenCV or RealSense camera + # "head(RGDB)": OpenCVCameraConfig( + # index_or_path="/dev/video2", fps=30, width=640, height=480, rotation=Cv2Rotation.NO_ROTATION + # ), + "head": RealSenseCameraConfig( + serial_number_or_name="142422251177", # Replace with camera SN + fps=30, + width=640, + height=480, + color_mode=ColorMode.BGR, + rotation=Cv2Rotation.NO_ROTATION, + use_depth=True + ), + } + +teleop_keys_default = { + # Movement + "forward": "i", + "backward": "k", + "left": "j", + "right": "l", + "rotate_left": "u", + "rotate_right": "o", + # Speed control + "speed_up": "n", + "speed_down": "m", + # quit teleop + "quit": "b", + } + +def default_motor_ids() -> dict[str, int | None]: + """ + Provide a configurable mapping for servo IDs. + + Leaving a value as ``None`` tells the robot runtime to fall back to either + the calibration file (if available) or the hardware defaults from + :mod:`robot_hardware`. Override a subset of entries by specifying a value. + """ + return {name: None for name in ROBOT_PARTS} + + +@RobotConfig.register_subclass("xlerobot") +@dataclass +class XLerobotConfig(RobotConfig): + + # Motor configuration layout: "4_bus" or "2_bus" + motor_layout: str = "4_bus" + + # Map of logical bus roles to device ports + # You can bind them to stable names like /dev/left-hand + # 1. Get serian number udevadm info --attribute-walk --name=/dev/ttyACM1 | grep -E 'idVendor|idProduct|serial|DEVPATH' -n + # 2. Create udev rule (example for neck): + # sudo tee -a /etc/udev/rules.d/99-my-tty.rules >/dev/null <<'EOF' + # ATTRS{serial}=="YOUR_SERIAL_HERE", SYMLINK+="neck" + # EOF + ports: dict[str, str | None] = field( + default_factory=lambda: { + "left_arm": "/dev/left-hand", + "right_arm": "/dev/right-hand", + "head": "/dev/neck", + "base": "/dev/wheels", + } + ) + 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=xlerobot_cameras_config) + motor_ids: dict[str, int | None] = field(default_factory=default_motor_ids) + verify_motors_on_connect: bool = True + + # Set to `True` for backward compatibility with previous policies/dataset + use_degrees: bool = False + + teleop_keys: dict[str, str] = field( + default_factory=lambda: teleop_keys_default + ) + + +@dataclass +class XLerobotHostConfig: + # Network Configuration + port_zmq_cmd: int = 5555 + port_zmq_observations: int = 5556 + + # Duration of the application + connection_time_s: int = 3600 + + # Watchdog: stop the robot if no command is received for over 30 seconds. + watchdog_timeout_ms: int = 30000 + + # If robot jitters decrease the frequency and monitor cpu load with `top` in cmd + max_loop_freq_hz: int = 30 + +@RobotConfig.register_subclass("xlerobot_client") +@dataclass +class XLerobotClientConfig(RobotConfig): + # Network Configuration + remote_ip: str + port_zmq_cmd: int = 5555 + port_zmq_observations: int = 5556 + + teleop_keys: dict[str, str] = field( + default_factory=lambda: teleop_keys_default + ) + + cameras: dict[str, CameraConfig] = field(default_factory=xlerobot_cameras_config) + + polling_timeout_ms: int = 15 + connect_timeout_s: int = 5 diff --git a/src/lerobot/robots/xlerobot/xlerobot_form_factor.py b/src/lerobot/robots/xlerobot/xlerobot_form_factor.py new file mode 100644 index 0000000000..a1c063fffe --- /dev/null +++ b/src/lerobot/robots/xlerobot/xlerobot_form_factor.py @@ -0,0 +1,131 @@ +# 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. + +""" +Robot hardware specification - source of truth for all servos and their properties. + +This module defines the complete robot hardware configuration, including all servos, +their roles, operating modes, and control parameters. The hardware definition is +independent of how servos are physically connected to control buses. + +This design allows the robot to run on various numbers of buses: +- 4-bus layout: Each role (left_arm, right_arm, head, base) gets its own bus +- 2-bus layout: Mixed roles on fewer buses (e.g., left_arm+head on bus1, right_arm+base on bus2) +- Custom layouts: Any combination of roles on any number of buses + +The bus mapping is configured in this same module, making the system +flexible for different hardware setups while maintaining the same robot capabilities. +""" + +from dataclasses import dataclass +from typing import Dict, List +from lerobot.motors import MotorNormMode + + +@dataclass +class RobotPart: + """Definition of a single robot servo/motor.""" + id: int + model: str + role: str # "left_arm", "right_arm", "head", "base" + norm_mode: MotorNormMode + operating_mode: str = "position" # "position" or "velocity" + pid_p: int = 16 + pid_i: int = 0 + pid_d: int = 43 + + +# Source of truth: All robot servos defined once +ROBOT_PARTS = { + # Left arm servos + "left_arm_shoulder_pan": RobotPart(1, "sts3215", "left_arm", MotorNormMode.DEGREES), + "left_arm_shoulder_lift": RobotPart(2, "sts3215", "left_arm", MotorNormMode.DEGREES), + "left_arm_elbow_flex": RobotPart(3, "sts3215", "left_arm", MotorNormMode.DEGREES), + "left_arm_wrist_flex": RobotPart(4, "sts3215", "left_arm", MotorNormMode.DEGREES), + "left_arm_wrist_roll": RobotPart(5, "sts3215", "left_arm", MotorNormMode.DEGREES), + "left_arm_gripper": RobotPart(6, "sts3215", "left_arm", MotorNormMode.RANGE_0_100), + + # Right arm servos + "right_arm_shoulder_pan": RobotPart(1, "sts3215", "right_arm", MotorNormMode.DEGREES), + "right_arm_shoulder_lift": RobotPart(2, "sts3215", "right_arm", MotorNormMode.DEGREES), + "right_arm_elbow_flex": RobotPart(3, "sts3215", "right_arm", MotorNormMode.DEGREES), + "right_arm_wrist_flex": RobotPart(4, "sts3215", "right_arm", MotorNormMode.DEGREES), + "right_arm_wrist_roll": RobotPart(5, "sts3215", "right_arm", MotorNormMode.DEGREES), + "right_arm_gripper": RobotPart(6, "sts3215", "right_arm", MotorNormMode.RANGE_0_100), + + # Head servos + "head_motor_1": RobotPart(7, "sts3215", "head", MotorNormMode.DEGREES), + "head_motor_2": RobotPart(8, "sts3215", "head", MotorNormMode.DEGREES), + + # Base servos (wheels) + "base_left_wheel": RobotPart(9, "sts3215", "base", MotorNormMode.RANGE_M100_100, "velocity"), + "base_back_wheel": RobotPart(10, "sts3215", "base", MotorNormMode.RANGE_M100_100, "velocity"), + "base_right_wheel": RobotPart(11, "sts3215", "base", MotorNormMode.RANGE_M100_100, "velocity"), +} + +DEFAULT_IDS_BY_LAYOUT = { + "4_bus": { + # Left arm bus + "left_arm_shoulder_pan": 1, + "left_arm_shoulder_lift": 2, + "left_arm_elbow_flex": 3, + "left_arm_wrist_flex": 4, + "left_arm_wrist_roll": 5, + "left_arm_gripper": 6, + # Right arm bus + "right_arm_shoulder_pan": 1, + "right_arm_shoulder_lift": 2, + "right_arm_elbow_flex": 3, + "right_arm_wrist_flex": 4, + "right_arm_wrist_roll": 5, + "right_arm_gripper": 6, + # Head bus + "head_motor_1": 1, + "head_motor_2": 2, + # Base bus + "base_left_wheel": 1, + "base_back_wheel": 2, + "base_right_wheel": 3, + }, + "2_bus": {name: part.id for name, part in ROBOT_PARTS.items()}, +} + + +@dataclass +class BusMapping: + """Defines which robot parts go on which bus.""" + bus_name: str + port_key: str # key in config.ports dict + motor_patterns: List[str] # patterns like ["left_arm_*", "head_*"] + + +# 4-bus mapping: each role gets its own bus +BUS_MAPPING_4_BUS = { + "left_arm_bus": BusMapping("left_arm_bus", "left_arm", ["left_arm_*"]), + "right_arm_bus": BusMapping("right_arm_bus", "right_arm", ["right_arm_*"]), + "head_bus": BusMapping("head_bus", "head", ["head_*"]), + "base_bus": BusMapping("base_bus", "base", ["base_*"]), +} + +# 2-bus mapping: mixed roles on fewer buses +BUS_MAPPING_2_BUS = { + "board_A": BusMapping("board_A", "left_arm", ["left_arm_*", "head_*"]), + "board_B": BusMapping("board_B", "right_arm", ["right_arm_*", "base_*"]), +} + +# Available bus mappings +BUS_MAPPINGS = { + "4_bus": BUS_MAPPING_4_BUS, + "2_bus": BUS_MAPPING_2_BUS, +} diff --git a/src/lerobot/robots/xlerobot/xlerobot_host.py b/src/lerobot/robots/xlerobot/xlerobot_host.py new file mode 100644 index 0000000000..50345ebe6a --- /dev/null +++ b/src/lerobot/robots/xlerobot/xlerobot_host.py @@ -0,0 +1,127 @@ +#!/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 base64 +import json +import logging +import time + +import cv2 +import zmq + +from .xlerobot import XLerobot +from .xlerobot_config import XLerobotConfig, XLerobotHostConfig + + +class XLerobotHost: + def __init__(self, config: XLerobotHostConfig): + 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://*:{config.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://*:{config.port_zmq_observations}") + + self.connection_time_s = config.connection_time_s + self.watchdog_timeout_ms = config.watchdog_timeout_ms + self.max_loop_freq_hz = config.max_loop_freq_hz + + def disconnect(self): + self.zmq_observation_socket.close() + self.zmq_cmd_socket.close() + self.zmq_context.term() + + +def main(): + logging.info("Configuring Xlerobot") + robot_config = XLerobotConfig(id="my_xlerobot_pc") + robot = XLerobot(robot_config) + + logging.info("Connecting Xlerobot") + robot.connect() + + logging.info("Starting HostAgent") + host_config = XLerobotHostConfig() + host = XLerobotHost(host_config) + + last_cmd_time = time.time() + watchdog_active = False + logging.info("Waiting for commands...") + try: + # Loop + start = time.perf_counter() + duration = 0 + while duration < host.connection_time_s: + loop_start_time = time.time() + try: + msg = host.zmq_cmd_socket.recv_string(zmq.NOBLOCK) + data = dict(json.loads(msg)) + _action_sent = robot.send_action(data) + last_cmd_time = time.time() + watchdog_active = False + except zmq.Again: + if not watchdog_active: + logging.warning("No command available") + except Exception as e: + logging.error("Message fetching failed: %s", e) + + now = time.time() + if (now - last_cmd_time > host.watchdog_timeout_ms / 1000) and not watchdog_active: + logging.warning( + f"Command not received for more than {host.watchdog_timeout_ms} milliseconds. Stopping the base." + ) + watchdog_active = True + robot.stop_base() + + last_observation = robot.get_observation() + + # Encode ndarrays to base64 strings + for cam_key, _ in robot.cameras.items(): + ret, buffer = cv2.imencode( + ".jpg", last_observation[cam_key], [int(cv2.IMWRITE_JPEG_QUALITY), 90] + ) + if ret: + last_observation[cam_key] = base64.b64encode(buffer).decode("utf-8") + else: + last_observation[cam_key] = "" + + # Send the observation to the remote agent + try: + host.zmq_observation_socket.send_string(json.dumps(last_observation), flags=zmq.NOBLOCK) + except zmq.Again: + logging.info("Dropping observation, no client connected") + + # Ensure a short sleep to avoid overloading the CPU. + elapsed = time.time() - loop_start_time + + time.sleep(max(1 / host.max_loop_freq_hz - elapsed, 0)) + duration = time.perf_counter() - start + print("Cycle time reached.") + + except KeyboardInterrupt: + print("Keyboard interrupt received. Exiting...") + finally: + print("Shutting down XLerobot Host.") + robot.disconnect() + host.disconnect() + + logging.info("Finished XLerobot host cleanly") + + +if __name__ == "__main__": + main() diff --git a/src/lerobot/scripts/lerobot_calibrate.py b/src/lerobot/scripts/lerobot_calibrate.py index 0f247caefe..15ed80791f 100644 --- a/src/lerobot/scripts/lerobot_calibrate.py +++ b/src/lerobot/scripts/lerobot_calibrate.py @@ -42,6 +42,7 @@ make_robot_from_config, so100_follower, so101_follower, + xlerobot, ) from lerobot.teleoperators import ( # noqa: F401 Teleoperator, diff --git a/src/lerobot/scripts/lerobot_find_joint_limits.py b/src/lerobot/scripts/lerobot_find_joint_limits.py index 07d57a7608..3581c87cba 100644 --- a/src/lerobot/scripts/lerobot_find_joint_limits.py +++ b/src/lerobot/scripts/lerobot_find_joint_limits.py @@ -42,6 +42,7 @@ koch_follower, make_robot_from_config, so100_follower, + xlerobot, ) from lerobot.teleoperators import ( # noqa: F401 TeleoperatorConfig, diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index 6df92d893b..d9617d6983 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -98,6 +98,7 @@ make_robot_from_config, so100_follower, so101_follower, + xlerobot, ) from lerobot.teleoperators import ( # noqa: F401 Teleoperator, diff --git a/src/lerobot/scripts/lerobot_replay.py b/src/lerobot/scripts/lerobot_replay.py index ffd7b2b22a..3f1a5a4472 100644 --- a/src/lerobot/scripts/lerobot_replay.py +++ b/src/lerobot/scripts/lerobot_replay.py @@ -59,6 +59,7 @@ make_robot_from_config, so100_follower, so101_follower, + xlerobot, ) from lerobot.utils.constants import ACTION from lerobot.utils.import_utils import register_third_party_devices diff --git a/src/lerobot/scripts/lerobot_setup_motors.py b/src/lerobot/scripts/lerobot_setup_motors.py index c1d256c211..fa777f4cc8 100644 --- a/src/lerobot/scripts/lerobot_setup_motors.py +++ b/src/lerobot/scripts/lerobot_setup_motors.py @@ -35,6 +35,7 @@ make_robot_from_config, so100_follower, so101_follower, + xlerobot, ) from lerobot.teleoperators import ( # noqa: F401 TeleoperatorConfig, diff --git a/src/lerobot/scripts/lerobot_teleoperate.py b/src/lerobot/scripts/lerobot_teleoperate.py index 0a418f3bca..8772dd98a8 100644 --- a/src/lerobot/scripts/lerobot_teleoperate.py +++ b/src/lerobot/scripts/lerobot_teleoperate.py @@ -76,6 +76,7 @@ make_robot_from_config, so100_follower, so101_follower, + xlerobot, ) from lerobot.teleoperators import ( # noqa: F401 Teleoperator, @@ -83,6 +84,7 @@ bi_so100_leader, gamepad, homunculus, + keyboard, koch_leader, make_teleoperator_from_config, so100_leader,