mirror of
https://github.com/huggingface/lerobot.git
synced 2026-05-11 14:49:43 +00:00
feat(robots): add earth rover robot support (#2575)
Co-authored-by: somthecoder <sbaner64@gmail.com> Co-authored-by: randomSmarts <Aarshsmittal@gmail.com> Co-authored-by: Hassoonu <halsae2@illinois.edu> Co-authored-by: Saketh06 <saketh.kantipudi@gmail.com> Co-authored-by: sairajshetye <sairajshetye2@gmail.com> Co-authored-by: Khalil Meftah <kmeftah.khalil@gmail.com>
This commit is contained in:
@@ -85,6 +85,8 @@
|
||||
title: Reachy 2
|
||||
- local: unitree_g1
|
||||
title: Unitree G1
|
||||
- local: earthrover_mini_plus
|
||||
title: Earth Rover Mini
|
||||
title: "Robots"
|
||||
- sections:
|
||||
- local: phone_teleop
|
||||
|
||||
@@ -0,0 +1,206 @@
|
||||
# EarthRover Mini Plus
|
||||
|
||||
The EarthRover Mini Plus is a fully open source mobile robot that connects through the cloud using the Frodobots SDK. This lets you control the robot and record datasets for training AI models.
|
||||
|
||||
## What You Need
|
||||
|
||||
### Hardware
|
||||
|
||||
- EarthRover Mini robot
|
||||
- Computer with Python 3.10 or newer
|
||||
- Internet connection
|
||||
|
||||
### Setting Up the Frodobots SDK
|
||||
|
||||
The robot needs the [Frodobots SDK](https://github.com/Frodobots/earth-rovers-sdk) running on your computer. Here's how:
|
||||
|
||||
1. Download and install the SDK:
|
||||
|
||||
```bash
|
||||
git clone https://github.com/Frodobots/earth-rovers-sdk.git
|
||||
cd earth-rovers-sdk
|
||||
pip install -r requirements.txt
|
||||
```
|
||||
|
||||
2. Start the SDK:
|
||||
|
||||
```bash
|
||||
hypercorn main:app --reload
|
||||
```
|
||||
|
||||
3. Open your web browser and go to `http://localhost:8000`, then click "Join"
|
||||
|
||||
The SDK gives you:
|
||||
|
||||
- Live video from front and rear cameras
|
||||
|
||||
> [!IMPORTANT]
|
||||
> The SDK must be running before you can use the robot.
|
||||
|
||||
## Install LeRobot
|
||||
|
||||
Follow our [Installation Guide](./installation) to install LeRobot.
|
||||
|
||||
In addition to the base installation, install the EarthRover Mini dependencies:
|
||||
|
||||
```bash
|
||||
pip install -e .
|
||||
```
|
||||
|
||||
## How It Works
|
||||
|
||||
The robot uses the internet to communicate:
|
||||
|
||||
- **Movement commands**: Sent through the SDK
|
||||
- **Camera video**: Received from the SDK
|
||||
- **Robot info**: Battery, location, speed from the SDK
|
||||
|
||||
You don't need to plug anything in - it all works through the SDK.
|
||||
|
||||
## Calibration
|
||||
|
||||
No calibration needed! The robot is ready to use as soon as the SDK is running.
|
||||
|
||||
## Controlling the Robot
|
||||
|
||||
You control the robot using your keyboard - just like playing a video game with WASD keys.
|
||||
|
||||
### Keyboard Controls
|
||||
|
||||
| Key | Action |
|
||||
| --- | -------------------------------- |
|
||||
| W | Move forward |
|
||||
| S | Move backward |
|
||||
| A | Turn left (with forward motion) |
|
||||
| D | Turn right (with forward motion) |
|
||||
| Q | Rotate left in place |
|
||||
| E | Rotate right in place |
|
||||
| X | Stop all movement |
|
||||
| +/= | Increase speed |
|
||||
| - | Decrease speed |
|
||||
| ESC | Disconnect |
|
||||
|
||||
### Speed Settings
|
||||
|
||||
You can adjust how fast the robot moves:
|
||||
|
||||
- **Forward/backward speed**: Default is full speed (1.0)
|
||||
- **Turning speed**: Default is full speed (1.0)
|
||||
- **Speed changes**: Use +/- keys to adjust by 0.1 each time
|
||||
|
||||
### Try It Out
|
||||
|
||||
Test driving the robot before recording data:
|
||||
|
||||
```python
|
||||
from lerobot.robots.earthrover_mini_plus import EarthRoverMiniPlus, EarthRoverMiniPlusConfig
|
||||
from lerobot.teleoperators.keyboard import KeyboardRoverTeleop, KeyboardRoverTeleopConfig
|
||||
|
||||
# Initialize robot
|
||||
robot_config = EarthRoverMiniPlusConfig()
|
||||
robot = EarthRoverMiniPlus(robot_config)
|
||||
|
||||
# Initialize teleoperator
|
||||
teleop_config = KeyboardRoverTeleopConfig(
|
||||
linear_speed=1.0,
|
||||
angular_speed=1.0,
|
||||
speed_increment=0.1
|
||||
)
|
||||
teleop = KeyboardRoverTeleop(teleop_config)
|
||||
|
||||
# Connect
|
||||
robot.connect()
|
||||
teleop.connect()
|
||||
|
||||
# Teleoperate (use keyboard controls)
|
||||
try:
|
||||
while True:
|
||||
action = teleop.get_action()
|
||||
robot.send_action(action)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
robot.disconnect()
|
||||
teleop.disconnect()
|
||||
```
|
||||
|
||||
> [!TIP]
|
||||
> If you're using a Mac, you might need to give Terminal permission to access your keyboard for teleoperation. Go to System Preferences > Security & Privacy > Input Monitoring and check the box for Terminal.
|
||||
|
||||
## Recording Data
|
||||
|
||||
Once you can drive the robot well, you can start recording data to train AI models. The system records:
|
||||
|
||||
- **What you do**: How you move the robot (forward, backward, turning)
|
||||
- **What the robot sees**:
|
||||
- Videos from both cameras
|
||||
- Robot speed and direction
|
||||
- Battery level and location
|
||||
- GPS position and signal
|
||||
- Other sensor data
|
||||
- **When it happened**: Timestamps for everything
|
||||
|
||||
### Setting Up Hugging Face
|
||||
|
||||
We use Hugging Face to store your data online. First, log in with your token from [Hugging Face settings](https://huggingface.co/settings/tokens):
|
||||
|
||||
```bash
|
||||
huggingface-cli login --token ${HUGGINGFACE_TOKEN} --add-to-git-credential
|
||||
```
|
||||
|
||||
Store your Hugging Face username:
|
||||
|
||||
```bash
|
||||
HF_USER=$(huggingface-cli whoami | head -n 1)
|
||||
echo $HF_USER
|
||||
```
|
||||
|
||||
### Start Recording
|
||||
|
||||
Use the standard recording command:
|
||||
|
||||
```bash
|
||||
python src/lerobot/scripts/lerobot_record.py \
|
||||
--robot.type=earthrover_mini_plus \
|
||||
--teleop.type=keyboard_rover \
|
||||
--dataset.repo_id=your_username/dataset_name \
|
||||
--dataset.num_episodes=2 \
|
||||
--dataset.fps=10 \
|
||||
--dataset.single_task="Navigate around obstacles" \
|
||||
--display_data=true
|
||||
```
|
||||
|
||||
Replace `your_username/dataset_name` with your Hugging Face username and a name for your dataset.
|
||||
|
||||
### What Gets Saved
|
||||
|
||||
Your dataset includes:
|
||||
|
||||
**Your Actions (2 things)**:
|
||||
|
||||
- How much you moved forward/backward
|
||||
- How much you turned left/right
|
||||
|
||||
**Robot Observations (12 things)**:
|
||||
|
||||
- Front camera video
|
||||
- Rear camera video
|
||||
- Current speed
|
||||
- Battery level
|
||||
- Which way the robot is facing
|
||||
- GPS location (latitude, longitude, signal strength)
|
||||
- Network signal strength
|
||||
- Vibration level
|
||||
- Lamp status (on/off)
|
||||
|
||||
### Where Your Data Goes
|
||||
|
||||
On your computer: `~/.cache/huggingface/lerobot/{repo-id}`
|
||||
|
||||
After recording, your data automatically uploads to your Hugging Face page:
|
||||
|
||||
```bash
|
||||
echo https://huggingface.co/datasets/${HF_USER}/earthrover-navigation
|
||||
```
|
||||
|
||||
Your dataset will be tagged with `LeRobot` for community discovery.
|
||||
@@ -0,0 +1,20 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 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 .config_earthrover_mini_plus import EarthRoverMiniPlusConfig
|
||||
from .robot_earthrover_mini_plus import EarthRoverMiniPlus
|
||||
|
||||
__all__ = ["EarthRoverMiniPlus", "EarthRoverMiniPlusConfig"]
|
||||
@@ -0,0 +1,35 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 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.
|
||||
"""Configuration for EarthRover Mini Plus robot."""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
from ..config import RobotConfig
|
||||
|
||||
|
||||
@RobotConfig.register_subclass("earthrover_mini_plus")
|
||||
@dataclass
|
||||
class EarthRoverMiniPlusConfig(RobotConfig):
|
||||
"""Configuration for EarthRover Mini Plus robot using Frodobots SDK.
|
||||
|
||||
This robot uses cloud-based control via the Frodobots SDK HTTP API.
|
||||
Camera frames are accessed directly through SDK HTTP endpoints.
|
||||
|
||||
Attributes:
|
||||
sdk_url: URL of the Frodobots SDK server (default: http://localhost:8000)
|
||||
"""
|
||||
|
||||
sdk_url: str = "http://localhost:8000"
|
||||
@@ -0,0 +1 @@
|
||||
../../../../docs/source/earthrover_mini_plus.mdx
|
||||
@@ -0,0 +1,473 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 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.
|
||||
"""EarthRover Mini Plus robot using Frodobots SDK."""
|
||||
|
||||
import base64
|
||||
import logging
|
||||
from functools import cached_property
|
||||
from typing import Any
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import requests
|
||||
|
||||
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
|
||||
|
||||
from ..robot import Robot
|
||||
from .config_earthrover_mini_plus import EarthRoverMiniPlusConfig
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# Action feature keys
|
||||
ACTION_LINEAR_VEL = "linear.vel"
|
||||
ACTION_ANGULAR_VEL = "angular.vel"
|
||||
|
||||
# Observation feature keys
|
||||
OBS_FRONT = "front"
|
||||
OBS_REAR = "rear"
|
||||
OBS_LINEAR_VEL = "linear.vel"
|
||||
OBS_BATTERY_LEVEL = "battery.level"
|
||||
OBS_ORIENTATION_DEG = "orientation.deg"
|
||||
OBS_GPS_LATITUDE = "gps.latitude"
|
||||
OBS_GPS_LONGITUDE = "gps.longitude"
|
||||
OBS_GPS_SIGNAL = "gps.signal"
|
||||
OBS_SIGNAL_LEVEL = "signal.level"
|
||||
OBS_VIBRATION = "vibration"
|
||||
OBS_LAMP_STATE = "lamp.state"
|
||||
|
||||
|
||||
class EarthRoverMiniPlus(Robot):
|
||||
"""
|
||||
EarthRover Mini Plus robot controlled via Frodobots SDK HTTP API.
|
||||
|
||||
This robot uses cloud-based control through the Frodobots SDK instead of direct
|
||||
hardware connection. Cameras stream via WebRTC through Agora cloud, and control
|
||||
commands are sent via HTTP POST requests.
|
||||
|
||||
The robot supports:
|
||||
- Dual cameras (front and rear) accessed via SDK HTTP endpoints
|
||||
- Linear and angular velocity control
|
||||
- Battery and orientation telemetry
|
||||
|
||||
Attributes:
|
||||
config: Robot configuration
|
||||
sdk_base_url: URL of the Frodobots SDK server (default: http://localhost:8000)
|
||||
"""
|
||||
|
||||
config_class = EarthRoverMiniPlusConfig
|
||||
name = "earthrover_mini_plus"
|
||||
|
||||
def __init__(self, config: EarthRoverMiniPlusConfig):
|
||||
"""Initialize EarthRover Mini Plus robot.
|
||||
|
||||
Args:
|
||||
config: Robot configuration including SDK URL
|
||||
"""
|
||||
super().__init__(config)
|
||||
self.config = config
|
||||
self.sdk_base_url = "http://localhost:8000"
|
||||
|
||||
# Empty cameras dict for compatibility with recording script
|
||||
# Cameras are accessed directly via SDK, not through Camera objects
|
||||
self.cameras = {}
|
||||
self._is_connected = False
|
||||
|
||||
# Cache for camera frames (fallback when requests fail)
|
||||
self._last_front_frame = None
|
||||
self._last_rear_frame = None
|
||||
|
||||
# Cache for robot telemetry data (fallback when requests fail)
|
||||
self._last_robot_data = None
|
||||
|
||||
logger.info(f"Initialized {self.name} with SDK at {self.sdk_base_url}")
|
||||
|
||||
@property
|
||||
def is_connected(self) -> bool:
|
||||
"""Check if robot is connected to SDK."""
|
||||
return self._is_connected
|
||||
|
||||
def connect(self, calibrate: bool = True) -> None:
|
||||
"""Connect to robot via Frodobots SDK.
|
||||
|
||||
Args:
|
||||
calibrate: Not used for SDK-based robot (kept for API compatibility)
|
||||
|
||||
Raises:
|
||||
DeviceAlreadyConnectedError: If robot is already connected
|
||||
DeviceNotConnectedError: If cannot connect to SDK server
|
||||
"""
|
||||
if self._is_connected:
|
||||
raise DeviceAlreadyConnectedError(f"{self.name} is already connected")
|
||||
|
||||
# Verify SDK is running and accessible
|
||||
try:
|
||||
response = requests.get(f"{self.sdk_base_url}/data", timeout=10.0)
|
||||
if response.status_code != 200:
|
||||
raise DeviceNotConnectedError(
|
||||
f"Cannot connect to SDK at {self.sdk_base_url}. "
|
||||
"Make sure it's running: hypercorn main:app --reload"
|
||||
)
|
||||
except requests.RequestException as e:
|
||||
raise DeviceNotConnectedError(f"Cannot connect to SDK at {self.sdk_base_url}: {e}") from e
|
||||
|
||||
self._is_connected = True
|
||||
logger.info(f"{self.name} connected to SDK")
|
||||
|
||||
if calibrate:
|
||||
self.calibrate()
|
||||
|
||||
def calibrate(self) -> None:
|
||||
"""Calibration not needed for SDK-based robot."""
|
||||
logger.info("Calibration not required for SDK-based robot")
|
||||
|
||||
@property
|
||||
def is_calibrated(self) -> bool:
|
||||
"""SDK robot doesn't require calibration.
|
||||
|
||||
Returns:
|
||||
bool: Always True for SDK-based robots
|
||||
"""
|
||||
return True
|
||||
|
||||
def configure(self) -> None:
|
||||
"""Configure robot (no-op for SDK-based robot)."""
|
||||
pass
|
||||
|
||||
@cached_property
|
||||
def observation_features(self) -> dict[str, type | tuple]:
|
||||
"""Define the observation space for dataset recording.
|
||||
|
||||
Returns:
|
||||
dict: Observation features with types/shapes:
|
||||
- front: (480, 640, 3) - Front camera RGB image
|
||||
- rear: (480, 640, 3) - Rear camera RGB image
|
||||
- linear.vel: float - Current speed (0-1, SDK reports only positive speeds)
|
||||
- battery.level: float - Battery level (0-1, normalized from 0-100)
|
||||
- orientation.deg: float - Robot orientation (0-1, normalized from raw value)
|
||||
- gps.latitude: float - GPS latitude coordinate
|
||||
- gps.longitude: float - GPS longitude coordinate
|
||||
- gps.signal: float - GPS signal strength (0-1, normalized from percentage)
|
||||
- signal.level: float - Network signal level (0-1, normalized from 0-5)
|
||||
- vibration: float - Vibration sensor reading
|
||||
- lamp.state: float - Lamp state (0=off, 1=on)
|
||||
"""
|
||||
return {
|
||||
# Cameras (height, width, channels)
|
||||
OBS_FRONT: (480, 640, 3),
|
||||
OBS_REAR: (480, 640, 3),
|
||||
# Motion state
|
||||
OBS_LINEAR_VEL: float,
|
||||
# Robot state
|
||||
OBS_BATTERY_LEVEL: float,
|
||||
OBS_ORIENTATION_DEG: float,
|
||||
# GPS
|
||||
OBS_GPS_LATITUDE: float,
|
||||
OBS_GPS_LONGITUDE: float,
|
||||
OBS_GPS_SIGNAL: float,
|
||||
# Sensors
|
||||
OBS_SIGNAL_LEVEL: float,
|
||||
OBS_VIBRATION: float,
|
||||
OBS_LAMP_STATE: float,
|
||||
}
|
||||
|
||||
@cached_property
|
||||
def action_features(self) -> dict[str, type]:
|
||||
"""Define the action space.
|
||||
|
||||
Returns:
|
||||
dict: Action features with types:
|
||||
- linear.vel: float - Target linear velocity
|
||||
- angular.vel: float - Target angular velocity
|
||||
"""
|
||||
return {
|
||||
ACTION_LINEAR_VEL: float,
|
||||
ACTION_ANGULAR_VEL: float,
|
||||
}
|
||||
|
||||
def get_observation(self) -> dict[str, Any]:
|
||||
"""Get current robot observation from SDK.
|
||||
|
||||
Returns:
|
||||
dict: Observation containing:
|
||||
- front: Front camera image (480, 640, 3) in RGB format
|
||||
- rear: Rear camera image (480, 640, 3) in RGB format
|
||||
- linear.vel: Current speed (0-1, SDK reports only positive speeds)
|
||||
- battery.level: Battery level (0-1, normalized from 0-100)
|
||||
- orientation.deg: Robot orientation (0-1, normalized from raw value)
|
||||
- gps.latitude: GPS latitude coordinate
|
||||
- gps.longitude: GPS longitude coordinate
|
||||
- gps.signal: GPS signal strength (0-1, normalized from percentage)
|
||||
- signal.level: Network signal level (0-1, normalized from 0-5)
|
||||
- vibration: Vibration sensor reading
|
||||
- lamp.state: Lamp state (0=off, 1=on)
|
||||
|
||||
Raises:
|
||||
DeviceNotConnectedError: If robot is not connected
|
||||
|
||||
Note:
|
||||
Camera frames are retrieved from SDK endpoints /v2/front and /v2/rear.
|
||||
Frames are decoded from base64 and converted from BGR to RGB format.
|
||||
Robot telemetry is retrieved from /data endpoint.
|
||||
All SDK values are normalized to appropriate ranges for dataset recording.
|
||||
"""
|
||||
if not self._is_connected:
|
||||
raise DeviceNotConnectedError(f"{self.name} is not connected")
|
||||
|
||||
observation = {}
|
||||
|
||||
# Get camera images from SDK
|
||||
frames = self._get_camera_frames()
|
||||
observation[OBS_FRONT] = frames["front"]
|
||||
observation[OBS_REAR] = frames["rear"]
|
||||
|
||||
# Get robot state from SDK
|
||||
robot_data = self._get_robot_data()
|
||||
|
||||
# Motion state
|
||||
observation[OBS_LINEAR_VEL] = robot_data["speed"] / 100.0 # Normalize 0-100 to 0-1
|
||||
|
||||
# Robot state
|
||||
observation[OBS_BATTERY_LEVEL] = robot_data["battery"] / 100.0 # Normalize 0-100 to 0-1
|
||||
observation[OBS_ORIENTATION_DEG] = robot_data["orientation"] / 360.0 # Normalize to 0-1
|
||||
|
||||
# GPS data
|
||||
observation[OBS_GPS_LATITUDE] = robot_data["latitude"]
|
||||
observation[OBS_GPS_LONGITUDE] = robot_data["longitude"]
|
||||
observation[OBS_GPS_SIGNAL] = robot_data["gps_signal"] / 100.0 # Normalize percentage to 0-1
|
||||
|
||||
# Sensors
|
||||
observation[OBS_SIGNAL_LEVEL] = robot_data["signal_level"] / 5.0 # Normalize 0-5 to 0-1
|
||||
observation[OBS_VIBRATION] = robot_data["vibration"]
|
||||
observation[OBS_LAMP_STATE] = float(robot_data["lamp"]) # 0 or 1
|
||||
|
||||
return observation
|
||||
|
||||
def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
|
||||
"""Send action to robot via SDK.
|
||||
|
||||
Args:
|
||||
action: Action dict with keys:
|
||||
- linear.vel: Target linear velocity (-1 to 1)
|
||||
- angular.vel: Target angular velocity (-1 to 1)
|
||||
|
||||
Returns:
|
||||
dict: The action that was sent (matches action_features keys)
|
||||
|
||||
Raises:
|
||||
DeviceNotConnectedError: If robot is not connected
|
||||
|
||||
Note:
|
||||
Actions are sent to SDK via POST /control endpoint.
|
||||
SDK expects commands in range [-1, 1].
|
||||
"""
|
||||
if not self._is_connected:
|
||||
raise DeviceNotConnectedError(f"{self.name} is not connected")
|
||||
|
||||
# Extract action values and convert to float
|
||||
linear = float(action.get(ACTION_LINEAR_VEL, 0.0))
|
||||
angular = float(action.get(ACTION_ANGULAR_VEL, 0.0))
|
||||
|
||||
# Send command to SDK
|
||||
try:
|
||||
self._send_command_to_sdk(linear, angular)
|
||||
except Exception as e:
|
||||
logger.error(f"Error sending action: {e}")
|
||||
|
||||
# Return action in format matching action_features
|
||||
return {
|
||||
ACTION_LINEAR_VEL: linear,
|
||||
ACTION_ANGULAR_VEL: angular,
|
||||
}
|
||||
|
||||
def disconnect(self) -> None:
|
||||
"""Disconnect from robot.
|
||||
|
||||
Stops the robot and closes connection to SDK.
|
||||
|
||||
Raises:
|
||||
DeviceNotConnectedError: If robot is not connected
|
||||
"""
|
||||
if not self._is_connected:
|
||||
raise DeviceNotConnectedError(f"{self.name} is not connected")
|
||||
|
||||
# Stop the robot before disconnecting
|
||||
try:
|
||||
self._send_command_to_sdk(0.0, 0.0)
|
||||
except Exception as e:
|
||||
logger.warning(f"Failed to stop robot during disconnect: {e}")
|
||||
|
||||
self._is_connected = False
|
||||
logger.info(f"{self.name} disconnected")
|
||||
|
||||
# Private helper methods for SDK communication
|
||||
|
||||
def _get_camera_frames(self) -> dict[str, np.ndarray]:
|
||||
"""Get camera frames from SDK using v2 endpoints with caching fallback.
|
||||
|
||||
Returns:
|
||||
dict: Dictionary with 'front' and 'rear' keys containing:
|
||||
- Current frame (if request succeeds)
|
||||
- Cached frame (if request fails but cache exists)
|
||||
- Zero array (if request fails and no cache exists yet)
|
||||
|
||||
Note:
|
||||
Uses /v2/front and /v2/rear endpoints which are 15x faster than /screenshot.
|
||||
Images are base64 encoded, resized to 640x480, and converted from BGR to RGB.
|
||||
If request fails, returns the last successfully retrieved frame (cached).
|
||||
"""
|
||||
frames = {}
|
||||
|
||||
# Get front camera
|
||||
try:
|
||||
response = requests.get(f"{self.sdk_base_url}/v2/front", timeout=2.0)
|
||||
if response.status_code == 200:
|
||||
data = response.json()
|
||||
if "front_frame" in data and data["front_frame"]:
|
||||
front_img = self._decode_base64_image(data["front_frame"])
|
||||
if front_img is not None:
|
||||
# Resize and convert BGR to RGB
|
||||
front_img = cv2.resize(front_img, (640, 480))
|
||||
front_rgb = cv2.cvtColor(front_img, cv2.COLOR_BGR2RGB)
|
||||
frames["front"] = front_rgb
|
||||
# Cache the successful frame
|
||||
self._last_front_frame = front_rgb
|
||||
except Exception as e:
|
||||
logger.warning(f"Error fetching front camera: {e}")
|
||||
|
||||
# Fallback: use cache or zero array
|
||||
if "front" not in frames:
|
||||
if self._last_front_frame is not None:
|
||||
frames["front"] = self._last_front_frame
|
||||
else:
|
||||
frames["front"] = np.zeros((480, 640, 3), dtype=np.uint8)
|
||||
|
||||
# Get rear camera
|
||||
try:
|
||||
response = requests.get(f"{self.sdk_base_url}/v2/rear", timeout=2.0)
|
||||
if response.status_code == 200:
|
||||
data = response.json()
|
||||
if "rear_frame" in data and data["rear_frame"]:
|
||||
rear_img = self._decode_base64_image(data["rear_frame"])
|
||||
if rear_img is not None:
|
||||
# Resize and convert BGR to RGB
|
||||
rear_img = cv2.resize(rear_img, (640, 480))
|
||||
rear_rgb = cv2.cvtColor(rear_img, cv2.COLOR_BGR2RGB)
|
||||
frames["rear"] = rear_rgb
|
||||
# Cache the successful frame
|
||||
self._last_rear_frame = rear_rgb
|
||||
except Exception as e:
|
||||
logger.warning(f"Error fetching rear camera: {e}")
|
||||
|
||||
# Fallback: use cache or zero array
|
||||
if "rear" not in frames:
|
||||
if self._last_rear_frame is not None:
|
||||
frames["rear"] = self._last_rear_frame
|
||||
else:
|
||||
frames["rear"] = np.zeros((480, 640, 3), dtype=np.uint8)
|
||||
|
||||
return frames
|
||||
|
||||
def _decode_base64_image(self, base64_string: str) -> np.ndarray | None:
|
||||
"""Decode base64 string to image.
|
||||
|
||||
Args:
|
||||
base64_string: Base64 encoded image string
|
||||
|
||||
Returns:
|
||||
np.ndarray: Decoded image in BGR format (OpenCV default), or None if decoding fails
|
||||
"""
|
||||
try:
|
||||
img_bytes = base64.b64decode(base64_string)
|
||||
nparr = np.frombuffer(img_bytes, np.uint8)
|
||||
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
|
||||
return img # Return in BGR format (OpenCV default)
|
||||
except Exception as e:
|
||||
logger.error(f"Error decoding image: {e}")
|
||||
return None
|
||||
|
||||
def _get_robot_data(self) -> dict:
|
||||
"""Get robot telemetry data from SDK.
|
||||
|
||||
Returns:
|
||||
dict: Robot telemetry data including battery, speed, orientation, GPS, etc:
|
||||
- Current data (if request succeeds)
|
||||
- Cached data (if request fails but cache exists)
|
||||
- Default values (if request fails and no cache exists yet)
|
||||
|
||||
Note:
|
||||
Uses /data endpoint which provides comprehensive robot state.
|
||||
If request fails, returns the last successfully retrieved data (cached).
|
||||
"""
|
||||
try:
|
||||
response = requests.get(f"{self.sdk_base_url}/data", timeout=2.0)
|
||||
if response.status_code == 200:
|
||||
data = response.json()
|
||||
# Cache the successful data
|
||||
self._last_robot_data = data
|
||||
return data
|
||||
except Exception as e:
|
||||
logger.warning(f"Error fetching robot data: {e}")
|
||||
|
||||
# Fallback: use cache or default values
|
||||
if self._last_robot_data is not None:
|
||||
return self._last_robot_data
|
||||
else:
|
||||
# Return dict with default values (used only on first failure before any cache exists)
|
||||
return {
|
||||
"speed": 0,
|
||||
"battery": 0,
|
||||
"orientation": 0,
|
||||
"latitude": 0.0,
|
||||
"longitude": 0.0,
|
||||
"gps_signal": 0,
|
||||
"signal_level": 0,
|
||||
"vibration": 0.0,
|
||||
"lamp": 0,
|
||||
}
|
||||
|
||||
def _send_command_to_sdk(self, linear: float, angular: float, lamp: int = 0) -> bool:
|
||||
"""Send control command to SDK.
|
||||
|
||||
Args:
|
||||
linear: Linear velocity command (-1 to 1)
|
||||
angular: Angular velocity command (-1 to 1)
|
||||
lamp: Lamp control (0=off, 1=on)
|
||||
|
||||
Returns:
|
||||
bool: True if command sent successfully, False otherwise
|
||||
|
||||
Note:
|
||||
Uses POST /control endpoint. Commands are sent as JSON payload.
|
||||
"""
|
||||
try:
|
||||
payload = {
|
||||
"command": {
|
||||
"linear": linear,
|
||||
"angular": angular,
|
||||
"lamp": lamp,
|
||||
}
|
||||
}
|
||||
|
||||
response = requests.post(
|
||||
f"{self.sdk_base_url}/control",
|
||||
json=payload,
|
||||
timeout=1.0,
|
||||
)
|
||||
|
||||
return response.status_code == 200
|
||||
except Exception as e:
|
||||
logger.error(f"Error sending command: {e}")
|
||||
return False
|
||||
@@ -93,6 +93,7 @@ from lerobot.robots import ( # noqa: F401
|
||||
Robot,
|
||||
RobotConfig,
|
||||
bi_so100_follower,
|
||||
earthrover_mini_plus,
|
||||
hope_jr,
|
||||
koch_follower,
|
||||
make_robot_from_config,
|
||||
|
||||
@@ -54,6 +54,7 @@ from lerobot.robots import ( # noqa: F401
|
||||
Robot,
|
||||
RobotConfig,
|
||||
bi_so100_follower,
|
||||
earthrover_mini_plus,
|
||||
hope_jr,
|
||||
koch_follower,
|
||||
make_robot_from_config,
|
||||
|
||||
@@ -71,6 +71,7 @@ from lerobot.robots import ( # noqa: F401
|
||||
Robot,
|
||||
RobotConfig,
|
||||
bi_so100_follower,
|
||||
earthrover_mini_plus,
|
||||
hope_jr,
|
||||
koch_follower,
|
||||
make_robot_from_config,
|
||||
@@ -83,6 +84,7 @@ from lerobot.teleoperators import ( # noqa: F401
|
||||
bi_so100_leader,
|
||||
gamepad,
|
||||
homunculus,
|
||||
keyboard,
|
||||
koch_leader,
|
||||
make_teleoperator_from_config,
|
||||
so100_leader,
|
||||
|
||||
@@ -14,12 +14,18 @@
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from .configuration_keyboard import KeyboardEndEffectorTeleopConfig, KeyboardTeleopConfig
|
||||
from .teleop_keyboard import KeyboardEndEffectorTeleop, KeyboardTeleop
|
||||
from .configuration_keyboard import (
|
||||
KeyboardEndEffectorTeleopConfig,
|
||||
KeyboardRoverTeleopConfig,
|
||||
KeyboardTeleopConfig,
|
||||
)
|
||||
from .teleop_keyboard import KeyboardEndEffectorTeleop, KeyboardRoverTeleop, KeyboardTeleop
|
||||
|
||||
__all__ = [
|
||||
"KeyboardTeleopConfig",
|
||||
"KeyboardTeleop",
|
||||
"KeyboardEndEffectorTeleopConfig",
|
||||
"KeyboardEndEffectorTeleop",
|
||||
"KeyboardRoverTeleopConfig",
|
||||
"KeyboardRoverTeleop",
|
||||
]
|
||||
|
||||
@@ -13,6 +13,7 @@
|
||||
# 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.
|
||||
"""Configuration for keyboard teleoperators."""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
@@ -30,4 +31,38 @@ class KeyboardTeleopConfig(TeleoperatorConfig):
|
||||
@TeleoperatorConfig.register_subclass("keyboard_ee")
|
||||
@dataclass
|
||||
class KeyboardEndEffectorTeleopConfig(KeyboardTeleopConfig):
|
||||
"""Configuration for keyboard end-effector teleoperator.
|
||||
|
||||
Used for controlling robot end-effectors with keyboard inputs.
|
||||
|
||||
Attributes:
|
||||
use_gripper: Whether to include gripper control in actions
|
||||
"""
|
||||
|
||||
use_gripper: bool = True
|
||||
|
||||
|
||||
@TeleoperatorConfig.register_subclass("keyboard_rover")
|
||||
@dataclass
|
||||
class KeyboardRoverTeleopConfig(TeleoperatorConfig):
|
||||
"""Configuration for keyboard rover teleoperator.
|
||||
|
||||
Used for controlling mobile robots like EarthRover Mini Plus with WASD controls.
|
||||
|
||||
Attributes:
|
||||
linear_speed: Default linear velocity magnitude (-1 to 1 range for SDK robots)
|
||||
angular_speed: Default angular velocity magnitude (-1 to 1 range for SDK robots)
|
||||
speed_increment: Amount to increase/decrease speed with +/- keys
|
||||
turn_assist_ratio: Forward motion multiplier when turning with A/D keys (0.0-1.0)
|
||||
angular_speed_ratio: Ratio of angular to linear speed for synchronized adjustments
|
||||
min_linear_speed: Minimum linear speed when decreasing (prevents zero speed)
|
||||
min_angular_speed: Minimum angular speed when decreasing (prevents zero speed)
|
||||
"""
|
||||
|
||||
linear_speed: float = 1.0
|
||||
angular_speed: float = 1.0
|
||||
speed_increment: float = 0.1
|
||||
turn_assist_ratio: float = 0.3
|
||||
angular_speed_ratio: float = 0.6
|
||||
min_linear_speed: float = 0.1
|
||||
min_angular_speed: float = 0.05
|
||||
|
||||
@@ -25,7 +25,11 @@ from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnected
|
||||
|
||||
from ..teleoperator import Teleoperator
|
||||
from ..utils import TeleopEvents
|
||||
from .configuration_keyboard import KeyboardEndEffectorTeleopConfig, KeyboardTeleopConfig
|
||||
from .configuration_keyboard import (
|
||||
KeyboardEndEffectorTeleopConfig,
|
||||
KeyboardRoverTeleopConfig,
|
||||
KeyboardTeleopConfig,
|
||||
)
|
||||
|
||||
PYNPUT_AVAILABLE = True
|
||||
try:
|
||||
@@ -289,3 +293,158 @@ class KeyboardEndEffectorTeleop(KeyboardTeleop):
|
||||
TeleopEvents.SUCCESS: success,
|
||||
TeleopEvents.RERECORD_EPISODE: rerecord_episode,
|
||||
}
|
||||
|
||||
|
||||
class KeyboardRoverTeleop(KeyboardTeleop):
|
||||
"""
|
||||
Keyboard teleoperator for mobile robots like EarthRover Mini Plus.
|
||||
|
||||
Provides intuitive WASD-style controls for driving a mobile robot:
|
||||
- Linear movement (forward/backward)
|
||||
- Angular movement (turning/rotation)
|
||||
- Speed adjustment
|
||||
- Emergency stop
|
||||
|
||||
Keyboard Controls:
|
||||
Movement:
|
||||
- W: Move forward
|
||||
- S: Move backward
|
||||
- A: Turn left (with forward motion)
|
||||
- D: Turn right (with forward motion)
|
||||
- Q: Rotate left in place
|
||||
- E: Rotate right in place
|
||||
- X: Emergency stop
|
||||
|
||||
Speed Control:
|
||||
- +/=: Increase speed
|
||||
- -: Decrease speed
|
||||
|
||||
System:
|
||||
- ESC: Disconnect teleoperator
|
||||
|
||||
Attributes:
|
||||
config: Teleoperator configuration
|
||||
current_linear_speed: Current linear velocity magnitude
|
||||
current_angular_speed: Current angular velocity magnitude
|
||||
|
||||
Example:
|
||||
```python
|
||||
from lerobot.teleoperators.keyboard import KeyboardRoverTeleop, KeyboardRoverTeleopConfig
|
||||
|
||||
teleop = KeyboardRoverTeleop(
|
||||
KeyboardRoverTeleopConfig(linear_speed=1.0, angular_speed=1.0, speed_increment=0.1)
|
||||
)
|
||||
teleop.connect()
|
||||
|
||||
while teleop.is_connected:
|
||||
action = teleop.get_action()
|
||||
robot.send_action(action)
|
||||
```
|
||||
"""
|
||||
|
||||
config_class = KeyboardRoverTeleopConfig
|
||||
name = "keyboard_rover"
|
||||
|
||||
def __init__(self, config: KeyboardRoverTeleopConfig):
|
||||
super().__init__(config)
|
||||
# Add rover-specific speed settings
|
||||
self.current_linear_speed = config.linear_speed
|
||||
self.current_angular_speed = config.angular_speed
|
||||
|
||||
@property
|
||||
def action_features(self) -> dict:
|
||||
"""Return action format for rover (linear and angular velocities)."""
|
||||
return {
|
||||
"linear.vel": float,
|
||||
"angular.vel": float,
|
||||
}
|
||||
|
||||
@property
|
||||
def is_calibrated(self) -> bool:
|
||||
"""Rover teleop doesn't require calibration."""
|
||||
return True
|
||||
|
||||
def _drain_pressed_keys(self):
|
||||
"""Update current_pressed state from event queue without clearing held keys"""
|
||||
while not self.event_queue.empty():
|
||||
key_char, is_pressed = self.event_queue.get_nowait()
|
||||
if is_pressed:
|
||||
self.current_pressed[key_char] = True
|
||||
else:
|
||||
# Only remove key if it's being released
|
||||
self.current_pressed.pop(key_char, None)
|
||||
|
||||
def get_action(self) -> dict[str, Any]:
|
||||
"""
|
||||
Get the current action based on pressed keys.
|
||||
|
||||
Returns:
|
||||
dict with 'linear.vel' and 'angular.vel' keys
|
||||
"""
|
||||
before_read_t = time.perf_counter()
|
||||
|
||||
if not self.is_connected:
|
||||
raise DeviceNotConnectedError(
|
||||
"KeyboardRoverTeleop is not connected. You need to run `connect()` before `get_action()`."
|
||||
)
|
||||
|
||||
self._drain_pressed_keys()
|
||||
|
||||
linear_velocity = 0.0
|
||||
angular_velocity = 0.0
|
||||
|
||||
# Check which keys are currently pressed (not released)
|
||||
active_keys = {key for key, is_pressed in self.current_pressed.items() if is_pressed}
|
||||
|
||||
# Linear movement (W/S) - these take priority
|
||||
if "w" in active_keys:
|
||||
linear_velocity = self.current_linear_speed
|
||||
elif "s" in active_keys:
|
||||
linear_velocity = -self.current_linear_speed
|
||||
|
||||
# Turning (A/D/Q/E)
|
||||
if "d" in active_keys:
|
||||
angular_velocity = -self.current_angular_speed
|
||||
if linear_velocity == 0: # If not moving forward/back, add slight forward motion
|
||||
linear_velocity = self.current_linear_speed * self.config.turn_assist_ratio
|
||||
elif "a" in active_keys:
|
||||
angular_velocity = self.current_angular_speed
|
||||
if linear_velocity == 0: # If not moving forward/back, add slight forward motion
|
||||
linear_velocity = self.current_linear_speed * self.config.turn_assist_ratio
|
||||
elif "q" in active_keys:
|
||||
angular_velocity = self.current_angular_speed
|
||||
linear_velocity = 0 # Rotate in place
|
||||
elif "e" in active_keys:
|
||||
angular_velocity = -self.current_angular_speed
|
||||
linear_velocity = 0 # Rotate in place
|
||||
|
||||
# Stop (X) - overrides everything
|
||||
if "x" in active_keys:
|
||||
linear_velocity = 0
|
||||
angular_velocity = 0
|
||||
|
||||
# Speed adjustment
|
||||
if "+" in active_keys or "=" in active_keys:
|
||||
self.current_linear_speed += self.config.speed_increment
|
||||
self.current_angular_speed += self.config.speed_increment * self.config.angular_speed_ratio
|
||||
logging.info(
|
||||
f"Speed increased: linear={self.current_linear_speed:.2f}, angular={self.current_angular_speed:.2f}"
|
||||
)
|
||||
if "-" in active_keys:
|
||||
self.current_linear_speed = max(
|
||||
self.config.min_linear_speed, self.current_linear_speed - self.config.speed_increment
|
||||
)
|
||||
self.current_angular_speed = max(
|
||||
self.config.min_angular_speed,
|
||||
self.current_angular_speed - self.config.speed_increment * self.config.angular_speed_ratio,
|
||||
)
|
||||
logging.info(
|
||||
f"Speed decreased: linear={self.current_linear_speed:.2f}, angular={self.current_angular_speed:.2f}"
|
||||
)
|
||||
|
||||
self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
|
||||
|
||||
return {
|
||||
"linear.vel": linear_velocity,
|
||||
"angular.vel": angular_velocity,
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user