mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-25 02:36:11 +00:00
Hardware API redesign (#777)
Co-authored-by: Pepijn <138571049+pkooij@users.noreply.github.com> Co-authored-by: Steven Palma <imstevenpmwork@ieee.org> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Steven Palma <steven.palma@huggingface.co> Co-authored-by: Adil Zouitine <adilzouitinegm@gmail.com> Co-authored-by: Pepijn <pepijn@huggingface.co>
This commit is contained in:
Simon Alibert
@@ -0,0 +1,3 @@
|
||||
from .config_lekiwi import LeKiwiClientConfig, LeKiwiConfig
|
||||
from .lekiwi import LeKiwi
|
||||
from .lekiwi_client import LeKiwiClient
|
||||
@@ -0,0 +1,90 @@
|
||||
# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from lerobot.common.cameras.configs import CameraConfig, Cv2Rotation
|
||||
from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig
|
||||
|
||||
from ..config import RobotConfig
|
||||
|
||||
|
||||
@RobotConfig.register_subclass("lekiwi")
|
||||
@dataclass
|
||||
class LeKiwiConfig(RobotConfig):
|
||||
port = "/dev/ttyACM0" # port to connect to the bus
|
||||
|
||||
disable_torque_on_disconnect: bool = True
|
||||
|
||||
# `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes.
|
||||
# Set this to a positive scalar to have the same value for all motors, or a list that is the same length as
|
||||
# the number of motors in your follower arms.
|
||||
max_relative_target: int | None = None
|
||||
|
||||
cameras: dict[str, CameraConfig] = field(
|
||||
default_factory=lambda: {
|
||||
"front": OpenCVCameraConfig(index_or_path="/dev/video0", fps=30, width=640, height=480),
|
||||
"wrist": OpenCVCameraConfig(
|
||||
index_or_path="/dev/video2", fps=30, width=640, height=480, rotation=Cv2Rotation.ROTATE_180
|
||||
),
|
||||
}
|
||||
)
|
||||
|
||||
# Set to `True` for backward compatibility with previous policies/dataset
|
||||
use_degrees: bool = False
|
||||
|
||||
|
||||
@dataclass
|
||||
class LeKiwiHostConfig:
|
||||
# Network Configuration
|
||||
port_zmq_cmd: int = 5555
|
||||
port_zmq_observations: int = 5556
|
||||
|
||||
# Duration of the application
|
||||
connection_time_s: int = 30
|
||||
|
||||
# Watchdog: stop the robot if no command is received for over 0.5 seconds.
|
||||
watchdog_timeout_ms: int = 500
|
||||
|
||||
# If robot jitters decrease the frequency and monitor cpu load with `top` in cmd
|
||||
max_loop_freq_hz: int = 30
|
||||
|
||||
|
||||
@RobotConfig.register_subclass("lekiwi_client")
|
||||
@dataclass
|
||||
class LeKiwiClientConfig(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: {
|
||||
# Movement
|
||||
"forward": "w",
|
||||
"backward": "s",
|
||||
"left": "a",
|
||||
"right": "d",
|
||||
"rotate_left": "z",
|
||||
"rotate_right": "x",
|
||||
# Speed control
|
||||
"speed_up": "r",
|
||||
"speed_down": "f",
|
||||
# quit teleop
|
||||
"quit": "q",
|
||||
}
|
||||
)
|
||||
|
||||
polling_timeout_ms: int = 15
|
||||
connect_timeout_s: int = 5
|
||||
@@ -0,0 +1,240 @@
|
||||
# LeKiwi
|
||||
|
||||
In the steps below, we explain how to assemble the LeKiwi mobile robot.
|
||||
|
||||
## Source the parts
|
||||
|
||||
Follow this [README](https://github.com/SIGRobotics-UIUC/LeKiwi). It contains the bill of materials, with a link to source the parts, as well as the instructions to 3D print the parts.
|
||||
And advise if it's your first time printing or if you don't own a 3D printer.
|
||||
|
||||
### Wired version
|
||||
If you have the **wired** LeKiwi version, you can skip the installation of the Raspberry Pi and setting up SSH. You can also run all commands directly on your PC for both the LeKiwi scripts and the leader arm scripts for teleoperating.
|
||||
|
||||
## Install software on Pi
|
||||
Now we have to set up the remote PC that will run on the LeKiwi Robot. This is normally a Raspberry Pi, but can be any PC that can run on 5V and has enough usb ports (2 or more) for the cameras and motor control board.
|
||||
|
||||
### Install OS
|
||||
For setting up the Raspberry Pi and its SD-card see: [Setup PI](https://www.raspberrypi.com/documentation/computers/getting-started.html). Here is explained how to download the [Imager](https://www.raspberrypi.com/software/) to install Raspberry Pi OS or Ubuntu.
|
||||
|
||||
### Setup SSH
|
||||
After setting up your Pi, you should enable and set up [SSH](https://www.raspberrypi.com/news/coding-on-raspberry-pi-remotely-with-visual-studio-code/) (Secure Shell Protocol) so you can log in to the Pi from your laptop without requiring a screen, keyboard, and mouse on the Pi. A great tutorial on how to do this can be found [here](https://www.raspberrypi.com/documentation/computers/remote-access.html#ssh). Logging into your Pi can be done in your Command Prompt (cmd) or, if you use VSCode you can use [this](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.remote-ssh) extension.
|
||||
|
||||
### Install LeRobot on Pi 🤗
|
||||
|
||||
On your Raspberry Pi install LeRobot using our [Installation Guide](./installation)
|
||||
|
||||
In addition to these instructions, you need to install the Feetech sdk on your Pi:
|
||||
```bash
|
||||
pip install -e ".[feetech]"
|
||||
```
|
||||
|
||||
## Install LeRobot locally
|
||||
If you already have installed LeRobot on your laptop/pc you can skip this step; otherwise, please follow along as we do the same steps we did on the Pi.
|
||||
|
||||
Follow our [Installation Guide](./installation)
|
||||
|
||||
Great :hugs:! You are now done installing LeRobot, and we can begin assembling the SO100/SO101 arms and the mobile base :robot:.
|
||||
Every time you now want to use LeRobot, you can go to the `~/lerobot` folder where we installed LeRobot and run one of the commands.
|
||||
|
||||
# Step-by-Step Assembly Instructions
|
||||
|
||||
First, we will assemble the two SO100/SO101 arms. One to attach to the mobile base and one for teleoperation. Then we will assemble the mobile base. The instructions for assembling can be found on these two pages:
|
||||
|
||||
- [Assemble SO101](./so101#step-by-step-assembly-instructions)
|
||||
- [Assemble LeKiwi](https://github.com/SIGRobotics-UIUC/LeKiwi/blob/main/Assembly.md)
|
||||
|
||||
### Configure motors
|
||||
The instructions for configuring the motors can be found in the SO101 [docs](./so101#configure-the-motors). Besides the ids for the arm motors, we also need to set the motor ids for the mobile base. These need to be in a specific order to work. Below an image of the motor ids and motor mounting positions for the mobile base. Note that we only use one Motor Control board on LeKiwi. This means the motor ids for the wheels are 7, 8 and 9.
|
||||
|
||||
<img src="https://huggingface.co/datasets/huggingface/documentation-images/resolve/main/lerobot/motor_ids.webp" alt="Motor ID's for mobile robot" title="Motor ID's for mobile robot" width="60%">
|
||||
|
||||
### Troubleshoot communication
|
||||
|
||||
If you are having trouble connecting to the Mobile SO100, follow these steps to diagnose and resolve the issue.
|
||||
|
||||
#### 1. Verify IP Address Configuration
|
||||
Make sure that the correct IP for the Pi is used in the commands or in your code. To check the Raspberry Pi's IP address, run (on the Pi command line):
|
||||
```bash
|
||||
hostname -I
|
||||
```
|
||||
|
||||
#### 2. Check if Pi is reachable from laptop/pc
|
||||
Try pinging the Raspberry Pi from your laptop:
|
||||
```bach
|
||||
ping <your_pi_ip_address>
|
||||
```
|
||||
|
||||
If the ping fails:
|
||||
- Ensure the Pi is powered on and connected to the same network.
|
||||
- Check if SSH is enabled on the Pi.
|
||||
|
||||
#### 3. Try SSH connection
|
||||
If you can't SSH into the Pi, it might not be properly connected. Use:
|
||||
```bash
|
||||
ssh <your_pi_user_name>@<your_pi_ip_address>
|
||||
```
|
||||
If you get a connection error:
|
||||
- Ensure SSH is enabled on the Pi by running:
|
||||
```bash
|
||||
sudo raspi-config
|
||||
```
|
||||
Then navigate to: **Interfacing Options -> SSH** and enable it.
|
||||
|
||||
### Calibration
|
||||
|
||||
Now we have to calibrate the leader arm and the follower arm. The wheel motors don't have to be calibrated.
|
||||
The calibration process is very important because it allows a neural network trained on one robot to work on another.
|
||||
|
||||
### Calibrate follower arm (on mobile base)
|
||||
|
||||
Make sure the arm is connected to the Raspberry Pi and run this script or API example (on the Raspberry Pi via SSH) to launch calibration of the follower arm:
|
||||
|
||||
```bash
|
||||
python -m lerobot.calibrate \
|
||||
--robot.type=lekiwi \
|
||||
--robot.id=my_awesome_kiwi # <- Give the robot a unique name
|
||||
```
|
||||
|
||||
We unified the calibration method for most robots, thus, the calibration steps for this SO100 arm are the same as the steps for the Koch and SO101. First, we have to move the robot to the position where each joint is in the middle of its range, then we press `Enter`. Secondly, we move all joints through their full range of motion. A video of this same process for the SO101 as reference can be found [here](https://huggingface.co/docs/lerobot/en/so101#calibration-video).
|
||||
|
||||
### Wired version
|
||||
If you have the **wired** LeKiwi version, please run all commands on your laptop.
|
||||
|
||||
### Calibrate leader arm
|
||||
Then, to calibrate the leader arm (which is attached to the laptop/pc). Run the following command of API example on your laptop:
|
||||
<hfoptions id="calibrate_leader">
|
||||
<hfoption id="Command">
|
||||
|
||||
```bash
|
||||
python -m lerobot.calibrate \
|
||||
--teleop.type=so100_leader \
|
||||
--teleop.port=/dev/tty.usbmodem58760431551 \ # <- The port of your robot
|
||||
--teleop.id=my_awesome_leader_arm # <- Give the robot a unique name
|
||||
```
|
||||
</hfoption>
|
||||
<hfoption id="API example">
|
||||
|
||||
```python
|
||||
from lerobot.common.teleoperators.so100_leader import SO100LeaderConfig, SO100Leader
|
||||
|
||||
config = SO100LeaderConfig(
|
||||
port="/dev/tty.usbmodem58760431551",
|
||||
id="my_awesome_leader_arm",
|
||||
)
|
||||
|
||||
leader = SO100Leader(config)
|
||||
leader.connect(calibrate=False)
|
||||
leader.calibrate()
|
||||
leader.disconnect()
|
||||
```
|
||||
</hfoption>
|
||||
</hfoptions>
|
||||
|
||||
## Teleoperate LeKiwi
|
||||
|
||||
> [!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.
|
||||
|
||||
To teleoperate, SSH into your Raspberry Pi, and run `conda activate lerobot` and this command:
|
||||
```bash
|
||||
python -m lerobot.common.robots.lekiwi.lekiwi_host
|
||||
```
|
||||
|
||||
Then on your laptop, also run `conda activate lerobot` and run the API example, make sure you set the correct `remote_ip` and `port`.
|
||||
|
||||
```bash
|
||||
python examples/lekiwi/teleoperate.py
|
||||
```
|
||||
|
||||
You should see on your laptop something like this: ```[INFO] Connected to remote robot at tcp://172.17.133.91:5555 and video stream at tcp://172.17.133.91:5556.``` Now you can move the leader arm and use the keyboard (w,a,s,d) to drive forward, left, backwards, right. And use (z,x) to turn left or turn right. You can use (r,f) to increase and decrease the speed of the mobile robot. There are three speed modes, see the table below:
|
||||
|
||||
| Speed Mode | Linear Speed (m/s) | Rotation Speed (deg/s) |
|
||||
| ---------- | ------------------ | ---------------------- |
|
||||
| Fast | 0.4 | 90 |
|
||||
| Medium | 0.25 | 60 |
|
||||
| Slow | 0.1 | 30 |
|
||||
|
||||
|
||||
| Key | Action |
|
||||
| --- | -------------- |
|
||||
| W | Move forward |
|
||||
| A | Move left |
|
||||
| S | Move backward |
|
||||
| D | Move right |
|
||||
| Z | Turn left |
|
||||
| X | Turn right |
|
||||
| R | Increase speed |
|
||||
| F | Decrease speed |
|
||||
|
||||
> [!TIP]
|
||||
> If you use a different keyboard, you can change the keys for each command in the [`LeKiwiConfig`](../lerobot/common/robot_devices/robots/configs.py).
|
||||
|
||||
### Wired version
|
||||
If you have the **wired** LeKiwi version, please run all commands on your laptop.
|
||||
|
||||
## Record a dataset
|
||||
|
||||
Once you're familiar with teleoperation, you can record your first dataset.
|
||||
|
||||
We use the Hugging Face hub features for uploading your dataset. If you haven't previously used the Hub, make sure you can login via the cli using a write-access token, this token can be generated from the [Hugging Face settings](https://huggingface.co/settings/tokens).
|
||||
|
||||
Add your token to the CLI by running this command:
|
||||
```bash
|
||||
huggingface-cli login --token ${HUGGINGFACE_TOKEN} --add-to-git-credential
|
||||
```
|
||||
|
||||
Then store your Hugging Face repository name in a variable:
|
||||
```bash
|
||||
HF_USER=$(huggingface-cli whoami | head -n 1)
|
||||
echo $HF_USER
|
||||
```
|
||||
|
||||
Now you can record a dataset. To record episodes and upload your dataset to the hub, execute this API example tailored for LeKiwi. Make sure to first adapt the `remote_ip`, `repo_id`, `port` and `task` in the script. If you would like to run the script for longer you can increase `NB_CYCLES_CLIENT_CONNECTION`.
|
||||
```bash
|
||||
python examples/lekiwi/record.py
|
||||
```
|
||||
|
||||
#### Dataset upload
|
||||
Locally, your dataset is stored in this folder: `~/.cache/huggingface/lerobot/{repo-id}`. At the end of data recording, your dataset will be uploaded on your Hugging Face page (e.g. https://huggingface.co/datasets/cadene/so101_test) that you can obtain by running:
|
||||
```bash
|
||||
echo https://huggingface.co/datasets/${HF_USER}/so101_test
|
||||
```
|
||||
Your dataset will be automatically tagged with `LeRobot` for the community to find it easily, and you can also add custom tags (in this case `tutorial` for example).
|
||||
|
||||
You can look for other LeRobot datasets on the hub by searching for `LeRobot` [tags](https://huggingface.co/datasets?other=LeRobot).
|
||||
|
||||
#### Tips for gathering data
|
||||
|
||||
Once you're comfortable with data recording, you can create a larger dataset for training. A good starting task is grasping an object at different locations and placing it in a bin. We suggest recording at least 50 episodes, with 10 episodes per location. Keep the cameras fixed and maintain consistent grasping behavior throughout the recordings. Also make sure the object you are manipulating is visible on the camera's. A good rule of thumb is you should be able to do the task yourself by only looking at the camera images.
|
||||
|
||||
In the following sections, you’ll train your neural network. After achieving reliable grasping performance, you can start introducing more variations during data collection, such as additional grasp locations, different grasping techniques, and altering camera positions.
|
||||
|
||||
Avoid adding too much variation too quickly, as it may hinder your results.
|
||||
|
||||
If you want to dive deeper into this important topic, you can check out the [blog post](https://huggingface.co/blog/lerobot-datasets#what-makes-a-good-dataset) we wrote on what makes a good dataset.
|
||||
|
||||
#### Troubleshooting:
|
||||
- On Linux, if the left and right arrow keys and escape key don't have any effect during data recording, make sure you've set the `$DISPLAY` environment variable. See [pynput limitations](https://pynput.readthedocs.io/en/latest/limitations.html#linux).
|
||||
|
||||
|
||||
## Replay an episode
|
||||
|
||||
To replay an episode run the API example below, make sure to change `remote_ip`, `port`, LeRobotDatasetId and episode index.
|
||||
|
||||
|
||||
```bash
|
||||
python examples/lekiwi/replay.py
|
||||
```
|
||||
|
||||
Congrats 🎉, your robot is all set to learn a task on its own. Start training it by the training part of this tutorial: [Getting started with real-world robots](./getting_started_real_world_robot)
|
||||
|
||||
## Evaluate your policy
|
||||
|
||||
To evaluate your policy run the `evaluate.py` API example, make sure to change `remote_ip`, `port`, model..
|
||||
|
||||
```bash
|
||||
python examples/lekiwi/evaluate.py
|
||||
```
|
||||
|
||||
> [!TIP]
|
||||
> If you have any questions or need help, please reach out on [Discord](https://discord.com/invite/s3KuuzsPFb).
|
||||
@@ -0,0 +1,416 @@
|
||||
#!/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.common.cameras.utils import make_cameras_from_configs
|
||||
from lerobot.common.constants import OBS_IMAGES, OBS_STATE
|
||||
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
|
||||
from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode
|
||||
from lerobot.common.motors.feetech import (
|
||||
FeetechMotorsBus,
|
||||
OperatingMode,
|
||||
)
|
||||
|
||||
from ..robot import Robot
|
||||
from ..utils import ensure_safe_goal_position
|
||||
from .config_lekiwi import LeKiwiConfig
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class LeKiwi(Robot):
|
||||
"""
|
||||
The robot includes a three omniwheel mobile base and a remote follower arm.
|
||||
The leader arm is connected locally (on the laptop) and its joint positions are recorded and then
|
||||
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 = LeKiwiConfig
|
||||
name = "lekiwi"
|
||||
|
||||
def __init__(self, config: LeKiwiConfig):
|
||||
super().__init__(config)
|
||||
self.config = config
|
||||
norm_mode_body = MotorNormMode.DEGREES if config.use_degrees else MotorNormMode.RANGE_M100_100
|
||||
self.bus = FeetechMotorsBus(
|
||||
port=self.config.port,
|
||||
motors={
|
||||
# arm
|
||||
"arm_shoulder_pan": Motor(1, "sts3215", norm_mode_body),
|
||||
"arm_shoulder_lift": Motor(2, "sts3215", norm_mode_body),
|
||||
"arm_elbow_flex": Motor(3, "sts3215", norm_mode_body),
|
||||
"arm_wrist_flex": Motor(4, "sts3215", norm_mode_body),
|
||||
"arm_wrist_roll": Motor(5, "sts3215", norm_mode_body),
|
||||
"arm_gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100),
|
||||
# base
|
||||
"base_left_wheel": Motor(7, "sts3215", MotorNormMode.RANGE_M100_100),
|
||||
"base_right_wheel": Motor(8, "sts3215", MotorNormMode.RANGE_M100_100),
|
||||
"base_back_wheel": Motor(9, "sts3215", MotorNormMode.RANGE_M100_100),
|
||||
},
|
||||
calibration=self.calibration,
|
||||
)
|
||||
self.arm_motors = [motor for motor in self.bus.motors if motor.startswith("arm")]
|
||||
self.base_motors = [motor for motor in self.bus.motors if motor.startswith("base")]
|
||||
self.cameras = make_cameras_from_configs(config.cameras)
|
||||
|
||||
@property
|
||||
def _state_ft(self) -> dict[str, type]:
|
||||
return dict.fromkeys(
|
||||
(
|
||||
"arm_shoulder_pan.pos",
|
||||
"arm_shoulder_lift.pos",
|
||||
"arm_elbow_flex.pos",
|
||||
"arm_wrist_flex.pos",
|
||||
"arm_wrist_roll.pos",
|
||||
"arm_gripper.pos",
|
||||
"x.vel",
|
||||
"y.vel",
|
||||
"theta.vel",
|
||||
),
|
||||
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 self.bus.is_connected 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")
|
||||
|
||||
self.bus.connect()
|
||||
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 self.bus.is_calibrated
|
||||
|
||||
def calibrate(self) -> None:
|
||||
logger.info(f"\nRunning calibration of {self}")
|
||||
|
||||
motors = self.arm_motors + self.base_motors
|
||||
|
||||
self.bus.disable_torque(self.arm_motors)
|
||||
for name in self.arm_motors:
|
||||
self.bus.write("Operating_Mode", name, OperatingMode.POSITION.value)
|
||||
|
||||
input("Move robot to the middle of its range of motion and press ENTER....")
|
||||
homing_offsets = self.bus.set_half_turn_homings(self.arm_motors)
|
||||
|
||||
homing_offsets.update(dict.fromkeys(self.base_motors, 0))
|
||||
|
||||
full_turn_motor = [
|
||||
motor for motor in motors if any(keyword in motor for keyword in ["wheel", "wrist"])
|
||||
]
|
||||
unknown_range_motors = [motor for motor in motors if motor not in full_turn_motor]
|
||||
|
||||
print(
|
||||
f"Move all arm joints except '{full_turn_motor}' sequentially through their "
|
||||
"entire ranges of motion.\nRecording positions. Press ENTER to stop..."
|
||||
)
|
||||
range_mins, range_maxes = self.bus.record_ranges_of_motion(unknown_range_motors)
|
||||
for name in full_turn_motor:
|
||||
range_mins[name] = 0
|
||||
range_maxes[name] = 4095
|
||||
|
||||
self.calibration = {}
|
||||
for name, motor in self.bus.motors.items():
|
||||
self.calibration[name] = MotorCalibration(
|
||||
id=motor.id,
|
||||
drive_mode=0,
|
||||
homing_offset=homing_offsets[name],
|
||||
range_min=range_mins[name],
|
||||
range_max=range_maxes[name],
|
||||
)
|
||||
|
||||
self.bus.write_calibration(self.calibration)
|
||||
self._save_calibration()
|
||||
print("Calibration saved to", self.calibration_fpath)
|
||||
|
||||
def configure(self):
|
||||
# Set-up arm actuators (position mode)
|
||||
# We assume that at connection time, arm is in a rest position,
|
||||
# and torque can be safely disabled to run calibration.
|
||||
self.bus.disable_torque()
|
||||
self.bus.configure_motors()
|
||||
for name in self.arm_motors:
|
||||
self.bus.write("Operating_Mode", name, OperatingMode.POSITION.value)
|
||||
# Set P_Coefficient to lower value to avoid shakiness (Default is 32)
|
||||
self.bus.write("P_Coefficient", name, 16)
|
||||
# Set I_Coefficient and D_Coefficient to default value 0 and 32
|
||||
self.bus.write("I_Coefficient", name, 0)
|
||||
self.bus.write("D_Coefficient", name, 32)
|
||||
|
||||
for name in self.base_motors:
|
||||
self.bus.write("Operating_Mode", name, OperatingMode.VELOCITY.value)
|
||||
|
||||
self.bus.enable_torque()
|
||||
|
||||
def setup_motors(self) -> None:
|
||||
for motor in chain(reversed(self.arm_motors), reversed(self.base_motors)):
|
||||
input(f"Connect the controller board to the '{motor}' motor only and press enter.")
|
||||
self.bus.setup_motor(motor)
|
||||
print(f"'{motor}' motor id set to {self.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, 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 = [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_cmd, y_cmd, theta_cmd) where:
|
||||
OBS_STATE.x_cmd : Linear velocity in x (m/s).
|
||||
OBS_STATE.y_cmd : Linear velocity in y (m/s).
|
||||
OBS_STATE.theta_cmd : Rotational velocity in deg/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, 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, 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 get_observation(self) -> dict[str, Any]:
|
||||
if not self.is_connected:
|
||||
raise DeviceNotConnectedError(f"{self} is not connected.")
|
||||
|
||||
# Read actuators position for arm and vel for base
|
||||
start = time.perf_counter()
|
||||
arm_pos = self.bus.sync_read("Present_Position", self.arm_motors)
|
||||
base_wheel_vel = self.bus.sync_read("Present_Velocity", self.base_motors)
|
||||
|
||||
base_vel = self._wheel_raw_to_body(
|
||||
base_wheel_vel["base_left_wheel"],
|
||||
base_wheel_vel["base_back_wheel"],
|
||||
base_wheel_vel["base_right_wheel"],
|
||||
)
|
||||
|
||||
arm_state = {f"{k}.pos": v for k, v in arm_pos.items()}
|
||||
|
||||
flat_states = {**arm_state, **base_vel}
|
||||
|
||||
obs_dict = {f"{OBS_STATE}": flat_states}
|
||||
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[f"{OBS_IMAGES}.{cam_key}"] = cam.async_read()
|
||||
dt_ms = (time.perf_counter() - start) * 1e3
|
||||
logger.debug(f"{self} read {cam_key}: {dt_ms:.1f}ms")
|
||||
|
||||
return obs_dict
|
||||
|
||||
def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
|
||||
"""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.
|
||||
|
||||
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.")
|
||||
|
||||
arm_goal_pos = {k: v for k, v in action.items() if k.endswith(".pos")}
|
||||
base_goal_vel = {k: v for k, v in action.items() if k.endswith(".vel")}
|
||||
|
||||
base_wheel_goal_vel = self._body_to_wheel_raw(
|
||||
base_goal_vel["x.vel"], base_goal_vel["y.vel"], base_goal_vel["theta.vel"]
|
||||
)
|
||||
|
||||
# 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.bus.sync_read("Present_Position", self.arm_motors)
|
||||
goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in arm_goal_pos.items()}
|
||||
arm_safe_goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target)
|
||||
arm_goal_pos = arm_safe_goal_pos
|
||||
|
||||
# Send goal position to the actuators
|
||||
arm_goal_pos_raw = {k.replace(".pos", ""): v for k, v in arm_goal_pos.items()}
|
||||
self.bus.sync_write("Goal_Position", arm_goal_pos_raw)
|
||||
self.bus.sync_write("Goal_Velocity", base_wheel_goal_vel)
|
||||
|
||||
return {**arm_goal_pos, **base_goal_vel}
|
||||
|
||||
def stop_base(self):
|
||||
self.bus.sync_write("Goal_Velocity", dict.fromkeys(self.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()
|
||||
self.bus.disconnect(self.config.disable_torque_on_disconnect)
|
||||
for cam in self.cameras.values():
|
||||
cam.disconnect()
|
||||
|
||||
logger.info(f"{self} disconnected.")
|
||||
@@ -0,0 +1,342 @@
|
||||
# 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 torch
|
||||
import zmq
|
||||
|
||||
from lerobot.common.constants import OBS_IMAGES, OBS_STATE
|
||||
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
|
||||
|
||||
from ..robot import Robot
|
||||
from .config_lekiwi import LeKiwiClientConfig
|
||||
|
||||
|
||||
class LeKiwiClient(Robot):
|
||||
config_class = LeKiwiClientConfig
|
||||
name = "lekiwi_client"
|
||||
|
||||
def __init__(self, config: LeKiwiClientConfig):
|
||||
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.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 = {}
|
||||
|
||||
# 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._is_connected = False
|
||||
self.logs = {}
|
||||
|
||||
@cached_property
|
||||
def _state_ft(self) -> dict[str, type]:
|
||||
return dict.fromkeys(
|
||||
(
|
||||
"arm_shoulder_pan.pos",
|
||||
"arm_shoulder_lift.pos",
|
||||
"arm_elbow_flex.pos",
|
||||
"arm_wrist_flex.pos",
|
||||
"arm_wrist_roll.pos",
|
||||
"arm_gripper.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]:
|
||||
return {
|
||||
"front": (480, 640, 3),
|
||||
"wrist": (640, 480, 3),
|
||||
}
|
||||
|
||||
@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(
|
||||
"LeKiwi Daemon is already connected. Do not run `robot.connect()` twice."
|
||||
)
|
||||
|
||||
self.zmq_context = zmq.Context()
|
||||
self.zmq_cmd_socket = self.zmq_context.socket(zmq.PUSH)
|
||||
zmq_cmd_locator = f"tcp://{self.remote_ip}:{self.port_zmq_cmd}"
|
||||
self.zmq_cmd_socket.connect(zmq_cmd_locator)
|
||||
self.zmq_cmd_socket.setsockopt(zmq.CONFLATE, 1)
|
||||
|
||||
self.zmq_observation_socket = self.zmq_context.socket(zmq.PULL)
|
||||
zmq_observations_locator = f"tcp://{self.remote_ip}:{self.port_zmq_observations}"
|
||||
self.zmq_observation_socket.connect(zmq_observations_locator)
|
||||
self.zmq_observation_socket.setsockopt(zmq.CONFLATE, 1)
|
||||
|
||||
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 LeKiwi 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 = observation[OBS_STATE]
|
||||
|
||||
state_vec = np.array(
|
||||
[flat_state.get(k, 0.0) for k in self._state_order],
|
||||
dtype=np.float32,
|
||||
)
|
||||
|
||||
# Decode images
|
||||
image_observation = {k: v for k, v in observation.items() if k.startswith(OBS_IMAGES)}
|
||||
current_frames: Dict[str, np.ndarray] = {}
|
||||
for cam_name, image_b64 in image_observation.items():
|
||||
frame = self._decode_image_from_b64(image_b64)
|
||||
if frame is not None:
|
||||
current_frames[cam_name] = frame
|
||||
|
||||
return current_frames, {"observation.state": state_vec}
|
||||
|
||||
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("LeKiwiClient 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] = torch.from_numpy(frame)
|
||||
|
||||
return obs_dict
|
||||
|
||||
def _from_keyboard_to_base_action(self, pressed_keys: np.ndarray):
|
||||
# Speed control
|
||||
if self.teleop_keys["speed_up"] in pressed_keys:
|
||||
self.speed_index = min(self.speed_index + 1, 2)
|
||||
if self.teleop_keys["speed_down"] in pressed_keys:
|
||||
self.speed_index = max(self.speed_index - 1, 0)
|
||||
speed_setting = self.speed_levels[self.speed_index]
|
||||
xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4
|
||||
theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90
|
||||
|
||||
x_cmd = 0.0 # m/s forward/backward
|
||||
y_cmd = 0.0 # m/s lateral
|
||||
theta_cmd = 0.0 # deg/s rotation
|
||||
|
||||
if self.teleop_keys["forward"] in pressed_keys:
|
||||
x_cmd += xy_speed
|
||||
if self.teleop_keys["backward"] in pressed_keys:
|
||||
x_cmd -= xy_speed
|
||||
if self.teleop_keys["left"] in pressed_keys:
|
||||
y_cmd += xy_speed
|
||||
if self.teleop_keys["right"] in pressed_keys:
|
||||
y_cmd -= xy_speed
|
||||
if self.teleop_keys["rotate_left"] in pressed_keys:
|
||||
theta_cmd += theta_speed
|
||||
if self.teleop_keys["rotate_right"] in pressed_keys:
|
||||
theta_cmd -= theta_speed
|
||||
return {
|
||||
"x.vel": x_cmd,
|
||||
"y.vel": y_cmd,
|
||||
"theta.vel": theta_cmd,
|
||||
}
|
||||
|
||||
def configure(self):
|
||||
pass
|
||||
|
||||
def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
|
||||
"""Command lekiwi to move to a target joint configuration. Translates to motor space + sends over ZMQ
|
||||
|
||||
Args:
|
||||
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()`."
|
||||
)
|
||||
|
||||
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)
|
||||
return {"action": actions}
|
||||
|
||||
def disconnect(self):
|
||||
"""Cleans ZMQ comms"""
|
||||
|
||||
if not self._is_connected:
|
||||
raise DeviceNotConnectedError(
|
||||
"LeKiwi is not connected. You need to run `robot.connect()` before disconnecting."
|
||||
)
|
||||
self.zmq_observation_socket.close()
|
||||
self.zmq_cmd_socket.close()
|
||||
self.zmq_context.term()
|
||||
self._is_connected = False
|
||||
@@ -0,0 +1,129 @@
|
||||
#!/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 lerobot.common.constants import OBS_IMAGES
|
||||
|
||||
from .config_lekiwi import LeKiwiConfig, LeKiwiHostConfig
|
||||
from .lekiwi import LeKiwi
|
||||
|
||||
|
||||
class LeKiwiHost:
|
||||
def __init__(self, config: LeKiwiHostConfig):
|
||||
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 LeKiwi")
|
||||
robot_config = LeKiwiConfig()
|
||||
robot = LeKiwi(robot_config)
|
||||
|
||||
logging.info("Connecting LeKiwi")
|
||||
robot.connect()
|
||||
|
||||
logging.info("Starting HostAgent")
|
||||
host_config = LeKiwiHostConfig()
|
||||
host = LeKiwiHost(host_config)
|
||||
|
||||
last_cmd_time = time.time()
|
||||
watchdog_active = False
|
||||
logging.info("Waiting for commands...")
|
||||
try:
|
||||
# Business logic
|
||||
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[f"{OBS_IMAGES}.{cam_key}"], [int(cv2.IMWRITE_JPEG_QUALITY), 90]
|
||||
)
|
||||
if ret:
|
||||
last_observation[f"{OBS_IMAGES}.{cam_key}"] = base64.b64encode(buffer).decode("utf-8")
|
||||
else:
|
||||
last_observation[f"{OBS_IMAGES}.{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 Lekiwi Host.")
|
||||
robot.disconnect()
|
||||
host.disconnect()
|
||||
|
||||
logging.info("Finished LeKiwi cleanly")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user