+
+ 
+ 
+
-
+
+ Meet the SO-100 – Just $110 per arm!
+ +Meet the updated SO100, the SO-101 – Just €114 per arm!
Train it in minutes with a few simple moves on your laptop.
Then sit back and watch your creation act autonomously! 🤯
-- Get the full SO-100 tutorial here.
++ See the full SO-101 tutorial here.
-Want to take it to the next level? Make your SO-100 mobile by building LeKiwi!
+Want to take it to the next level? Make your SO-101 mobile by building LeKiwi!
Check out the LeKiwi tutorial and bring your robot to life on wheels.
@@ -51,7 +65,6 @@
---
-
🤗 LeRobot aims to provide models, datasets, and tools for real-world robotics in PyTorch. The goal is to lower the barrier to entry to robotics so that everyone can contribute and benefit from sharing datasets and pretrained models.
🤗 LeRobot contains state-of-the-art approaches that have been shown to transfer to the real-world with a focus on imitation learning and reinforcement learning.
@@ -103,13 +116,20 @@ When using `miniconda`, install `ffmpeg` in your environment:
conda install ffmpeg -c conda-forge
```
+> **NOTE:** This usually installs `ffmpeg 7.X` for your platform compiled with the `libsvtav1` encoder. If `libsvtav1` is not supported (check supported encoders with `ffmpeg -encoders`), you can:
+> - _[On any platform]_ Explicitly install `ffmpeg 7.X` using:
+> ```bash
+> conda install ffmpeg=7.1.1 -c conda-forge
+> ```
+> - _[On Linux only]_ Install [ffmpeg build dependencies](https://trac.ffmpeg.org/wiki/CompilationGuide/Ubuntu#GettheDependencies) and [compile ffmpeg from source with libsvtav1](https://trac.ffmpeg.org/wiki/CompilationGuide/Ubuntu#libsvtav1), and make sure you use the corresponding ffmpeg binary to your install with `which ffmpeg`.
+
Install 🤗 LeRobot:
```bash
pip install -e .
```
> **NOTE:** If you encounter build errors, you may need to install additional dependencies (`cmake`, `build-essential`, and `ffmpeg libs`). On Linux, run:
-`sudo apt-get install cmake build-essential python-dev pkg-config libavformat-dev libavcodec-dev libavdevice-dev libavutil-dev libswscale-dev libswresample-dev libavfilter-dev pkg-config`. For other systems, see: [Compiling PyAV](https://pyav.org/docs/develop/overview/installation.html#bring-your-own-ffmpeg)
+`sudo apt-get install cmake build-essential python3-dev pkg-config libavformat-dev libavcodec-dev libavdevice-dev libavutil-dev libswscale-dev libswresample-dev libavfilter-dev pkg-config`. For other systems, see: [Compiling PyAV](https://pyav.org/docs/develop/overview/installation.html#bring-your-own-ffmpeg)
For simulations, 🤗 LeRobot comes with gymnasium environments that can be installed as extras:
- [aloha](https://github.com/huggingface/gym-aloha)
@@ -201,7 +221,7 @@ dataset attributes:
│ ├ episode_index (int64): index of the episode for this sample
│ ├ frame_index (int64): index of the frame for this sample in the episode ; starts at 0 for each episode
│ ├ timestamp (float32): timestamp in the episode
- │ ├ next.done (bool): indicates the end of en episode ; True for the last frame in each episode
+ │ ├ next.done (bool): indicates the end of an episode ; True for the last frame in each episode
│ └ index (int64): general index in the whole dataset
├ episode_data_index: contains 2 tensors with the start and end indices of each episode
│ ├ from (1D int64 tensor): first frame index for each episode — shape (num episodes,) starts with 0
@@ -250,7 +270,7 @@ See `python lerobot/scripts/eval.py --help` for more instructions.
### Train your own policy
-Check out [example 3](./examples/3_train_policy.py) that illustrate how to train a model using our core library in python, and [example 4](./examples/4_train_policy_with_script.md) that shows how to use our training script from command line.
+Check out [example 3](./examples/3_train_policy.py) that illustrates how to train a model using our core library in python, and [example 4](./examples/4_train_policy_with_script.md) that shows how to use our training script from command line.
To use wandb for logging training and evaluation curves, make sure you've run `wandb login` as a one-time setup step. Then, when running the training command above, enable WandB in the configuration by adding `--wandb.enable=true`.
@@ -301,7 +321,7 @@ Once you have trained a policy you may upload it to the Hugging Face hub using a
You first need to find the checkpoint folder located inside your experiment directory (e.g. `outputs/train/2024-05-05/20-21-12_aloha_act_default/checkpoints/002500`). Within that there is a `pretrained_model` directory which should contain:
- `config.json`: A serialized version of the policy configuration (following the policy's dataclass config).
- `model.safetensors`: A set of `torch.nn.Module` parameters, saved in [Hugging Face Safetensors](https://huggingface.co/docs/safetensors/index) format.
-- `train_config.json`: A consolidated configuration containing all parameter userd for training. The policy configuration should match `config.json` exactly. Thisis useful for anyone who wants to evaluate your policy or for reproducibility.
+- `train_config.json`: A consolidated configuration containing all parameters used for training. The policy configuration should match `config.json` exactly. This is useful for anyone who wants to evaluate your policy or for reproducibility.
To upload these to the hub, run the following:
```bash
diff --git a/benchmarks/video/run_video_benchmark.py b/benchmarks/video/run_video_benchmark.py
index c62578c46..9d587ee9f 100644
--- a/benchmarks/video/run_video_benchmark.py
+++ b/benchmarks/video/run_video_benchmark.py
@@ -416,7 +416,7 @@ if __name__ == "__main__":
"--vcodec",
type=str,
nargs="*",
- default=["libx264", "libx265", "libsvtav1"],
+ default=["libx264", "hevc", "libsvtav1"],
help="Video codecs to be tested",
)
parser.add_argument(
@@ -446,7 +446,7 @@ if __name__ == "__main__":
# nargs="*",
# default=[0, 1],
# help="Use the fastdecode tuning option. 0 disables it. "
- # "For libx264 and libx265, only 1 is possible. "
+ # "For libx264 and libx265/hevc, only 1 is possible. "
# "For libsvtav1, 1, 2 or 3 are possible values with a higher number meaning a faster decoding optimization",
# )
parser.add_argument(
diff --git a/docker/lerobot-gpu-dev/Dockerfile b/docker/lerobot-gpu-dev/Dockerfile
index 561a7cff6..4d25b2550 100644
--- a/docker/lerobot-gpu-dev/Dockerfile
+++ b/docker/lerobot-gpu-dev/Dockerfile
@@ -14,7 +14,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
tcpdump sysstat screen tmux \
libglib2.0-0 libgl1-mesa-glx libegl1-mesa \
speech-dispatcher portaudio19-dev libgeos-dev \
- python${PYTHON_VERSION} python${PYTHON_VERSION}-venv \
+ python${PYTHON_VERSION} python${PYTHON_VERSION}-venv python${PYTHON_VERSION}-dev \
&& apt-get clean && rm -rf /var/lib/apt/lists/*
# Install ffmpeg build dependencies. See:
diff --git a/docs/README.md b/docs/README.md
new file mode 100644
index 000000000..275fee46b
--- /dev/null
+++ b/docs/README.md
@@ -0,0 +1,137 @@
+
+
+# Generating the documentation
+
+To generate the documentation, you first have to build it. Several packages are necessary to build the doc,
+you can install them with the following command, at the root of the code repository:
+
+```bash
+pip install -e ".[docs]"
+```
+
+You will also need `nodejs`. Please refer to their [installation page](https://nodejs.org/en/download)
+
+---
+**NOTE**
+
+You only need to generate the documentation to inspect it locally (if you're planning changes and want to
+check how they look before committing for instance). You don't have to `git commit` the built documentation.
+
+---
+
+## Building the documentation
+
+Once you have setup the `doc-builder` and additional packages, you can generate the documentation by
+typing the following command:
+
+```bash
+doc-builder build lerobot docs/source/ --build_dir ~/tmp/test-build
+```
+
+You can adapt the `--build_dir` to set any temporary folder that you prefer. This command will create it and generate
+the MDX files that will be rendered as the documentation on the main website. You can inspect them in your favorite
+Markdown editor.
+
+## Previewing the documentation
+
+To preview the docs, first install the `watchdog` module with:
+
+```bash
+pip install watchdog
+```
+
+Then run the following command:
+
+```bash
+doc-builder preview lerobot docs/source/
+```
+
+The docs will be viewable at [http://localhost:3000](http://localhost:3000). You can also preview the docs once you have opened a PR. You will see a bot add a comment to a link where the documentation with your changes lives.
+
+---
+**NOTE**
+
+The `preview` command only works with existing doc files. When you add a completely new file, you need to update `_toctree.yml` & restart `preview` command (`ctrl-c` to stop it & call `doc-builder preview ...` again).
+
+---
+
+## Adding a new element to the navigation bar
+
+Accepted files are Markdown (.md).
+
+Create a file with its extension and put it in the source directory. You can then link it to the toc-tree by putting
+the filename without the extension in the [`_toctree.yml`](https://github.com/huggingface/lerobot/blob/main/docs/source/_toctree.yml) file.
+
+## Renaming section headers and moving sections
+
+It helps to keep the old links working when renaming the section header and/or moving sections from one document to another. This is because the old links are likely to be used in Issues, Forums, and Social media and it'd make for a much more superior user experience if users reading those months later could still easily navigate to the originally intended information.
+
+Therefore, we simply keep a little map of moved sections at the end of the document where the original section was. The key is to preserve the original anchor.
+
+So if you renamed a section from: "Section A" to "Section B", then you can add at the end of the file:
+
+```
+Sections that were moved:
+
+[ Section A ]
+```
+and of course, if you moved it to another file, then:
+
+```
+Sections that were moved:
+
+[ Section A ]
+```
+
+Use the relative style to link to the new file so that the versioned docs continue to work.
+
+For an example of a rich moved sections set please see the very end of [the transformers Trainer doc](https://github.com/huggingface/transformers/blob/main/docs/source/en/main_classes/trainer.md).
+
+### Adding a new tutorial
+
+Adding a new tutorial or section is done in two steps:
+
+- Add a new file under `./source`. This file can either be ReStructuredText (.rst) or Markdown (.md).
+- Link that file in `./source/_toctree.yml` on the correct toc-tree.
+
+Make sure to put your new file under the proper section. If you have a doubt, feel free to ask in a Github Issue or PR.
+
+### Writing source documentation
+
+Values that should be put in `code` should either be surrounded by backticks: \`like so\`. Note that argument names
+and objects like True, None or any strings should usually be put in `code`.
+
+#### Writing a multi-line code block
+
+Multi-line code blocks can be useful for displaying examples. They are done between two lines of three backticks as usual in Markdown:
+
+
+````
+```
+# first line of code
+# second line
+# etc
+```
+````
+
+#### Adding an image
+
+Due to the rapidly growing repository, it is important to make sure that no files that would significantly weigh down the repository are added. This includes images, videos, and other non-text files. We prefer to leverage a hf.co hosted `dataset` like
+the ones hosted on [`hf-internal-testing`](https://huggingface.co/hf-internal-testing) in which to place these files and reference
+them by URL. We recommend putting them in the following dataset: [huggingface/documentation-images](https://huggingface.co/datasets/huggingface/documentation-images).
+If an external contribution, feel free to add the images to your PR and ask a Hugging Face member to migrate your images
+to this dataset.
diff --git a/docs/source/_toctree.yml b/docs/source/_toctree.yml
new file mode 100644
index 000000000..a0f69d0ac
--- /dev/null
+++ b/docs/source/_toctree.yml
@@ -0,0 +1,12 @@
+- sections:
+ - local: index
+ title: LeRobot
+ - local: installation
+ title: Installation
+ title: Get started
+- sections:
+ - local: assemble_so101
+ title: Assemble SO-101
+ - local: getting_started_real_world_robot
+ title: Getting Started with Real-World Robots
+ title: "Tutorials"
diff --git a/docs/source/assemble_so101.mdx b/docs/source/assemble_so101.mdx
new file mode 100644
index 000000000..de280a392
--- /dev/null
+++ b/docs/source/assemble_so101.mdx
@@ -0,0 +1,348 @@
+# Assemble SO-101
+
+In the steps below we explain how to assemble our flagship robot, the SO-101.
+
+## Source the parts
+
+Follow this [README](https://github.com/TheRobotStudio/SO-ARM100). It contains the bill of materials, with a link to source the parts, as well as the instructions to 3D print the parts,
+and advice if it's your first time printing or if you don't own a 3D printer.
+
+Before assembling, you will first need to configure your motors. To this end, we provide a nice script, so let's first install LeRobot. After configuration, we will also guide you through assembly.
+
+## Install LeRobot
+
+To install LeRobot follow our [Installation Guide](./installation)
+
+## Configure motors
+
+To configure the motors designate one bus servo adapter and 6 motors for your leader arm, and similarly the other bus servo adapter and 6 motors for the follower arm. It's convenient to label them and write on each motor if it's for the follower `F` or for the leader `L` and it's ID from 1 to 6.
+
+You now should plug the 5V or 12V power supply to the motor bus. 5V for the STS3215 7.4V motors and 12V for the STS3215 12V motors. Note that the leader arm always uses the 7.4V motors, so watch out that you plug in the right power supply if you have 12V and 7.4V motors, otherwise you might burn your motors! Now, connect the motor bus to your computer via USB. Note that the USB doesn't provide any power, and both the power supply and USB have to be plugged in.
+
+### Find the USB ports associated to each arm
+
+To find the port for each bus servo adapter, run this script:
+```bash
+python lerobot/scripts/find_motors_bus_port.py
+```
+##### Example outputs of script
+
+
+
+
+
+## Step-by-Step Assembly Instructions
+
+The follower arm uses 6x STS3215 motors with 1/345 gearing. The leader however uses three differently geared motors to make sure it can both sustain its own weight and it can be moved without requiring much force. Which motor is needed for which joint is shown in table below.
+
+| Leader-Arm Axis | Motor | Gear Ratio |
+|-----------------|:-------:|:----------:|
+| Base / Shoulder Yaw | 1 | 1 / 191 |
+| Shoulder Pitch | 2 | 1 / 345 |
+| Elbow | 3 | 1 / 191 |
+| Wrist Roll | 4 | 1 / 147 |
+| Wrist Pitch | 5 | 1 / 147 |
+| Gripper | 6 | 1 / 147 |
+
+### Set motor IDs
+
+Plug your motor in one of the two ports of the motor bus and run this script to set its ID to 1. Replace the text after --port to the corresponding control board port.
+```bash
+python lerobot/scripts/configure_motor.py \
+ --port /dev/tty.usbmodem58760432961 \
+ --brand feetech \
+ --model sts3215 \
+ --baudrate 1000000 \
+ --ID 1
+```
+
+Then unplug your motor and plug the second motor and set its ID to 2.
+```bash
+python lerobot/scripts/configure_motor.py \
+ --port /dev/tty.usbmodem58760432961 \
+ --brand feetech \
+ --model sts3215 \
+ --baudrate 1000000 \
+ --ID 2
+```
+
+Redo this process for all your motors until ID 6. Do the same for the 6 motors of the leader arm, but make sure to change the power supply if you use motors with different voltage and make sure you give the right ID to the right motor according to the table above.
+
+Here is a video of the process:
+
+
+
+
+### Clean Parts
+Remove all support material from the 3D-printed parts, the easiest way to do this is using a small screwdriver to get underneath the support material.
+
+### Joint 1
+
+- Place the first motor into the base.
+- Fasten the motor with 4 M2x6mm screws (smallest screws). Two from the top and two from bottom.
+- Slide over the first motor holder and fasten it using two M2x6mm screws (one on each side).
+- Install both motor horns, securing the top horn with a M3x6mm screw.
+- Attach the shoulder part.
+- Tighten the shoulder part with 4 M3x6mm screws on top and 4 M3x6mm screws on the bottom
+- Add the shoulder motor holder.
+
+
+
+
+
+### Joint 2
+
+- Slide the second motor in from the top.
+- Fasten the second motor with 4 M2x6mm screws.
+- Attach both motor horns to motor 2, again use the M3x6mm horn screw.
+- Attach the upper arm with 4 M3x6mm screws on each side.
+
+
+
+
+
+### Joint 3
+
+- Insert motor 3 and fasten using 4 M2x6mm screws
+- Attach both motor horns to motor 3 and secure one again with a M3x6mm horn screw.
+- Connect the forearm to motor 3 using 4 M3x6mm screws on each side.
+
+
+
+
+
+### Joint 4
+
+- Slide over motor holder 4.
+- Slide in motor 4.
+- Fasten motor 4 with 4 M2x6mm screws and attach its motor horns, use a M3x6mm horn screw.
+
+
+
+
+
+### Joint 5
+
+- Insert motor 5 into the wrist holder and secure it with 2 M2x6mm front screws.
+- Install only one motor horn on the wrist motor and secure it with a M3x6mm horn screw.
+- Secure the wrist to motor 4 using 4 M3x6mm screws on both sides.
+
+
+
+
+
+### Gripper / Handle
+
+
+
+
+
+
+
+
+
+
+
+
+
+## Calibrate
+
+Next, you'll need to calibrate your SO-101 robot to ensure that the leader and follower arms have the same position values when they are in the same physical position.
+The calibration process is very important because it allows a neural network trained on one SO-101 robot to work on another.
+
+#### Manual calibration of follower arm
+
+You will need to move the follower arm to these positions sequentially, note that the rotated position is on the right side of the robot and you have to open the gripper fully.
+
+| 1. Middle position | 2. Zero position | 3. Rotated position | 4. Rest position |
+| ------------ |------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| 
| 
| 
| 
|
+
+Make sure both arms are connected and run this script to launch manual calibration:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --robot.cameras='{}' \
+ --control.type=calibrate \
+ --control.arms='["main_follower"]'
+```
+
+#### Manual calibration of leader arm
+You will also need to move the leader arm to these positions sequentially:
+
+| 1. Middle position | 2. Zero position | 3. Rotated position | 4. Rest position |
+| ------------ |------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| 
| 
| 
| 
|
+
+Run this script to launch manual calibration:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --robot.cameras='{}' \
+ --control.type=calibrate \
+ --control.arms='["main_leader"]'
+```
+
+Congrats 🎉, your robot is all set to learn a task on its own. Start training it by following this tutorial: [Getting started with real-world robots](./getting_started_real_world_robot)
diff --git a/docs/source/getting_started_real_world_robot.mdx b/docs/source/getting_started_real_world_robot.mdx
new file mode 100644
index 000000000..dc7bb7419
--- /dev/null
+++ b/docs/source/getting_started_real_world_robot.mdx
@@ -0,0 +1,370 @@
+# Getting Started with Real-World Robots
+
+This tutorial will explain you how to train a neural network to autonomously control a real robot.
+
+**You'll learn:**
+1. How to record and visualize your dataset.
+2. How to train a policy using your data and prepare it for evaluation.
+3. How to evaluate your policy and visualize the results.
+
+By following these steps, you'll be able to replicate tasks like picking up a Lego block and placing it in a bin with a high success rate, as demonstrated in [this video](https://x.com/RemiCadene/status/1814680760592572934).
+
+This tutorial is specifically made for the affordable [SO-101](https://github.com/TheRobotStudio/SO-ARM100) robot, but it contains additional information to be easily adapted to various types of robots like [Aloha bimanual robot](https://aloha-2.github.io) by changing some configurations. The SO-101 consists of a leader arm and a follower arm, each with 6 motors. It can work with one or several cameras to record the scene, which serve as visual sensors for the robot.
+
+During the data collection phase, you will control the follower arm by moving the leader arm. This process is known as "teleoperation." This technique is used to collect robot trajectories. Afterward, you'll train a neural network to imitate these trajectories and deploy the network to enable your robot to operate autonomously.
+
+If you encounter any issues at any step of the tutorial, feel free to seek help on [Discord](https://discord.com/invite/s3KuuzsPFb) or don't hesitate to iterate with us on the tutorial by creating issues or pull requests.
+
+## Setup and Calibrate
+
+If you haven't yet setup and calibrate the SO-101 follow these steps:
+1. [Find ports and update config file](./assemble_so101#find-the-usb-ports-associated-to-each-arm)
+2. [Calibrate](./assemble_so101#calibrate)
+
+## Teleoperate
+
+Run this simple script to teleoperate your robot (it won't connect and display the cameras):
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --robot.cameras='{}' \
+ --control.type=teleoperate
+```
+
+The teleoperate command will automatically:
+1. Identify any missing calibrations and initiate the calibration procedure.
+2. Connect the robot and start teleoperation.
+
+## Setup Cameras
+
+To connect a camera you have three options:
+1. OpenCVCamera which allows us to use any camera: usb, realsense, laptop webcam
+2. iPhone camera with MacOS
+3. Phone camera on Linux
+
+### Use OpenCVCamera
+
+The [`OpenCVCamera`](../lerobot/common/robot_devices/cameras/opencv.py) class allows you to efficiently record frames from most cameras using the [`opencv2`](https://docs.opencv.org) library. For more details on compatibility, see [Video I/O with OpenCV Overview](https://docs.opencv.org/4.x/d0/da7/videoio_overview.html).
+
+To instantiate an [`OpenCVCamera`](../lerobot/common/robot_devices/cameras/opencv.py), you need a camera index (e.g. `OpenCVCamera(camera_index=0)`). When you only have one camera like a webcam of a laptop, the camera index is usually `0` but it might differ, and the camera index might change if you reboot your computer or re-plug your camera. This behavior depends on your operating system.
+
+To find the camera indices, run the following utility script, which will save a few frames from each detected camera:
+```bash
+python lerobot/common/robot_devices/cameras/opencv.py \
+ --images-dir outputs/images_from_opencv_cameras
+```
+
+The output will look something like this if you have two cameras connected:
+```
+Mac or Windows detected. Finding available camera indices through scanning all indices from 0 to 60
+[...]
+Camera found at index 0
+Camera found at index 1
+[...]
+Connecting cameras
+OpenCVCamera(0, fps=30.0, width=1920.0, height=1080.0, color_mode=rgb)
+OpenCVCamera(1, fps=24.0, width=1920.0, height=1080.0, color_mode=rgb)
+Saving images to outputs/images_from_opencv_cameras
+Frame: 0000 Latency (ms): 39.52
+[...]
+Frame: 0046 Latency (ms): 40.07
+Images have been saved to outputs/images_from_opencv_cameras
+```
+
+Check the saved images in `outputs/images_from_opencv_cameras` to identify which camera index corresponds to which physical camera (e.g. `0` for `camera_00` or `1` for `camera_01`):
+```
+camera_00_frame_000000.png
+[...]
+camera_00_frame_000047.png
+camera_01_frame_000000.png
+[...]
+camera_01_frame_000047.png
+```
+
+Note: Some cameras may take a few seconds to warm up, and the first frame might be black or green.
+
+Now that you have the camera indexes, you should specify the camera's in the config.
+
+### Use your phone
+
+ 
+
+
+## Replay an episode
+
+A useful feature is the `replay` function, which allows to replay on your robot any episode that you've recorded or episodes from any dataset out there. This function helps you test the repeatability of your robot's actions and assess transferability across robots of the same model.
+
+You can replay the first episode on your robot with:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --control.type=replay \
+ --control.fps=30 \
+ --control.repo_id=${HF_USER}/so101_test \
+ --control.episode=0
+```
+
+Your robot should replicate movements similar to those you recorded. For example, check out [this video](https://x.com/RemiCadene/status/1793654950905680090) where we use `replay` on a Aloha robot from [Trossen Robotics](https://www.trossenrobotics.com).
+
+## Train a policy
+
+To train a policy to control your robot, use the [`python lerobot/scripts/train.py`](../lerobot/scripts/train.py) script. A few arguments are required. Here is an example command:
+```bash
+python lerobot/scripts/train.py \
+ --dataset.repo_id=${HF_USER}/so101_test \
+ --policy.type=act \
+ --output_dir=outputs/train/act_so101_test \
+ --job_name=act_so101_test \
+ --policy.device=cuda \
+ --wandb.enable=true
+```
+
+Let's explain the command:
+1. We provided the dataset as argument with `--dataset.repo_id=${HF_USER}/so101_test`.
+2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor states, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
+4. We provided `policy.device=cuda` since we are training on a Nvidia GPU, but you could use `policy.device=mps` to train on Apple silicon.
+5. We provided `wandb.enable=true` to use [Weights and Biases](https://docs.wandb.ai/quickstart) for visualizing training plots. This is optional but if you use it, make sure you are logged in by running `wandb login`.
+
+Training should take several hours. You will find checkpoints in `outputs/train/act_so101_test/checkpoints`.
+
+To resume training from a checkpoint, below is an example command to resume from `last` checkpoint of the `act_so101_test` policy:
+```bash
+python lerobot/scripts/train.py \
+ --config_path=outputs/train/act_so101_test/checkpoints/last/pretrained_model/train_config.json \
+ --resume=true
+```
+
+#### Upload policy checkpoints
+
+Once training is done, upload the latest checkpoint with:
+```bash
+huggingface-cli upload ${HF_USER}/act_so101_test \
+ outputs/train/act_so101_test/checkpoints/last/pretrained_model
+```
+
+You can also upload intermediate checkpoints with:
+```bash
+CKPT=010000
+huggingface-cli upload ${HF_USER}/act_so101_test${CKPT} \
+ outputs/train/act_so101_test/checkpoints/${CKPT}/pretrained_model
+```
+
+## Evaluate your policy
+
+You can use the `record` function from [`lerobot/scripts/control_robot.py`](../lerobot/scripts/control_robot.py) but with a policy checkpoint as input. For instance, run this command to record 10 evaluation episodes:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --control.type=record \
+ --control.fps=30 \
+ --control.single_task="Grasp a lego block and put it in the bin." \
+ --control.repo_id=${HF_USER}/eval_act_so101_test \
+ --control.tags='["tutorial"]' \
+ --control.warmup_time_s=5 \
+ --control.episode_time_s=30 \
+ --control.reset_time_s=30 \
+ --control.num_episodes=10 \
+ --control.push_to_hub=true \
+ --control.policy.path=outputs/train/act_so101_test/checkpoints/last/pretrained_model
+```
+
+As you can see, it's almost the same command as previously used to record your training dataset. Two things changed:
+1. There is an additional `--control.policy.path` argument which indicates the path to your policy checkpoint with (e.g. `outputs/train/eval_act_so101_test/checkpoints/last/pretrained_model`). You can also use the model repository if you uploaded a model checkpoint to the hub (e.g. `${HF_USER}/act_so101_test`).
+2. The name of dataset begins by `eval` to reflect that you are running inference (e.g. `${HF_USER}/eval_act_so101_test`).
diff --git a/docs/source/index.mdx b/docs/source/index.mdx
new file mode 100644
index 000000000..b8ff56ea7
--- /dev/null
+++ b/docs/source/index.mdx
@@ -0,0 +1,19 @@
+
+
+
+ 
+
+
+
+# LeRobot
+
+**State-of-the-art machine learning for real-world robotics**
+
+🤗 LeRobot aims to provide models, datasets, and tools for real-world robotics in PyTorch. The goal is to lower the barrier for entry to robotics so that everyone can contribute and benefit from sharing datasets and pretrained models.
+
+🤗 LeRobot contains state-of-the-art approaches that have been shown to transfer to the real-world with a focus on imitation learning and reinforcement learning.
+
+🤗 LeRobot already provides a set of pretrained models, datasets with human collected demonstrations, and simulated environments so that everyone can get started.
+
+🤗 LeRobot hosts pretrained models and datasets on the LeRobot HuggingFace page.
+
+Join the LeRobot community on [Discord](https://discord.gg/s3KuuzsPFb)
diff --git a/docs/source/installation.mdx b/docs/source/installation.mdx
new file mode 100644
index 000000000..3823d30e7
--- /dev/null
+++ b/docs/source/installation.mdx
@@ -0,0 +1,84 @@
+# Installation
+
+## Install LeRobot
+
+Download our source code:
+```bash
+git clone https://github.com/huggingface/lerobot.git
+cd lerobot
+```
+
+Create a virtual environment with Python 3.10, using [`Miniconda`](https://docs.anaconda.com/miniconda/install/#quick-command-line-install)
+```bash
+conda create -y -n lerobot python=3.10
+```
+
+Now restart the shell by running:
+
+
+
+ 
+
+
+For a visual walkthrough of the assembly process, you can refer to [this video tutorial](https://youtu.be/8nQIg9BwwTk).
+
+> [!IMPORTANT]
+> Since the production of this video, we simplified the configuration phase (detailed in [section C](#c-configure-the-motors)) of the motors.
+> Because of this, two things differ from the instructions in that video:
+> - Don't plug all the motors cables right away and wait for being instructed to do so in [section C](#c-configure-the-motors).
+> - Don't screw in the controller board (PCB) to the base right away and wait for being instructed to do so in [section C](#c-configure-the-motors).
+
+
+## B. Install LeRobot
+
+> [!TIP]
+> We use the Command Prompt (cmd) quite a lot. If you are not comfortable using the cmd or want to brush up using the command line you can have a look here: [Command line crash course](https://developer.mozilla.org/en-US/docs/Learn_web_development/Getting_started/Environment_setup/Command_line)
+
+Follow instructions on our [README](https://github.com/huggingface/lerobot) to install LeRobot.
+
+In addition to these instructions, you need to install the dynamixel sdk:
+```bash
+pip install -e ".[dynamixel]"
+```
+
+## C. Configure the motors
+
+### 1. Find the USB ports associated to each arm
+
+For each controller board (Waveshare Serial Bus Servo Driver Board, one for the leader arm and one for the follower), connect it first to your computer through usb. To then find the internal port its connected to -which we will need later on- run the utility script:
+```bash
+python -m lerobot.find_port
+```
+
+> [!NOTE]
+> Note: On Linux, you might need to give access to the USB ports by running:
+> ```bash
+> sudo chmod 666 /dev/ttyACM0
+> sudo chmod 666 /dev/ttyACM1
+> ```
+
+This will first display all currently available ports on your computer. As prompted by the script, unplug the controller board usb cable from your computer. The script will then detect which port has been disconnected and will display it.
+
+
+Example output when identifying the leader arm's port (e.g., `/dev/tty.usbmodem575E0031751` on Mac, or possibly `/dev/ttyACM0` on Linux):
+```
+Finding all available ports for the MotorBus.
+['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751']
+Remove the usb cable from your MotorsBus and press Enter when done.
+
+[...Disconnect leader arm and press Enter...]
+
+The port of this MotorsBus is /dev/tty.usbmodem575E0031751
+Reconnect the usb cable.
+```
+
+You can now reconnect the usb cable to your computer.
+
+### 2. Set the motors ids and baudrate
+
+Each motor is identified by a unique id on the bus. When brand new, motors usually come with a default id of `1`. For the communication to work properly between the motors and the controller, we first need to set a unique, different id to each motor. Additionally, the speed at which data is transmitted on the bus is determined by the baudrate. In order to talk to each other, the controller and all the motors need to be configured with the same baudrate.
+
+To that end, we first need to connect to each motor individually with the controller in order to set these. Since we will write these parameters in the non-volatile section of the motors' internal memory (EEPROM), we'll only need to do this once.
+
+> [!NOTE]
+> Note: If you are repurposing motors from another robot, you will probably also need to perform this step as the ids and baudrate likely won't match.
+
+Connect the usb cable from your computer and the 5V power supply to the leader arm's controller board. Then, run the following command with the port you got from the previous step. You'll also need to give your leader arm a name with the `id` parameter.
+
+```bash
+python -m lerobot.setup_motors \
+ --device.type=so100_leader \
+ --device.port=/dev/tty.usbmodem575E0031751 \ # <- paste here the port found at previous step
+ --device.id=my_awesome_leader_arm # <- give it a nice, unique name
+```
+
+Note that the command above is equivalent to running the following script:
+
+
+
+
+
+You should see the following instruction
+```
+Connect the controller board to the 'gripper' motor only and press enter.
+```
+
+As instructed, plug the gripper's motor. Make sure it's the only motor connected to the board, and that the motor itself is not yet daisy chained to any other motor. As you press `[Enter]`, the script will automatically set the id and baudrate for that motor.
+
+
+Setup script
+ + ```python + from lerobot.common.teleoperators.koch import KochLeader, KochLeaderConfig + + config = KochLeaderConfig( + port="/dev/tty.usbmodem575E0031751", + id="my_awesome_leader_arm", + ) + leader = KochLeader(config) + leader.setup_motors() + ``` +
+
+
+You should then see the following message:
+```
+'gripper' motor id set to 6
+```
+
+Followed by the next instruction:
+```
+Connect the controller board to the 'wrist_roll' motor only and press enter.
+```
+
+You can disconnect the 3-pin cable from the controller board but you can leave it connected to the gripper motor on the other end as it will already be in the right place. Now, plug-in another 3-pin cable to the wrist roll motor and connect it to the controller board. As with the previous motor, make sure it is the only motor connected to the board and that the motor itself isn't connected to any other one.
+
+Repeat the operation for each motor as instructed.
+
+> [!TIP]
+> Check your cabling at each step before pressing Enter. For instance, the power supply cable is not solidly anchored to the board and might disconnect easily as you manipulate the board.
+
+When you are done, the script will simply finish, at which point the motors are ready to be used. You can now plug the 3-pin cable from each motor to the next one, and the cable from the first motor (the 'shoulder pan' with id=1) to the controller board, which can now be attached to the base of the arm.
+
+## D. Calibrate
+
+Next, you'll need to calibrate your SO-100 robot to ensure that the leader and follower arms have the same position values when they are in the same physical position. This calibration is essential because it allows a neural network trained on one SO-100 robot to work on another.
+
+#### a. Manual calibration of follower arm
+
+> [!IMPORTANT]
+> Contrarily to step 6 of the [assembly video](https://youtu.be/FioA2oeFZ5I?t=724) which illustrates the auto calibration, we will actually do manual calibration of follower for now.
+
+You will need to move the follower arm to these positions sequentially:
+
+| 1. Zero position | 2. Rotated position | 3. Rest position |
+| ------------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| Troubleshooting
+ + If you get an error at that point, check your cables and make sure they are plugged-in properly: + - Power supply + - USB cable between from your computer to the controller board + - The 3-pin cable from the controller board to the motor. + + If you are using a Waveshare controller board, make sure that the two jumpers are set on the `B` channel (USB). +
| 
| 
|
+
+Make sure both arms are connected and run this script to launch manual calibration:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so100 \
+ --robot.cameras='{}' \
+ --control.type=calibrate \
+ --control.arms='["main_follower"]'
+```
+
+#### b. Manual calibration of leader arm
+Follow step 6 of the [assembly video](https://youtu.be/FioA2oeFZ5I?t=724) which illustrates the manual calibration. You will need to move the leader arm to these positions sequentially:
+
+| 1. Zero position | 2. Rotated position | 3. Rest position |
+| ------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| 
| 
| 
|
+
+Run this script to launch manual calibration:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so100 \
+ --robot.cameras='{}' \
+ --control.type=calibrate \
+ --control.arms='["main_leader"]'
+```
+
+## E. Teleoperate
+
+**Simple teleop**
+Then you are ready to teleoperate your robot! Run this simple script (it won't connect and display the cameras):
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so100 \
+ --robot.cameras='{}' \
+ --control.type=teleoperate
+```
+
+
+#### a. Teleop with displaying cameras
+Follow [this guide to setup your cameras](https://github.com/huggingface/lerobot/blob/main/examples/7_get_started_with_real_robot.md#c-add-your-cameras-with-opencvcamera). Then you will be able to display the cameras on your computer while you are teleoperating by running the following code. This is useful to prepare your setup before recording your first dataset.
+
+> **NOTE:** To visualize the data, enable `--control.display_data=true`. This streams the data using `rerun`.
+
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so100 \
+ --control.type=teleoperate
+```
+
+## F. Record a dataset
+
+Once you're familiar with teleoperation, you can record your first dataset with SO-100.
+
+If you want to use the Hugging Face hub features for uploading your dataset and you haven't previously done it, make sure you've logged in using a write-access token, which can be generated from the [Hugging Face settings](https://huggingface.co/settings/tokens):
+```bash
+huggingface-cli login --token ${HUGGINGFACE_TOKEN} --add-to-git-credential
+```
+
+Store your Hugging Face repository name in a variable to run these commands:
+```bash
+HF_USER=$(huggingface-cli whoami | head -n 1)
+echo $HF_USER
+```
+
+Record 2 episodes and upload your dataset to the hub:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so100 \
+ --control.type=record \
+ --control.fps=30 \
+ --control.single_task="Grasp a lego block and put it in the bin." \
+ --control.repo_id=${HF_USER}/so100_test \
+ --control.tags='["so100","tutorial"]' \
+ --control.warmup_time_s=5 \
+ --control.episode_time_s=30 \
+ --control.reset_time_s=30 \
+ --control.num_episodes=2 \
+ --control.push_to_hub=true
+```
+
+Note: You can resume recording by adding `--control.resume=true`.
+
+## G. Visualize a dataset
+
+If you uploaded your dataset to the hub with `--control.push_to_hub=true`, you can [visualize your dataset online](https://huggingface.co/spaces/lerobot/visualize_dataset) by copy pasting your repo id given by:
+```bash
+echo ${HF_USER}/so100_test
+```
+
+If you didn't upload with `--control.push_to_hub=false`, you can also visualize it locally with (a window can be opened in the browser `http://127.0.0.1:9090` with the visualization tool):
+```bash
+python lerobot/scripts/visualize_dataset_html.py \
+ --repo-id ${HF_USER}/so100_test \
+ --local-files-only 1
+```
+
+## H. Replay an episode
+
+Now try to replay the first episode on your robot:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so100 \
+ --control.type=replay \
+ --control.fps=30 \
+ --control.repo_id=${HF_USER}/so100_test \
+ --control.episode=0
+```
+
+## I. Train a policy
+
+To train a policy to control your robot, use the [`python lerobot/scripts/train.py`](../lerobot/scripts/train.py) script. A few arguments are required. Here is an example command:
+```bash
+python lerobot/scripts/train.py \
+ --dataset.repo_id=${HF_USER}/so100_test \
+ --policy.type=act \
+ --output_dir=outputs/train/act_so100_test \
+ --job_name=act_so100_test \
+ --policy.device=cuda \
+ --wandb.enable=true
+```
+
+Let's explain it:
+1. We provided the dataset as argument with `--dataset.repo_id=${HF_USER}/so100_test`.
+2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor sates, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
+4. We provided `policy.device=cuda` since we are training on a Nvidia GPU, but you could use `policy.device=mps` to train on Apple silicon.
+5. We provided `wandb.enable=true` to use [Weights and Biases](https://docs.wandb.ai/quickstart) for visualizing training plots. This is optional but if you use it, make sure you are logged in by running `wandb login`.
+
+Training should take several hours. You will find checkpoints in `outputs/train/act_so100_test/checkpoints`.
+
+To resume training from a checkpoint, below is an example command to resume from `last` checkpoint of the `act_so100_test` policy:
+```bash
+python lerobot/scripts/train.py \
+ --config_path=outputs/train/act_so100_test/checkpoints/last/pretrained_model/train_config.json \
+ --resume=true
+```
+
+## J. Evaluate your policy
+
+You can use the `record` function from [`lerobot/scripts/control_robot.py`](../lerobot/scripts/control_robot.py) but with a policy checkpoint as input. For instance, run this command to record 10 evaluation episodes:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so100 \
+ --control.type=record \
+ --control.fps=30 \
+ --control.single_task="Grasp a lego block and put it in the bin." \
+ --control.repo_id=${HF_USER}/eval_act_so100_test \
+ --control.tags='["tutorial"]' \
+ --control.warmup_time_s=5 \
+ --control.episode_time_s=30 \
+ --control.reset_time_s=30 \
+ --control.num_episodes=10 \
+ --control.push_to_hub=true \
+ --control.policy.path=outputs/train/act_so100_test/checkpoints/last/pretrained_model
+```
+
+As you can see, it's almost the same command as previously used to record your training dataset. Two things changed:
+1. There is an additional `--control.policy.path` argument which indicates the path to your policy checkpoint with (e.g. `outputs/train/eval_act_so100_test/checkpoints/last/pretrained_model`). You can also use the model repository if you uploaded a model checkpoint to the hub (e.g. `${HF_USER}/act_so100_test`).
+2. The name of dataset begins by `eval` to reflect that you are running inference (e.g. `${HF_USER}/eval_act_so100_test`).
+
+## K. More Information
+
+Follow this [previous tutorial](https://github.com/huggingface/lerobot/blob/main/examples/7_get_started_with_real_robot.md#4-train-a-policy-on-your-data) for a more in-depth tutorial on controlling real robots with LeRobot.
+
+> [!TIP]
+> If you have any questions or need help, please reach out on [Discord](https://discord.com/invite/s3KuuzsPFb) in the channel [`#so100-arm`](https://discord.com/channels/1216765309076115607/1237741463832363039).
diff --git a/lerobot/common/robots/koch/__init__.py b/lerobot/common/robots/koch_follower/__init__.py
similarity index 100%
rename from lerobot/common/robots/koch/__init__.py
rename to lerobot/common/robots/koch_follower/__init__.py
diff --git a/lerobot/common/robots/koch/config_koch_follower.py b/lerobot/common/robots/koch_follower/config_koch_follower.py
similarity index 100%
rename from lerobot/common/robots/koch/config_koch_follower.py
rename to lerobot/common/robots/koch_follower/config_koch_follower.py
diff --git a/lerobot/common/robots/koch/koch_follower.py b/lerobot/common/robots/koch_follower/koch_follower.py
similarity index 77%
rename from lerobot/common/robots/koch/koch_follower.py
rename to lerobot/common/robots/koch_follower/koch_follower.py
index 2395118db..d37f50ac8 100644
--- a/lerobot/common/robots/koch/koch_follower.py
+++ b/lerobot/common/robots/koch_follower/koch_follower.py
@@ -16,10 +16,11 @@
import logging
import time
+from functools import cached_property
from typing import Any
from lerobot.common.cameras.utils import make_cameras_from_configs
-from lerobot.common.constants import OBS_IMAGES, OBS_STATE
+from lerobot.common.constants import OBS_STATE
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode
from lerobot.common.motors.dynamixel import (
@@ -62,34 +63,29 @@ class KochFollower(Robot):
self.cameras = make_cameras_from_configs(config.cameras)
@property
- def state_feature(self) -> dict:
+ def _motors_ft(self) -> dict[str, type]:
+ return {f"{motor}.pos": float for motor in self.arm.motors}
+
+ @property
+ def _cameras_ft(self) -> dict[str, tuple]:
return {
- "dtype": "float32",
- "shape": (len(self.arm),),
- "names": {"motors": list(self.arm.motors)},
+ cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras
}
- @property
- def action_feature(self) -> dict:
- return self.state_feature
+ @cached_property
+ def observation_features(self) -> dict[str, type | tuple]:
+ return {**self._motors_ft, **self._cameras_ft}
- @property
- def camera_features(self) -> dict[str, dict]:
- cam_ft = {}
- for cam_key, cam in self.cameras.items():
- cam_ft[cam_key] = {
- "shape": (cam.height, cam.width, cam.channels),
- "names": ["height", "width", "channels"],
- "info": None,
- }
- return cam_ft
+ @cached_property
+ def action_features(self) -> dict[str, type]:
+ return self._motors_ft
@property
def is_connected(self) -> bool:
# TODO(aliberts): add cam.is_connected for cam in self.cameras
return self.arm.is_connected
- def connect(self) -> None:
+ def connect(self, calibrate: bool = True) -> None:
"""
We assume that at connection time, arm is in a rest position,
and torque can be safely disabled to run calibration.
@@ -98,7 +94,7 @@ class KochFollower(Robot):
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.arm.connect()
- if not self.is_calibrated:
+ if not self.is_calibrated and calibrate:
self.calibrate()
for cam in self.cameras.values():
@@ -114,31 +110,31 @@ class KochFollower(Robot):
def calibrate(self) -> None:
logger.info(f"\nRunning calibration of {self}")
self.arm.disable_torque()
- for name in self.arm.names:
- self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
- input("Move robot to the middle of its range of motion and press ENTER....")
+ input(f"Move {self} to the middle of its range of motion and press ENTER....")
homing_offsets = self.arm.set_half_turn_homings()
full_turn_motors = ["shoulder_pan", "wrist_roll"]
- unknown_range_motors = [name for name in self.arm.names if name not in full_turn_motors]
- logger.info(
+ unknown_range_motors = [motor for motor in self.arm.motors if motor not in full_turn_motors]
+ print(
f"Move all joints except {full_turn_motors} sequentially through their entire "
"ranges of motion.\nRecording positions. Press ENTER to stop..."
)
range_mins, range_maxes = self.arm.record_ranges_of_motion(unknown_range_motors)
- for name in full_turn_motors:
- range_mins[name] = 0
- range_maxes[name] = 4095
+ for motor in full_turn_motors:
+ range_mins[motor] = 0
+ range_maxes[motor] = 4095
self.calibration = {}
- for name, motor in self.arm.motors.items():
- self.calibration[name] = MotorCalibration(
- id=motor.id,
+ for motor, m in self.arm.motors.items():
+ self.calibration[motor] = MotorCalibration(
+ id=m.id,
drive_mode=0,
- homing_offset=homing_offsets[name],
- range_min=range_mins[name],
- range_max=range_maxes[name],
+ homing_offset=homing_offsets[motor],
+ range_min=range_mins[motor],
+ range_max=range_maxes[motor],
)
self.arm.write_calibration(self.calibration)
@@ -151,9 +147,9 @@ class KochFollower(Robot):
# Use 'extended position mode' for all motors except gripper, because in joint mode the servos
# can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling
# the arm, you could end up with a servo with a position 0 or 4095 at a crucial point
- for name in self.arm.names:
- if name != "gripper":
- self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
+ for motor in self.arm.motors:
+ if motor != "gripper":
+ self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
# Use 'position control current based' for gripper to be limited by the limit of the current. For
# the follower gripper, it means it can grasp an object without forcing too much even tho, its
@@ -169,6 +165,12 @@ class KochFollower(Robot):
self.arm.write("Position_I_Gain", "elbow_flex", 0)
self.arm.write("Position_D_Gain", "elbow_flex", 600)
+ def setup_motors(self) -> None:
+ for motor in reversed(self.arm.motors):
+ input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+ self.arm.setup_motor(motor)
+ print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
+
def get_observation(self) -> dict[str, Any]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
@@ -178,13 +180,14 @@ class KochFollower(Robot):
# Read arm position
start = time.perf_counter()
obs_dict[OBS_STATE] = self.arm.sync_read("Present_Position")
+ obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()}
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()
+ 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")
@@ -206,7 +209,7 @@ class KochFollower(Robot):
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
- goal_pos = action
+ goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")}
# Cap goal position when too far away from present position.
# /!\ Slower fps expected due to reading from the follower.
@@ -217,7 +220,7 @@ class KochFollower(Robot):
# Send goal position to the arm
self.arm.sync_write("Goal_Position", goal_pos)
- return goal_pos
+ return {f"{motor}.pos": val for motor, val in goal_pos.items()}
def disconnect(self):
if not self.is_connected:
diff --git a/lerobot/common/robots/lekiwi/README.md b/lerobot/common/robots/lekiwi/README.md
index 1be7cbc4a..ffee8ce5d 100644
--- a/lerobot/common/robots/lekiwi/README.md
+++ b/lerobot/common/robots/lekiwi/README.md
@@ -134,7 +134,7 @@ First we will assemble the two SO100 arms. One to attach to the mobile base and
## SO100 Arms
### Configure motors
-The instructions for configuring the motors can be found [Here](https://github.com/huggingface/lerobot/blob/main/examples/10_use_so100.md#c-configure-the-motors) in step C of the SO100 tutorial. Besides the ID's for the arm motors we also need to set the motor ID's for the mobile base. These needs to be in a specific order to work. Below an image of the motor ID's and motor mounting positions for the mobile base. Note that we only use one Motor Control board on LeKiwi. This means the motor ID's for the wheels are 7, 8 and 9.
+The instructions for configuring the motors can be found [Here](https://github.com/huggingface/lerobot/blob/main/examples/10_use_so100.md#c-configure-the-motors) in step C of the SO100 tutorial. Besides the ID's for the arm motors we also need to set the motor ID's for the mobile base. These need to be in a specific order to work. Below an image of the motor ID's and motor mounting positions for the mobile base. Note that we only use one Motor Control board on LeKiwi. This means the motor ID's for the wheels are 7, 8 and 9.
@@ -194,11 +194,11 @@ sudo chmod 666 /dev/ttyACM1
#### d. Update config file
-IMPORTANTLY: Now that you have your ports of leader and follower arm and ip address of the mobile-so100, update the **ip** in Network configuration, **port** in leader_arms and **port** in lekiwi. In the [`LeKiwiRobotConfig`](../lerobot/common/robot_devices/robots/configs.py) file. Where you will find something like:
+IMPORTANTLY: Now that you have your ports of leader and follower arm and ip address of the mobile-so100, update the **ip** in Network configuration, **port** in leader_arms and **port** in lekiwi. In the [`LeKiwiConfig`](../lerobot/common/robot_devices/robots/configs.py) file. Where you will find something like:
```python
@RobotConfig.register_subclass("lekiwi")
@dataclass
-class LeKiwiRobotConfig(RobotConfig):
+class LeKiwiConfig(RobotConfig):
# `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes.
# Set this to a positive scalar to have the same value for all motors, or a list that is the same length as
# the number of motors in your follower arms.
@@ -281,7 +281,7 @@ For the wired LeKiwi version your configured IP address should refer to your own
```python
@RobotConfig.register_subclass("lekiwi")
@dataclass
-class LeKiwiRobotConfig(RobotConfig):
+class LeKiwiConfig(RobotConfig):
# `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes.
# Set this to a positive scalar to have the same value for all motors, or a list that is the same length as
# the number of motors in your follower arms.
@@ -446,7 +446,7 @@ You should see on your laptop something like this: ```[INFO] Connected to remote
| F | Decrease speed |
> [!TIP]
-> If you use a different keyboard you can change the keys for each command in the [`LeKiwiRobotConfig`](../lerobot/common/robot_devices/robots/configs.py).
+> If you use a different keyboard you can change the keys for each command in the [`LeKiwiConfig`](../lerobot/common/robot_devices/robots/configs.py).
### Wired version
If you have the **wired** LeKiwi version please run all commands including both these teleoperation commands on your laptop.
@@ -567,7 +567,7 @@ python lerobot/scripts/train.py \
Let's explain it:
1. We provided the dataset as argument with `--dataset.repo_id=${HF_USER}/lekiwi_test`.
-2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor sates, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
+2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor states, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
4. We provided `policy.device=cuda` since we are training on a Nvidia GPU, but you could use `policy.device=mps` to train on Apple silicon.
5. We provided `wandb.enable=true` to use [Weights and Biases](https://docs.wandb.ai/quickstart) for visualizing training plots. This is optional but if you use it, make sure you are logged in by running `wandb login`.
diff --git a/lerobot/common/robots/lekiwi/__init__.py b/lerobot/common/robots/lekiwi/__init__.py
new file mode 100644
index 000000000..e3d10c5c1
--- /dev/null
+++ b/lerobot/common/robots/lekiwi/__init__.py
@@ -0,0 +1,3 @@
+from .config_lekiwi import LeKiwiClientConfig, LeKiwiConfig
+from .lekiwi import LeKiwi
+from .lekiwi_client import LeKiwiClient
diff --git a/lerobot/common/robots/lekiwi/config_lekiwi.py b/lerobot/common/robots/lekiwi/config_lekiwi.py
new file mode 100644
index 000000000..946718a15
--- /dev/null
+++ b/lerobot/common/robots/lekiwi/config_lekiwi.py
@@ -0,0 +1,89 @@
+# 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
+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(
+ camera_index="/dev/video0", fps=30, width=640, height=480, rotation=None
+ ),
+ "wrist": OpenCVCameraConfig(
+ camera_index="/dev/video2", fps=30, width=640, height=480, rotation=180
+ ),
+ }
+ )
+
+
+@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
diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py
deleted file mode 100644
index 076dbbc25..000000000
--- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py
+++ /dev/null
@@ -1,89 +0,0 @@
-from dataclasses import dataclass, field
-
-from lerobot.common.cameras.configs import CameraConfig
-from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig
-from lerobot.common.motors.configs import FeetechMotorsBusConfig, MotorsBusConfig
-from lerobot.common.robots.config import RobotConfig
-
-
-@RobotConfig.register_subclass("lekiwi")
-@dataclass
-class LeKiwiRobotConfig(RobotConfig):
- # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes.
- # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as
- # the number of motors in your follower arms.
- max_relative_target: int | None = None
-
- # Network Configuration
- ip: str = "192.168.0.193"
- port: int = 5555
- video_port: int = 5556
-
- cameras: dict[str, CameraConfig] = field(
- default_factory=lambda: {
- "front": OpenCVCameraConfig(
- camera_index="/dev/video0", fps=30, width=640, height=480, rotation=90
- ),
- "wrist": OpenCVCameraConfig(
- camera_index="/dev/video2", fps=30, width=640, height=480, rotation=180
- ),
- }
- )
-
- calibration_dir: str = ".cache/calibration/lekiwi"
-
- leader_arms: dict[str, MotorsBusConfig] = field(
- default_factory=lambda: {
- "main": FeetechMotorsBusConfig(
- port="/dev/tty.usbmodem585A0077581",
- motors={
- # name: (index, model)
- "shoulder_pan": [1, "sts3215"],
- "shoulder_lift": [2, "sts3215"],
- "elbow_flex": [3, "sts3215"],
- "wrist_flex": [4, "sts3215"],
- "wrist_roll": [5, "sts3215"],
- "gripper": [6, "sts3215"],
- },
- ),
- }
- )
-
- follower_arms: dict[str, MotorsBusConfig] = field(
- default_factory=lambda: {
- "main": FeetechMotorsBusConfig(
- port="/dev/ttyACM0",
- motors={
- # name: (index, model)
- "shoulder_pan": [1, "sts3215"],
- "shoulder_lift": [2, "sts3215"],
- "elbow_flex": [3, "sts3215"],
- "wrist_flex": [4, "sts3215"],
- "wrist_roll": [5, "sts3215"],
- "gripper": [6, "sts3215"],
- "left_wheel": (7, "sts3215"),
- "back_wheel": (8, "sts3215"),
- "right_wheel": (9, "sts3215"),
- },
- ),
- }
- )
-
- teleop_keys: dict[str, str] = field(
- default_factory=lambda: {
- # Movement
- "forward": "w",
- "backward": "s",
- "left": "a",
- "right": "d",
- "rotate_left": "z",
- "rotate_right": "x",
- # Speed control
- "speed_up": "r",
- "speed_down": "f",
- # quit teleop
- "quit": "q",
- }
- )
-
- mock: bool = False
diff --git a/lerobot/common/robots/lekiwi/lekiwi.py b/lerobot/common/robots/lekiwi/lekiwi.py
new file mode 100644
index 000000000..02474898a
--- /dev/null
+++ b/lerobot/common/robots/lekiwi/lekiwi.py
@@ -0,0 +1,254 @@
+#!/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 typing import Any
+
+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
+ self.bus = FeetechMotorsBus(
+ port=self.config.port,
+ motors={
+ # arm
+ "arm_shoulder_pan": Motor(1, "sts3215", MotorNormMode.RANGE_M100_100),
+ "arm_shoulder_lift": Motor(2, "sts3215", MotorNormMode.RANGE_M100_100),
+ "arm_elbow_flex": Motor(3, "sts3215", MotorNormMode.RANGE_M100_100),
+ "arm_wrist_flex": Motor(4, "sts3215", MotorNormMode.RANGE_M100_100),
+ "arm_wrist_roll": Motor(5, "sts3215", MotorNormMode.RANGE_M100_100),
+ "arm_gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100),
+ # base
+ "base_left_wheel": Motor(7, "sts3215", MotorNormMode.RANGE_M100_100),
+ "base_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_feature(self) -> dict:
+ state_ft = {
+ "arm_shoulder_pan": {"dtype": "float32"},
+ "arm_shoulder_lift": {"dtype": "float32"},
+ "arm_elbow_flex": {"dtype": "float32"},
+ "arm_wrist_flex": {"dtype": "float32"},
+ "arm_wrist_roll": {"dtype": "float32"},
+ "arm_gripper": {"dtype": "float32"},
+ "base_left_wheel": {"dtype": "float32"},
+ "base_right_wheel": {"dtype": "float32"},
+ "base_back_wheel": {"dtype": "float32"},
+ }
+ return state_ft
+
+ @property
+ def action_feature(self) -> dict:
+ return self.state_feature
+
+ @property
+ def camera_features(self) -> dict[str, dict]:
+ cam_ft = {}
+ for cam_key, cam in self.cameras.items():
+ cam_ft[cam_key] = {
+ "shape": (cam.height, cam.width, cam.channels),
+ "names": ["height", "width", "channels"],
+ "info": None,
+ }
+ return cam_ft
+
+ @property
+ def is_connected(self) -> bool:
+ # TODO(aliberts): add cam.is_connected for cam in self.cameras
+ return self.bus.is_connected
+
+ def connect(self, 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 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_vel = self.bus.sync_read("Present_Velocity", self.base_motors)
+ obs_dict = {**arm_pos, **base_vel}
+ obs_dict = {f"{OBS_STATE}." + key: value for key, value in obs_dict.items()}
+ 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 in self.arm_motors}
+ base_goal_vel = {k: v for k, v in action.items() if k in self.base_motors}
+
+ # 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
+ self.bus.sync_write("Goal_Position", arm_goal_pos)
+ self.bus.sync_write("Goal_Velocity", base_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.")
diff --git a/lerobot/common/robots/lekiwi/lekiwi_client.py b/lerobot/common/robots/lekiwi/lekiwi_client.py
new file mode 100644
index 000000000..a4e791000
--- /dev/null
+++ b/lerobot/common/robots/lekiwi/lekiwi_client.py
@@ -0,0 +1,495 @@
+# 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
+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_arm_state = {}
+ self.last_remote_base_state = {"base_left_wheel": 0, "base_back_wheel": 0, "base_right_wheel": 0}
+
+ # 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 = {}
+
+ @property
+ def state_feature(self) -> dict:
+ state_ft = {
+ "arm_shoulder_pan": {"shape": (1,), "info": None, "dtype": "float32"},
+ "arm_shoulder_lift": {"shape": (1,), "info": None, "dtype": "float32"},
+ "arm_elbow_flex": {"shape": (1,), "info": None, "dtype": "float32"},
+ "arm_wrist_flex": {"shape": (1,), "info": None, "dtype": "float32"},
+ "arm_wrist_roll": {"shape": (1,), "info": None, "dtype": "float32"},
+ "arm_gripper": {"shape": (1,), "info": None, "dtype": "float32"},
+ "x_cmd": {"shape": (1,), "info": None, "dtype": "float32"},
+ "y_cmd": {"shape": (1,), "info": None, "dtype": "float32"},
+ "theta_cmd": {"shape": (1,), "info": None, "dtype": "float32"},
+ }
+ return state_ft
+
+ @property
+ def action_feature(self) -> dict:
+ action_ft = {
+ "arm_shoulder_pan": {"shape": (1,), "info": None, "dtype": "float32"},
+ "arm_shoulder_lift": {"shape": (1,), "info": None, "dtype": "float32"},
+ "arm_elbow_flex": {"shape": (1,), "info": None, "dtype": "float32"},
+ "arm_wrist_flex": {"shape": (1,), "info": None, "dtype": "float32"},
+ "arm_wrist_roll": {"shape": (1,), "info": None, "dtype": "float32"},
+ "arm_gripper": {"shape": (1,), "info": None, "dtype": "float32"},
+ "base_left_wheel": {"shape": (1,), "info": None, "dtype": "float32"},
+ "base_right_wheel": {"shape": (1,), "info": None, "dtype": "float32"},
+ "base_back_wheel": {"shape": (1,), "info": None, "dtype": "float32"},
+ }
+ return action_ft
+
+ @property
+ def camera_features(self) -> dict[str, dict]:
+ cam_ft = {
+ f"{OBS_IMAGES}.front": {
+ "shape": (480, 640, 3),
+ "names": ["height", "width", "channels"],
+ "info": None,
+ "dtype": "image",
+ },
+ f"{OBS_IMAGES}.wrist": {
+ "shape": (480, 640, 3),
+ "names": ["height", "width", "channels"],
+ "dtype": "image",
+ "info": None,
+ },
+ }
+ return cam_ft
+
+ @property
+ def is_connected(self) -> bool:
+ return self._is_connected
+
+ @property
+ def is_calibrated(self) -> bool:
+ pass
+
+ def connect(self) -> None:
+ """Establishes ZMQ sockets with the remote mobile robot"""
+
+ if self._is_connected:
+ 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
+
+ @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_cmd: float,
+ y_cmd: float,
+ theta_cmd: float,
+ wheel_radius: float = 0.05,
+ base_radius: float = 0.125,
+ max_raw: int = 3000,
+ ) -> dict:
+ """
+ Convert desired body-frame velocities into wheel raw commands.
+
+ Parameters:
+ x_cmd : Linear velocity in x (m/s).
+ y_cmd : Linear velocity in y (m/s).
+ theta_cmd : Rotational velocity (deg/s).
+ wheel_radius: Radius of each wheel (meters).
+ base_radius : Distance from the center of rotation to each wheel (meters).
+ max_raw : Maximum allowed raw command (ticks) per wheel.
+
+ Returns:
+ A dictionary with wheel raw commands:
+ {"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_cmd * (np.pi / 180.0)
+ # Create the body velocity vector [x, y, theta_rad].
+ velocity_vector = np.array([x_cmd, y_cmd, theta_rad])
+
+ # Define the wheel mounting angles with a -90° offset.
+ angles = np.radians(np.array([240, 120, 0]) - 90)
+ # Build the kinematic matrix: each row maps body velocities to a wheel’s linear speed.
+ # The third column (base_radius) accounts for the effect of rotation.
+ m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
+
+ # Compute each wheel’s linear speed (m/s) and then its angular speed (rad/s).
+ wheel_linear_speeds = m.dot(velocity_vector)
+ wheel_angular_speeds = wheel_linear_speeds / wheel_radius
+
+ # Convert wheel angular speeds from rad/s to deg/s.
+ wheel_degps = wheel_angular_speeds * (180.0 / np.pi)
+
+ # Scaling
+ steps_per_deg = 4096.0 / 360.0
+ raw_floats = [abs(degps) * steps_per_deg for degps in wheel_degps]
+ max_raw_computed = max(raw_floats)
+ if max_raw_computed > max_raw:
+ scale = max_raw / max_raw_computed
+ wheel_degps = wheel_degps * scale
+
+ # Convert each wheel’s angular speed (deg/s) to a raw integer.
+ wheel_raw = [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, wheel_raw: dict[str, Any], wheel_radius: float = 0.05, base_radius: float = 0.125
+ ) -> dict[str, Any]:
+ """
+ Convert wheel raw command feedback back into body-frame velocities.
+
+ 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([LeKiwiClient._raw_to_degps(int(v)) for _, v in wheel_raw.items()])
+ # Convert from deg/s to rad/s.
+ wheel_radps = wheel_degps * (np.pi / 180.0)
+ # Compute each wheel’s linear speed (m/s) from its angular speed.
+ wheel_linear_speeds = wheel_radps * wheel_radius
+
+ # Define the wheel mounting angles with a -90° offset.
+ angles = np.radians(np.array([240, 120, 0]) - 90)
+ m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
+
+ # Solve the inverse kinematics: body_velocity = M⁻¹ · wheel_linear_speeds.
+ m_inv = np.linalg.inv(m)
+ velocity_vector = m_inv.dot(wheel_linear_speeds)
+ x_cmd, y_cmd, theta_rad = velocity_vector
+ theta_cmd = theta_rad * (180.0 / np.pi)
+ return {
+ f"{OBS_STATE}.x_cmd": x_cmd * 1000,
+ f"{OBS_STATE}.y_cmd": y_cmd * 1000,
+ f"{OBS_STATE}.theta_cmd": theta_cmd,
+ } # Convert to mm/s
+
+ 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], Dict[str, Any]]:
+ """Extracts frames, speed, and arm state from the parsed observation."""
+
+ # Separate image and state data
+ image_observation = {k: v for k, v in observation.items() if k.startswith(OBS_IMAGES)}
+ state_observation = {k: v for k, v in observation.items() if k.startswith(OBS_STATE)}
+
+ # Decode 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
+
+ # Extract state components
+ current_arm_state = {k: v for k, v in state_observation.items() if k.startswith(f"{OBS_STATE}.arm")}
+ current_base_state = {k: v for k, v in state_observation.items() if k.startswith(f"{OBS_STATE}.base")}
+
+ return current_frames, current_arm_state, current_base_state
+
+ 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_arm_state, self.last_remote_base_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_arm_state, self.last_remote_base_state
+
+ # 5. Process the valid observation data
+ try:
+ new_frames, new_arm_state, new_base_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_arm_state, self.last_remote_base_state
+
+ self.last_frames = new_frames
+ self.last_remote_arm_state = new_arm_state
+ self.last_remote_base_state = new_base_state
+
+ return new_frames, new_arm_state, new_base_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, remote_arm_state, remote_base_state = self._get_data()
+ remote_body_state = self._wheel_raw_to_body(remote_base_state)
+
+ obs_dict = {**remote_arm_state, **remote_body_state}
+
+ # TODO(Steven): Remove this when it is possible to record a non-numpy array value
+ obs_dict = {k: np.array([v], dtype=np.float32) for k, v in obs_dict.items()}
+
+ # 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_wheel_action(self, pressed_keys: np.ndarray):
+ # Speed control
+ if self.teleop_keys["speed_up"] in pressed_keys:
+ self.speed_index = min(self.speed_index + 1, 2)
+ 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 self._body_to_wheel_raw(x_cmd, y_cmd, 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()`."
+ )
+
+ goal_pos = {}
+
+ common_keys = [
+ key
+ for key in action
+ if key in (motor.replace("arm_", "") for motor, _ in self.action_feature.items())
+ ]
+
+ arm_actions = {"arm_" + arm_motor: action[arm_motor] for arm_motor in common_keys}
+ goal_pos = arm_actions
+
+ keyboard_keys = np.array(list(set(action.keys()) - set(common_keys)))
+ wheel_actions = self._from_keyboard_to_wheel_action(keyboard_keys)
+ goal_pos = {**arm_actions, **wheel_actions}
+
+ self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) # action is in motor space
+
+ # TODO(Steven): Remove the np conversion when it is possible to record a non-numpy array value
+ goal_pos = {"action." + k: np.array([v], dtype=np.float32) for k, v in goal_pos.items()}
+ return goal_pos
+
+ 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
diff --git a/lerobot/common/robots/lekiwi/lekiwi_host.py b/lerobot/common/robots/lekiwi/lekiwi_host.py
new file mode 100644
index 000000000..014c965b7
--- /dev/null
+++ b/lerobot/common/robots/lekiwi/lekiwi_host.py
@@ -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()
diff --git a/lerobot/common/robots/lekiwi/lekiwi_remote.py b/lerobot/common/robots/lekiwi/lekiwi_remote.py
deleted file mode 100644
index 1a3af5cf0..000000000
--- a/lerobot/common/robots/lekiwi/lekiwi_remote.py
+++ /dev/null
@@ -1,224 +0,0 @@
-# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import base64
-import json
-import threading
-import time
-from pathlib import Path
-
-import cv2
-import zmq
-
-from lerobot.common.robots.mobile_manipulator import LeKiwi
-
-
-def setup_zmq_sockets(config):
- context = zmq.Context()
- cmd_socket = context.socket(zmq.PULL)
- cmd_socket.setsockopt(zmq.CONFLATE, 1)
- cmd_socket.bind(f"tcp://*:{config.port}")
-
- video_socket = context.socket(zmq.PUSH)
- video_socket.setsockopt(zmq.CONFLATE, 1)
- video_socket.bind(f"tcp://*:{config.video_port}")
-
- return context, cmd_socket, video_socket
-
-
-def run_camera_capture(cameras, images_lock, latest_images_dict, stop_event):
- while not stop_event.is_set():
- local_dict = {}
- for name, cam in cameras.items():
- frame = cam.async_read()
- ret, buffer = cv2.imencode(".jpg", frame, [int(cv2.IMWRITE_JPEG_QUALITY), 90])
- if ret:
- local_dict[name] = base64.b64encode(buffer).decode("utf-8")
- else:
- local_dict[name] = ""
- with images_lock:
- latest_images_dict.update(local_dict)
- time.sleep(0.01)
-
-
-def calibrate_follower_arm(motors_bus, calib_dir_str):
- """
- Calibrates the follower arm. Attempts to load an existing calibration file;
- if not found, runs manual calibration and saves the result.
- """
- calib_dir = Path(calib_dir_str)
- calib_dir.mkdir(parents=True, exist_ok=True)
- calib_file = calib_dir / "main_follower.json"
- try:
- from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration
- except ImportError:
- print("[WARNING] Calibration function not available. Skipping calibration.")
- return
-
- if calib_file.exists():
- with open(calib_file) as f:
- calibration = json.load(f)
- print(f"[INFO] Loaded calibration from {calib_file}")
- else:
- print("[INFO] Calibration file not found. Running manual calibration...")
- calibration = run_full_arm_calibration(motors_bus, "lekiwi", "follower_arm", "follower")
- print(f"[INFO] Calibration complete. Saving to {calib_file}")
- with open(calib_file, "w") as f:
- json.dump(calibration, f)
- try:
- motors_bus.set_calibration(calibration)
- print("[INFO] Applied calibration for follower arm.")
- except Exception as e:
- print(f"[WARNING] Could not apply calibration: {e}")
-
-
-def run_lekiwi(robot_config):
- """
- Runs the LeKiwi robot:
- - Sets up cameras and connects them.
- - Initializes the follower arm motors.
- - Calibrates the follower arm if necessary.
- - Creates ZeroMQ sockets for receiving commands and streaming observations.
- - Processes incoming commands (arm and wheel commands) and sends back sensor and camera data.
- """
- # Import helper functions and classes
- from lerobot.common.cameras.utils import make_cameras_from_configs
- from lerobot.common.motors.feetech.feetech import FeetechMotorsBus, TorqueMode
-
- # Initialize cameras from the robot configuration.
- cameras = make_cameras_from_configs(robot_config.cameras)
- for cam in cameras.values():
- cam.connect()
-
- # Initialize the motors bus using the follower arm configuration.
- motor_config = robot_config.follower_arms.get("main")
- if motor_config is None:
- print("[ERROR] Follower arm 'main' configuration not found.")
- return
- motors_bus = FeetechMotorsBus(motor_config)
- motors_bus.connect()
-
- # Calibrate the follower arm.
- calibrate_follower_arm(motors_bus, robot_config.calibration_dir)
-
- # Create the LeKiwi robot instance.
- robot = LeKiwi(motors_bus)
-
- # Define the expected arm motor IDs.
- arm_motor_ids = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]
-
- # Disable torque for each arm motor.
- for motor in arm_motor_ids:
- motors_bus.write("Torque_Enable", TorqueMode.DISABLED.value, motor)
-
- # Set up ZeroMQ sockets.
- context, cmd_socket, video_socket = setup_zmq_sockets(robot_config)
-
- # Start the camera capture thread.
- latest_images_dict = {}
- images_lock = threading.Lock()
- stop_event = threading.Event()
- cam_thread = threading.Thread(
- target=run_camera_capture, args=(cameras, images_lock, latest_images_dict, stop_event), daemon=True
- )
- cam_thread.start()
-
- last_cmd_time = time.time()
- print("LeKiwi robot server started. Waiting for commands...")
-
- try:
- while True:
- loop_start_time = time.time()
-
- # Process incoming commands (non-blocking).
- while True:
- try:
- msg = cmd_socket.recv_string(zmq.NOBLOCK)
- except zmq.Again:
- break
- try:
- data = json.loads(msg)
- # Process arm position commands.
- if "arm_positions" in data:
- arm_positions = data["arm_positions"]
- if not isinstance(arm_positions, list):
- print(f"[ERROR] Invalid arm_positions: {arm_positions}")
- elif len(arm_positions) < len(arm_motor_ids):
- print(
- f"[WARNING] Received {len(arm_positions)} arm positions, expected {len(arm_motor_ids)}"
- )
- else:
- for motor, pos in zip(arm_motor_ids, arm_positions, strict=False):
- motors_bus.write("Goal_Position", pos, motor)
- # Process wheel (base) commands.
- if "raw_velocity" in data:
- raw_command = data["raw_velocity"]
- # Expect keys: "left_wheel", "back_wheel", "right_wheel".
- command_speeds = [
- int(raw_command.get("left_wheel", 0)),
- int(raw_command.get("back_wheel", 0)),
- int(raw_command.get("right_wheel", 0)),
- ]
- robot.set_velocity(command_speeds)
- last_cmd_time = time.time()
- except Exception as e:
- print(f"[ERROR] Parsing message failed: {e}")
-
- # Watchdog: stop the robot if no command is received for over 0.5 seconds.
- now = time.time()
- if now - last_cmd_time > 0.5:
- robot.stop()
- last_cmd_time = now
-
- # Read current wheel speeds from the robot.
- current_velocity = robot.read_velocity()
-
- # Read the follower arm state from the motors bus.
- follower_arm_state = []
- for motor in arm_motor_ids:
- try:
- pos = motors_bus.read("Present_Position", motor)
- # Convert the position to a float (or use as is if already numeric).
- follower_arm_state.append(float(pos) if not isinstance(pos, (int, float)) else pos)
- except Exception as e:
- print(f"[ERROR] Reading motor {motor} failed: {e}")
-
- # Get the latest camera images.
- with images_lock:
- images_dict_copy = dict(latest_images_dict)
-
- # Build the observation dictionary.
- observation = {
- "images": images_dict_copy,
- "present_speed": current_velocity,
- "follower_arm_state": follower_arm_state,
- }
- # Send the observation over the video socket.
- video_socket.send_string(json.dumps(observation))
-
- # Ensure a short sleep to avoid overloading the CPU.
- elapsed = time.time() - loop_start_time
- time.sleep(
- max(0.033 - elapsed, 0)
- ) # If robot jitters increase the sleep and monitor cpu load with `top` in cmd
- except KeyboardInterrupt:
- print("Shutting down LeKiwi server.")
- finally:
- stop_event.set()
- cam_thread.join()
- robot.stop()
- motors_bus.disconnect()
- cmd_socket.close()
- video_socket.close()
- context.term()
diff --git a/lerobot/common/robots/lekiwi/robot_lekiwi.py b/lerobot/common/robots/lekiwi/robot_lekiwi.py
deleted file mode 100644
index 8f2f9037e..000000000
--- a/lerobot/common/robots/lekiwi/robot_lekiwi.py
+++ /dev/null
@@ -1,692 +0,0 @@
-import base64
-import json
-import os
-import sys
-from pathlib import Path
-
-import cv2
-import numpy as np
-import torch
-import zmq
-
-from lerobot.common.cameras.utils import make_cameras_from_configs
-from lerobot.common.errors import DeviceNotConnectedError
-from lerobot.common.motors.feetech.feetech import TorqueMode
-from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration
-from lerobot.common.motors.motors_bus import MotorsBus
-from lerobot.common.motors.utils import make_motors_buses_from_configs
-from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig
-from lerobot.common.robots.utils import get_arm_id
-
-PYNPUT_AVAILABLE = True
-try:
- # Only import if there's a valid X server or if we're not on a Pi
- if ("DISPLAY" not in os.environ) and ("linux" in sys.platform):
- print("No DISPLAY set. Skipping pynput import.")
- raise ImportError("pynput blocked intentionally due to no display.")
-
- from pynput import keyboard
-except ImportError:
- keyboard = None
- PYNPUT_AVAILABLE = False
-except Exception as e:
- keyboard = None
- PYNPUT_AVAILABLE = False
- print(f"Could not import pynput: {e}")
-
-
-class MobileManipulator:
- """
- MobileManipulator is a class for connecting to and controlling a remote mobile manipulator robot.
- The robot includes a three omniwheel mobile base and a remote follower arm.
- The leader arm is connected locally (on the laptop) and its joint positions are recorded and then
- forwarded to the remote follower arm (after applying a safety clamp).
- In parallel, keyboard teleoperation is used to generate raw velocity commands for the wheels.
- """
-
- def __init__(self, config: LeKiwiRobotConfig):
- """
- Expected keys in config:
- - ip, port, video_port for the remote connection.
- - calibration_dir, leader_arms, follower_arms, max_relative_target, etc.
- """
- self.robot_type = config.type
- self.config = config
- self.remote_ip = config.ip
- self.remote_port = config.port
- self.remote_port_video = config.video_port
- self.calibration_dir = Path(self.config.calibration_dir)
- self.logs = {}
-
- self.teleop_keys = self.config.teleop_keys
-
- # For teleoperation, the leader arm (local) is used to record the desired arm pose.
- self.leader_arms = make_motors_buses_from_configs(self.config.leader_arms)
-
- self.follower_arms = make_motors_buses_from_configs(self.config.follower_arms)
-
- self.cameras = make_cameras_from_configs(self.config.cameras)
-
- self.is_connected = False
-
- self.last_frames = {}
- self.last_present_speed = {}
- self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32)
-
- # Define three speed levels and a current index
- self.speed_levels = [
- {"xy": 0.1, "theta": 30}, # slow
- {"xy": 0.2, "theta": 60}, # medium
- {"xy": 0.3, "theta": 90}, # fast
- ]
- self.speed_index = 0 # Start at slow
-
- # ZeroMQ context and sockets.
- self.context = None
- self.cmd_socket = None
- self.video_socket = None
-
- # Keyboard state for base teleoperation.
- self.running = True
- self.pressed_keys = {
- "forward": False,
- "backward": False,
- "left": False,
- "right": False,
- "rotate_left": False,
- "rotate_right": False,
- }
-
- if PYNPUT_AVAILABLE:
- print("pynput is available - enabling local keyboard listener.")
- self.listener = keyboard.Listener(
- on_press=self.on_press,
- on_release=self.on_release,
- )
- self.listener.start()
- else:
- print("pynput not available - skipping local keyboard listener.")
- self.listener = None
-
- def get_motor_names(self, arms: dict[str, MotorsBus]) -> list:
- return [f"{arm}_{motor}" for arm, bus in arms.items() for motor in bus.motors]
-
- @property
- def camera_features(self) -> dict:
- cam_ft = {}
- for cam_key, cam in self.cameras.items():
- key = f"observation.images.{cam_key}"
- cam_ft[key] = {
- "shape": (cam.height, cam.width, cam.channels),
- "names": ["height", "width", "channels"],
- "info": None,
- }
- return cam_ft
-
- @property
- def motor_features(self) -> dict:
- follower_arm_names = [
- "shoulder_pan",
- "shoulder_lift",
- "elbow_flex",
- "wrist_flex",
- "wrist_roll",
- "gripper",
- ]
- observations = ["x_mm", "y_mm", "theta"]
- combined_names = follower_arm_names + observations
- return {
- "action": {
- "dtype": "float32",
- "shape": (len(combined_names),),
- "names": combined_names,
- },
- "observation.state": {
- "dtype": "float32",
- "shape": (len(combined_names),),
- "names": combined_names,
- },
- }
-
- @property
- def features(self):
- return {**self.motor_features, **self.camera_features}
-
- @property
- def has_camera(self):
- return len(self.cameras) > 0
-
- @property
- def num_cameras(self):
- return len(self.cameras)
-
- @property
- def available_arms(self):
- available = []
- for name in self.leader_arms:
- available.append(get_arm_id(name, "leader"))
- for name in self.follower_arms:
- available.append(get_arm_id(name, "follower"))
- return available
-
- def on_press(self, key):
- try:
- # Movement
- if key.char == self.teleop_keys["forward"]:
- self.pressed_keys["forward"] = True
- elif key.char == self.teleop_keys["backward"]:
- self.pressed_keys["backward"] = True
- elif key.char == self.teleop_keys["left"]:
- self.pressed_keys["left"] = True
- elif key.char == self.teleop_keys["right"]:
- self.pressed_keys["right"] = True
- elif key.char == self.teleop_keys["rotate_left"]:
- self.pressed_keys["rotate_left"] = True
- elif key.char == self.teleop_keys["rotate_right"]:
- self.pressed_keys["rotate_right"] = True
-
- # Quit teleoperation
- elif key.char == self.teleop_keys["quit"]:
- self.running = False
- return False
-
- # Speed control
- elif key.char == self.teleop_keys["speed_up"]:
- self.speed_index = min(self.speed_index + 1, 2)
- print(f"Speed index increased to {self.speed_index}")
- elif key.char == self.teleop_keys["speed_down"]:
- self.speed_index = max(self.speed_index - 1, 0)
- print(f"Speed index decreased to {self.speed_index}")
-
- except AttributeError:
- # e.g., if key is special like Key.esc
- if key == keyboard.Key.esc:
- self.running = False
- return False
-
- def on_release(self, key):
- try:
- if hasattr(key, "char"):
- if key.char == self.teleop_keys["forward"]:
- self.pressed_keys["forward"] = False
- elif key.char == self.teleop_keys["backward"]:
- self.pressed_keys["backward"] = False
- elif key.char == self.teleop_keys["left"]:
- self.pressed_keys["left"] = False
- elif key.char == self.teleop_keys["right"]:
- self.pressed_keys["right"] = False
- elif key.char == self.teleop_keys["rotate_left"]:
- self.pressed_keys["rotate_left"] = False
- elif key.char == self.teleop_keys["rotate_right"]:
- self.pressed_keys["rotate_right"] = False
- except AttributeError:
- pass
-
- def connect(self):
- if not self.leader_arms:
- raise ValueError("MobileManipulator has no leader arm to connect.")
- for name in self.leader_arms:
- print(f"Connecting {name} leader arm.")
- self.calibrate_leader()
-
- # Set up ZeroMQ sockets to communicate with the remote mobile robot.
- self.context = zmq.Context()
- self.cmd_socket = self.context.socket(zmq.PUSH)
- connection_string = f"tcp://{self.remote_ip}:{self.remote_port}"
- self.cmd_socket.connect(connection_string)
- self.cmd_socket.setsockopt(zmq.CONFLATE, 1)
- self.video_socket = self.context.socket(zmq.PULL)
- video_connection = f"tcp://{self.remote_ip}:{self.remote_port_video}"
- self.video_socket.connect(video_connection)
- self.video_socket.setsockopt(zmq.CONFLATE, 1)
- print(
- f"[INFO] Connected to remote robot at {connection_string} and video stream at {video_connection}."
- )
- self.is_connected = True
-
- def load_or_run_calibration_(self, name, arm, arm_type):
- arm_id = get_arm_id(name, arm_type)
- arm_calib_path = self.calibration_dir / f"{arm_id}.json"
-
- if arm_calib_path.exists():
- with open(arm_calib_path) as f:
- calibration = json.load(f)
- else:
- print(f"Missing calibration file '{arm_calib_path}'")
- calibration = run_full_arm_calibration(arm, self.robot_type, name, arm_type)
- print(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
- arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
- with open(arm_calib_path, "w") as f:
- json.dump(calibration, f)
-
- return calibration
-
- def calibrate_leader(self):
- for name, arm in self.leader_arms.items():
- # Connect the bus
- arm.connect()
-
- # Disable torque on all motors
- for motor_id in arm.motors:
- arm.write("Torque_Enable", TorqueMode.DISABLED.value, motor_id)
-
- # Now run calibration
- calibration = self.load_or_run_calibration_(name, arm, "leader")
- arm.set_calibration(calibration)
-
- def calibrate_follower(self):
- for name, bus in self.follower_arms.items():
- bus.connect()
-
- # Disable torque on all motors
- for motor_id in bus.motors:
- bus.write("Torque_Enable", 0, motor_id)
-
- # Then filter out wheels
- arm_only_dict = {k: v for k, v in bus.motors.items() if not k.startswith("wheel_")}
- if not arm_only_dict:
- continue
-
- original_motors = bus.motors
- bus.motors = arm_only_dict
-
- calibration = self.load_or_run_calibration_(name, bus, "follower")
- bus.set_calibration(calibration)
-
- bus.motors = original_motors
-
- def _get_data(self):
- """
- Polls the video socket for up to 15 ms. If data arrives, decode only
- the *latest* message, returning frames, speed, and arm state. If
- nothing arrives for any field, use the last known values.
- """
- frames = {}
- present_speed = {}
- remote_arm_state_tensor = torch.zeros(6, dtype=torch.float32)
-
- # Poll up to 15 ms
- poller = zmq.Poller()
- poller.register(self.video_socket, zmq.POLLIN)
- socks = dict(poller.poll(15))
- if self.video_socket not in socks or socks[self.video_socket] != zmq.POLLIN:
- # No new data arrived → reuse ALL old data
- return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
-
- # Drain all messages, keep only the last
- last_msg = None
- while True:
- try:
- obs_string = self.video_socket.recv_string(zmq.NOBLOCK)
- last_msg = obs_string
- except zmq.Again:
- break
-
- if not last_msg:
- # No new message → also reuse old
- return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
-
- # Decode only the final message
- try:
- observation = json.loads(last_msg)
-
- images_dict = observation.get("images", {})
- new_speed = observation.get("present_speed", {})
- new_arm_state = observation.get("follower_arm_state", None)
-
- # Convert images
- for cam_name, image_b64 in images_dict.items():
- if image_b64:
- jpg_data = base64.b64decode(image_b64)
- np_arr = np.frombuffer(jpg_data, dtype=np.uint8)
- frame_candidate = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
- if frame_candidate is not None:
- frames[cam_name] = frame_candidate
-
- # If remote_arm_state is None and frames is None there is no message then use the previous message
- if new_arm_state is not None and frames is not None:
- self.last_frames = frames
-
- remote_arm_state_tensor = torch.tensor(new_arm_state, dtype=torch.float32)
- self.last_remote_arm_state = remote_arm_state_tensor
-
- present_speed = new_speed
- self.last_present_speed = new_speed
- else:
- frames = self.last_frames
-
- remote_arm_state_tensor = self.last_remote_arm_state
-
- present_speed = self.last_present_speed
-
- except Exception as e:
- print(f"[DEBUG] Error decoding video message: {e}")
- # If decode fails, fall back to old data
- return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
-
- return frames, present_speed, remote_arm_state_tensor
-
- def _process_present_speed(self, present_speed: dict) -> torch.Tensor:
- state_tensor = torch.zeros(3, dtype=torch.int32)
- if present_speed:
- decoded = {key: MobileManipulator.raw_to_degps(value) for key, value in present_speed.items()}
- if "1" in decoded:
- state_tensor[0] = decoded["1"]
- if "2" in decoded:
- state_tensor[1] = decoded["2"]
- if "3" in decoded:
- state_tensor[2] = decoded["3"]
- return state_tensor
-
- def teleop_step(
- self, record_data: bool = False
- ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
- if not self.is_connected:
- raise DeviceNotConnectedError("MobileManipulator is not connected. Run `connect()` first.")
-
- speed_setting = self.speed_levels[self.speed_index]
- xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4
- theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90
-
- # Prepare to assign the position of the leader to the follower
- arm_positions = []
- for name in self.leader_arms:
- pos = self.leader_arms[name].read("Present_Position")
- pos_tensor = torch.from_numpy(pos).float()
- # Instead of pos_tensor.item(), use tolist() to convert the entire tensor to a list
- arm_positions.extend(pos_tensor.tolist())
-
- # (The rest of your code for generating wheel commands remains unchanged)
- x_cmd = 0.0 # m/s forward/backward
- y_cmd = 0.0 # m/s lateral
- theta_cmd = 0.0 # deg/s rotation
- if self.pressed_keys["forward"]:
- x_cmd += xy_speed
- if self.pressed_keys["backward"]:
- x_cmd -= xy_speed
- if self.pressed_keys["left"]:
- y_cmd += xy_speed
- if self.pressed_keys["right"]:
- y_cmd -= xy_speed
- if self.pressed_keys["rotate_left"]:
- theta_cmd += theta_speed
- if self.pressed_keys["rotate_right"]:
- theta_cmd -= theta_speed
-
- wheel_commands = self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd)
-
- message = {"raw_velocity": wheel_commands, "arm_positions": arm_positions}
- self.cmd_socket.send_string(json.dumps(message))
-
- if not record_data:
- return
-
- obs_dict = self.capture_observation()
-
- arm_state_tensor = torch.tensor(arm_positions, dtype=torch.float32)
-
- wheel_velocity_tuple = self.wheel_raw_to_body(wheel_commands)
- wheel_velocity_mm = (
- wheel_velocity_tuple[0] * 1000.0,
- wheel_velocity_tuple[1] * 1000.0,
- wheel_velocity_tuple[2],
- )
- wheel_tensor = torch.tensor(wheel_velocity_mm, dtype=torch.float32)
- action_tensor = torch.cat([arm_state_tensor, wheel_tensor])
- action_dict = {"action": action_tensor}
-
- return obs_dict, action_dict
-
- def capture_observation(self) -> dict:
- """
- Capture observations from the remote robot: current follower arm positions,
- present wheel speeds (converted to body-frame velocities: x, y, theta),
- and a camera frame.
- """
- if not self.is_connected:
- raise DeviceNotConnectedError("Not connected. Run `connect()` first.")
-
- frames, present_speed, remote_arm_state_tensor = self._get_data()
-
- body_state = self.wheel_raw_to_body(present_speed)
-
- body_state_mm = (body_state[0] * 1000.0, body_state[1] * 1000.0, body_state[2]) # Convert x,y to mm/s
- wheel_state_tensor = torch.tensor(body_state_mm, dtype=torch.float32)
- combined_state_tensor = torch.cat((remote_arm_state_tensor, wheel_state_tensor), dim=0)
-
- obs_dict = {"observation.state": combined_state_tensor}
-
- # Loop over each configured camera
- for cam_name, cam in self.cameras.items():
- frame = frames.get(cam_name, None)
- if frame is None:
- # Create a black image using the camera's configured width, height, and channels
- frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8)
- obs_dict[f"observation.images.{cam_name}"] = torch.from_numpy(frame)
-
- return obs_dict
-
- def send_action(self, action: torch.Tensor) -> torch.Tensor:
- if not self.is_connected:
- raise DeviceNotConnectedError("Not connected. Run `connect()` first.")
-
- # Ensure the action tensor has at least 9 elements:
- # - First 6: arm positions.
- # - Last 3: base commands.
- if action.numel() < 9:
- # Pad with zeros if there are not enough elements.
- padded = torch.zeros(9, dtype=action.dtype)
- padded[: action.numel()] = action
- action = padded
-
- # Extract arm and base actions.
- arm_actions = action[:6].flatten()
- base_actions = action[6:].flatten()
-
- x_cmd_mm = base_actions[0].item() # mm/s
- y_cmd_mm = base_actions[1].item() # mm/s
- theta_cmd = base_actions[2].item() # deg/s
-
- # Convert mm/s to m/s for the kinematics calculations.
- x_cmd = x_cmd_mm / 1000.0 # m/s
- y_cmd = y_cmd_mm / 1000.0 # m/s
-
- # Compute wheel commands from body commands.
- wheel_commands = self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd)
-
- arm_positions_list = arm_actions.tolist()
-
- message = {"raw_velocity": wheel_commands, "arm_positions": arm_positions_list}
- self.cmd_socket.send_string(json.dumps(message))
-
- return action
-
- def print_logs(self):
- pass
-
- def disconnect(self):
- if not self.is_connected:
- raise DeviceNotConnectedError("Not connected.")
- if self.cmd_socket:
- stop_cmd = {
- "raw_velocity": {"left_wheel": 0, "back_wheel": 0, "right_wheel": 0},
- "arm_positions": {},
- }
- self.cmd_socket.send_string(json.dumps(stop_cmd))
- self.cmd_socket.close()
- if self.video_socket:
- self.video_socket.close()
- if self.context:
- self.context.term()
- if PYNPUT_AVAILABLE:
- self.listener.stop()
- self.is_connected = False
- print("[INFO] Disconnected from remote robot.")
-
- def __del__(self):
- if getattr(self, "is_connected", False):
- self.disconnect()
- if PYNPUT_AVAILABLE:
- self.listener.stop()
-
- @staticmethod
- def degps_to_raw(degps: float) -> int:
- steps_per_deg = 4096.0 / 360.0
- speed_in_steps = abs(degps) * steps_per_deg
- speed_int = int(round(speed_in_steps))
- if speed_int > 0x7FFF:
- speed_int = 0x7FFF
- if degps < 0:
- return speed_int | 0x8000
- else:
- return speed_int & 0x7FFF
-
- @staticmethod
- def raw_to_degps(raw_speed: int) -> float:
- steps_per_deg = 4096.0 / 360.0
- magnitude = raw_speed & 0x7FFF
- degps = magnitude / steps_per_deg
- if raw_speed & 0x8000:
- degps = -degps
- return degps
-
- def body_to_wheel_raw(
- self,
- x_cmd: float,
- y_cmd: float,
- theta_cmd: float,
- wheel_radius: float = 0.05,
- base_radius: float = 0.125,
- max_raw: int = 3000,
- ) -> dict:
- """
- Convert desired body-frame velocities into wheel raw commands.
-
- Parameters:
- x_cmd : Linear velocity in x (m/s).
- y_cmd : Linear velocity in y (m/s).
- theta_cmd : Rotational velocity (deg/s).
- wheel_radius: Radius of each wheel (meters).
- base_radius : Distance from the center of rotation to each wheel (meters).
- max_raw : Maximum allowed raw command (ticks) per wheel.
-
- Returns:
- A dictionary with wheel raw commands:
- {"left_wheel": value, "back_wheel": value, "right_wheel": value}.
-
- Notes:
- - Internally, the method converts theta_cmd to rad/s for the kinematics.
- - The raw command is computed from the wheels angular speed in deg/s
- using degps_to_raw(). If any command exceeds max_raw, all commands
- are scaled down proportionally.
- """
- # Convert rotational velocity from deg/s to rad/s.
- theta_rad = theta_cmd * (np.pi / 180.0)
- # Create the body velocity vector [x, y, theta_rad].
- velocity_vector = np.array([x_cmd, y_cmd, theta_rad])
-
- # Define the wheel mounting angles with a -90° offset.
- angles = np.radians(np.array([240, 120, 0]) - 90)
- # Build the kinematic matrix: each row maps body velocities to a wheel’s linear speed.
- # The third column (base_radius) accounts for the effect of rotation.
- m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
-
- # Compute each wheel’s linear speed (m/s) and then its angular speed (rad/s).
- wheel_linear_speeds = m.dot(velocity_vector)
- wheel_angular_speeds = wheel_linear_speeds / wheel_radius
-
- # Convert wheel angular speeds from rad/s to deg/s.
- wheel_degps = wheel_angular_speeds * (180.0 / np.pi)
-
- # Scaling
- steps_per_deg = 4096.0 / 360.0
- raw_floats = [abs(degps) * steps_per_deg for degps in wheel_degps]
- max_raw_computed = max(raw_floats)
- if max_raw_computed > max_raw:
- scale = max_raw / max_raw_computed
- wheel_degps = wheel_degps * scale
-
- # Convert each wheel’s angular speed (deg/s) to a raw integer.
- wheel_raw = [MobileManipulator.degps_to_raw(deg) for deg in wheel_degps]
-
- return {"left_wheel": wheel_raw[0], "back_wheel": wheel_raw[1], "right_wheel": wheel_raw[2]}
-
- def wheel_raw_to_body(
- self, wheel_raw: dict, wheel_radius: float = 0.05, base_radius: float = 0.125
- ) -> tuple:
- """
- Convert wheel raw command feedback back into body-frame velocities.
-
- Parameters:
- wheel_raw : Dictionary with raw wheel commands (keys: "left_wheel", "back_wheel", "right_wheel").
- wheel_radius: Radius of each wheel (meters).
- base_radius : Distance from the robot center to each wheel (meters).
-
- Returns:
- A tuple (x_cmd, y_cmd, theta_cmd) where:
- x_cmd : Linear velocity in x (m/s).
- y_cmd : Linear velocity in y (m/s).
- theta_cmd : Rotational velocity in deg/s.
- """
- # Extract the raw values in order.
- raw_list = [
- int(wheel_raw.get("left_wheel", 0)),
- int(wheel_raw.get("back_wheel", 0)),
- int(wheel_raw.get("right_wheel", 0)),
- ]
-
- # Convert each raw command back to an angular speed in deg/s.
- wheel_degps = np.array([MobileManipulator.raw_to_degps(r) for r in raw_list])
- # Convert from deg/s to rad/s.
- wheel_radps = wheel_degps * (np.pi / 180.0)
- # Compute each wheel’s linear speed (m/s) from its angular speed.
- wheel_linear_speeds = wheel_radps * wheel_radius
-
- # Define the wheel mounting angles with a -90° offset.
- angles = np.radians(np.array([240, 120, 0]) - 90)
- m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
-
- # Solve the inverse kinematics: body_velocity = M⁻¹ · wheel_linear_speeds.
- m_inv = np.linalg.inv(m)
- velocity_vector = m_inv.dot(wheel_linear_speeds)
- x_cmd, y_cmd, theta_rad = velocity_vector
- theta_cmd = theta_rad * (180.0 / np.pi)
- return (x_cmd, y_cmd, theta_cmd)
-
-
-class LeKiwi:
- def __init__(self, motor_bus):
- """
- Initializes the LeKiwi with Feetech motors bus.
- """
- self.motor_bus = motor_bus
- self.motor_ids = ["left_wheel", "back_wheel", "right_wheel"]
-
- # Initialize motors in velocity mode.
- self.motor_bus.write("Lock", 0)
- self.motor_bus.write("Mode", [1, 1, 1], self.motor_ids)
- self.motor_bus.write("Lock", 1)
- print("Motors set to velocity mode.")
-
- def read_velocity(self):
- """
- Reads the raw speeds for all wheels. Returns a dictionary with motor names:
- """
- raw_speeds = self.motor_bus.read("Present_Speed", self.motor_ids)
- return {
- "left_wheel": int(raw_speeds[0]),
- "back_wheel": int(raw_speeds[1]),
- "right_wheel": int(raw_speeds[2]),
- }
-
- def set_velocity(self, command_speeds):
- """
- Sends raw velocity commands (16-bit encoded values) directly to the motor bus.
- The order of speeds must correspond to self.motor_ids.
- """
- self.motor_bus.write("Goal_Speed", command_speeds, self.motor_ids)
-
- def stop(self):
- """Stops the robot by setting all motor speeds to zero."""
- self.motor_bus.write("Goal_Speed", [0, 0, 0], self.motor_ids)
- print("Motors stopped.")
diff --git a/lerobot/common/robots/manipulator.py b/lerobot/common/robots/manipulator.py
deleted file mode 100644
index c5507ecf2..000000000
--- a/lerobot/common/robots/manipulator.py
+++ /dev/null
@@ -1,605 +0,0 @@
-# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-"""Contains logic to instantiate a robot, read information from its motors and cameras,
-and send orders to its motors.
-"""
-# TODO(rcadene, aliberts): reorganize the codebase into one file per robot, with the associated
-# calibration procedure, to make it easy for people to add their own robot.
-
-import time
-import warnings
-from dataclasses import dataclass, field
-from pathlib import Path
-from typing import Sequence
-
-import numpy as np
-import torch
-
-from lerobot.common.cameras.configs import CameraConfig
-from lerobot.common.cameras.utils import make_cameras_from_configs
-from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
-from lerobot.common.motors.configs import MotorsBusConfig
-from lerobot.common.motors.motors_bus import MotorsBus
-from lerobot.common.motors.utils import make_motors_buses_from_configs
-from lerobot.common.robots.config import RobotConfig
-from lerobot.common.robots.utils import ensure_safe_goal_position, get_arm_id
-
-
-@dataclass
-class ManipulatorRobotConfig(RobotConfig):
- leader_arms: dict[str, MotorsBusConfig] = field(default_factory=lambda: {})
- follower_arms: dict[str, MotorsBusConfig] = field(default_factory=lambda: {})
- cameras: dict[str, CameraConfig] = field(default_factory=lambda: {})
-
- # Optionally limit 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 (assumes all follower arms have the same number of
- # motors).
- max_relative_target: list[float] | float | None = None
-
- # Optionally set the leader arm in torque mode with the gripper motor set to this angle. This makes it
- # possible to squeeze the gripper and have it spring back to an open position on its own. If None, the
- # gripper is not put in torque mode.
- gripper_open_degree: float | None = None
-
- mock: bool = False
-
- def __post_init__(self):
- if self.mock:
- for arm in self.leader_arms.values():
- if not arm.mock:
- arm.mock = True
- for arm in self.follower_arms.values():
- if not arm.mock:
- arm.mock = True
- for cam in self.cameras.values():
- if not cam.mock:
- cam.mock = True
-
- if self.max_relative_target is not None and isinstance(self.max_relative_target, Sequence):
- for name in self.follower_arms:
- if len(self.follower_arms[name].motors) != len(self.max_relative_target):
- raise ValueError(
- f"len(max_relative_target)={len(self.max_relative_target)} but the follower arm with name {name} has "
- f"{len(self.follower_arms[name].motors)} motors. Please make sure that the "
- f"`max_relative_target` list has as many parameters as there are motors per arm. "
- "Note: This feature does not yet work with robots where different follower arms have "
- "different numbers of motors."
- )
-
-
-class ManipulatorRobot:
- # TODO(rcadene): Implement force feedback
- """This class allows to control any manipulator robot of various number of motors.
-
- Non exhaustive list of robots:
- - [Koch v1.0](https://github.com/AlexanderKoch-Koch/low_cost_robot), with and without the wrist-to-elbow expansion, developed
- by Alexander Koch from [Tau Robotics](https://tau-robotics.com)
- - [Koch v1.1](https://github.com/jess-moss/koch-v1-1) developed by Jess Moss
- - [Aloha](https://www.trossenrobotics.com/aloha-kits) developed by Trossen Robotics
-
- Example of instantiation, a pre-defined robot config is required:
- ```python
- robot = ManipulatorRobot(KochRobotConfig())
- ```
-
- Example of overwriting motors during instantiation:
- ```python
- # Defines how to communicate with the motors of the leader and follower arms
- leader_arms = {
- "main": DynamixelMotorsBusConfig(
- port="/dev/tty.usbmodem575E0031751",
- motors={
- # name: (index, model)
- "shoulder_pan": (1, "xl330-m077"),
- "shoulder_lift": (2, "xl330-m077"),
- "elbow_flex": (3, "xl330-m077"),
- "wrist_flex": (4, "xl330-m077"),
- "wrist_roll": (5, "xl330-m077"),
- "gripper": (6, "xl330-m077"),
- },
- ),
- }
- follower_arms = {
- "main": DynamixelMotorsBusConfig(
- port="/dev/tty.usbmodem575E0032081",
- motors={
- # name: (index, model)
- "shoulder_pan": (1, "xl430-w250"),
- "shoulder_lift": (2, "xl430-w250"),
- "elbow_flex": (3, "xl330-m288"),
- "wrist_flex": (4, "xl330-m288"),
- "wrist_roll": (5, "xl330-m288"),
- "gripper": (6, "xl330-m288"),
- },
- ),
- }
- robot_config = KochRobotConfig(leader_arms=leader_arms, follower_arms=follower_arms)
- robot = ManipulatorRobot(robot_config)
- ```
-
- Example of overwriting cameras during instantiation:
- ```python
- # Defines how to communicate with 2 cameras connected to the computer.
- # Here, the webcam of the laptop and the phone (connected in USB to the laptop)
- # can be reached respectively using the camera indices 0 and 1. These indices can be
- # arbitrary. See the documentation of `OpenCVCamera` to find your own camera indices.
- cameras = {
- "laptop": OpenCVCamera(camera_index=0, fps=30, width=640, height=480),
- "phone": OpenCVCamera(camera_index=1, fps=30, width=640, height=480),
- }
- robot = ManipulatorRobot(KochRobotConfig(cameras=cameras))
- ```
-
- Once the robot is instantiated, connect motors buses and cameras if any (Required):
- ```python
- robot.connect()
- ```
-
- Example of highest frequency teleoperation, which doesn't require cameras:
- ```python
- while True:
- robot.teleop_step()
- ```
-
- Example of highest frequency data collection from motors and cameras (if any):
- ```python
- while True:
- observation, action = robot.teleop_step(record_data=True)
- ```
-
- Example of controlling the robot with a policy:
- ```python
- while True:
- # Uses the follower arms and cameras to capture an observation
- observation = robot.capture_observation()
-
- # Assumes a policy has been instantiated
- with torch.inference_mode():
- action = policy.select_action(observation)
-
- # Orders the robot to move
- robot.send_action(action)
- ```
-
- Example of disconnecting which is not mandatory since we disconnect when the object is deleted:
- ```python
- robot.disconnect()
- ```
- """
-
- def __init__(
- self,
- config: ManipulatorRobotConfig,
- ):
- self.config = config
- self.robot_type = self.config.type
- self.calibration_dir = Path(self.config.calibration_dir)
- self.leader_arms = make_motors_buses_from_configs(self.config.leader_arms)
- self.follower_arms = make_motors_buses_from_configs(self.config.follower_arms)
- self.cameras = make_cameras_from_configs(self.config.cameras)
- self.is_connected = False
- self.logs = {}
-
- def get_motor_names(self, arm: dict[str, MotorsBus]) -> list:
- return [f"{arm}_{motor}" for arm, bus in arm.items() for motor in bus.motors]
-
- @property
- def camera_features(self) -> dict:
- cam_ft = {}
- for cam_key, cam in self.cameras.items():
- key = f"observation.images.{cam_key}"
- cam_ft[key] = {
- "shape": (cam.height, cam.width, cam.channels),
- "names": ["height", "width", "channels"],
- "info": None,
- }
- return cam_ft
-
- @property
- def motor_features(self) -> dict:
- action_names = self.get_motor_names(self.leader_arms)
- state_names = self.get_motor_names(self.leader_arms)
- return {
- "action": {
- "dtype": "float32",
- "shape": (len(action_names),),
- "names": action_names,
- },
- "observation.state": {
- "dtype": "float32",
- "shape": (len(state_names),),
- "names": state_names,
- },
- }
-
- @property
- def features(self):
- return {**self.motor_features, **self.camera_features}
-
- @property
- def has_camera(self):
- return len(self.cameras) > 0
-
- @property
- def num_cameras(self):
- return len(self.cameras)
-
- @property
- def available_arms(self):
- available_arms = []
- for name in self.follower_arms:
- arm_id = get_arm_id(name, "follower")
- available_arms.append(arm_id)
- for name in self.leader_arms:
- arm_id = get_arm_id(name, "leader")
- available_arms.append(arm_id)
- return available_arms
-
- def connect(self):
- if self.is_connected:
- raise DeviceAlreadyConnectedError(
- "ManipulatorRobot is already connected. Do not run `robot.connect()` twice."
- )
-
- if not self.leader_arms and not self.follower_arms and not self.cameras:
- raise ValueError(
- "ManipulatorRobot doesn't have any device to connect. See example of usage in docstring of the class."
- )
-
- # Connect the arms
- for name in self.follower_arms:
- print(f"Connecting {name} follower arm.")
- self.follower_arms[name].connect()
- for name in self.leader_arms:
- print(f"Connecting {name} leader arm.")
- self.leader_arms[name].connect()
-
- if self.robot_type in ["koch", "koch_bimanual", "aloha"]:
- from lerobot.common.motors.dynamixel.dynamixel import TorqueMode
- elif self.robot_type in ["so100", "moss", "lekiwi"]:
- from lerobot.common.motors.feetech.feetech import TorqueMode
-
- # We assume that at connection time, arms are in a rest position, and torque can
- # be safely disabled to run calibration and/or set robot preset configurations.
- for name in self.follower_arms:
- self.follower_arms[name].write("Torque_Enable", TorqueMode.DISABLED.value)
- for name in self.leader_arms:
- self.leader_arms[name].write("Torque_Enable", TorqueMode.DISABLED.value)
-
- # Set robot preset (e.g. torque in leader gripper for Koch v1.1)
- if self.robot_type in ["koch", "koch_bimanual"]:
- self.set_koch_robot_preset()
- elif self.robot_type == "aloha":
- self.set_aloha_robot_preset()
- elif self.robot_type in ["so100", "moss", "lekiwi"]:
- self.set_so100_robot_preset()
-
- # Enable torque on all motors of the follower arms
- for name in self.follower_arms:
- print(f"Activating torque on {name} follower arm.")
- self.follower_arms[name].write("Torque_Enable", 1)
-
- if self.config.gripper_open_degree is not None:
- if self.robot_type not in ["koch", "koch_bimanual"]:
- raise NotImplementedError(
- f"{self.robot_type} does not support position AND current control in the handle, which is require to set the gripper open."
- )
- # Set the leader arm in torque mode with the gripper motor set to an angle. This makes it possible
- # to squeeze the gripper and have it spring back to an open position on its own.
- for name in self.leader_arms:
- self.leader_arms[name].write("Torque_Enable", 1, "gripper")
- self.leader_arms[name].write("Goal_Position", self.config.gripper_open_degree, "gripper")
-
- # Check both arms can be read
- for name in self.follower_arms:
- self.follower_arms[name].read("Present_Position")
- for name in self.leader_arms:
- self.leader_arms[name].read("Present_Position")
-
- # Connect the cameras
- for name in self.cameras:
- self.cameras[name].connect()
-
- self.is_connected = True
-
- def set_koch_robot_preset(self):
- def set_operating_mode_(arm):
- from lerobot.common.motors.dynamixel.dynamixel import TorqueMode
-
- if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
- raise ValueError("To run set robot preset, the torque must be disabled on all motors.")
-
- # Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't
- # rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm,
- # you could end up with a servo with a position 0 or 4095 at a crucial point See [
- # https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11]
- all_motors_except_gripper = [name for name in arm.motor_names if name != "gripper"]
- if len(all_motors_except_gripper) > 0:
- # 4 corresponds to Extended Position on Koch motors
- arm.write("Operating_Mode", 4, all_motors_except_gripper)
-
- # Use 'position control current based' for gripper to be limited by the limit of the current.
- # For the follower gripper, it means it can grasp an object without forcing too much even tho,
- # it's goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch).
- # For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger
- # to make it move, and it will move back to its original target position when we release the force.
- # 5 corresponds to Current Controlled Position on Koch gripper motors "xl330-m077, xl330-m288"
- arm.write("Operating_Mode", 5, "gripper")
-
- for name in self.follower_arms:
- set_operating_mode_(self.follower_arms[name])
-
- # Set better PID values to close the gap between recorded states and actions
- # TODO(rcadene): Implement an automatic procedure to set optimal PID values for each motor
- self.follower_arms[name].write("Position_P_Gain", 1500, "elbow_flex")
- self.follower_arms[name].write("Position_I_Gain", 0, "elbow_flex")
- self.follower_arms[name].write("Position_D_Gain", 600, "elbow_flex")
-
- if self.config.gripper_open_degree is not None:
- for name in self.leader_arms:
- set_operating_mode_(self.leader_arms[name])
-
- # Enable torque on the gripper of the leader arms, and move it to 45 degrees,
- # so that we can use it as a trigger to close the gripper of the follower arms.
- self.leader_arms[name].write("Torque_Enable", 1, "gripper")
- self.leader_arms[name].write("Goal_Position", self.config.gripper_open_degree, "gripper")
-
- def set_aloha_robot_preset(self):
- def set_shadow_(arm):
- # Set secondary/shadow ID for shoulder and elbow. These joints have two motors.
- # As a result, if only one of them is required to move to a certain position,
- # the other will follow. This is to avoid breaking the motors.
- if "shoulder_shadow" in arm.motor_names:
- shoulder_idx = arm.read("ID", "shoulder")
- arm.write("Secondary_ID", shoulder_idx, "shoulder_shadow")
-
- if "elbow_shadow" in arm.motor_names:
- elbow_idx = arm.read("ID", "elbow")
- arm.write("Secondary_ID", elbow_idx, "elbow_shadow")
-
- for name in self.follower_arms:
- set_shadow_(self.follower_arms[name])
-
- for name in self.leader_arms:
- set_shadow_(self.leader_arms[name])
-
- for name in self.follower_arms:
- # Set a velocity limit of 131 as advised by Trossen Robotics
- self.follower_arms[name].write("Velocity_Limit", 131)
-
- # Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't
- # rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm,
- # you could end up with a servo with a position 0 or 4095 at a crucial point See [
- # https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11]
- all_motors_except_gripper = [
- name for name in self.follower_arms[name].motor_names if name != "gripper"
- ]
- if len(all_motors_except_gripper) > 0:
- # 4 corresponds to Extended Position on Aloha motors
- self.follower_arms[name].write("Operating_Mode", 4, all_motors_except_gripper)
-
- # Use 'position control current based' for follower gripper to be limited by the limit of the current.
- # It can grasp an object without forcing too much even tho,
- # it's goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch).
- # 5 corresponds to Current Controlled Position on Aloha gripper follower "xm430-w350"
- self.follower_arms[name].write("Operating_Mode", 5, "gripper")
-
- # Note: We can't enable torque on the leader gripper since "xc430-w150" doesn't have
- # a Current Controlled Position mode.
-
- if self.config.gripper_open_degree is not None:
- warnings.warn(
- f"`gripper_open_degree` is set to {self.config.gripper_open_degree}, but None is expected for Aloha instead",
- stacklevel=1,
- )
-
- def set_so100_robot_preset(self):
- for name in self.follower_arms:
- # Mode=0 for Position Control
- self.follower_arms[name].write("Mode", 0)
- # Set P_Coefficient to lower value to avoid shakiness (Default is 32)
- self.follower_arms[name].write("P_Coefficient", 16)
- # Set I_Coefficient and D_Coefficient to default value 0 and 32
- self.follower_arms[name].write("I_Coefficient", 0)
- self.follower_arms[name].write("D_Coefficient", 32)
- # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of
- # the motors. Note: this configuration is not in the official STS3215 Memory Table
- self.follower_arms[name].write("Maximum_Acceleration", 254)
- self.follower_arms[name].write("Acceleration", 254)
-
- def teleop_step(
- self, record_data=False
- ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
- if not self.is_connected:
- raise DeviceNotConnectedError(
- "ManipulatorRobot is not connected. You need to run `robot.connect()`."
- )
-
- # Prepare to assign the position of the leader to the follower
- leader_pos = {}
- for name in self.leader_arms:
- before_lread_t = time.perf_counter()
- leader_pos[name] = self.leader_arms[name].read("Present_Position")
- leader_pos[name] = torch.from_numpy(leader_pos[name])
- self.logs[f"read_leader_{name}_pos_dt_s"] = time.perf_counter() - before_lread_t
-
- # Send goal position to the follower
- follower_goal_pos = {}
- for name in self.follower_arms:
- before_fwrite_t = time.perf_counter()
- goal_pos = leader_pos[name]
-
- # 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.follower_arms[name].read("Present_Position")
- present_pos = torch.from_numpy(present_pos)
- goal_pos = ensure_safe_goal_position(goal_pos, present_pos, self.config.max_relative_target)
-
- # Used when record_data=True
- follower_goal_pos[name] = goal_pos
-
- goal_pos = goal_pos.numpy().astype(np.float32)
- self.follower_arms[name].write("Goal_Position", goal_pos)
- self.logs[f"write_follower_{name}_goal_pos_dt_s"] = time.perf_counter() - before_fwrite_t
-
- # Early exit when recording data is not requested
- if not record_data:
- return
-
- # TODO(rcadene): Add velocity and other info
- # Read follower position
- follower_pos = {}
- for name in self.follower_arms:
- before_fread_t = time.perf_counter()
- follower_pos[name] = self.follower_arms[name].read("Present_Position")
- follower_pos[name] = torch.from_numpy(follower_pos[name])
- self.logs[f"read_follower_{name}_pos_dt_s"] = time.perf_counter() - before_fread_t
-
- # Create state by concatenating follower current position
- state = []
- for name in self.follower_arms:
- if name in follower_pos:
- state.append(follower_pos[name])
- state = torch.cat(state)
-
- # Create action by concatenating follower goal position
- action = []
- for name in self.follower_arms:
- if name in follower_goal_pos:
- action.append(follower_goal_pos[name])
- action = torch.cat(action)
-
- # Capture images from cameras
- images = {}
- for name in self.cameras:
- before_camread_t = time.perf_counter()
- images[name] = self.cameras[name].async_read()
- images[name] = torch.from_numpy(images[name])
- self.logs[f"read_camera_{name}_dt_s"] = self.cameras[name].logs["delta_timestamp_s"]
- self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t
-
- # Populate output dictionaries
- obs_dict, action_dict = {}, {}
- obs_dict["observation.state"] = state
- action_dict["action"] = action
- for name in self.cameras:
- obs_dict[f"observation.images.{name}"] = images[name]
-
- return obs_dict, action_dict
-
- def capture_observation(self):
- """The returned observations do not have a batch dimension."""
- if not self.is_connected:
- raise DeviceNotConnectedError(
- "ManipulatorRobot is not connected. You need to run `robot.connect()`."
- )
-
- # Read follower position
- follower_pos = {}
- for name in self.follower_arms:
- before_fread_t = time.perf_counter()
- follower_pos[name] = self.follower_arms[name].read("Present_Position")
- follower_pos[name] = torch.from_numpy(follower_pos[name])
- self.logs[f"read_follower_{name}_pos_dt_s"] = time.perf_counter() - before_fread_t
-
- # Create state by concatenating follower current position
- state = []
- for name in self.follower_arms:
- if name in follower_pos:
- state.append(follower_pos[name])
- state = torch.cat(state)
-
- # Capture images from cameras
- images = {}
- for name in self.cameras:
- before_camread_t = time.perf_counter()
- images[name] = self.cameras[name].async_read()
- images[name] = torch.from_numpy(images[name])
- self.logs[f"read_camera_{name}_dt_s"] = self.cameras[name].logs["delta_timestamp_s"]
- self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t
-
- # Populate output dictionaries and format to pytorch
- obs_dict = {}
- obs_dict["observation.state"] = state
- for name in self.cameras:
- obs_dict[f"observation.images.{name}"] = images[name]
- return obs_dict
-
- def send_action(self, action: torch.Tensor) -> torch.Tensor:
- """Command the follower arms to move to a target joint configuration.
-
- The relative action magnitude may be clipped depending on the configuration parameter
- `max_relative_target`. In this case, the action sent differs from original action.
- Thus, this function always returns the action actually sent.
-
- Args:
- action: tensor containing the concatenated goal positions for the follower arms.
- """
- if not self.is_connected:
- raise DeviceNotConnectedError(
- "ManipulatorRobot is not connected. You need to run `robot.connect()`."
- )
-
- from_idx = 0
- to_idx = 0
- action_sent = []
- for name in self.follower_arms:
- # Get goal position of each follower arm by splitting the action vector
- to_idx += len(self.follower_arms[name].motor_names)
- goal_pos = action[from_idx:to_idx]
- from_idx = to_idx
-
- # 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.follower_arms[name].read("Present_Position")
- present_pos = torch.from_numpy(present_pos)
- goal_pos = ensure_safe_goal_position(goal_pos, present_pos, self.config.max_relative_target)
-
- # Save tensor to concat and return
- action_sent.append(goal_pos)
-
- # Send goal position to each follower
- goal_pos = goal_pos.numpy().astype(np.float32)
- self.follower_arms[name].write("Goal_Position", goal_pos)
-
- return torch.cat(action_sent)
-
- def print_logs(self):
- pass
- # TODO(aliberts): move robot-specific logs logic here
-
- def disconnect(self):
- if not self.is_connected:
- raise DeviceNotConnectedError(
- "ManipulatorRobot is not connected. You need to run `robot.connect()` before disconnecting."
- )
-
- for name in self.follower_arms:
- self.follower_arms[name].disconnect()
-
- for name in self.leader_arms:
- self.leader_arms[name].disconnect()
-
- for name in self.cameras:
- self.cameras[name].disconnect()
-
- self.is_connected = False
-
- def __del__(self):
- if getattr(self, "is_connected", False):
- self.disconnect()
diff --git a/lerobot/common/robots/mobile_manipulator.py b/lerobot/common/robots/mobile_manipulator.py
deleted file mode 100644
index 330277915..000000000
--- a/lerobot/common/robots/mobile_manipulator.py
+++ /dev/null
@@ -1,704 +0,0 @@
-# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import base64
-import json
-import os
-import sys
-from pathlib import Path
-
-import cv2
-import numpy as np
-import torch
-import zmq
-
-from lerobot.common.cameras.utils import make_cameras_from_configs
-from lerobot.common.errors import DeviceNotConnectedError
-from lerobot.common.motors.feetech.feetech import TorqueMode
-from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration
-from lerobot.common.motors.motors_bus import MotorsBus
-from lerobot.common.motors.utils import make_motors_buses_from_configs
-from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig
-from lerobot.common.robots.utils import get_arm_id
-
-PYNPUT_AVAILABLE = True
-try:
- # Only import if there's a valid X server or if we're not on a Pi
- if ("DISPLAY" not in os.environ) and ("linux" in sys.platform):
- print("No DISPLAY set. Skipping pynput import.")
- raise ImportError("pynput blocked intentionally due to no display.")
-
- from pynput import keyboard
-except ImportError:
- keyboard = None
- PYNPUT_AVAILABLE = False
-except Exception as e:
- keyboard = None
- PYNPUT_AVAILABLE = False
- print(f"Could not import pynput: {e}")
-
-
-class MobileManipulator:
- """
- MobileManipulator is a class for connecting to and controlling a remote mobile manipulator robot.
- The robot includes a three omniwheel mobile base and a remote follower arm.
- The leader arm is connected locally (on the laptop) and its joint positions are recorded and then
- forwarded to the remote follower arm (after applying a safety clamp).
- In parallel, keyboard teleoperation is used to generate raw velocity commands for the wheels.
- """
-
- def __init__(self, config: LeKiwiRobotConfig):
- """
- Expected keys in config:
- - ip, port, video_port for the remote connection.
- - calibration_dir, leader_arms, follower_arms, max_relative_target, etc.
- """
- self.robot_type = config.type
- self.config = config
- self.remote_ip = config.ip
- self.remote_port = config.port
- self.remote_port_video = config.video_port
- self.calibration_dir = Path(self.config.calibration_dir)
- self.logs = {}
-
- self.teleop_keys = self.config.teleop_keys
-
- # For teleoperation, the leader arm (local) is used to record the desired arm pose.
- self.leader_arms = make_motors_buses_from_configs(self.config.leader_arms)
-
- self.follower_arms = make_motors_buses_from_configs(self.config.follower_arms)
-
- self.cameras = make_cameras_from_configs(self.config.cameras)
-
- self.is_connected = False
-
- self.last_frames = {}
- self.last_present_speed = {}
- self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32)
-
- # Define three speed levels and a current index
- self.speed_levels = [
- {"xy": 0.1, "theta": 30}, # slow
- {"xy": 0.2, "theta": 60}, # medium
- {"xy": 0.3, "theta": 90}, # fast
- ]
- self.speed_index = 0 # Start at slow
-
- # ZeroMQ context and sockets.
- self.context = None
- self.cmd_socket = None
- self.video_socket = None
-
- # Keyboard state for base teleoperation.
- self.running = True
- self.pressed_keys = {
- "forward": False,
- "backward": False,
- "left": False,
- "right": False,
- "rotate_left": False,
- "rotate_right": False,
- }
-
- if PYNPUT_AVAILABLE:
- print("pynput is available - enabling local keyboard listener.")
- self.listener = keyboard.Listener(
- on_press=self.on_press,
- on_release=self.on_release,
- )
- self.listener.start()
- else:
- print("pynput not available - skipping local keyboard listener.")
- self.listener = None
-
- def get_motor_names(self, arms: dict[str, MotorsBus]) -> list:
- return [f"{arm}_{motor}" for arm, bus in arms.items() for motor in bus.motors]
-
- @property
- def camera_features(self) -> dict:
- cam_ft = {}
- for cam_key, cam in self.cameras.items():
- key = f"observation.images.{cam_key}"
- cam_ft[key] = {
- "shape": (cam.height, cam.width, cam.channels),
- "names": ["height", "width", "channels"],
- "info": None,
- }
- return cam_ft
-
- @property
- def motor_features(self) -> dict:
- follower_arm_names = [
- "shoulder_pan",
- "shoulder_lift",
- "elbow_flex",
- "wrist_flex",
- "wrist_roll",
- "gripper",
- ]
- observations = ["x_mm", "y_mm", "theta"]
- combined_names = follower_arm_names + observations
- return {
- "action": {
- "dtype": "float32",
- "shape": (len(combined_names),),
- "names": combined_names,
- },
- "observation.state": {
- "dtype": "float32",
- "shape": (len(combined_names),),
- "names": combined_names,
- },
- }
-
- @property
- def features(self):
- return {**self.motor_features, **self.camera_features}
-
- @property
- def has_camera(self):
- return len(self.cameras) > 0
-
- @property
- def num_cameras(self):
- return len(self.cameras)
-
- @property
- def available_arms(self):
- available = []
- for name in self.leader_arms:
- available.append(get_arm_id(name, "leader"))
- for name in self.follower_arms:
- available.append(get_arm_id(name, "follower"))
- return available
-
- def on_press(self, key):
- try:
- # Movement
- if key.char == self.teleop_keys["forward"]:
- self.pressed_keys["forward"] = True
- elif key.char == self.teleop_keys["backward"]:
- self.pressed_keys["backward"] = True
- elif key.char == self.teleop_keys["left"]:
- self.pressed_keys["left"] = True
- elif key.char == self.teleop_keys["right"]:
- self.pressed_keys["right"] = True
- elif key.char == self.teleop_keys["rotate_left"]:
- self.pressed_keys["rotate_left"] = True
- elif key.char == self.teleop_keys["rotate_right"]:
- self.pressed_keys["rotate_right"] = True
-
- # Quit teleoperation
- elif key.char == self.teleop_keys["quit"]:
- self.running = False
- return False
-
- # Speed control
- elif key.char == self.teleop_keys["speed_up"]:
- self.speed_index = min(self.speed_index + 1, 2)
- print(f"Speed index increased to {self.speed_index}")
- elif key.char == self.teleop_keys["speed_down"]:
- self.speed_index = max(self.speed_index - 1, 0)
- print(f"Speed index decreased to {self.speed_index}")
-
- except AttributeError:
- # e.g., if key is special like Key.esc
- if key == keyboard.Key.esc:
- self.running = False
- return False
-
- def on_release(self, key):
- try:
- if hasattr(key, "char"):
- if key.char == self.teleop_keys["forward"]:
- self.pressed_keys["forward"] = False
- elif key.char == self.teleop_keys["backward"]:
- self.pressed_keys["backward"] = False
- elif key.char == self.teleop_keys["left"]:
- self.pressed_keys["left"] = False
- elif key.char == self.teleop_keys["right"]:
- self.pressed_keys["right"] = False
- elif key.char == self.teleop_keys["rotate_left"]:
- self.pressed_keys["rotate_left"] = False
- elif key.char == self.teleop_keys["rotate_right"]:
- self.pressed_keys["rotate_right"] = False
- except AttributeError:
- pass
-
- def connect(self):
- if not self.leader_arms:
- raise ValueError("MobileManipulator has no leader arm to connect.")
- for name in self.leader_arms:
- print(f"Connecting {name} leader arm.")
- self.calibrate_leader()
-
- # Set up ZeroMQ sockets to communicate with the remote mobile robot.
- self.context = zmq.Context()
- self.cmd_socket = self.context.socket(zmq.PUSH)
- connection_string = f"tcp://{self.remote_ip}:{self.remote_port}"
- self.cmd_socket.connect(connection_string)
- self.cmd_socket.setsockopt(zmq.CONFLATE, 1)
- self.video_socket = self.context.socket(zmq.PULL)
- video_connection = f"tcp://{self.remote_ip}:{self.remote_port_video}"
- self.video_socket.connect(video_connection)
- self.video_socket.setsockopt(zmq.CONFLATE, 1)
- print(
- f"[INFO] Connected to remote robot at {connection_string} and video stream at {video_connection}."
- )
- self.is_connected = True
-
- def load_or_run_calibration_(self, name, arm, arm_type):
- arm_id = get_arm_id(name, arm_type)
- arm_calib_path = self.calibration_dir / f"{arm_id}.json"
-
- if arm_calib_path.exists():
- with open(arm_calib_path) as f:
- calibration = json.load(f)
- else:
- print(f"Missing calibration file '{arm_calib_path}'")
- calibration = run_full_arm_calibration(arm, self.robot_type, name, arm_type)
- print(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
- arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
- with open(arm_calib_path, "w") as f:
- json.dump(calibration, f)
-
- return calibration
-
- def calibrate_leader(self):
- for name, arm in self.leader_arms.items():
- # Connect the bus
- arm.connect()
-
- # Disable torque on all motors
- for motor_id in arm.motors:
- arm.write("Torque_Enable", TorqueMode.DISABLED.value, motor_id)
-
- # Now run calibration
- calibration = self.load_or_run_calibration_(name, arm, "leader")
- arm.set_calibration(calibration)
-
- def calibrate_follower(self):
- for name, bus in self.follower_arms.items():
- bus.connect()
-
- # Disable torque on all motors
- for motor_id in bus.motors:
- bus.write("Torque_Enable", 0, motor_id)
-
- # Then filter out wheels
- arm_only_dict = {k: v for k, v in bus.motors.items() if not k.startswith("wheel_")}
- if not arm_only_dict:
- continue
-
- original_motors = bus.motors
- bus.motors = arm_only_dict
-
- calibration = self.load_or_run_calibration_(name, bus, "follower")
- bus.set_calibration(calibration)
-
- bus.motors = original_motors
-
- def _get_data(self):
- """
- Polls the video socket for up to 15 ms. If data arrives, decode only
- the *latest* message, returning frames, speed, and arm state. If
- nothing arrives for any field, use the last known values.
- """
- frames = {}
- present_speed = {}
- remote_arm_state_tensor = torch.zeros(6, dtype=torch.float32)
-
- # Poll up to 15 ms
- poller = zmq.Poller()
- poller.register(self.video_socket, zmq.POLLIN)
- socks = dict(poller.poll(15))
- if self.video_socket not in socks or socks[self.video_socket] != zmq.POLLIN:
- # No new data arrived → reuse ALL old data
- return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
-
- # Drain all messages, keep only the last
- last_msg = None
- while True:
- try:
- obs_string = self.video_socket.recv_string(zmq.NOBLOCK)
- last_msg = obs_string
- except zmq.Again:
- break
-
- if not last_msg:
- # No new message → also reuse old
- return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
-
- # Decode only the final message
- try:
- observation = json.loads(last_msg)
-
- images_dict = observation.get("images", {})
- new_speed = observation.get("present_speed", {})
- new_arm_state = observation.get("follower_arm_state", None)
-
- # Convert images
- for cam_name, image_b64 in images_dict.items():
- if image_b64:
- jpg_data = base64.b64decode(image_b64)
- np_arr = np.frombuffer(jpg_data, dtype=np.uint8)
- frame_candidate = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
- if frame_candidate is not None:
- frames[cam_name] = frame_candidate
-
- # If remote_arm_state is None and frames is None there is no message then use the previous message
- if new_arm_state is not None and frames is not None:
- self.last_frames = frames
-
- remote_arm_state_tensor = torch.tensor(new_arm_state, dtype=torch.float32)
- self.last_remote_arm_state = remote_arm_state_tensor
-
- present_speed = new_speed
- self.last_present_speed = new_speed
- else:
- frames = self.last_frames
-
- remote_arm_state_tensor = self.last_remote_arm_state
-
- present_speed = self.last_present_speed
-
- except Exception as e:
- print(f"[DEBUG] Error decoding video message: {e}")
- # If decode fails, fall back to old data
- return (self.last_frames, self.last_present_speed, self.last_remote_arm_state)
-
- return frames, present_speed, remote_arm_state_tensor
-
- def _process_present_speed(self, present_speed: dict) -> torch.Tensor:
- state_tensor = torch.zeros(3, dtype=torch.int32)
- if present_speed:
- decoded = {key: MobileManipulator.raw_to_degps(value) for key, value in present_speed.items()}
- if "1" in decoded:
- state_tensor[0] = decoded["1"]
- if "2" in decoded:
- state_tensor[1] = decoded["2"]
- if "3" in decoded:
- state_tensor[2] = decoded["3"]
- return state_tensor
-
- def teleop_step(
- self, record_data: bool = False
- ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
- if not self.is_connected:
- raise DeviceNotConnectedError("MobileManipulator is not connected. Run `connect()` first.")
-
- speed_setting = self.speed_levels[self.speed_index]
- xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4
- theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90
-
- # Prepare to assign the position of the leader to the follower
- arm_positions = []
- for name in self.leader_arms:
- pos = self.leader_arms[name].read("Present_Position")
- pos_tensor = torch.from_numpy(pos).float()
- arm_positions.extend(pos_tensor.tolist())
-
- y_cmd = 0.0 # m/s forward/backward
- x_cmd = 0.0 # m/s lateral
- theta_cmd = 0.0 # deg/s rotation
- if self.pressed_keys["forward"]:
- y_cmd += xy_speed
- if self.pressed_keys["backward"]:
- y_cmd -= xy_speed
- if self.pressed_keys["left"]:
- x_cmd += xy_speed
- if self.pressed_keys["right"]:
- x_cmd -= xy_speed
- if self.pressed_keys["rotate_left"]:
- theta_cmd += theta_speed
- if self.pressed_keys["rotate_right"]:
- theta_cmd -= theta_speed
-
- wheel_commands = self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd)
-
- message = {"raw_velocity": wheel_commands, "arm_positions": arm_positions}
- self.cmd_socket.send_string(json.dumps(message))
-
- if not record_data:
- return
-
- obs_dict = self.capture_observation()
-
- arm_state_tensor = torch.tensor(arm_positions, dtype=torch.float32)
-
- wheel_velocity_tuple = self.wheel_raw_to_body(wheel_commands)
- wheel_velocity_mm = (
- wheel_velocity_tuple[0] * 1000.0,
- wheel_velocity_tuple[1] * 1000.0,
- wheel_velocity_tuple[2],
- )
- wheel_tensor = torch.tensor(wheel_velocity_mm, dtype=torch.float32)
- action_tensor = torch.cat([arm_state_tensor, wheel_tensor])
- action_dict = {"action": action_tensor}
-
- return obs_dict, action_dict
-
- def capture_observation(self) -> dict:
- """
- Capture observations from the remote robot: current follower arm positions,
- present wheel speeds (converted to body-frame velocities: x, y, theta),
- and a camera frame.
- """
- if not self.is_connected:
- raise DeviceNotConnectedError("Not connected. Run `connect()` first.")
-
- frames, present_speed, remote_arm_state_tensor = self._get_data()
-
- body_state = self.wheel_raw_to_body(present_speed)
-
- body_state_mm = (body_state[0] * 1000.0, body_state[1] * 1000.0, body_state[2]) # Convert x,y to mm/s
- wheel_state_tensor = torch.tensor(body_state_mm, dtype=torch.float32)
- combined_state_tensor = torch.cat((remote_arm_state_tensor, wheel_state_tensor), dim=0)
-
- obs_dict = {"observation.state": combined_state_tensor}
-
- # Loop over each configured camera
- for cam_name, cam in self.cameras.items():
- frame = frames.get(cam_name, None)
- if frame is None:
- # Create a black image using the camera's configured width, height, and channels
- frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8)
- obs_dict[f"observation.images.{cam_name}"] = torch.from_numpy(frame)
-
- return obs_dict
-
- def send_action(self, action: torch.Tensor) -> torch.Tensor:
- if not self.is_connected:
- raise DeviceNotConnectedError("Not connected. Run `connect()` first.")
-
- # Ensure the action tensor has at least 9 elements:
- # - First 6: arm positions.
- # - Last 3: base commands.
- if action.numel() < 9:
- # Pad with zeros if there are not enough elements.
- padded = torch.zeros(9, dtype=action.dtype)
- padded[: action.numel()] = action
- action = padded
-
- # Extract arm and base actions.
- arm_actions = action[:6].flatten()
- base_actions = action[6:].flatten()
-
- x_cmd_mm = base_actions[0].item() # mm/s
- y_cmd_mm = base_actions[1].item() # mm/s
- theta_cmd = base_actions[2].item() # deg/s
-
- # Convert mm/s to m/s for the kinematics calculations.
- x_cmd = x_cmd_mm / 1000.0 # m/s
- y_cmd = y_cmd_mm / 1000.0 # m/s
-
- # Compute wheel commands from body commands.
- wheel_commands = self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd)
-
- arm_positions_list = arm_actions.tolist()
-
- message = {"raw_velocity": wheel_commands, "arm_positions": arm_positions_list}
- self.cmd_socket.send_string(json.dumps(message))
-
- return action
-
- def print_logs(self):
- pass
-
- def disconnect(self):
- if not self.is_connected:
- raise DeviceNotConnectedError("Not connected.")
- if self.cmd_socket:
- stop_cmd = {
- "raw_velocity": {"left_wheel": 0, "back_wheel": 0, "right_wheel": 0},
- "arm_positions": {},
- }
- self.cmd_socket.send_string(json.dumps(stop_cmd))
- self.cmd_socket.close()
- if self.video_socket:
- self.video_socket.close()
- if self.context:
- self.context.term()
- if PYNPUT_AVAILABLE:
- self.listener.stop()
- self.is_connected = False
- print("[INFO] Disconnected from remote robot.")
-
- def __del__(self):
- if getattr(self, "is_connected", False):
- self.disconnect()
- if PYNPUT_AVAILABLE:
- self.listener.stop()
-
- @staticmethod
- def degps_to_raw(degps: float) -> int:
- steps_per_deg = 4096.0 / 360.0
- speed_in_steps = abs(degps) * steps_per_deg
- speed_int = int(round(speed_in_steps))
- if speed_int > 0x7FFF:
- speed_int = 0x7FFF
- if degps < 0:
- return speed_int | 0x8000
- else:
- return speed_int & 0x7FFF
-
- @staticmethod
- def raw_to_degps(raw_speed: int) -> float:
- steps_per_deg = 4096.0 / 360.0
- magnitude = raw_speed & 0x7FFF
- degps = magnitude / steps_per_deg
- if raw_speed & 0x8000:
- degps = -degps
- return degps
-
- def body_to_wheel_raw(
- self,
- x_cmd: float,
- y_cmd: float,
- theta_cmd: float,
- wheel_radius: float = 0.05,
- base_radius: float = 0.125,
- max_raw: int = 3000,
- ) -> dict:
- """
- Convert desired body-frame velocities into wheel raw commands.
-
- Parameters:
- x_cmd : Linear velocity in x (m/s).
- y_cmd : Linear velocity in y (m/s).
- theta_cmd : Rotational velocity (deg/s).
- wheel_radius: Radius of each wheel (meters).
- base_radius : Distance from the center of rotation to each wheel (meters).
- max_raw : Maximum allowed raw command (ticks) per wheel.
-
- Returns:
- A dictionary with wheel raw commands:
- {"left_wheel": value, "back_wheel": value, "right_wheel": value}.
-
- Notes:
- - Internally, the method converts theta_cmd to rad/s for the kinematics.
- - The raw command is computed from the wheels angular speed in deg/s
- using degps_to_raw(). If any command exceeds max_raw, all commands
- are scaled down proportionally.
- """
- # Convert rotational velocity from deg/s to rad/s.
- theta_rad = theta_cmd * (np.pi / 180.0)
- # Create the body velocity vector [x, y, theta_rad].
- velocity_vector = np.array([x_cmd, y_cmd, theta_rad])
-
- # Define the wheel mounting angles (defined from y axis cw)
- angles = np.radians(np.array([300, 180, 60]))
- # Build the kinematic matrix: each row maps body velocities to a wheel’s linear speed.
- # The third column (base_radius) accounts for the effect of rotation.
- m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
-
- # Compute each wheel’s linear speed (m/s) and then its angular speed (rad/s).
- wheel_linear_speeds = m.dot(velocity_vector)
- wheel_angular_speeds = wheel_linear_speeds / wheel_radius
-
- # Convert wheel angular speeds from rad/s to deg/s.
- wheel_degps = wheel_angular_speeds * (180.0 / np.pi)
-
- # Scaling
- steps_per_deg = 4096.0 / 360.0
- raw_floats = [abs(degps) * steps_per_deg for degps in wheel_degps]
- max_raw_computed = max(raw_floats)
- if max_raw_computed > max_raw:
- scale = max_raw / max_raw_computed
- wheel_degps = wheel_degps * scale
-
- # Convert each wheel’s angular speed (deg/s) to a raw integer.
- wheel_raw = [MobileManipulator.degps_to_raw(deg) for deg in wheel_degps]
-
- return {"left_wheel": wheel_raw[0], "back_wheel": wheel_raw[1], "right_wheel": wheel_raw[2]}
-
- def wheel_raw_to_body(
- self, wheel_raw: dict, wheel_radius: float = 0.05, base_radius: float = 0.125
- ) -> tuple:
- """
- Convert wheel raw command feedback back into body-frame velocities.
-
- Parameters:
- wheel_raw : Dictionary with raw wheel commands (keys: "left_wheel", "back_wheel", "right_wheel").
- wheel_radius: Radius of each wheel (meters).
- base_radius : Distance from the robot center to each wheel (meters).
-
- Returns:
- A tuple (x_cmd, y_cmd, theta_cmd) where:
- x_cmd : Linear velocity in x (m/s).
- y_cmd : Linear velocity in y (m/s).
- theta_cmd : Rotational velocity in deg/s.
- """
- # Extract the raw values in order.
- raw_list = [
- int(wheel_raw.get("left_wheel", 0)),
- int(wheel_raw.get("back_wheel", 0)),
- int(wheel_raw.get("right_wheel", 0)),
- ]
-
- # Convert each raw command back to an angular speed in deg/s.
- wheel_degps = np.array([MobileManipulator.raw_to_degps(r) for r in raw_list])
- # Convert from deg/s to rad/s.
- wheel_radps = wheel_degps * (np.pi / 180.0)
- # Compute each wheel’s linear speed (m/s) from its angular speed.
- wheel_linear_speeds = wheel_radps * wheel_radius
-
- # Define the wheel mounting angles (defined from y axis cw)
- angles = np.radians(np.array([300, 180, 60]))
- m = np.array([[np.cos(a), np.sin(a), base_radius] for a in angles])
-
- # Solve the inverse kinematics: body_velocity = M⁻¹ · wheel_linear_speeds.
- m_inv = np.linalg.inv(m)
- velocity_vector = m_inv.dot(wheel_linear_speeds)
- x_cmd, y_cmd, theta_rad = velocity_vector
- theta_cmd = theta_rad * (180.0 / np.pi)
- return (x_cmd, y_cmd, theta_cmd)
-
-
-class LeKiwi:
- def __init__(self, motor_bus):
- """
- Initializes the LeKiwi with Feetech motors bus.
- """
- self.motor_bus = motor_bus
- self.motor_ids = ["left_wheel", "back_wheel", "right_wheel"]
-
- # Initialize motors in velocity mode.
- self.motor_bus.write("Lock", 0)
- self.motor_bus.write("Mode", [1, 1, 1], self.motor_ids)
- self.motor_bus.write("Lock", 1)
- print("Motors set to velocity mode.")
-
- def read_velocity(self):
- """
- Reads the raw speeds for all wheels. Returns a dictionary with motor names:
- """
- raw_speeds = self.motor_bus.read("Present_Speed", self.motor_ids)
- return {
- "left_wheel": int(raw_speeds[0]),
- "back_wheel": int(raw_speeds[1]),
- "right_wheel": int(raw_speeds[2]),
- }
-
- def set_velocity(self, command_speeds):
- """
- Sends raw velocity commands (16-bit encoded values) directly to the motor bus.
- The order of speeds must correspond to self.motor_ids.
- """
- self.motor_bus.write("Goal_Speed", command_speeds, self.motor_ids)
-
- def stop(self):
- """Stops the robot by setting all motor speeds to zero."""
- self.motor_bus.write("Goal_Speed", [0, 0, 0], self.motor_ids)
- print("Motors stopped.")
diff --git a/lerobot/common/robots/moss/__init__.py b/lerobot/common/robots/moss/__init__.py
deleted file mode 100644
index f7c840a3f..000000000
--- a/lerobot/common/robots/moss/__init__.py
+++ /dev/null
@@ -1,4 +0,0 @@
-from .configuration_moss import MossRobotConfig
-from .robot_moss import MossRobot
-
-__all__ = ["MossRobotConfig", "MossRobot"]
diff --git a/lerobot/common/robots/moss/robot_moss.py b/lerobot/common/robots/moss/robot_moss.py
deleted file mode 100644
index 0f7e5e778..000000000
--- a/lerobot/common/robots/moss/robot_moss.py
+++ /dev/null
@@ -1,223 +0,0 @@
-#!/usr/bin/env python
-
-# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import json
-import logging
-import time
-
-import numpy as np
-
-from lerobot.common.cameras.utils import make_cameras_from_configs
-from lerobot.common.constants import OBS_IMAGES, OBS_STATE
-from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
-from lerobot.common.motors import TorqueMode
-from lerobot.common.motors.feetech import (
- FeetechMotorsBus,
- apply_feetech_offsets_from_calibration,
- run_full_arm_calibration,
-)
-
-from ..robot import Robot
-from ..utils import ensure_safe_goal_position
-from .configuration_moss import MossRobotConfig
-
-
-class MossRobot(Robot):
- """
- [Moss Arm](https://github.com/jess-moss/moss-robot-arms) designed by Jess Moss
- """
-
- config_class = MossRobotConfig
- name = "moss"
-
- def __init__(self, config: MossRobotConfig):
- super().__init__(config)
- self.config = config
- self.robot_type = config.type
-
- self.arm = FeetechMotorsBus(
- port=self.config.port,
- motors={
- "shoulder_pan": config.shoulder_pan,
- "shoulder_lift": config.shoulder_lift,
- "elbow_flex": config.elbow_flex,
- "wrist_flex": config.wrist_flex,
- "wrist_roll": config.wrist_roll,
- "gripper": config.gripper,
- },
- )
- self.cameras = make_cameras_from_configs(config.cameras)
-
- self.is_connected = False
- self.logs = {}
-
- @property
- def state_feature(self) -> dict:
- return {
- "dtype": "float32",
- "shape": (len(self.arm),),
- "names": {"motors": list(self.arm.motors)},
- }
-
- @property
- def action_feature(self) -> dict:
- return self.state_feature
-
- @property
- def camera_features(self) -> dict[str, dict]:
- cam_ft = {}
- for cam_key, cam in self.cameras.items():
- cam_ft[cam_key] = {
- "shape": (cam.height, cam.width, cam.channels),
- "names": ["height", "width", "channels"],
- "info": None,
- }
- return cam_ft
-
- def connect(self) -> None:
- if self.is_connected:
- raise DeviceAlreadyConnectedError(
- "ManipulatorRobot is already connected. Do not run `robot.connect()` twice."
- )
-
- logging.info("Connecting arm.")
- self.arm.connect()
-
- # We assume that at connection time, arm is in a rest position,
- # and torque can be safely disabled to run calibration.
- self.arm.write("Torque_Enable", TorqueMode.DISABLED.value)
- self.calibrate()
-
- # Mode=0 for Position Control
- self.arm.write("Mode", 0)
- # Set P_Coefficient to lower value to avoid shakiness (Default is 32)
- self.arm.write("P_Coefficient", 16)
- # Set I_Coefficient and D_Coefficient to default value 0 and 32
- self.arm.write("I_Coefficient", 0)
- self.arm.write("D_Coefficient", 32)
- # Close the write lock so that Maximum_Acceleration gets written to EPROM address,
- # which is mandatory for Maximum_Acceleration to take effect after rebooting.
- self.arm.write("Lock", 0)
- # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of
- # the motors. Note: this configuration is not in the official STS3215 Memory Table
- self.arm.write("Maximum_Acceleration", 254)
- self.arm.write("Acceleration", 254)
-
- logging.info("Activating torque.")
- self.arm.write("Torque_Enable", TorqueMode.ENABLED.value)
-
- # Check arm can be read
- self.arm.read("Present_Position")
-
- # Connect the cameras
- for cam in self.cameras.values():
- cam.connect()
-
- self.is_connected = True
-
- def calibrate(self) -> None:
- """After calibration all motors function in human interpretable ranges.
- Rotations are expressed in degrees in nominal range of [-180, 180],
- and linear motions (like gripper of Aloha) in nominal range of [0, 100].
- """
- if self.calibration_fpath.exists():
- with open(self.calibration_fpath) as f:
- calibration = json.load(f)
- else:
- # TODO(rcadene): display a warning in __init__ if calibration file not available
- logging.info(f"Missing calibration file '{self.calibration_fpath}'")
- calibration = run_full_arm_calibration(self.arm, self.robot_type, self.name, "follower")
-
- logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
- self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
- with open(self.calibration_fpath, "w") as f:
- json.dump(calibration, f)
-
- self.arm.set_calibration(calibration)
- apply_feetech_offsets_from_calibration(self.arm, calibration)
-
- def get_observation(self) -> dict[str, np.ndarray]:
- """The returned observations do not have a batch dimension."""
- if not self.is_connected:
- raise DeviceNotConnectedError(
- "ManipulatorRobot is not connected. You need to run `robot.connect()`."
- )
-
- obs_dict = {}
-
- # Read arm position
- before_read_t = time.perf_counter()
- obs_dict[OBS_STATE] = self.arm.read("Present_Position")
- self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-
- # Capture images from cameras
- for cam_key, cam in self.cameras.items():
- before_camread_t = time.perf_counter()
- obs_dict[f"{OBS_IMAGES}.{cam_key}"] = cam.async_read()
- self.logs[f"read_camera_{cam_key}_dt_s"] = cam.logs["delta_timestamp_s"]
- self.logs[f"async_read_camera_{cam_key}_dt_s"] = time.perf_counter() - before_camread_t
-
- return obs_dict
-
- def send_action(self, action: np.ndarray) -> np.ndarray:
- """Command arm to move to a target joint configuration.
-
- The relative action magnitude may be clipped depending on the configuration parameter
- `max_relative_target`. In this case, the action sent differs from original action.
- Thus, this function always returns the action actually sent.
-
- Args:
- action (np.ndarray): array containing the goal positions for the motors.
-
- Raises:
- RobotDeviceNotConnectedError: if robot is not connected.
-
- Returns:
- np.ndarray: the action sent to the motors, potentially clipped.
- """
- if not self.is_connected:
- raise DeviceNotConnectedError(
- "ManipulatorRobot is not connected. You need to run `robot.connect()`."
- )
-
- goal_pos = action
-
- # Cap goal position when too far away from present position.
- # /!\ Slower fps expected due to reading from the follower.
- if self.config.max_relative_target is not None:
- present_pos = self.arm.read("Present_Position")
- goal_pos = ensure_safe_goal_position(goal_pos, present_pos, self.config.max_relative_target)
-
- # Send goal position to the arm
- self.arm.write("Goal_Position", goal_pos.astype(np.int32))
-
- return goal_pos
-
- def print_logs(self):
- # TODO(aliberts): move robot-specific logs logic here
- pass
-
- def disconnect(self):
- if not self.is_connected:
- raise DeviceNotConnectedError(
- "ManipulatorRobot is not connected. You need to run `robot.connect()` before disconnecting."
- )
-
- self.arm.disconnect()
- for cam in self.cameras.values():
- cam.disconnect()
-
- self.is_connected = False
diff --git a/lerobot/common/robots/moss/README.md b/lerobot/common/robots/moss_follower/README.md
similarity index 97%
rename from lerobot/common/robots/moss/README.md
rename to lerobot/common/robots/moss_follower/README.md
index f716cd6c0..91419528a 100644
--- a/lerobot/common/robots/moss/README.md
+++ b/lerobot/common/robots/moss_follower/README.md
@@ -44,7 +44,7 @@ cd ~/lerobot && pip install -e ".[feetech]"
## Configure the motors
-Follow steps 1 of the [assembly video](https://www.youtube.com/watch?v=DA91NJOtMic) which illustrates the use of our scripts below.
+Follow step 1 of the [assembly video](https://www.youtube.com/watch?v=DA91NJOtMic) which illustrates the use of our scripts below.
**Find USB ports associated to your arms**
To find the correct ports for each arm, run the utility script twice:
@@ -164,7 +164,7 @@ Try to avoid rotating the motor while doing so to keep position 2048 set during
## Assemble the arms
-Follow step 4 of the [assembly video](https://www.youtube.com/watch?v=DA91NJOtMic). The first arm should take a bit more than 1 hour to assemble, but once you get use to it, you can do it under 1 hour for the second arm.
+Follow step 4 of the [assembly video](https://www.youtube.com/watch?v=DA91NJOtMic). The first arm should take a bit more than 1 hour to assemble, but once you get used to it, you can do it under 1 hour for the second arm.
## Calibrate
@@ -301,7 +301,7 @@ python lerobot/scripts/train.py \
Let's explain it:
1. We provided the dataset as argument with `--dataset.repo_id=${HF_USER}/moss_test`.
-2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor sates, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
+2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor states, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
4. We provided `policy.device=cuda` since we are training on a Nvidia GPU, but you could use `policy.device=mps` to train on Apple silicon.
5. We provided `wandb.enable=true` to use [Weights and Biases](https://docs.wandb.ai/quickstart) for visualizing training plots. This is optional but if you use it, make sure you are logged in by running `wandb login`.
diff --git a/lerobot/common/robots/moss_follower/__init__.py b/lerobot/common/robots/moss_follower/__init__.py
new file mode 100644
index 000000000..2ab82c1df
--- /dev/null
+++ b/lerobot/common/robots/moss_follower/__init__.py
@@ -0,0 +1,2 @@
+from .configuration_moss import MossRobotConfig
+from .moss_follower import MossRobot
diff --git a/lerobot/common/robots/moss/configuration_moss.py b/lerobot/common/robots/moss_follower/configuration_moss.py
similarity index 100%
rename from lerobot/common/robots/moss/configuration_moss.py
rename to lerobot/common/robots/moss_follower/configuration_moss.py
diff --git a/lerobot/common/robots/moss_follower/moss_follower.py b/lerobot/common/robots/moss_follower/moss_follower.py
new file mode 100644
index 000000000..982e2d47a
--- /dev/null
+++ b/lerobot/common/robots/moss_follower/moss_follower.py
@@ -0,0 +1,215 @@
+#!/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 typing import Any
+
+from lerobot.common.cameras.utils import make_cameras_from_configs
+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 .configuration_moss import MossRobotConfig
+
+logger = logging.getLogger(__name__)
+
+
+class MossRobot(Robot):
+ """
+ [Moss Arm](https://github.com/jess-moss/moss-robot-arms) designed by Jess Moss
+ """
+
+ config_class = MossRobotConfig
+ name = "moss_follower"
+
+ def __init__(self, config: MossRobotConfig):
+ super().__init__(config)
+ self.config = config
+ self.arm = FeetechMotorsBus(
+ port=self.config.port,
+ motors={
+ "shoulder_pan": Motor(1, "sts3215", MotorNormMode.RANGE_M100_100),
+ "shoulder_lift": Motor(2, "sts3215", MotorNormMode.RANGE_M100_100),
+ "elbow_flex": Motor(3, "sts3215", MotorNormMode.RANGE_M100_100),
+ "wrist_flex": Motor(4, "sts3215", MotorNormMode.RANGE_M100_100),
+ "wrist_roll": Motor(5, "sts3215", MotorNormMode.RANGE_M100_100),
+ "gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100),
+ },
+ calibration=self.calibration,
+ )
+ self.cameras = make_cameras_from_configs(config.cameras)
+
+ @property
+ def _motors_ft(self) -> dict[str, type]:
+ return {f"{motor}.pos": float for motor in self.arm.motors}
+
+ @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._motors_ft, **self._cameras_ft}
+
+ @cached_property
+ def action_features(self) -> dict[str, type]:
+ return self._motors_ft
+
+ @property
+ def is_connected(self) -> bool:
+ # TODO(aliberts): add cam.is_connected for cam in self.cameras
+ return self.arm.is_connected
+
+ def connect(self, calibrate: bool = True) -> None:
+ """
+ We assume that at connection time, arm is in a rest position,
+ and torque can be safely disabled to run calibration.
+ """
+ if self.is_connected:
+ raise DeviceAlreadyConnectedError(f"{self} already connected")
+
+ self.arm.connect()
+ if not self.is_calibrated and calibrate:
+ self.calibrate()
+
+ # Connect the cameras
+ for cam in self.cameras.values():
+ cam.connect()
+
+ self.configure()
+ logger.info(f"{self} connected.")
+
+ @property
+ def is_calibrated(self) -> bool:
+ return self.arm.is_calibrated
+
+ def calibrate(self) -> None:
+ logger.info(f"\nRunning calibration of {self}")
+ self.arm.disable_torque()
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+
+ input(f"Move {self} to the middle of its range of motion and press ENTER....")
+ homing_offsets = self.arm.set_half_turn_homings()
+
+ full_turn_motor = "wrist_roll"
+ unknown_range_motors = [motor for motor in self.arm.motors if motor != full_turn_motor]
+ print(
+ f"Move all joints except '{full_turn_motor}' sequentially through their "
+ "entire ranges of motion.\nRecording positions. Press ENTER to stop..."
+ )
+ range_mins, range_maxes = self.arm.record_ranges_of_motion(unknown_range_motors)
+ range_mins[full_turn_motor] = 0
+ range_maxes[full_turn_motor] = 4095
+
+ self.calibration = {}
+ for motor, m in self.arm.motors.items():
+ self.calibration[motor] = MotorCalibration(
+ id=m.id,
+ drive_mode=0,
+ homing_offset=homing_offsets[motor],
+ range_min=range_mins[motor],
+ range_max=range_maxes[motor],
+ )
+
+ self.arm.write_calibration(self.calibration)
+ self._save_calibration()
+ print("Calibration saved to", self.calibration_fpath)
+
+ def configure(self) -> None:
+ with self.arm.torque_disabled():
+ self.arm.configure_motors()
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+ # Set P_Coefficient to lower value to avoid shakiness (Default is 32)
+ self.arm.write("P_Coefficient", motor, 16)
+ # Set I_Coefficient and D_Coefficient to default value 0 and 32
+ self.arm.write("I_Coefficient", motor, 0)
+ self.arm.write("D_Coefficient", motor, 32)
+
+ def setup_motors(self) -> None:
+ for motor in reversed(self.arm.motors):
+ input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+ self.arm.setup_motor(motor)
+ print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
+
+ def get_observation(self) -> dict[str, Any]:
+ if not self.is_connected:
+ raise DeviceNotConnectedError(f"{self} is not connected.")
+
+ # Read arm position
+ start = time.perf_counter()
+ obs_dict = self.arm.sync_read("Present_Position")
+ obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()}
+ 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 arm 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:
+ the action sent to the motors, potentially clipped.
+ """
+ if not self.is_connected:
+ raise DeviceNotConnectedError(f"{self} is not connected.")
+
+ goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")}
+
+ # 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.arm.sync_read("Present_Position")
+ goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in goal_pos.items()}
+ goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target)
+
+ # Send goal position to the arm
+ self.arm.sync_write("Goal_Position", goal_pos)
+ return {f"{motor}.pos": val for motor, val in goal_pos.items()}
+
+ def disconnect(self):
+ if not self.is_connected:
+ raise DeviceNotConnectedError(f"{self} is not connected.")
+
+ self.arm.disconnect(self.config.disable_torque_on_disconnect)
+ for cam in self.cameras.values():
+ cam.disconnect()
+
+ logger.info(f"{self} disconnected.")
diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py
index a7ec4eda8..bd643c179 100644
--- a/lerobot/common/robots/robot.py
+++ b/lerobot/common/robots/robot.py
@@ -36,15 +36,11 @@ class Robot(abc.ABC):
# TODO(aliberts): create a proper Feature class for this that links with datasets
@abc.abstractproperty
- def state_feature(self) -> dict:
+ def observation_features(self) -> dict:
pass
@abc.abstractproperty
- def action_feature(self) -> dict:
- pass
-
- @abc.abstractproperty
- def camera_features(self) -> dict[str, dict]:
+ def action_features(self) -> dict:
pass
@abc.abstractproperty
@@ -52,7 +48,7 @@ class Robot(abc.ABC):
pass
@abc.abstractmethod
- def connect(self) -> None:
+ def connect(self, calibrate: bool = True) -> None:
"""Connects to the robot."""
pass
diff --git a/lerobot/common/robots/so100/README.md b/lerobot/common/robots/so100_follower/README.md
similarity index 88%
rename from lerobot/common/robots/so100/README.md
rename to lerobot/common/robots/so100_follower/README.md
index 97433e939..5b04de705 100644
--- a/lerobot/common/robots/so100/README.md
+++ b/lerobot/common/robots/so100_follower/README.md
@@ -128,7 +128,7 @@ sudo chmod 666 /dev/ttyACM1
#### d. Update config file
IMPORTANTLY: Now that you have your ports, update the **port** default values of [`SO100RobotConfig`](../lerobot/common/robot_devices/robots/configs.py). You will find something like:
-```python
+```diff
@RobotConfig.register_subclass("so100")
@dataclass
class So100RobotConfig(ManipulatorRobotConfig):
@@ -141,7 +141,8 @@ class So100RobotConfig(ManipulatorRobotConfig):
leader_arms: dict[str, MotorsBusConfig] = field(
default_factory=lambda: {
"main": FeetechMotorsBusConfig(
- port="/dev/tty.usbmodem58760431091", <-- UPDATE HERE
+- port="/dev/tty.usbmodem58760431091",
++ port="{ADD YOUR LEADER PORT}",
motors={
# name: (index, model)
"shoulder_pan": [1, "sts3215"],
@@ -158,7 +159,8 @@ class So100RobotConfig(ManipulatorRobotConfig):
follower_arms: dict[str, MotorsBusConfig] = field(
default_factory=lambda: {
"main": FeetechMotorsBusConfig(
- port="/dev/tty.usbmodem585A0076891", <-- UPDATE HERE
+- port="/dev/tty.usbmodem585A0076891",
++ port="{ADD YOUR FOLLOWER PORT}",
motors={
# name: (index, model)
"shoulder_pan": [1, "sts3215"],
@@ -445,18 +447,16 @@ For the leader configuration, perform **Steps 1–23**. Make sure that you remov
## E. Calibrate
-Next, you'll need to calibrate your SO-100 robot to ensure that the leader and follower arms have the same position values when they are in the same physical position. This calibration is essential because it allows a neural network trained on one SO-100 robot to work on another.
+Next, you'll need to calibrate your SO-100 robot to ensure that the leader and follower arms have the same position values when they are in the same physical position.
+The calibration process is very important because it allows a neural network trained on one SO-100 robot to work on another.
-#### a. Manual calibration of follower arm
+#### Manual calibration of follower arm
-> [!IMPORTANT]
-> Contrarily to step 6 of the [assembly video](https://youtu.be/FioA2oeFZ5I?t=724) which illustrates the auto calibration, we will actually do manual calibration of follower for now.
+You will need to move the follower arm to these positions sequentially, note that the rotated position is on the right side of the robot and you have to open the gripper fully.
-You will need to move the follower arm to these positions sequentially:
-
-| 1. Zero position | 2. Rotated position | 3. Rest position |
-| ------------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| | | |
+| 1. Middle position | 2. Zero position | 3. Rotated position | 4. Rest position |
+| ------------ |------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| 
| 
| 
| 
|
Make sure both arms are connected and run this script to launch manual calibration:
```bash
@@ -467,12 +467,12 @@ python lerobot/scripts/control_robot.py \
--control.arms='["main_follower"]'
```
-#### b. Manual calibration of leader arm
-Follow step 6 of the [assembly video](https://youtu.be/FioA2oeFZ5I?t=724) which illustrates the manual calibration. You will need to move the leader arm to these positions sequentially:
+#### Manual calibration of leader arm
+You will also need to move the leader arm to these positions sequentially:
-| 1. Zero position | 2. Rotated position | 3. Rest position |
-| ------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| | | |
+| 1. Middle position | 2. Zero position | 3. Rotated position | 4. Rest position |
+| ------------ |------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| 
| 
| 
| 
|
Run this script to launch manual calibration:
```bash
@@ -580,7 +580,7 @@ python lerobot/scripts/train.py \
Let's explain it:
1. We provided the dataset as argument with `--dataset.repo_id=${HF_USER}/so100_test`.
-2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor sates, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
+2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor states, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
4. We provided `policy.device=cuda` since we are training on a Nvidia GPU, but you could use `policy.device=mps` to train on Apple silicon.
5. We provided `wandb.enable=true` to use [Weights and Biases](https://docs.wandb.ai/quickstart) for visualizing training plots. This is optional but if you use it, make sure you are logged in by running `wandb login`.
diff --git a/lerobot/common/robots/so100/__init__.py b/lerobot/common/robots/so100_follower/__init__.py
similarity index 100%
rename from lerobot/common/robots/so100/__init__.py
rename to lerobot/common/robots/so100_follower/__init__.py
diff --git a/lerobot/common/robots/so100/config_so100_follower.py b/lerobot/common/robots/so100_follower/config_so100_follower.py
similarity index 100%
rename from lerobot/common/robots/so100/config_so100_follower.py
rename to lerobot/common/robots/so100_follower/config_so100_follower.py
diff --git a/lerobot/common/robots/so100/so100_follower.py b/lerobot/common/robots/so100_follower/so100_follower.py
similarity index 73%
rename from lerobot/common/robots/so100/so100_follower.py
rename to lerobot/common/robots/so100_follower/so100_follower.py
index 50361fc9e..5f999ae58 100644
--- a/lerobot/common/robots/so100/so100_follower.py
+++ b/lerobot/common/robots/so100_follower/so100_follower.py
@@ -16,10 +16,10 @@
import logging
import time
+from functools import cached_property
from typing import Any
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 (
@@ -60,34 +60,29 @@ class SO100Follower(Robot):
self.cameras = make_cameras_from_configs(config.cameras)
@property
- def state_feature(self) -> dict:
+ def _motors_ft(self) -> dict[str, type]:
+ return {f"{motor}.pos": float for motor in self.arm.motors}
+
+ @property
+ def _cameras_ft(self) -> dict[str, tuple]:
return {
- "dtype": "float32",
- "shape": (len(self.arm),),
- "names": {"motors": list(self.arm.motors)},
+ cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras
}
- @property
- def action_feature(self) -> dict:
- return self.state_feature
+ @cached_property
+ def observation_features(self) -> dict[str, type | tuple]:
+ return {**self._motors_ft, **self._cameras_ft}
- @property
- def camera_features(self) -> dict[str, dict]:
- cam_ft = {}
- for cam_key, cam in self.cameras.items():
- cam_ft[cam_key] = {
- "shape": (cam.height, cam.width, cam.channels),
- "names": ["height", "width", "channels"],
- "info": None,
- }
- return cam_ft
+ @cached_property
+ def action_features(self) -> dict[str, type]:
+ return self._motors_ft
@property
def is_connected(self) -> bool:
# TODO(aliberts): add cam.is_connected for cam in self.cameras
return self.arm.is_connected
- def connect(self) -> None:
+ def connect(self, calibrate: bool = True) -> None:
"""
We assume that at connection time, arm is in a rest position,
and torque can be safely disabled to run calibration.
@@ -96,7 +91,7 @@ class SO100Follower(Robot):
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.arm.connect()
- if not self.is_calibrated:
+ if not self.is_calibrated and calibrate:
self.calibrate()
# Connect the cameras
@@ -113,15 +108,15 @@ class SO100Follower(Robot):
def calibrate(self) -> None:
logger.info(f"\nRunning calibration of {self}")
self.arm.disable_torque()
- for name in self.arm.names:
- self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
- input("Move robot to the middle of its range of motion and press ENTER....")
+ input(f"Move {self} to the middle of its range of motion and press ENTER....")
homing_offsets = self.arm.set_half_turn_homings()
full_turn_motor = "wrist_roll"
- unknown_range_motors = [name for name in self.arm.names if name != full_turn_motor]
- logger.info(
+ unknown_range_motors = [motor for motor in self.arm.motors if motor != full_turn_motor]
+ print(
f"Move all joints except '{full_turn_motor}' sequentially through their "
"entire ranges of motion.\nRecording positions. Press ENTER to stop..."
)
@@ -130,13 +125,13 @@ class SO100Follower(Robot):
range_maxes[full_turn_motor] = 4095
self.calibration = {}
- for name, motor in self.arm.motors.items():
- self.calibration[name] = MotorCalibration(
- id=motor.id,
+ for motor, m in self.arm.motors.items():
+ self.calibration[motor] = MotorCalibration(
+ id=m.id,
drive_mode=0,
- homing_offset=homing_offsets[name],
- range_min=range_mins[name],
- range_max=range_maxes[name],
+ homing_offset=homing_offsets[motor],
+ range_min=range_mins[motor],
+ range_max=range_maxes[motor],
)
self.arm.write_calibration(self.calibration)
@@ -146,30 +141,35 @@ class SO100Follower(Robot):
def configure(self) -> None:
with self.arm.torque_disabled():
self.arm.configure_motors()
- for name in self.arm.names:
- self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
# Set P_Coefficient to lower value to avoid shakiness (Default is 32)
- self.arm.write("P_Coefficient", name, 16)
+ self.arm.write("P_Coefficient", motor, 16)
# Set I_Coefficient and D_Coefficient to default value 0 and 32
- self.arm.write("I_Coefficient", name, 0)
- self.arm.write("D_Coefficient", name, 32)
+ self.arm.write("I_Coefficient", motor, 0)
+ self.arm.write("D_Coefficient", motor, 32)
+
+ def setup_motors(self) -> None:
+ for motor in reversed(self.arm.motors):
+ input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+ self.arm.setup_motor(motor)
+ print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
def get_observation(self) -> dict[str, Any]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
- obs_dict = {}
-
# Read arm position
start = time.perf_counter()
- obs_dict[OBS_STATE] = self.arm.sync_read("Present_Position")
+ obs_dict = self.arm.sync_read("Present_Position")
+ obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()}
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()
+ 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")
@@ -191,7 +191,7 @@ class SO100Follower(Robot):
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
- goal_pos = action
+ goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")}
# Cap goal position when too far away from present position.
# /!\ Slower fps expected due to reading from the follower.
@@ -202,7 +202,7 @@ class SO100Follower(Robot):
# Send goal position to the arm
self.arm.sync_write("Goal_Position", goal_pos)
- return goal_pos
+ return {f"{motor}.pos": val for motor, val in goal_pos.items()}
def disconnect(self):
if not self.is_connected:
diff --git a/lerobot/common/robots/so101_follower/README.md b/lerobot/common/robots/so101_follower/README.md
new file mode 100644
index 000000000..60e5e6ad2
--- /dev/null
+++ b/lerobot/common/robots/so101_follower/README.md
@@ -0,0 +1,711 @@
+# Assemble and use SO-101
+
+In the steps below we explain how to assemble and use our flagship robot, the SO-101 with LeRobot 🤗.
+
+## Source the parts
+
+Follow this [README](https://github.com/TheRobotStudio/SO-ARM100). It contains the bill of materials, with a link to source the parts, as well as the instructions to 3D print the parts,
+and advice if it's your first time printing or if you don't own a 3D printer.
+
+Before assembling, you will first need to configure your motors. To this end, we provide a nice script, so let's first install LeRobot. After configuration, we will also guide you through assembly.
+
+## Install LeRobot
+
+> [!TIP]
+> We use the Command Prompt (cmd) quite a lot. If you are not comfortable using the cmd or want to brush up using the command line you can have a look here: [Command line crash course](https://developer.mozilla.org/en-US/docs/Learn_web_development/Getting_started/Environment_setup/Command_line)
+
+Download our source code:
+```bash
+git clone https://github.com/huggingface/lerobot.git
+cd lerobot
+```
+
+Create a virtual environment with Python 3.10 and activate it, e.g. with [`miniconda`](https://docs.anaconda.com/miniconda/install/#quick-command-line-install):
+```bash
+conda create -y -n lerobot python=3.10
+```
+Now restart the shell by running:
+
+##### Windows:
+```bash
+`source ~/.bashrc`
+```
+
+##### Mac:
+```bash
+`source ~/.bash_profile`
+```
+
+##### zshell:
+```bash
+`source ~/.zshrc`
+```
+
+Then activate your conda environment, you have to do this each time you open a shell to use lerobot:
+```bash
+conda activate lerobot
+```
+
+When using `miniconda`, install `ffmpeg` in your environment:
+```bash
+conda install ffmpeg -c conda-forge
+```
+
+> [!NOTE]
+> This usually installs `ffmpeg 7.X` for your platform compiled with the `libsvtav1` encoder. If `libsvtav1` is not supported (check supported encoders with `ffmpeg -encoders`), you can:
+> - _[On any platform]_ Explicitly install `ffmpeg 7.X` using:
+> ```bash
+> conda install ffmpeg=7.1.1 -c conda-forge
+> ```
+> - _[On Linux only]_ Install [ffmpeg build dependencies](https://trac.ffmpeg.org/wiki/CompilationGuide/Ubuntu#GettheDependencies) and [compile ffmpeg from source with libsvtav1](https://trac.ffmpeg.org/wiki/CompilationGuide/Ubuntu#libsvtav1), and make sure you use the corresponding ffmpeg binary to your install with `which ffmpeg`.
+
+Install 🤗 LeRobot:
+```bash
+cd lerobot && pip install ".[feetech]"
+```
+
+> [!NOTE]
+> If you encounter build errors, you may need to install additional dependencies (`cmake`, `build-essential`, and `ffmpeg libs`). On Linux, run: `sudo apt-get install cmake build-essential python3-dev pkg-config libavformat-dev libavcodec-dev libavdevice-dev libavutil-dev libswscale-dev libswresample-dev libavfilter-dev pkg-config`. For other systems, see: [Compiling PyAV](https://pyav.org/docs/develop/overview/installation.html#bring-your-own-ffmpeg)
+
+
+## Configure motors
+
+To configure the motors designate one bus servo adapter and 6 motors for your leader arm, and similarly the other bus servo adapter and 6 motors for the follower arm. It's convenient to label them and write on each motor if it's for the follower `F` or for the leader `L` and it's ID from 1 to 6.
+
+You now should plug the 5V or 12V power supply to the motor bus. 5V for the STS3215 7.4V motors and 12V for the STS3215 12V motors. Note that the leader arm always uses the 7.4V motors, so watch out that you plug in the right power supply if you have 12V and 7.4V motors, otherwise you might burn your motors! Now, connect the motor bus to your computer via USB. Note that the USB doesn't provide any power, and both the power supply and USB have to be plugged in.
+
+### Find the USB ports associated to each arm
+
+To find the port for each bus servo adapter, run this script:
+```bash
+python lerobot/scripts/find_motors_bus_port.py
+```
+#### Example outputs of script
+
+##### Mac:
+Example output leader arm's port: `/dev/tty.usbmodem575E0031751`
+
+```bash
+Finding all available ports for the MotorBus.
+['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751']
+Remove the usb cable from your MotorsBus and press Enter when done.
+
+[...Disconnect leader arm and press Enter...]
+
+The port of this MotorsBus is /dev/tty.usbmodem575E0031751
+Reconnect the usb cable.
+```
+
+Example output follower arm port: `/dev/tty.usbmodem575E0032081`
+
+```
+Finding all available ports for the MotorBus.
+['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751']
+Remove the usb cable from your MotorsBus and press Enter when done.
+
+[...Disconnect follower arm and press Enter...]
+
+The port of this MotorsBus is /dev/tty.usbmodem575E0032081
+Reconnect the usb cable.
+```
+
+##### Linux:
+On Linux, you might need to give access to the USB ports by running:
+```bash
+sudo chmod 666 /dev/ttyACM0
+sudo chmod 666 /dev/ttyACM1
+```
+
+Example output leader arm port: `/dev/ttyACM0`
+
+```bash
+Finding all available ports for the MotorBus.
+['/dev/ttyACM0', '/dev/ttyACM1']
+Remove the usb cable from your MotorsBus and press Enter when done.
+
+[...Disconnect leader arm and press Enter...]
+
+The port of this MotorsBus is /dev/ttyACM0
+Reconnect the usb cable.
+```
+
+Example output follower arm port: `/dev/ttyACM1`
+
+```
+Finding all available ports for the MotorBus.
+['/dev/ttyACM0', '/dev/ttyACM1']
+Remove the usb cable from your MotorsBus and press Enter when done.
+
+[...Disconnect follower arm and press Enter...]
+
+The port of this MotorsBus is /dev/ttyACM1
+Reconnect the usb cable.
+```
+
+#### Update config file
+
+Now that you have your ports, update the **port** default values of [`SO101RobotConfig`](https://github.com/huggingface/lerobot/blob/main/lerobot/common/robot_devices/robots/configs.py).
+You will find a class called `so101` where you can update the `port` values with your actual motor ports:
+```diff
+@RobotConfig.register_subclass("so101")
+@dataclass
+class So101RobotConfig(ManipulatorRobotConfig):
+ calibration_dir: str = ".cache/calibration/so101"
+ # `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
+
+ leader_arms: dict[str, MotorsBusConfig] = field(
+ default_factory=lambda: {
+ "main": FeetechMotorsBusConfig(
+- port="/dev/tty.usbmodem58760431091",
++ port="{ADD YOUR LEADER PORT}",
+ motors={
+ # name: (index, model)
+ "shoulder_pan": [1, "sts3215"],
+ "shoulder_lift": [2, "sts3215"],
+ "elbow_flex": [3, "sts3215"],
+ "wrist_flex": [4, "sts3215"],
+ "wrist_roll": [5, "sts3215"],
+ "gripper": [6, "sts3215"],
+ },
+ ),
+ }
+ )
+
+ follower_arms: dict[str, MotorsBusConfig] = field(
+ default_factory=lambda: {
+ "main": FeetechMotorsBusConfig(
+- port="/dev/tty.usbmodem585A0076891",
++ port="{ADD YOUR FOLLOWER PORT}",
+ motors={
+ # name: (index, model)
+ "shoulder_pan": [1, "sts3215"],
+ "shoulder_lift": [2, "sts3215"],
+ "elbow_flex": [3, "sts3215"],
+ "wrist_flex": [4, "sts3215"],
+ "wrist_roll": [5, "sts3215"],
+ "gripper": [6, "sts3215"],
+ },
+ ),
+ }
+ )
+```
+
+Here is a video of the process:
+
+
+
+### Set motor IDs
+
+Now we need to set the motor ID for each motor. Plug your motor in only one of the two ports of the motor bus and run this script to set its ID to 1. Replace the text after --port to the corresponding control board port.
+```bash
+python lerobot/scripts/configure_motor.py \
+ --port /dev/tty.usbmodem58760432961 \
+ --brand feetech \
+ --model sts3215 \
+ --baudrate 1000000 \
+ --ID 1
+```
+
+Then unplug your motor and plug the second motor and set its ID to 2.
+```bash
+python lerobot/scripts/configure_motor.py \
+ --port /dev/tty.usbmodem58760432961 \
+ --brand feetech \
+ --model sts3215 \
+ --baudrate 1000000 \
+ --ID 2
+```
+
+Redo this process for all your motors until ID 6. Do the same for the 6 motors of the leader arm, but make sure to change the power supply if you use motors with different voltage.
+
+Here is a video of the process:
+
+
+
+## Step-by-Step Assembly Instructions
+
+The follower arm uses 6x STS3215 motors with 1/345 gearing. The leader however uses three differently geared motors to make sure it can both sustain its own weight and it can be moved without requiring much force. Which motor is needed for which joint is shown in table below.
+
+| Leader-Arm Axis | Motor | Gear Ratio |
+|-----------------|:-------:|:----------:|
+| Base / Shoulder Yaw | 1 | 1 / 191 |
+| Shoulder Pitch | 2 | 1 / 345 |
+| Elbow | 3 | 1 / 191 |
+| Wrist Roll | 4 | 1 / 147 |
+| Wrist Pitch | 5 | 1 / 147 |
+| Gripper | 6 | 1 / 147 |
+
+
+### Clean Parts
+Remove all support material from the 3D-printed parts.
+
+### Joint 1
+
+- Place the first motor into the base.
+- Fasten the motor with 4 M2x6mm screws (smallest screws). Two from the top and two from bottom.
+- Slide over the first motor holder and fasten it using two M2x6mm screws (one on each side).
+- Install both motor horns, securing the top horn with a M3x6mm screw.
+- Attach the shoulder part.
+- Tighten the shoulder part with 4 M3x6mm screws on top and 4 M3x6mm screws on the bottom
+- Add the shoulder motor holder.
+
+
+
+### Joint 2
+
+- Slide the second motor in from the top.
+- Fasten the second motor with 4 M2x6mm screws.
+- Attach both motor horns to motor 2, again use the M3x6mm horn screw.
+- Attach the upper arm with 4 M3x6mm screws on each side.
+
+
+
+### Joint 3
+
+- Insert motor 3 and fasten using 4 M2x6mm screws
+- Attach both motor horns to motor 3 and secure one again with a M3x6mm horn screw.
+- Connect the forearm to motor 3 using 4 M3x6mm screws on each side.
+
+
+
+### Joint 4
+
+- Slide over motor holder 4.
+- Slide in motor 4.
+- Fasten motor 4 with 4 M2x6mm screws and attach its motor horns, use a M3x6mm horn screw.
+
+
+
+### Joint 5
+
+- Insert motor 5 into the wrist holder and secure it with 2 M2x6mm front screws.
+- Install only one motor horn on the wrist motor and secure it with a M3x6mm horn screw.
+- Secure the wrist to motor 4 using 4 M3x6mm screws on both sides.
+
+
+
+### Gripper / Handle
+
+#### Follower:
+
+- Attach the gripper to motor 5, attach it to the motor horn on the wrist using 4 M3x6mm screws.
+- Insert the gripper motor and secure it with 2 M2x6mm screws on each side.
+- Attach the motor horns and again use a M3x6mm horn screw.
+- Install the gripper claw and secure it with 4 M3x6mm screws on both sides.
+
+
+
+#### Leader:
+
+- Mount the leader holder onto the wrist and secure it with 4 M3x6mm screws.
+- Attach the handle to motor 5 using 1 M2x6mm screw.
+- Insert the gripper motor, secure it with 2 M2x6mm screws on each side, attach a motor horn using a M3x6mm horn screw.
+- Attach the follower trigger with 4 M3x6mm screws.
+
+
+
+##### Wiring
+
+- Attach the motor controller on the back.
+- Then insert all wires, use the wire guides everywhere to make sure the wires don't unplug themselves and stay in place.
+
+
+
+## Calibrate
+
+Next, you'll need to calibrate your SO-101 robot to ensure that the leader and follower arms have the same position values when they are in the same physical position.
+The calibration process is very important because it allows a neural network trained on one SO-101 robot to work on another.
+
+#### Manual calibration of follower arm
+
+You will need to move the follower arm to these positions sequentially, note that the rotated position is on the right side of the robot and you have to open the gripper fully.
+
+| 1. Middle position | 2. Zero position | 3. Rotated position | 4. Rest position |
+| ------------ |------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| | | | |
+
+Make sure both arms are connected and run this script to launch manual calibration:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --robot.cameras='{}' \
+ --control.type=calibrate \
+ --control.arms='["main_follower"]'
+```
+
+#### Manual calibration of leader arm
+You will also need to move the leader arm to these positions sequentially:
+
+| 1. Middle position | 2. Zero position | 3. Rotated position | 4. Rest position |
+| ------------ |------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| | | | |
+
+Run this script to launch manual calibration:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --robot.cameras='{}' \
+ --control.type=calibrate \
+ --control.arms='["main_leader"]'
+```
+## Control your robot
+
+Congrats 🎉, your robot is all set to learn a task on its own. Next we will explain to you how to train a neural network to autonomously control a real robot.
+
+**You'll learn to:**
+1. How to record and visualize your dataset.
+2. How to train a policy using your data and prepare it for evaluation.
+3. How to evaluate your policy and visualize the results.
+
+By following these steps, you'll be able to replicate tasks like picking up a Lego block and placing it in a bin with a high success rate, as demonstrated in [this video](https://x.com/RemiCadene/status/1814680760592572934).
+
+This tutorial is specifically made for the affordable [SO-101](https://github.com/TheRobotStudio/SO-ARM100) robot, but it contains additional information to be easily adapted to various types of robots like [Aloha bimanual robot](https://aloha-2.github.io) by changing some configurations. The SO-101 consists of a leader arm and a follower arm, each with 6 motors. It can work with one or several cameras to record the scene, which serve as visual sensors for the robot.
+
+During the data collection phase, you will control the follower arm by moving the leader arm. This process is known as "teleoperation." This technique is used to collect robot trajectories. Afterward, you'll train a neural network to imitate these trajectories and deploy the network to enable your robot to operate autonomously.
+
+If you encounter any issues at any step of the tutorial, feel free to seek help on [Discord](https://discord.com/invite/s3KuuzsPFb) or don't hesitate to iterate with us on the tutorial by creating issues or pull requests.
+
+## Teleoperate
+
+Run this simple script to teleoperate your robot (it won't connect and display the cameras):
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --robot.cameras='{}' \
+ --control.type=teleoperate
+```
+
+The teleoperate command will automatically:
+1. Identify any missing calibrations and initiate the calibration procedure.
+2. Connect the robot and start teleoperation.
+
+## Setup Cameras
+
+To connect a camera you have three options:
+1. OpenCVCamera which allows us to use any camera: usb, realsense, laptop webcam
+2. iPhone camera with MacOS
+3. Phone camera on Linux
+
+### Use OpenCVCamera
+
+The [`OpenCVCamera`](../lerobot/common/robot_devices/cameras/opencv.py) class allows you to efficiently record frames from most cameras using the [`opencv2`](https://docs.opencv.org) library. For more details on compatibility, see [Video I/O with OpenCV Overview](https://docs.opencv.org/4.x/d0/da7/videoio_overview.html).
+
+To instantiate an [`OpenCVCamera`](../lerobot/common/robot_devices/cameras/opencv.py), you need a camera index (e.g. `OpenCVCamera(camera_index=0)`). When you only have one camera like a webcam of a laptop, the camera index is usually `0` but it might differ, and the camera index might change if you reboot your computer or re-plug your camera. This behavior depends on your operating system.
+
+To find the camera indices, run the following utility script, which will save a few frames from each detected camera:
+```bash
+python lerobot/common/robot_devices/cameras/opencv.py \
+ --images-dir outputs/images_from_opencv_cameras
+```
+
+The output will look something like this if you have two cameras connected:
+```
+Mac or Windows detected. Finding available camera indices through scanning all indices from 0 to 60
+[...]
+Camera found at index 0
+Camera found at index 1
+[...]
+Connecting cameras
+OpenCVCamera(0, fps=30.0, width=1920.0, height=1080.0, color_mode=rgb)
+OpenCVCamera(1, fps=24.0, width=1920.0, height=1080.0, color_mode=rgb)
+Saving images to outputs/images_from_opencv_cameras
+Frame: 0000 Latency (ms): 39.52
+[...]
+Frame: 0046 Latency (ms): 40.07
+Images have been saved to outputs/images_from_opencv_cameras
+```
+
+Check the saved images in `outputs/images_from_opencv_cameras` to identify which camera index corresponds to which physical camera (e.g. `0` for `camera_00` or `1` for `camera_01`):
+```
+camera_00_frame_000000.png
+[...]
+camera_00_frame_000047.png
+camera_01_frame_000000.png
+[...]
+camera_01_frame_000047.png
+```
+
+Note: Some cameras may take a few seconds to warm up, and the first frame might be black or green.
+
+Now that you have the camera indexes, you should change them in the config. You can also change the fps, width or height of the camera.
+
+The camera config is defined per robot, can be found here [`RobotConfig`](https://github.com/huggingface/lerobot/blob/main/lerobot/common/robot_devices/robots/configs.py) and looks like this:
+```python
+cameras: dict[str, CameraConfig] = field(
+ default_factory=lambda: {
+ "wrist": OpenCVCameraConfig(
+ camera_index=0, <-- UPDATE HERE
+ fps=30,
+ width=640,
+ height=480,
+ ),
+ "base": OpenCVCameraConfig(
+ camera_index=1, <-- UPDATE HERE
+ fps=30,
+ width=640,
+ height=480,
+ ),
+ }
+ )
+```
+
+### Use your phone
+#### Mac:
+
+To use your iPhone as a camera on macOS, enable the Continuity Camera feature:
+- Ensure your Mac is running macOS 13 or later, and your iPhone is on iOS 16 or later.
+- Sign in both devices with the same Apple ID.
+- Connect your devices with a USB cable or turn on Wi-Fi and Bluetooth for a wireless connection.
+
+For more details, visit [Apple support](https://support.apple.com/en-gb/guide/mac-help/mchl77879b8a/mac).
+
+Your iPhone should be detected automatically when running the camera setup script in the next section.
+
+#### Linux:
+
+If you want to use your phone as a camera on Linux, follow these steps to set up a virtual camera
+
+1. *Install `v4l2loopback-dkms` and `v4l-utils`*. Those packages are required to create virtual camera devices (`v4l2loopback`) and verify their settings with the `v4l2-ctl` utility from `v4l-utils`. Install them using:
+```python
+sudo apt install v4l2loopback-dkms v4l-utils
+```
+2. *Install [DroidCam](https://droidcam.app) on your phone*. This app is available for both iOS and Android.
+3. *Install [OBS Studio](https://obsproject.com)*. This software will help you manage the camera feed. Install it using [Flatpak](https://flatpak.org):
+```python
+flatpak install flathub com.obsproject.Studio
+```
+4. *Install the DroidCam OBS plugin*. This plugin integrates DroidCam with OBS Studio. Install it with:
+```python
+flatpak install flathub com.obsproject.Studio.Plugin.DroidCam
+```
+5. *Start OBS Studio*. Launch with:
+```python
+flatpak run com.obsproject.Studio
+```
+6. *Add your phone as a source*. Follow the instructions [here](https://droidcam.app/obs/usage). Be sure to set the resolution to `640x480`.
+7. *Adjust resolution settings*. In OBS Studio, go to `File > Settings > Video`. Change the `Base(Canvas) Resolution` and the `Output(Scaled) Resolution` to `640x480` by manually typing it in.
+8. *Start virtual camera*. In OBS Studio, follow the instructions [here](https://obsproject.com/kb/virtual-camera-guide).
+9. *Verify the virtual camera setup*. Use `v4l2-ctl` to list the devices:
+```python
+v4l2-ctl --list-devices
+```
+You should see an entry like:
+```
+VirtualCam (platform:v4l2loopback-000):
+/dev/video1
+```
+10. *Check the camera resolution*. Use `v4l2-ctl` to ensure that the virtual camera output resolution is `640x480`. Change `/dev/video1` to the port of your virtual camera from the output of `v4l2-ctl --list-devices`.
+```python
+v4l2-ctl -d /dev/video1 --get-fmt-video
+```
+You should see an entry like:
+```
+>>> Format Video Capture:
+>>> Width/Height : 640/480
+>>> Pixel Format : 'YUYV' (YUYV 4:2:2)
+```
+
+Troubleshooting: If the resolution is not correct you will have to delete the Virtual Camera port and try again as it cannot be changed.
+
+If everything is set up correctly, you can proceed with the rest of the tutorial.
+
+### Add wrist camera
+If you have an additional camera you can add a wrist camera to the SO101. There are already many premade wrist camera holders that you can find in the SO101 repo: [Wrist camera's](https://github.com/TheRobotStudio/SO-ARM100#wrist-cameras)
+
+## Teleoperate with cameras
+
+We can now teleoperate again while at the same time visualizing the cameras and joint positions with `rerun`.
+
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --control.type=teleoperate \
+ --control.display_data=true
+```
+
+## Record a dataset
+
+Once you're familiar with teleoperation, you can record your first dataset with SO-101.
+
+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 2 episodes and upload your dataset to the hub execute this command:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --control.type=record \
+ --control.fps=30 \
+ --control.single_task="Grasp a lego block and put it in the bin." \
+ --control.repo_id=${HF_USER}/so101_test \
+ --control.tags='["so101","tutorial"]' \
+ --control.warmup_time_s=5 \
+ --control.episode_time_s=30 \
+ --control.reset_time_s=30 \
+ --control.num_episodes=2 \
+ --control.display_data=true \
+ --control.push_to_hub=true
+```
+
+You will see a lot of lines appearing like this one:
+```
+INFO 2024-08-10 15:02:58 ol_robot.py:219 dt:33.34 (30.0hz) dtRlead: 5.06 (197.5hz) dtWfoll: 0.25 (3963.7hz) dtRfoll: 6.22 (160.7hz) dtRlaptop: 32.57 (30.7hz) dtRphone: 33.84 (29.5hz)
+```
+It contains:
+- `2024-08-10 15:02:58` which is the date and time of the call to the print function,
+- `ol_robot.py:219` which is the end of the file name and the line number where the print function is called (`lerobot/scripts/control_robot.py` line `219`).
+- `dt:33.34 (30.0hz)` which is the "delta time" or the number of milliseconds spent between the previous call to `robot.teleop_step(record_data=True)` and the current one, associated with the frequency (33.34 ms equals 30.0 Hz) ; note that we use `--fps 30` so we expect 30.0 Hz ; when a step takes more time, the line appears in yellow.
+- `dtRlead: 5.06 (197.5hz)` which is the delta time of reading the present position of the leader arm.
+- `dtWfoll: 0.25 (3963.7hz)` which is the delta time of writing the goal position on the follower arm ; writing is asynchronous so it takes less time than reading.
+- `dtRfoll: 6.22 (160.7hz)` which is the delta time of reading the present position on the follower arm.
+- `dtRlaptop:32.57 (30.7hz) ` which is the delta time of capturing an image from the laptop camera in the thread running asynchronously.
+- `dtRphone:33.84 (29.5hz)` which is the delta time of capturing an image from the phone camera in the thread running asynchronously.
+
+#### Dataset upload
+Locally your dataset is stored in this folder: `~/.cache/huggingface/lerobot/{repo-id}` (e.g. `data/cadene/so101_test`). 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).
+
+#### Record function
+
+The `record` function provides a suite of tools for capturing and managing data during robot operation:
+1. Set the flow of data recording using command line arguments:
+ - `--control.warmup_time_s=10` defines the number of seconds before starting data collection. It allows the robot devices to warmup and synchronize (10 seconds by default).
+ - `--control.episode_time_s=60` defines the number of seconds for data recording for each episode (60 seconds by default).
+ - `--control.reset_time_s=60` defines the number of seconds for resetting the environment after each episode (60 seconds by default).
+ - `--control.num_episodes=50` defines the number of episodes to record (50 by default).
+2. Control the flow during data recording using keyboard keys:
+ - Press right arrow `->` at any time during episode recording to early stop and go to resetting. Same during resetting, to early stop and to go to the next episode recording.
+ - Press left arrow `<-` at any time during episode recording or resetting to early stop, cancel the current episode, and re-record it.
+ - Press escape `ESC` at any time during episode recording to end the session early and go straight to video encoding and dataset uploading.
+3. Checkpoints are done set during recording, so if any issue occurs, you can resume recording by re-running the same command again with `--control.resume=true`. You will need to manually delete the dataset directory if you want to start recording from scratch.
+
+#### 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.
+
+#### 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).
+
+## Visualize a dataset
+
+If you uploaded your dataset to the hub with `--control.push_to_hub=true`, you can [visualize your dataset online](https://huggingface.co/spaces/lerobot/visualize_dataset) by copy pasting your repo id given by:
+```bash
+echo ${HF_USER}/so101_test
+```
+
+If you didn't upload with `--control.push_to_hub=false`, you can visualize it locally with (via a window in the browser `http://127.0.0.1:9090` with the visualization tool):
+```bash
+python lerobot/scripts/visualize_dataset_html.py \
+ --repo-id ${HF_USER}/so101_test \
+ --local-files-only 1
+```
+
+This will launch a local web server that looks like this:
+
+
+ 
+
+
+## Replay an episode
+
+A useful feature is the `replay` function, which allows to replay on your robot any episode that you've recorded or episodes from any dataset out there. This function helps you test the repeatability of your robot's actions and assess transferability across robots of the same model.
+
+You can replay the first episode on your robot with:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --control.type=replay \
+ --control.fps=30 \
+ --control.repo_id=${HF_USER}/so101_test \
+ --control.episode=0
+```
+
+Your robot should replicate movements similar to those you recorded. For example, check out [this video](https://x.com/RemiCadene/status/1793654950905680090) where we use `replay` on a Aloha robot from [Trossen Robotics](https://www.trossenrobotics.com).
+
+## Train a policy
+
+To train a policy to control your robot, use the [`python lerobot/scripts/train.py`](../lerobot/scripts/train.py) script. A few arguments are required. Here is an example command:
+```bash
+python lerobot/scripts/train.py \
+ --dataset.repo_id=${HF_USER}/so101_test \
+ --policy.type=act \
+ --output_dir=outputs/train/act_so101_test \
+ --job_name=act_so101_test \
+ --policy.device=cuda \
+ --wandb.enable=true
+```
+
+Let's explain the command:
+1. We provided the dataset as argument with `--dataset.repo_id=${HF_USER}/so101_test`.
+2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor states, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
+4. We provided `policy.device=cuda` since we are training on a Nvidia GPU, but you could use `policy.device=mps` to train on Apple silicon.
+5. We provided `wandb.enable=true` to use [Weights and Biases](https://docs.wandb.ai/quickstart) for visualizing training plots. This is optional but if you use it, make sure you are logged in by running `wandb login`.
+
+Training should take several hours. You will find checkpoints in `outputs/train/act_so101_test/checkpoints`.
+
+To resume training from a checkpoint, below is an example command to resume from `last` checkpoint of the `act_so101_test` policy:
+```bash
+python lerobot/scripts/train.py \
+ --config_path=outputs/train/act_so101_test/checkpoints/last/pretrained_model/train_config.json \
+ --resume=true
+```
+
+#### Upload policy checkpoints
+
+Once training is done, upload the latest checkpoint with:
+```bash
+huggingface-cli upload ${HF_USER}/act_so101_test \
+ outputs/train/act_so101_test/checkpoints/last/pretrained_model
+```
+
+You can also upload intermediate checkpoints with:
+```bash
+CKPT=010000
+huggingface-cli upload ${HF_USER}/act_so101_test${CKPT} \
+ outputs/train/act_so101_test/checkpoints/${CKPT}/pretrained_model
+```
+
+## Evaluate your policy
+
+You can use the `record` function from [`lerobot/scripts/control_robot.py`](../lerobot/scripts/control_robot.py) but with a policy checkpoint as input. For instance, run this command to record 10 evaluation episodes:
+```bash
+python lerobot/scripts/control_robot.py \
+ --robot.type=so101 \
+ --control.type=record \
+ --control.fps=30 \
+ --control.single_task="Grasp a lego block and put it in the bin." \
+ --control.repo_id=${HF_USER}/eval_act_so101_test \
+ --control.tags='["tutorial"]' \
+ --control.warmup_time_s=5 \
+ --control.episode_time_s=30 \
+ --control.reset_time_s=30 \
+ --control.num_episodes=10 \
+ --control.push_to_hub=true \
+ --control.policy.path=outputs/train/act_so101_test/checkpoints/last/pretrained_model
+```
+
+As you can see, it's almost the same command as previously used to record your training dataset. Two things changed:
+1. There is an additional `--control.policy.path` argument which indicates the path to your policy checkpoint with (e.g. `outputs/train/eval_act_so101_test/checkpoints/last/pretrained_model`). You can also use the model repository if you uploaded a model checkpoint to the hub (e.g. `${HF_USER}/act_so101_test`).
+2. The name of dataset begins by `eval` to reflect that you are running inference (e.g. `${HF_USER}/eval_act_so101_test`).
diff --git a/lerobot/common/robots/so101_follower/__init__.py b/lerobot/common/robots/so101_follower/__init__.py
new file mode 100644
index 000000000..f6615b15b
--- /dev/null
+++ b/lerobot/common/robots/so101_follower/__init__.py
@@ -0,0 +1,2 @@
+from .config_so101_follower import SO101FollowerConfig
+from .so101_follower import SO101Follower
diff --git a/lerobot/common/robots/so101_follower/config_so101_follower.py b/lerobot/common/robots/so101_follower/config_so101_follower.py
new file mode 100644
index 000000000..c75f003bb
--- /dev/null
+++ b/lerobot/common/robots/so101_follower/config_so101_follower.py
@@ -0,0 +1,38 @@
+#!/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 dataclasses import dataclass, field
+
+from lerobot.common.cameras import CameraConfig
+
+from ..config import RobotConfig
+
+
+@RobotConfig.register_subclass("so101_follower")
+@dataclass
+class SO101FollowerConfig(RobotConfig):
+ # Port to connect to the arm
+ port: str
+
+ 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
+ cameras: dict[str, CameraConfig] = field(default_factory=dict)
diff --git a/lerobot/common/robots/so101_follower/so101_follower.py b/lerobot/common/robots/so101_follower/so101_follower.py
new file mode 100644
index 000000000..6d555005c
--- /dev/null
+++ b/lerobot/common/robots/so101_follower/so101_follower.py
@@ -0,0 +1,211 @@
+#!/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.
+
+import logging
+import time
+from functools import cached_property
+from typing import Any
+
+from lerobot.common.cameras.utils import make_cameras_from_configs
+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_so101_follower import SO101FollowerConfig
+
+logger = logging.getLogger(__name__)
+
+
+class SO101Follower(Robot):
+ """
+ SO-101 Follower Arm designed by TheRobotStudio and Hugging Face.
+ """
+
+ config_class = SO101FollowerConfig
+ name = "so101_follower"
+
+ def __init__(self, config: SO101FollowerConfig):
+ super().__init__(config)
+ self.config = config
+ self.arm = FeetechMotorsBus(
+ port=self.config.port,
+ motors={
+ "shoulder_pan": Motor(1, "sts3215", MotorNormMode.RANGE_M100_100),
+ "shoulder_lift": Motor(2, "sts3215", MotorNormMode.RANGE_M100_100),
+ "elbow_flex": Motor(3, "sts3215", MotorNormMode.RANGE_M100_100),
+ "wrist_flex": Motor(4, "sts3215", MotorNormMode.RANGE_M100_100),
+ "wrist_roll": Motor(5, "sts3215", MotorNormMode.RANGE_M100_100),
+ "gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100),
+ },
+ calibration=self.calibration,
+ )
+ self.cameras = make_cameras_from_configs(config.cameras)
+
+ @property
+ def _motors_ft(self) -> dict[str, type]:
+ return {f"{motor}.pos": float for motor in self.arm.motors}
+
+ @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._motors_ft, **self._cameras_ft}
+
+ @cached_property
+ def action_features(self) -> dict[str, type]:
+ return self._motors_ft
+
+ @property
+ def is_connected(self) -> bool:
+ # TODO(aliberts): add cam.is_connected for cam in self.cameras
+ return self.arm.is_connected
+
+ def connect(self, calibrate: bool = True) -> None:
+ """
+ We assume that at connection time, arm is in a rest position,
+ and torque can be safely disabled to run calibration.
+ """
+ if self.is_connected:
+ raise DeviceAlreadyConnectedError(f"{self} already connected")
+
+ self.arm.connect()
+ if not self.is_calibrated and calibrate:
+ self.calibrate()
+
+ # Connect the cameras
+ for cam in self.cameras.values():
+ cam.connect()
+
+ self.configure()
+ logger.info(f"{self} connected.")
+
+ @property
+ def is_calibrated(self) -> bool:
+ return self.arm.is_calibrated
+
+ def calibrate(self) -> None:
+ logger.info(f"\nRunning calibration of {self}")
+ self.arm.disable_torque()
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+
+ input(f"Move {self} to the middle of its range of motion and press ENTER....")
+ homing_offsets = self.arm.set_half_turn_homings()
+
+ print(
+ "Move all joints sequentially through their entire ranges "
+ "of motion.\nRecording positions. Press ENTER to stop..."
+ )
+ range_mins, range_maxes = self.arm.record_ranges_of_motion()
+
+ self.calibration = {}
+ for motor, m in self.arm.motors.items():
+ self.calibration[motor] = MotorCalibration(
+ id=m.id,
+ drive_mode=0,
+ homing_offset=homing_offsets[motor],
+ range_min=range_mins[motor],
+ range_max=range_maxes[motor],
+ )
+
+ self.arm.write_calibration(self.calibration)
+ self._save_calibration()
+ print("Calibration saved to", self.calibration_fpath)
+
+ def configure(self) -> None:
+ with self.arm.torque_disabled():
+ self.arm.configure_motors()
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+ # Set P_Coefficient to lower value to avoid shakiness (Default is 32)
+ self.arm.write("P_Coefficient", motor, 16)
+ # Set I_Coefficient and D_Coefficient to default value 0 and 32
+ self.arm.write("I_Coefficient", motor, 0)
+ self.arm.write("D_Coefficient", motor, 32)
+
+ def setup_motors(self) -> None:
+ for motor in reversed(self.arm.motors):
+ input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+ self.arm.setup_motor(motor)
+ print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
+
+ def get_observation(self) -> dict[str, Any]:
+ if not self.is_connected:
+ raise DeviceNotConnectedError(f"{self} is not connected.")
+
+ # Read arm position
+ start = time.perf_counter()
+ obs_dict = self.arm.sync_read("Present_Position")
+ obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()}
+ 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 arm 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:
+ the action sent to the motors, potentially clipped.
+ """
+ if not self.is_connected:
+ raise DeviceNotConnectedError(f"{self} is not connected.")
+
+ goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")}
+
+ # 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.arm.sync_read("Present_Position")
+ goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in goal_pos.items()}
+ goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target)
+
+ # Send goal position to the arm
+ self.arm.sync_write("Goal_Position", goal_pos)
+ return {f"{motor}.pos": val for motor, val in goal_pos.items()}
+
+ def disconnect(self):
+ if not self.is_connected:
+ raise DeviceNotConnectedError(f"{self} is not connected.")
+
+ self.arm.disconnect(self.config.disable_torque_on_disconnect)
+ for cam in self.cameras.values():
+ cam.disconnect()
+
+ logger.info(f"{self} disconnected.")
diff --git a/lerobot/common/robots/stretch3/README.md b/lerobot/common/robots/stretch3/README.md
index a7a7dde17..982e72571 100644
--- a/lerobot/common/robots/stretch3/README.md
+++ b/lerobot/common/robots/stretch3/README.md
@@ -99,7 +99,7 @@ This is equivalent to running `stretch_robot_home.py`
> **Note:** If you run any of the LeRobot scripts below and Stretch is not properly homed, it will automatically home/calibrate first.
**Teleoperate**
-Before trying teleoperation, you need activate the gamepad controller by pressing the middle button. For more info, see Stretch's [doc](https://docs.hello-robot.com/0.3/getting_started/hello_robot/#gamepad-teleoperation).
+Before trying teleoperation, you need to activate the gamepad controller by pressing the middle button. For more info, see Stretch's [doc](https://docs.hello-robot.com/0.3/getting_started/hello_robot/#gamepad-teleoperation).
Now try out teleoperation (see above documentation to learn about the gamepad controls):
diff --git a/lerobot/common/robots/stretch3/__init__.py b/lerobot/common/robots/stretch3/__init__.py
new file mode 100644
index 000000000..e2a859cde
--- /dev/null
+++ b/lerobot/common/robots/stretch3/__init__.py
@@ -0,0 +1,2 @@
+from .configuration_stretch3 import Stretch3RobotConfig
+from .robot_stretch3 import Stretch3Robot
diff --git a/lerobot/common/robots/stretch3/robot_stretch3.py b/lerobot/common/robots/stretch3/robot_stretch3.py
index e07e3f1e0..8a7ce9d57 100644
--- a/lerobot/common/robots/stretch3/robot_stretch3.py
+++ b/lerobot/common/robots/stretch3/robot_stretch3.py
@@ -72,7 +72,7 @@ class Stretch3Robot(Robot):
self.action_keys = None
@property
- def state_feature(self) -> dict:
+ def observation_features(self) -> dict:
return {
"dtype": "float32",
"shape": (len(STRETCH_MOTORS),),
@@ -80,8 +80,8 @@ class Stretch3Robot(Robot):
}
@property
- def action_feature(self) -> dict:
- return self.state_feature
+ def action_features(self) -> dict:
+ return self.observation_features
@property
def camera_features(self) -> dict[str, dict]:
diff --git a/lerobot/common/robots/utils.py b/lerobot/common/robots/utils.py
index 19b7c1b92..1d802776b 100644
--- a/lerobot/common/robots/utils.py
+++ b/lerobot/common/robots/utils.py
@@ -1,47 +1,27 @@
import logging
from pprint import pformat
-from typing import Protocol
from lerobot.common.robots import RobotConfig
-
-def get_arm_id(name, arm_type):
- """Returns the string identifier of a robot arm. For instance, for a bimanual manipulator
- like Aloha, it could be left_follower, right_follower, left_leader, or right_leader.
- """
- return f"{name}_{arm_type}"
-
-
-# TODO(aliberts): Remove and point to lerobot.common.robots.Robot
-class Robot(Protocol):
- robot_type: str
- features: dict
-
- def connect(self): ...
- def run_calibration(self): ...
- def teleop_step(self, record_data=False): ...
- def capture_observation(self): ...
- def send_action(self, action): ...
- def disconnect(self): ...
+from .robot import Robot
def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:
if robot_type == "aloha":
- from .aloha.configuration_aloha import AlohaRobotConfig
+ raise NotImplementedError # TODO
- return AlohaRobotConfig(**kwargs)
elif robot_type == "koch_follower":
- from .koch.config_koch_follower import KochFollowerConfig
+ from .koch_follower.config_koch_follower import KochFollowerConfig
return KochFollowerConfig(**kwargs)
# elif robot_type == "koch_bimanual":
# return KochBimanualRobotConfig(**kwargs)
- elif robot_type == "moss":
- from .moss.configuration_moss import MossRobotConfig
+ elif robot_type == "moss_follower":
+ from .moss_follower.configuration_moss import MossRobotConfig
return MossRobotConfig(**kwargs)
- elif robot_type == "so100_leader":
- from .so100.config_so100_follower import SO100FollowerConfig
+ elif robot_type == "so100_follower":
+ from .so100_follower.config_so100_follower import SO100FollowerConfig
return SO100FollowerConfig(**kwargs)
elif robot_type == "stretch":
@@ -49,34 +29,44 @@ def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:
return Stretch3RobotConfig(**kwargs)
elif robot_type == "lekiwi":
- from .lekiwi.configuration_lekiwi import LeKiwiRobotConfig
+ from .lekiwi.config_lekiwi import LeKiwiConfig
- return LeKiwiRobotConfig(**kwargs)
+ return LeKiwiConfig(**kwargs)
else:
raise ValueError(f"Robot type '{robot_type}' is not available.")
-def make_robot_from_config(config: RobotConfig):
- from .lekiwi.configuration_lekiwi import LeKiwiRobotConfig
- from .manipulator import ManipulatorRobotConfig
+def make_robot_from_config(config: RobotConfig) -> Robot:
+ if config.type == "koch_follower":
+ from .koch_follower import KochFollower
- if isinstance(config, ManipulatorRobotConfig):
- from lerobot.common.robots.manipulator import ManipulatorRobot
+ return KochFollower(config)
+ elif config.type == "so100_follower":
+ from .so100_follower import SO100Follower
- return ManipulatorRobot(config)
- elif isinstance(config, LeKiwiRobotConfig):
- from lerobot.common.robots.mobile_manipulator import MobileManipulator
+ return SO100Follower(config)
+ elif config.type == "so101_follower":
+ from .so101_follower import SO101Follower
- return MobileManipulator(config)
- else:
- from lerobot.common.robots.stretch3.robot_stretch3 import Stretch3Robot
+ return SO101Follower(config)
+ elif config.type == "lekiwi":
+ from .lekiwi import LeKiwiClient
+
+ return LeKiwiClient(config)
+ elif config.type == "stretch3":
+ from .stretch3 import Stretch3Robot
return Stretch3Robot(config)
+ elif config.type == "viperx":
+ from .viperx import ViperX
+ return ViperX(config)
+ elif config.type == "mock_robot":
+ from tests.mocks.mock_robot import MockRobot
-def make_robot(robot_type: str, **kwargs) -> Robot:
- config = make_robot_config(robot_type, **kwargs)
- return make_robot_from_config(config)
+ return MockRobot(config)
+ else:
+ raise ValueError(config.type)
def ensure_safe_goal_position(
@@ -115,3 +105,11 @@ def ensure_safe_goal_position(
)
return safe_goal_positions
+
+
+# TODO(aliberts): Remove
+def get_arm_id(name, arm_type):
+ """Returns the string identifier of a robot arm. For instance, for a bimanual manipulator
+ like Aloha, it could be left_follower, right_follower, left_leader, or right_leader.
+ """
+ return f"{name}_{arm_type}"
diff --git a/lerobot/common/robots/viperx/README.md b/lerobot/common/robots/viperx/README.md
index 77cff1611..be2a323b6 100644
--- a/lerobot/common/robots/viperx/README.md
+++ b/lerobot/common/robots/viperx/README.md
@@ -142,7 +142,7 @@ python lerobot/scripts/train.py \
Let's explain it:
1. We provided the dataset as argument with `--dataset.repo_id=${HF_USER}/aloha_test`.
-2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor sates, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
+2. We provided the policy with `policy.type=act`. This loads configurations from [`configuration_act.py`](../lerobot/common/policies/act/configuration_act.py). Importantly, this policy will automatically adapt to the number of motor states, motor actions and cameras of your robot (e.g. `laptop` and `phone`) which have been saved in your dataset.
4. We provided `policy.device=cuda` since we are training on a Nvidia GPU, but you could use `policy.device=mps` to train on Apple silicon.
5. We provided `wandb.enable=true` to use [Weights and Biases](https://docs.wandb.ai/quickstart) for visualizing training plots. This is optional but if you use it, make sure you are logged in by running `wandb login`.
diff --git a/lerobot/common/robots/viperx/viperx.py b/lerobot/common/robots/viperx/viperx.py
index 76287b2d2..3a497113a 100644
--- a/lerobot/common/robots/viperx/viperx.py
+++ b/lerobot/common/robots/viperx/viperx.py
@@ -6,10 +6,11 @@ and send orders to its motors.
import logging
import time
+from functools import cached_property
from typing import Any
from lerobot.common.cameras.utils import make_cameras_from_configs
-from lerobot.common.constants import OBS_IMAGES, OBS_STATE
+from lerobot.common.constants import OBS_STATE
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode
from lerobot.common.motors.dynamixel import (
@@ -55,35 +56,29 @@ class ViperX(Robot):
self.cameras = make_cameras_from_configs(config.cameras)
@property
- def state_feature(self) -> dict:
+ def _motors_ft(self) -> dict[str, type]:
+ return {f"{motor}.pos": float for motor in self.arm.motors}
+
+ @property
+ def _cameras_ft(self) -> dict[str, tuple]:
return {
- "dtype": "float32",
- "shape": (len(self.arm),),
- "names": {"motors": list(self.arm.motors)},
+ cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras
}
- @property
- def action_feature(self) -> dict:
- return self.state_feature
+ @cached_property
+ def observation_features(self) -> dict[str, type | tuple]:
+ return {**self._motors_ft, **self._cameras_ft}
- @property
- def camera_features(self) -> dict[str, dict]:
- cam_ft = {}
- for cam_key, cam in self.cameras.items():
- key = f"observation.images.{cam_key}"
- cam_ft[key] = {
- "shape": (cam.height, cam.width, cam.channels),
- "names": ["height", "width", "channels"],
- "info": None,
- }
- return cam_ft
+ @cached_property
+ def action_features(self) -> dict[str, type]:
+ return self._motors_ft
@property
def is_connected(self) -> bool:
# TODO(aliberts): add cam.is_connected for cam in self.cameras
return self.arm.is_connected
- def connect(self) -> None:
+ def connect(self, calibrate: bool = True) -> None:
"""
We assume that at connection time, arm is in a rest position,
and torque can be safely disabled to run calibration.
@@ -92,7 +87,7 @@ class ViperX(Robot):
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.arm.connect()
- if not self.is_calibrated:
+ if not self.is_calibrated and calibrate:
self.calibrate()
for cam in self.cameras.values():
@@ -109,31 +104,31 @@ class ViperX(Robot):
raise NotImplementedError # TODO(aliberts): adapt code below (copied from koch
logger.info(f"\nRunning calibration of {self}")
self.arm.disable_torque()
- for name in self.arm.names:
- self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
input("Move robot to the middle of its range of motion and press ENTER....")
homing_offsets = self.arm.set_half_turn_homings()
full_turn_motors = ["shoulder_pan", "wrist_roll"]
- unknown_range_motors = [name for name in self.arm.names if name not in full_turn_motors]
- logger.info(
+ unknown_range_motors = [motor for motor in self.arm.motors if motor not in full_turn_motors]
+ print(
f"Move all joints except {full_turn_motors} sequentially through their entire "
"ranges of motion.\nRecording positions. Press ENTER to stop..."
)
range_mins, range_maxes = self.arm.record_ranges_of_motion(unknown_range_motors)
- for name in full_turn_motors:
- range_mins[name] = 0
- range_maxes[name] = 4095
+ for motor in full_turn_motors:
+ range_mins[motor] = 0
+ range_maxes[motor] = 4095
self.calibration = {}
- for name, motor in self.arm.motors.items():
- self.calibration[name] = MotorCalibration(
- id=motor.id,
+ for motor, m in self.arm.motors.items():
+ self.calibration[motor] = MotorCalibration(
+ id=m.id,
drive_mode=0,
- homing_offset=homing_offsets[name],
- range_min=range_mins[name],
- range_max=range_maxes[name],
+ homing_offset=homing_offsets[motor],
+ range_min=range_mins[motor],
+ range_max=range_maxes[motor],
)
self.arm.write_calibration(self.calibration)
@@ -158,9 +153,9 @@ class ViperX(Robot):
# can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling
# the arm, you could end up with a servo with a position 0 or 4095 at a crucial point.
# See: https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11
- for name in self.arm.names:
- if name != "gripper":
- self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
+ for motor in self.arm.motors:
+ if motor != "gripper":
+ self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
# Use 'position control current based' for follower gripper to be limited by the limit of the
# current. It can grasp an object without forcing too much even tho, it's goal position is a
@@ -177,13 +172,14 @@ class ViperX(Robot):
# Read arm position
start = time.perf_counter()
obs_dict[OBS_STATE] = self.arm.sync_read("Present_Position")
+ obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()}
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()
+ 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")
@@ -205,7 +201,7 @@ class ViperX(Robot):
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
- goal_pos = action
+ goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")}
# Cap goal position when too far away from present position.
# /!\ Slower fps expected due to reading from the follower.
@@ -216,7 +212,7 @@ class ViperX(Robot):
# Send goal position to the arm
self.arm.sync_write("Goal_Position", goal_pos)
- return goal_pos
+ return {f"{motor}.pos": val for motor, val in goal_pos.items()}
def disconnect(self):
if not self.is_connected:
diff --git a/lerobot/common/teleoperators/__init__.py b/lerobot/common/teleoperators/__init__.py
index 9dd9b9626..ec93547f7 100644
--- a/lerobot/common/teleoperators/__init__.py
+++ b/lerobot/common/teleoperators/__init__.py
@@ -1,4 +1,3 @@
from .config import TeleoperatorConfig
from .teleoperator import Teleoperator
-
-__all__ = ["TeleoperatorConfig", "Teleoperator"]
+from .utils import make_teleoperator_from_config
diff --git a/lerobot/common/teleoperators/keyboard/configuration_keyboard.py b/lerobot/common/teleoperators/keyboard/configuration_keyboard.py
index 91b596bf7..ce6c9206e 100644
--- a/lerobot/common/teleoperators/keyboard/configuration_keyboard.py
+++ b/lerobot/common/teleoperators/keyboard/configuration_keyboard.py
@@ -22,4 +22,5 @@ from ..config import TeleoperatorConfig
@TeleoperatorConfig.register_subclass("keyboard")
@dataclass
class KeyboardTeleopConfig(TeleoperatorConfig):
+ # TODO(Steven): Consider setting in here the keys that we want to capture/listen
mock: bool = False
diff --git a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py
index 65f1dff71..a72710e9d 100644
--- a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py
+++ b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py
@@ -19,8 +19,7 @@ import os
import sys
import time
from queue import Queue
-
-import numpy as np
+from typing import Any
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
@@ -59,11 +58,10 @@ class KeyboardTeleop(Teleoperator):
self.event_queue = Queue()
self.current_pressed = {}
self.listener = None
- self.is_connected = False
self.logs = {}
@property
- def action_feature(self) -> dict:
+ def action_features(self) -> dict:
return {
"dtype": "float32",
"shape": (len(self.arm),),
@@ -71,36 +69,42 @@ class KeyboardTeleop(Teleoperator):
}
@property
- def feedback_feature(self) -> dict:
+ def feedback_features(self) -> dict:
return {}
+ @property
+ def is_connected(self) -> bool:
+ return PYNPUT_AVAILABLE and isinstance(self.listener, keyboard.Listener) and self.listener.is_alive()
+
+ @property
+ def is_calibrated(self) -> bool:
+ pass
+
def connect(self) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(
- "ManipulatorRobot is already connected. Do not run `robot.connect()` twice."
+ "Keyboard is already connected. Do not run `robot.connect()` twice."
)
if PYNPUT_AVAILABLE:
logging.info("pynput is available - enabling local keyboard listener.")
self.listener = keyboard.Listener(
- on_press=self.on_press,
- on_release=self.on_release,
+ on_press=self._on_press,
+ on_release=self._on_release,
)
self.listener.start()
else:
logging.info("pynput not available - skipping local keyboard listener.")
self.listener = None
- self.is_connected = True
-
def calibrate(self) -> None:
pass
- def on_press(self, key):
+ def _on_press(self, key):
if hasattr(key, "char"):
self.event_queue.put((key.char, True))
- def on_release(self, key):
+ def _on_release(self, key):
if hasattr(key, "char"):
self.event_queue.put((key.char, False))
if key == keyboard.Key.esc:
@@ -112,7 +116,10 @@ class KeyboardTeleop(Teleoperator):
key_char, is_pressed = self.event_queue.get_nowait()
self.current_pressed[key_char] = is_pressed
- def get_action(self) -> np.ndarray:
+ def configure(self):
+ pass
+
+ def get_action(self) -> dict[str, Any]:
before_read_t = time.perf_counter()
if not self.is_connected:
@@ -126,9 +133,9 @@ class KeyboardTeleop(Teleoperator):
action = {key for key, val in self.current_pressed.items() if val}
self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
- return np.array(list(action))
+ return dict.fromkeys(action, None)
- def send_feedback(self, feedback: np.ndarray) -> None:
+ def send_feedback(self, feedback: dict[str, Any]) -> None:
pass
def disconnect(self) -> None:
@@ -138,5 +145,3 @@ class KeyboardTeleop(Teleoperator):
)
if self.listener is not None:
self.listener.stop()
-
- self.is_connected = False
diff --git a/lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py b/lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py
deleted file mode 100755
index 4f463814e..000000000
--- a/lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py
+++ /dev/null
@@ -1,28 +0,0 @@
-import logging
-import time
-
-from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig
-
-
-def main():
- logging.info("Configuring Keyboard Teleop")
- keyboard_config = KeyboardTeleopConfig()
- keyboard = KeyboardTeleop(keyboard_config)
-
- logging.info("Connecting Keyboard Teleop")
- keyboard.connect()
-
- logging.info("Starting Keyboard capture")
- i = 0
- while i < 20:
- action = keyboard.get_action()
- print("Captured keys: %s", action)
- time.sleep(1)
- i += 1
-
- keyboard.disconnect()
- logging.info("Finished LeKiwiRobot cleanly")
-
-
-if __name__ == "__main__":
- main()
diff --git a/lerobot/common/teleoperators/koch/__init__.py b/lerobot/common/teleoperators/koch_leader/__init__.py
similarity index 100%
rename from lerobot/common/teleoperators/koch/__init__.py
rename to lerobot/common/teleoperators/koch_leader/__init__.py
diff --git a/lerobot/common/teleoperators/koch/config_koch_leader.py b/lerobot/common/teleoperators/koch_leader/config_koch_leader.py
similarity index 100%
rename from lerobot/common/teleoperators/koch/config_koch_leader.py
rename to lerobot/common/teleoperators/koch_leader/config_koch_leader.py
diff --git a/lerobot/common/teleoperators/koch/koch_leader.py b/lerobot/common/teleoperators/koch_leader/koch_leader.py
similarity index 76%
rename from lerobot/common/teleoperators/koch/koch_leader.py
rename to lerobot/common/teleoperators/koch_leader/koch_leader.py
index 85cad16c3..8f5ac4576 100644
--- a/lerobot/common/teleoperators/koch/koch_leader.py
+++ b/lerobot/common/teleoperators/koch_leader/koch_leader.py
@@ -58,27 +58,23 @@ class KochLeader(Teleoperator):
)
@property
- def action_feature(self) -> dict:
- return {
- "dtype": "float32",
- "shape": (len(self.arm),),
- "names": {"motors": list(self.arm.motors)},
- }
+ def action_features(self) -> dict[str, type]:
+ return {f"{motor}.pos": float for motor in self.arm.motors}
@property
- def feedback_feature(self) -> dict:
+ def feedback_features(self) -> dict[str, type]:
return {}
@property
def is_connected(self) -> bool:
return self.arm.is_connected
- def connect(self) -> None:
+ def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.arm.connect()
- if not self.is_calibrated:
+ if not self.is_calibrated and calibrate:
self.calibrate()
self.configure()
@@ -91,34 +87,34 @@ class KochLeader(Teleoperator):
def calibrate(self) -> None:
logger.info(f"\nRunning calibration of {self}")
self.arm.disable_torque()
- for name in self.arm.names:
- self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
self.arm.write("Drive_Mode", "elbow_flex", DriveMode.INVERTED.value)
- drive_modes = {name: 1 if name == "elbow_flex" else 0 for name in self.arm.names}
+ drive_modes = {motor: 1 if motor == "elbow_flex" else 0 for motor in self.arm.motors}
- input("Move robot to the middle of its range of motion and press ENTER....")
+ input(f"Move {self} to the middle of its range of motion and press ENTER....")
homing_offsets = self.arm.set_half_turn_homings()
full_turn_motors = ["shoulder_pan", "wrist_roll"]
- unknown_range_motors = [name for name in self.arm.names if name not in full_turn_motors]
- logger.info(
+ unknown_range_motors = [motor for motor in self.arm.motors if motor not in full_turn_motors]
+ print(
f"Move all joints except {full_turn_motors} sequentially through their "
"entire ranges of motion.\nRecording positions. Press ENTER to stop..."
)
range_mins, range_maxes = self.arm.record_ranges_of_motion(unknown_range_motors)
- for name in full_turn_motors:
- range_mins[name] = 0
- range_maxes[name] = 4095
+ for motor in full_turn_motors:
+ range_mins[motor] = 0
+ range_maxes[motor] = 4095
self.calibration = {}
- for name, motor in self.arm.motors.items():
- self.calibration[name] = MotorCalibration(
- id=motor.id,
- drive_mode=drive_modes[name],
- homing_offset=homing_offsets[name],
- range_min=range_mins[name],
- range_max=range_maxes[name],
+ for motor, m in self.arm.motors.items():
+ self.calibration[motor] = MotorCalibration(
+ id=m.id,
+ drive_mode=drive_modes[motor],
+ homing_offset=homing_offsets[motor],
+ range_min=range_mins[motor],
+ range_max=range_maxes[motor],
)
self.arm.write_calibration(self.calibration)
@@ -128,13 +124,13 @@ class KochLeader(Teleoperator):
def configure(self) -> None:
self.arm.disable_torque()
self.arm.configure_motors()
- for name in self.arm.names:
- if name != "gripper":
+ for motor in self.arm.motors:
+ if motor != "gripper":
# Use 'extended position mode' for all motors except gripper, because in joint mode the servos
# can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while
# assembling the arm, you could end up with a servo with a position 0 or 4095 at a crucial
# point
- self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
+ self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
# Use 'position control current based' for gripper to be limited by the limit of the current.
# For the follower gripper, it means it can grasp an object without forcing too much even tho,
@@ -146,12 +142,19 @@ class KochLeader(Teleoperator):
self.arm.enable_torque("gripper")
self.arm.write("Goal_Position", "gripper", self.config.gripper_open_pos)
+ def setup_motors(self) -> None:
+ for motor in reversed(self.arm.motors):
+ input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+ self.arm.setup_motor(motor)
+ print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
+
def get_action(self) -> dict[str, float]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
start = time.perf_counter()
action = self.arm.sync_read("Present_Position")
+ action = {f"{motor}.pos": val for motor, val in action.items()}
dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read action: {dt_ms:.1f}ms")
return action
diff --git a/lerobot/common/teleoperators/so100/__init__.py b/lerobot/common/teleoperators/so100_leader/__init__.py
similarity index 100%
rename from lerobot/common/teleoperators/so100/__init__.py
rename to lerobot/common/teleoperators/so100_leader/__init__.py
diff --git a/lerobot/common/teleoperators/so100/config_so100_leader.py b/lerobot/common/teleoperators/so100_leader/config_so100_leader.py
similarity index 100%
rename from lerobot/common/teleoperators/so100/config_so100_leader.py
rename to lerobot/common/teleoperators/so100_leader/config_so100_leader.py
diff --git a/lerobot/common/teleoperators/so100/so100_leader.py b/lerobot/common/teleoperators/so100_leader/so100_leader.py
similarity index 74%
rename from lerobot/common/teleoperators/so100/so100_leader.py
rename to lerobot/common/teleoperators/so100_leader/so100_leader.py
index 0ed5eafc8..a063edd1f 100644
--- a/lerobot/common/teleoperators/so100/so100_leader.py
+++ b/lerobot/common/teleoperators/so100_leader/so100_leader.py
@@ -55,27 +55,23 @@ class SO100Leader(Teleoperator):
)
@property
- def action_feature(self) -> dict:
- return {
- "dtype": "float32",
- "shape": (len(self.arm),),
- "names": {"motors": list(self.arm.motors)},
- }
+ def action_features(self) -> dict[str, type]:
+ return {f"{motor}.pos": float for motor in self.arm.motors}
@property
- def feedback_feature(self) -> dict:
+ def feedback_features(self) -> dict[str, type]:
return {}
@property
def is_connected(self) -> bool:
return self.arm.is_connected
- def connect(self) -> None:
+ def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.arm.connect()
- if not self.is_calibrated:
+ if not self.is_calibrated and calibrate:
self.calibrate()
self.configure()
@@ -88,15 +84,15 @@ class SO100Leader(Teleoperator):
def calibrate(self) -> None:
logger.info(f"\nRunning calibration of {self}")
self.arm.disable_torque()
- for name in self.arm.names:
- self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
- input("Move robot to the middle of its range of motion and press ENTER....")
+ input(f"Move {self} to the middle of its range of motion and press ENTER....")
homing_offsets = self.arm.set_half_turn_homings()
full_turn_motor = "wrist_roll"
- unknown_range_motors = [name for name in self.arm.names if name != full_turn_motor]
- logger.info(
+ unknown_range_motors = [motor for motor in self.arm.motors if motor != full_turn_motor]
+ print(
f"Move all joints except '{full_turn_motor}' sequentially through their "
"entire ranges of motion.\nRecording positions. Press ENTER to stop..."
)
@@ -105,13 +101,13 @@ class SO100Leader(Teleoperator):
range_maxes[full_turn_motor] = 4095
self.calibration = {}
- for name, motor in self.arm.motors.items():
- self.calibration[name] = MotorCalibration(
- id=motor.id,
+ for motor, m in self.arm.motors.items():
+ self.calibration[motor] = MotorCalibration(
+ id=m.id,
drive_mode=0,
- homing_offset=homing_offsets[name],
- range_min=range_mins[name],
- range_max=range_maxes[name],
+ homing_offset=homing_offsets[motor],
+ range_min=range_mins[motor],
+ range_max=range_maxes[motor],
)
self.arm.write_calibration(self.calibration)
@@ -121,12 +117,19 @@ class SO100Leader(Teleoperator):
def configure(self) -> None:
self.arm.disable_torque()
self.arm.configure_motors()
- for name in self.arm.names:
- self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+
+ def setup_motors(self) -> None:
+ for motor in reversed(self.arm.motors):
+ input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+ self.arm.setup_motor(motor)
+ print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
def get_action(self) -> dict[str, float]:
start = time.perf_counter()
action = self.arm.sync_read("Present_Position")
+ action = {f"{motor}.pos": val for motor, val in action.items()}
dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read action: {dt_ms:.1f}ms")
return action
diff --git a/lerobot/common/teleoperators/so101_leader/__init__.py b/lerobot/common/teleoperators/so101_leader/__init__.py
new file mode 100644
index 000000000..1f45170e9
--- /dev/null
+++ b/lerobot/common/teleoperators/so101_leader/__init__.py
@@ -0,0 +1,2 @@
+from .config_so101_leader import SO101LeaderConfig
+from .so101_leader import SO101Leader
diff --git a/lerobot/common/teleoperators/so101_leader/config_so101_leader.py b/lerobot/common/teleoperators/so101_leader/config_so101_leader.py
new file mode 100644
index 000000000..5f2e110da
--- /dev/null
+++ b/lerobot/common/teleoperators/so101_leader/config_so101_leader.py
@@ -0,0 +1,26 @@
+#!/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 dataclasses import dataclass
+
+from ..config import TeleoperatorConfig
+
+
+@TeleoperatorConfig.register_subclass("so101_leader")
+@dataclass
+class SO101LeaderConfig(TeleoperatorConfig):
+ # Port to connect to the arm
+ port: str
diff --git a/lerobot/common/teleoperators/so101_leader/so101_leader.py b/lerobot/common/teleoperators/so101_leader/so101_leader.py
new file mode 100644
index 000000000..00c82304a
--- /dev/null
+++ b/lerobot/common/teleoperators/so101_leader/so101_leader.py
@@ -0,0 +1,142 @@
+#!/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.
+
+import logging
+import time
+
+from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
+from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode
+from lerobot.common.motors.feetech import (
+ FeetechMotorsBus,
+ OperatingMode,
+)
+
+from ..teleoperator import Teleoperator
+from .config_so101_leader import SO101LeaderConfig
+
+logger = logging.getLogger(__name__)
+
+
+class SO101Leader(Teleoperator):
+ """
+ SO-101 Leader Arm designed by TheRobotStudio and Hugging Face.
+ """
+
+ config_class = SO101LeaderConfig
+ name = "so101_leader"
+
+ def __init__(self, config: SO101LeaderConfig):
+ super().__init__(config)
+ self.config = config
+ self.arm = FeetechMotorsBus(
+ port=self.config.port,
+ motors={
+ "shoulder_pan": Motor(1, "sts3215", MotorNormMode.RANGE_M100_100),
+ "shoulder_lift": Motor(2, "sts3215", MotorNormMode.RANGE_M100_100),
+ "elbow_flex": Motor(3, "sts3215", MotorNormMode.RANGE_M100_100),
+ "wrist_flex": Motor(4, "sts3215", MotorNormMode.RANGE_M100_100),
+ "wrist_roll": Motor(5, "sts3215", MotorNormMode.RANGE_M100_100),
+ "gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100),
+ },
+ calibration=self.calibration,
+ )
+
+ @property
+ def action_features(self) -> dict[str, type]:
+ return {f"{motor}.pos": float for motor in self.arm.motors}
+
+ @property
+ def feedback_features(self) -> dict[str, type]:
+ return {}
+
+ @property
+ def is_connected(self) -> bool:
+ return self.arm.is_connected
+
+ def connect(self, calibrate: bool = True) -> None:
+ if self.is_connected:
+ raise DeviceAlreadyConnectedError(f"{self} already connected")
+
+ self.arm.connect()
+ if not self.is_calibrated and calibrate:
+ self.calibrate()
+
+ self.configure()
+ logger.info(f"{self} connected.")
+
+ @property
+ def is_calibrated(self) -> bool:
+ return self.arm.is_calibrated
+
+ def calibrate(self) -> None:
+ logger.info(f"\nRunning calibration of {self}")
+ self.arm.disable_torque()
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+
+ input(f"Move {self} to the middle of its range of motion and press ENTER....")
+ homing_offsets = self.arm.set_half_turn_homings()
+
+ print(
+ "Move all joints sequentially through their entire ranges "
+ "of motion.\nRecording positions. Press ENTER to stop..."
+ )
+ range_mins, range_maxes = self.arm.record_ranges_of_motion()
+
+ self.calibration = {}
+ for motor, m in self.arm.motors.items():
+ self.calibration[motor] = MotorCalibration(
+ id=m.id,
+ drive_mode=0,
+ homing_offset=homing_offsets[motor],
+ range_min=range_mins[motor],
+ range_max=range_maxes[motor],
+ )
+
+ self.arm.write_calibration(self.calibration)
+ self._save_calibration()
+ logger.info(f"Calibration saved to {self.calibration_fpath}")
+
+ def configure(self) -> None:
+ self.arm.disable_torque()
+ self.arm.configure_motors()
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+
+ def setup_motors(self) -> None:
+ for motor in reversed(self.arm.motors):
+ input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+ self.arm.setup_motor(motor)
+ print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
+
+ def get_action(self) -> dict[str, float]:
+ start = time.perf_counter()
+ action = self.arm.sync_read("Present_Position")
+ action = {f"{motor}.pos": val for motor, val in action.items()}
+ dt_ms = (time.perf_counter() - start) * 1e3
+ logger.debug(f"{self} read action: {dt_ms:.1f}ms")
+ return action
+
+ def send_feedback(self, feedback: dict[str, float]) -> None:
+ # TODO(rcadene, aliberts): Implement force feedback
+ raise NotImplementedError
+
+ def disconnect(self) -> None:
+ if not self.is_connected:
+ DeviceNotConnectedError(f"{self} is not connected.")
+
+ self.arm.disconnect()
+ logger.info(f"{self} disconnected.")
diff --git a/lerobot/common/teleoperators/stretch3/__init__.py b/lerobot/common/teleoperators/stretch3/__init__.py
deleted file mode 100644
index 9931e5fa4..000000000
--- a/lerobot/common/teleoperators/stretch3/__init__.py
+++ /dev/null
@@ -1,4 +0,0 @@
-from .configuration_stretch3 import Stretch3GamePadConfig
-from .teleop_stretch3 import Stretch3GamePad
-
-__all__ = ["Stretch3GamePadConfig", "Stretch3GamePad"]
diff --git a/lerobot/common/teleoperators/stretch3_gamepad/__init__.py b/lerobot/common/teleoperators/stretch3_gamepad/__init__.py
new file mode 100644
index 000000000..ac45b6dd4
--- /dev/null
+++ b/lerobot/common/teleoperators/stretch3_gamepad/__init__.py
@@ -0,0 +1,2 @@
+from .configuration_stretch3 import Stretch3GamePadConfig
+from .stretch3_gamepad import Stretch3GamePad
diff --git a/lerobot/common/teleoperators/stretch3/configuration_stretch3.py b/lerobot/common/teleoperators/stretch3_gamepad/configuration_stretch3.py
similarity index 100%
rename from lerobot/common/teleoperators/stretch3/configuration_stretch3.py
rename to lerobot/common/teleoperators/stretch3_gamepad/configuration_stretch3.py
diff --git a/lerobot/common/teleoperators/stretch3/teleop_stretch3.py b/lerobot/common/teleoperators/stretch3_gamepad/stretch3_gamepad.py
similarity index 97%
rename from lerobot/common/teleoperators/stretch3/teleop_stretch3.py
rename to lerobot/common/teleoperators/stretch3_gamepad/stretch3_gamepad.py
index b23bd4027..adc973ee7 100644
--- a/lerobot/common/teleoperators/stretch3/teleop_stretch3.py
+++ b/lerobot/common/teleoperators/stretch3_gamepad/stretch3_gamepad.py
@@ -73,7 +73,7 @@ class Stretch3GamePad(Teleoperator):
RobotParams.set_logging_formatter("brief_console_formatter")
@property
- def action_feature(self) -> dict:
+ def action_features(self) -> dict:
return {
"dtype": "float32",
"shape": (len(GAMEPAD_BUTTONS),),
@@ -81,7 +81,7 @@ class Stretch3GamePad(Teleoperator):
}
@property
- def feedback_feature(self) -> dict:
+ def feedback_features(self) -> dict:
return {}
def connect(self) -> None:
diff --git a/lerobot/common/teleoperators/teleoperator.py b/lerobot/common/teleoperators/teleoperator.py
index d6285f5c0..c09f76adc 100644
--- a/lerobot/common/teleoperators/teleoperator.py
+++ b/lerobot/common/teleoperators/teleoperator.py
@@ -34,11 +34,11 @@ class Teleoperator(abc.ABC):
return f"{self.id} {self.__class__.__name__}"
@abc.abstractproperty
- def action_feature(self) -> dict:
+ def action_features(self) -> dict:
pass
@abc.abstractproperty
- def feedback_feature(self) -> dict:
+ def feedback_features(self) -> dict:
pass
@abc.abstractproperty
@@ -46,7 +46,7 @@ class Teleoperator(abc.ABC):
pass
@abc.abstractmethod
- def connect(self) -> None:
+ def connect(self, calibrate: bool = True) -> None:
"""Connects to the teleoperator."""
pass
diff --git a/lerobot/common/teleoperators/utils.py b/lerobot/common/teleoperators/utils.py
new file mode 100644
index 000000000..f9fbbea29
--- /dev/null
+++ b/lerobot/common/teleoperators/utils.py
@@ -0,0 +1,35 @@
+from .config import TeleoperatorConfig
+from .teleoperator import Teleoperator
+
+
+def make_teleoperator_from_config(config: TeleoperatorConfig) -> Teleoperator:
+ if config.type == "keyboard":
+ from .keyboard import KeyboardTeleop
+
+ return KeyboardTeleop(config)
+ elif config.type == "koch_leader":
+ from .koch_leader import KochLeader
+
+ return KochLeader(config)
+ elif config.type == "so100_leader":
+ from .so100_leader import SO100Leader
+
+ return SO100Leader(config)
+ elif config.type == "so101_leader":
+ from .so101_leader import SO101Leader
+
+ return SO101Leader(config)
+ elif config.type == "stretch3":
+ from .stretch3_gamepad import Stretch3GamePad
+
+ return Stretch3GamePad(config)
+ elif config.type == "widowx":
+ from .widowx import WidowX
+
+ return WidowX(config)
+ elif config.type == "mock_teleop":
+ from tests.mocks.mock_teleop import MockTeleop
+
+ return MockTeleop(config)
+ else:
+ raise ValueError(config.type)
diff --git a/lerobot/common/teleoperators/widowx/widowx.py b/lerobot/common/teleoperators/widowx/widowx.py
index 8b452055c..4b09f6d07 100644
--- a/lerobot/common/teleoperators/widowx/widowx.py
+++ b/lerobot/common/teleoperators/widowx/widowx.py
@@ -58,64 +58,64 @@ class WidowX(Teleoperator):
)
@property
- def action_feature(self) -> dict:
- return {
- "dtype": "float32",
- "shape": (len(self.arm),),
- "names": {"motors": list(self.arm.motors)},
- }
+ def action_features(self) -> dict[str, type]:
+ return {f"{motor}.pos": float for motor in self.arm.motors}
@property
- def feedback_feature(self) -> dict:
+ def feedback_features(self) -> dict[str, type]:
return {}
@property
def is_connected(self) -> bool:
return self.arm.is_connected
- def connect(self):
+ def connect(self, calibrate: bool = True):
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.arm.connect()
- if not self.is_calibrated:
+ if not self.is_calibrated and calibrate:
self.calibrate()
self.configure()
logger.info(f"{self} connected.")
+ @property
+ def is_calibrated(self) -> bool:
+ return self.arm.is_calibrated
+
def calibrate(self) -> None:
raise NotImplementedError # TODO(aliberts): adapt code below (copied from koch)
logger.info(f"\nRunning calibration of {self}")
self.arm.disable_torque()
- for name in self.arm.names:
- self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
+ for motor in self.arm.motors:
+ self.arm.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
self.arm.write("Drive_Mode", "elbow_flex", DriveMode.INVERTED.value)
- drive_modes = {name: 1 if name == "elbow_flex" else 0 for name in self.arm.names}
+ drive_modes = {motor: 1 if motor == "elbow_flex" else 0 for motor in self.arm.motors}
input("Move robot to the middle of its range of motion and press ENTER....")
homing_offsets = self.arm.set_half_turn_homings()
full_turn_motors = ["shoulder_pan", "wrist_roll"]
- unknown_range_motors = [name for name in self.arm.names if name not in full_turn_motors]
- logger.info(
+ unknown_range_motors = [motor for motor in self.arm.motors if motor not in full_turn_motors]
+ print(
f"Move all joints except {full_turn_motors} sequentially through their "
"entire ranges of motion.\nRecording positions. Press ENTER to stop..."
)
range_mins, range_maxes = self.arm.record_ranges_of_motion(unknown_range_motors)
- for name in full_turn_motors:
- range_mins[name] = 0
- range_maxes[name] = 4095
+ for motor in full_turn_motors:
+ range_mins[motor] = 0
+ range_maxes[motor] = 4095
self.calibration = {}
- for name, motor in self.arm.motors.items():
- self.calibration[name] = MotorCalibration(
- id=motor.id,
- drive_mode=drive_modes[name],
- homing_offset=homing_offsets[name],
- range_min=range_mins[name],
- range_max=range_maxes[name],
+ for motor, m in self.arm.motors.items():
+ self.calibration[motor] = MotorCalibration(
+ id=m.id,
+ drive_mode=drive_modes[motor],
+ homing_offset=homing_offsets[motor],
+ range_min=range_mins[motor],
+ range_max=range_maxes[motor],
)
self.arm.write_calibration(self.calibration)
@@ -137,7 +137,8 @@ class WidowX(Teleoperator):
raise DeviceNotConnectedError(f"{self} is not connected.")
start = time.perf_counter()
- action = self.arm.read("Present_Position")
+ action = self.arm.sync_read("Present_Position")
+ action = {f"{motor}.pos": val for motor, val in action.items()}
dt_ms = (time.perf_counter() - start) * 1e3
logger.debug(f"{self} read action: {dt_ms:.1f}ms")
return action
diff --git a/lerobot/common/utils/control_utils.py b/lerobot/common/utils/control_utils.py
index 12524b699..3ac792a5d 100644
--- a/lerobot/common/utils/control_utils.py
+++ b/lerobot/common/utils/control_utils.py
@@ -31,9 +31,9 @@ from termcolor import colored
from lerobot.common.datasets.image_writer import safe_stop_image_writer
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
-from lerobot.common.datasets.utils import get_features_from_robot
+from lerobot.common.datasets.utils import DEFAULT_FEATURES
from lerobot.common.policies.pretrained import PreTrainedPolicy
-from lerobot.common.robots.utils import Robot
+from lerobot.common.robots import Robot
from lerobot.common.utils.robot_utils import busy_wait
from lerobot.common.utils.utils import get_safe_torch_device, has_method
@@ -250,6 +250,7 @@ def control_loop(
observation, action = robot.teleop_step(record_data=True)
else:
observation = robot.capture_observation()
+ action = None
if policy is not None:
pred_action = predict_action(
@@ -266,9 +267,10 @@ def control_loop(
# TODO(Steven): This should be more general (for RemoteRobot instead of checking the name, but anyways it will change soon)
if (display_data and not is_headless()) or (display_data and robot.robot_type.startswith("lekiwi")):
- for k, v in action.items():
- for i, vv in enumerate(v):
- rr.log(f"sent_{k}_{i}", rr.Scalar(vv.numpy()))
+ if action is not None:
+ for k, v in action.items():
+ for i, vv in enumerate(v):
+ rr.log(f"sent_{k}_{i}", rr.Scalar(vv.numpy()))
image_keys = [key for key in observation if "image" in key]
for key in image_keys:
@@ -327,12 +329,12 @@ def sanity_check_dataset_name(repo_id, policy_cfg):
def sanity_check_dataset_robot_compatibility(
- dataset: LeRobotDataset, robot: Robot, fps: int, use_videos: bool
+ dataset: LeRobotDataset, robot: Robot, fps: int, features: dict
) -> None:
fields = [
("robot_type", dataset.meta.robot_type, robot.robot_type),
("fps", dataset.fps, fps),
- ("features", dataset.features, get_features_from_robot(robot, use_videos)),
+ ("features", dataset.features, {**features, **DEFAULT_FEATURES}),
]
mismatches = []
diff --git a/lerobot/common/utils/visualization_utils.py b/lerobot/common/utils/visualization_utils.py
new file mode 100644
index 000000000..2491706af
--- /dev/null
+++ b/lerobot/common/utils/visualization_utils.py
@@ -0,0 +1,12 @@
+import os
+
+import rerun as rr
+
+
+def _init_rerun(session_name: str = "lerobot_control_loop") -> None:
+ """Initializes the Rerun SDK for visualizing the control loop."""
+ batch_size = os.getenv("RERUN_FLUSH_NUM_BYTES", "8000")
+ os.environ["RERUN_FLUSH_NUM_BYTES"] = batch_size
+ rr.init(session_name)
+ memory_limit = os.getenv("LEROBOT_RERUN_MEMORY_LIMIT", "10%")
+ rr.spawn(memory_limit=memory_limit)
diff --git a/lerobot/configs/parser.py b/lerobot/configs/parser.py
index 39e315152..f69b5a7fa 100644
--- a/lerobot/configs/parser.py
+++ b/lerobot/configs/parser.py
@@ -26,7 +26,6 @@ from lerobot.common.utils.utils import has_method
PATH_KEY = "path"
PLUGIN_DISCOVERY_SUFFIX = "discover_packages_path"
-draccus.set_config_type("json")
def get_cli_overrides(field_name: str, args: Sequence[str] | None = None) -> list[str] | None:
diff --git a/lerobot/configs/policies.py b/lerobot/configs/policies.py
index 022d1fb52..22eae05fd 100644
--- a/lerobot/configs/policies.py
+++ b/lerobot/configs/policies.py
@@ -173,4 +173,5 @@ class PreTrainedConfig(draccus.ChoiceRegistry, HubMixin, abc.ABC):
# HACK: this is very ugly, ideally we'd like to be able to do that natively with draccus
# something like --policy.path (in addition to --policy.type)
cli_overrides = policy_kwargs.pop("cli_overrides", [])
- return draccus.parse(cls, config_file, args=cli_overrides)
+ with draccus.config_type("json"):
+ return draccus.parse(cls, config_file, args=cli_overrides)
diff --git a/lerobot/configs/train.py b/lerobot/configs/train.py
index 7a787b83e..98826294e 100644
--- a/lerobot/configs/train.py
+++ b/lerobot/configs/train.py
@@ -170,6 +170,5 @@ class TrainPipelineConfig(HubMixin):
) from e
cli_args = kwargs.pop("cli_args", [])
- cfg = draccus.parse(cls, config_file, args=cli_args)
-
- return cfg
+ with draccus.config_type("json"):
+ return draccus.parse(cls, config_file, args=cli_args)
diff --git a/lerobot/scripts/find_motors_bus_port.py b/lerobot/find_port.py
similarity index 90%
rename from lerobot/scripts/find_motors_bus_port.py
rename to lerobot/find_port.py
index 68f2315d7..05d0eb8fd 100644
--- a/lerobot/scripts/find_motors_bus_port.py
+++ b/lerobot/find_port.py
@@ -11,14 +11,25 @@
# 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.
+
+"""
+Helper to find the USB port associated with your MotorsBus.
+
+Example:
+
+```shell
+python -m lerobot.find_port
+```
+"""
+
import os
import time
from pathlib import Path
-from serial.tools import list_ports # Part of pyserial library
-
def find_available_ports():
+ from serial.tools import list_ports # Part of pyserial library
+
if os.name == "nt": # Windows
# List COM ports using pyserial
ports = [port.device for port in list_ports.comports()]
@@ -51,5 +62,4 @@ def find_port():
if __name__ == "__main__":
- # Helper to find the USB port associated with your MotorsBus.
find_port()
diff --git a/lerobot/record.py b/lerobot/record.py
new file mode 100644
index 000000000..0059752b8
--- /dev/null
+++ b/lerobot/record.py
@@ -0,0 +1,345 @@
+# 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.
+
+"""
+Records a dataset. Actions for the robot can be either generated by teleoperation or by a policy.
+
+Example:
+
+```shell
+python -m lerobot.record \
+ --robot.type=so100_follower \
+ --robot.port=/dev/tty.usbmodem58760431541 \
+ --robot.cameras="{laptop: {type: opencv, camera_index: 0, width: 640, height: 480}}" \
+ --robot.id=black \
+ --teleop.type=so100_leader \
+ --teleop.port=/dev/tty.usbmodem58760431551 \
+ --teleop.id=blue \
+ --dataset.repo_id=aliberts/record-test \
+ --dataset.num_episodes=2 \
+ --dataset.single_task="Grab the cube"
+```
+"""
+
+import logging
+import time
+from dataclasses import asdict, dataclass
+from pathlib import Path
+from pprint import pformat
+
+import draccus
+import numpy as np
+import rerun as rr
+
+from lerobot.common.cameras import ( # noqa: F401
+ CameraConfig, # noqa: F401
+ intel,
+ opencv,
+)
+from lerobot.common.datasets.image_writer import safe_stop_image_writer
+from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
+from lerobot.common.datasets.utils import build_dataset_frame, hw_to_dataset_features
+from lerobot.common.policies.factory import make_policy
+from lerobot.common.policies.pretrained import PreTrainedPolicy
+from lerobot.common.robots import ( # noqa: F401
+ Robot,
+ RobotConfig,
+ koch_follower,
+ make_robot_from_config,
+ so100_follower,
+)
+from lerobot.common.teleoperators import ( # noqa: F401
+ Teleoperator,
+ TeleoperatorConfig,
+ make_teleoperator_from_config,
+)
+from lerobot.common.utils.control_utils import (
+ init_keyboard_listener,
+ is_headless,
+ predict_action,
+ sanity_check_dataset_name,
+ sanity_check_dataset_robot_compatibility,
+)
+from lerobot.common.utils.robot_utils import busy_wait
+from lerobot.common.utils.utils import (
+ get_safe_torch_device,
+ init_logging,
+ log_say,
+)
+from lerobot.common.utils.visualization_utils import _init_rerun
+from lerobot.configs import parser
+from lerobot.configs.policies import PreTrainedConfig
+
+from .common.teleoperators import koch_leader, so100_leader # noqa: F401
+
+
+@dataclass
+class DatasetRecordConfig:
+ # Dataset identifier. By convention it should match '{hf_username}/{dataset_name}' (e.g. `lerobot/test`).
+ repo_id: str
+ # A short but accurate description of the task performed during the recording (e.g. "Pick the Lego block and drop it in the box on the right.")
+ single_task: str
+ # Root directory where the dataset will be stored (e.g. 'dataset/path').
+ root: str | Path | None = None
+ # Limit the frames per second. By default, uses the policy fps.
+ fps: int = 30
+ # Number of seconds for data recording for each episode.
+ episode_time_s: int | float = 60
+ # Number of seconds for resetting the environment after each episode.
+ reset_time_s: int | float = 60
+ # Number of episodes to record.
+ num_episodes: int = 50
+ # Encode frames in the dataset into video
+ video: bool = True
+ # Upload dataset to Hugging Face hub.
+ push_to_hub: bool = True
+ # Upload on private repository on the Hugging Face hub.
+ private: bool = False
+ # Add tags to your dataset on the hub.
+ tags: list[str] | None = None
+ # Number of subprocesses handling the saving of frames as PNG. Set to 0 to use threads only;
+ # set to ≥1 to use subprocesses, each using threads to write images. The best number of processes
+ # and threads depends on your system. We recommend 4 threads per camera with 0 processes.
+ # If fps is unstable, adjust the thread count. If still unstable, try using 1 or more subprocesses.
+ num_image_writer_processes: int = 0
+ # Number of threads writing the frames as png images on disk, per camera.
+ # Too many threads might cause unstable teleoperation fps due to main thread being blocked.
+ # Not enough threads might cause low camera fps.
+ num_image_writer_threads_per_camera: int = 4
+
+ def __post_init__(self):
+ if self.single_task is None:
+ raise ValueError("You need to provide a task as argument in `single_task`.")
+
+
+@dataclass
+class RecordConfig:
+ robot: RobotConfig
+ dataset: DatasetRecordConfig
+ # Whether to control the robot with a teleoperator
+ teleop: TeleoperatorConfig | None = None
+ # Whether to control the robot with a policy
+ policy: PreTrainedConfig | None = None
+ # Number of seconds before starting data collection. It allows the robot devices to warmup and synchronize.
+ warmup_time_s: int | float = 10
+ # Display all cameras on screen
+ display_data: bool = False
+ # Use vocal synthesis to read events.
+ play_sounds: bool = True
+ # Resume recording on an existing dataset.
+ resume: bool = False
+
+ def __post_init__(self):
+ if bool(self.teleop) == bool(self.policy):
+ raise ValueError("Choose either a policy or a teleoperator to control the robot")
+
+ # HACK: We parse again the cli args here to get the pretrained path if there was one.
+ policy_path = parser.get_path_arg("policy")
+ if policy_path:
+ cli_overrides = parser.get_cli_overrides("policy")
+ self.policy = PreTrainedConfig.from_pretrained(policy_path, cli_overrides=cli_overrides)
+ self.policy.pretrained_path = policy_path
+
+
+@safe_stop_image_writer
+def record_loop(
+ robot: Robot,
+ events: dict,
+ dataset: LeRobotDataset | None = None,
+ teleop: Teleoperator | None = None,
+ policy: PreTrainedPolicy | None = None,
+ control_time_s: int | None = None,
+ fps: int | None = None,
+ single_task: str | None = None,
+ display_data: bool = False,
+):
+ if dataset is not None and fps is not None and dataset.fps != fps:
+ raise ValueError(f"The dataset fps should be equal to requested fps ({dataset.fps} != {fps}).")
+
+ timestamp = 0
+ start_episode_t = time.perf_counter()
+ while timestamp < control_time_s:
+ start_loop_t = time.perf_counter()
+
+ observation = robot.get_observation()
+
+ if policy is not None:
+ action = predict_action(
+ observation, policy, get_safe_torch_device(policy.config.device), policy.config.use_amp
+ )
+ else:
+ action = teleop.get_action()
+
+ # Action can eventually be clipped using `max_relative_target`,
+ # so action actually sent is saved in the dataset.
+ sent_action = robot.send_action(action)
+
+ if dataset is not None:
+ observation_frame = build_dataset_frame(dataset.features, observation, prefix="observation")
+ action_frame = build_dataset_frame(dataset.features, sent_action, prefix="action")
+ frame = {**observation_frame, **action_frame}
+ dataset.add_frame(frame, task=single_task)
+
+ if display_data:
+ for obs, val in observation.items():
+ if isinstance(val, float):
+ rr.log(f"observation.{obs}", rr.Scalar(val))
+ elif isinstance(val, np.ndarray):
+ rr.log(f"observation.{obs}", rr.Image(val), static=True)
+ for act, val in action.items():
+ if isinstance(val, float):
+ rr.log(f"action.{act}", rr.Scalar(val))
+
+ if fps is not None:
+ dt_s = time.perf_counter() - start_loop_t
+ busy_wait(1 / fps - dt_s)
+
+ dt_s = time.perf_counter() - start_loop_t
+ # log_control_info(robot, dt_s, fps=fps)
+
+ timestamp = time.perf_counter() - start_episode_t
+ if events["exit_early"]:
+ events["exit_early"] = False
+ break
+
+
+@draccus.wrap()
+def record(cfg: RecordConfig) -> LeRobotDataset:
+ init_logging()
+ logging.info(pformat(asdict(cfg)))
+ if cfg.display_data:
+ _init_rerun(session_name="recording")
+
+ robot = make_robot_from_config(cfg.robot)
+ teleop = make_teleoperator_from_config(cfg.teleop) if cfg.teleop is not None else None
+
+ action_features = hw_to_dataset_features(robot.action_features, "action", cfg.dataset.video)
+ obs_features = hw_to_dataset_features(robot.observation_features, "observation", cfg.dataset.video)
+ dataset_features = {**action_features, **obs_features}
+
+ if cfg.resume:
+ dataset = LeRobotDataset(
+ cfg.dataset.repo_id,
+ root=cfg.dataset.root,
+ )
+ # for key, ft in dataset_features.items():
+ # for property in ["dtype", "shape", "names"]:
+ # if ft[property] != dataset.features[key][property]:
+ # raise ValueError(ft)
+
+ if hasattr(robot, "cameras") and len(robot.cameras) > 0:
+ dataset.start_image_writer(
+ num_processes=cfg.dataset.num_image_writer_processes,
+ num_threads=cfg.dataset.num_image_writer_threads_per_camera * len(robot.cameras),
+ )
+ sanity_check_dataset_robot_compatibility(dataset, robot, cfg.dataset.fps, dataset_features)
+ else:
+ # Create empty dataset or load existing saved episodes
+ sanity_check_dataset_name(cfg.dataset.repo_id, cfg.policy)
+ dataset = LeRobotDataset.create(
+ cfg.dataset.repo_id,
+ cfg.dataset.fps,
+ root=cfg.dataset.root,
+ robot_type=robot.name,
+ features=dataset_features,
+ use_videos=cfg.dataset.video,
+ image_writer_processes=cfg.dataset.num_image_writer_processes,
+ image_writer_threads=cfg.dataset.num_image_writer_threads_per_camera * len(robot.cameras),
+ )
+
+ # Load pretrained policy
+ policy = None if cfg.policy is None else make_policy(cfg.policy, ds_meta=dataset.meta)
+
+ robot.connect()
+ if teleop is not None:
+ teleop.connect()
+
+ listener, events = init_keyboard_listener()
+
+ # Execute a few seconds without recording to:
+ # 1. teleoperate the robot to move it in starting position if no policy provided,
+ # 2. give times to the robot devices to connect and start synchronizing,
+ # 3. place the cameras windows on screen
+ # enable_teleoperation = policy is None
+ # log_say("Warmup record", cfg.play_sounds)
+ # record_loop(
+ # robot=robot,
+ # control_time_s=cfg.warmup_time_s,
+ # display_data=cfg.display_data,
+ # events=events,
+ # fps=cfg.dataset.fps,
+ # teleoperate=enable_teleoperation,
+ # )
+ # warmup_record(robot, events, enable_teleoperation, cfg.warmup_time_s, cfg.display_data, cfg.dataset.fps)
+
+ for recorded_episodes in range(cfg.dataset.num_episodes):
+ log_say(f"Recording episode {dataset.num_episodes}", cfg.play_sounds)
+ record_loop(
+ robot=robot,
+ events=events,
+ teleop=teleop,
+ policy=policy,
+ dataset=dataset,
+ control_time_s=cfg.dataset.episode_time_s,
+ fps=cfg.dataset.fps,
+ single_task=cfg.dataset.single_task,
+ display_data=cfg.display_data,
+ )
+
+ # Execute a few seconds without recording to give time to manually reset the environment
+ # Skip reset for the last episode to be recorded
+ if not events["stop_recording"] and (
+ (recorded_episodes < cfg.dataset.num_episodes - 1) or events["rerecord_episode"]
+ ):
+ log_say("Reset the environment", cfg.play_sounds)
+ record_loop(
+ robot=robot,
+ events=events,
+ teleop=teleop,
+ control_time_s=cfg.dataset.reset_time_s,
+ fps=cfg.dataset.fps,
+ single_task=cfg.dataset.single_task,
+ display_data=cfg.display_data,
+ )
+ # reset_environment(robot, events, cfg.dataset.reset_time_s, cfg.dataset.fps)
+
+ if events["rerecord_episode"]:
+ log_say("Re-record episode", cfg.play_sounds)
+ events["rerecord_episode"] = False
+ events["exit_early"] = False
+ dataset.clear_episode_buffer()
+ continue
+
+ dataset.save_episode()
+
+ if events["stop_recording"]:
+ break
+
+ log_say("Stop recording", cfg.play_sounds, blocking=True)
+
+ robot.disconnect()
+ teleop.disconnect()
+
+ if not is_headless() and listener is not None:
+ listener.stop()
+
+ if cfg.dataset.push_to_hub:
+ dataset.push_to_hub(tags=cfg.dataset.tags, private=cfg.dataset.private)
+
+ log_say("Exiting", cfg.play_sounds)
+ return dataset
+
+
+if __name__ == "__main__":
+ record()
diff --git a/lerobot/replay.py b/lerobot/replay.py
new file mode 100644
index 000000000..5b8f86190
--- /dev/null
+++ b/lerobot/replay.py
@@ -0,0 +1,101 @@
+# 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.
+
+"""
+Replays the actions of an episode from a dataset on a robot.
+
+Example:
+
+```shell
+python -m lerobot.replay \
+ --robot.type=so100_follower \
+ --robot.port=/dev/tty.usbmodem58760431541 \
+ --robot.id=black \
+ --dataset.repo_id=aliberts/record-test \
+ --dataset.episode=2
+```
+"""
+
+import logging
+import time
+from dataclasses import asdict, dataclass
+from pathlib import Path
+from pprint import pformat
+
+import draccus
+
+from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
+from lerobot.common.robots import ( # noqa: F401
+ Robot,
+ RobotConfig,
+ koch_follower,
+ make_robot_from_config,
+ so100_follower,
+)
+from lerobot.common.utils.robot_utils import busy_wait
+from lerobot.common.utils.utils import (
+ init_logging,
+ log_say,
+)
+
+
+@dataclass
+class DatasetReplayConfig:
+ # Dataset identifier. By convention it should match '{hf_username}/{dataset_name}' (e.g. `lerobot/test`).
+ repo_id: str
+ # Episode to replay.
+ episode: int
+ # Root directory where the dataset will be stored (e.g. 'dataset/path').
+ root: str | Path | None = None
+ # Limit the frames per second. By default, uses the policy fps.
+ fps: int = 30
+
+
+@dataclass
+class ReplayConfig:
+ robot: RobotConfig
+ dataset: DatasetReplayConfig
+ # Use vocal synthesis to read events.
+ play_sounds: bool = True
+
+
+@draccus.wrap()
+def replay(cfg: ReplayConfig):
+ init_logging()
+ logging.info(pformat(asdict(cfg)))
+
+ robot = make_robot_from_config(cfg.robot)
+ dataset = LeRobotDataset(cfg.dataset.repo_id, root=cfg.dataset.root, episodes=[cfg.dataset.episode])
+ actions = dataset.hf_dataset.select_columns("action.joints")
+ robot.connect()
+
+ log_say("Replaying episode", cfg.play_sounds, blocking=True)
+ for idx in range(dataset.num_frames):
+ start_episode_t = time.perf_counter()
+
+ action_array = actions[idx]["action.joints"]
+ action = {}
+ for i, name in enumerate(dataset.features["action.joints"]["names"]):
+ action[name] = action_array[i]
+
+ robot.send_action(action)
+
+ dt_s = time.perf_counter() - start_episode_t
+ busy_wait(1 / dataset.fps - dt_s)
+
+ robot.disconnect()
+
+
+if __name__ == "__main__":
+ replay()
diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py
deleted file mode 100644
index b434fed6c..000000000
--- a/lerobot/scripts/configure_motor.py
+++ /dev/null
@@ -1,140 +0,0 @@
-# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-"""
-This script configure a single motor at a time to a given ID and baudrate.
-
-Example of usage:
-```bash
-python lerobot/scripts/configure_motor.py \
- --port /dev/tty.usbmodem585A0080521 \
- --brand feetech \
- --model sts3215 \
- --baudrate 1000000 \
- --id 1
-```
-"""
-
-import argparse
-import time
-
-from lerobot.common.motors.dynamixel.dynamixel import MODEL_RESOLUTION as DXL_MODEL_RESOLUTION
-from lerobot.common.motors.feetech.feetech import MODEL_RESOLUTION as FTCH_MODEL_RESOLUTION
-
-
-def configure_motor(port, brand, model, target_motor_idx, target_baudrate):
- if brand == "feetech":
- from lerobot.common.motors.feetech.feetech import FeetechMotorsBus
-
- motor_bus = FeetechMotorsBus(port=port, motors={"motor": (target_motor_idx, model)})
-
- elif brand == "dynamixel":
- from lerobot.common.motors.dynamixel.dynamixel import DynamixelMotorsBus
-
- motor_bus = DynamixelMotorsBus(port=port, motors={"motor": (target_motor_idx, model)})
-
- motor_bus.connect()
-
- # Motor bus is connected, proceed with the rest of the operations
- try:
- print("Scanning all baudrates and motor indices")
- model_baudrates = list(motor_bus.model_baudrate_table[model].values())
- motor_index = -1 # Set the motor index to an out-of-range value.
-
- for baudrate in model_baudrates:
- motor_bus.set_baudrate(baudrate)
- present_ids = motor_bus.find_motor_indices(list(range(1, 10)))
- if len(present_ids) > 1:
- raise ValueError(
- "Error: More than one motor ID detected. This script is designed to only handle one motor at a time. Please disconnect all but one motor."
- )
-
- if len(present_ids) == 1:
- if motor_index != -1:
- raise ValueError(
- "Error: More than one motor ID detected. This script is designed to only handle one motor at a time. Please disconnect all but one motor."
- )
- motor_index = present_ids[0]
- break
-
- if motor_index == -1:
- raise ValueError("No motors detected. Please ensure you have one motor connected.")
-
- print(f"Motor index found at: {motor_index}")
-
- if brand == "feetech":
- # Allows ID and BAUDRATE to be written in memory
- motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0)
-
- if baudrate != target_baudrate:
- print(f"Setting its baudrate to {target_baudrate}")
- baudrate_idx = model_baudrates.index(target_baudrate)
-
- # The write can fail, so we allow retries
- motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Baud_Rate", baudrate_idx)
- time.sleep(0.5)
- motor_bus.set_bus_baudrate(target_baudrate)
- present_baudrate_idx = motor_bus.read_with_motor_ids(
- motor_bus.motor_models, motor_index, "Baud_Rate", num_retry=2
- )
-
- if present_baudrate_idx != baudrate_idx:
- raise OSError("Failed to write baudrate.")
-
- print(f"Setting its index to desired index {target_motor_idx}")
- if brand == "feetech":
- motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0)
- motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "ID", target_motor_idx)
-
- present_idx = motor_bus.read_with_motor_ids(
- motor_bus.motor_models, target_motor_idx, "ID", num_retry=2
- )
- if present_idx != target_motor_idx:
- raise OSError("Failed to write index.")
-
- if brand == "feetech":
- # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of
- # the motors. Note: this configuration is not in the official STS3215 Memory Table
- motor_bus.write("Lock", 0)
- motor_bus.write("Maximum_Acceleration", 254)
- motor_bus.write("Max_Angle_Limit", 4095) # default 4095
- motor_bus.write("Min_Angle_Limit", 0) # default 0
- motor_bus.write("Offset", 0)
- motor_bus.write("Mode", 0)
- motor_bus.write("Goal_Position", 2048)
- motor_bus.write("Lock", 1)
- print("Offset", motor_bus.read("Offset"))
-
- except Exception as e:
- print(f"Error occurred during motor configuration: {e}")
-
- finally:
- if motor_bus.is_connected:
- motor_bus.disconnect()
- print("Disconnected from motor bus.")
-
-
-if __name__ == "__main__":
- model_choices = [*FTCH_MODEL_RESOLUTION.keys(), *DXL_MODEL_RESOLUTION.keys()]
- brand_choices = ["feetech", "dynamixel"]
- parser = argparse.ArgumentParser()
- parser.add_argument("--port", type=str, required=True, help="Motors bus port")
- parser.add_argument("--brand", type=str, required=True, choices=brand_choices, help="Motor brand")
- parser.add_argument("--model", type=str, required=True, choices=model_choices, help="Motor model")
- parser.add_argument("--id", type=int, required=True, help="Desired ID of the current motor (e.g. 1,2,3)")
- parser.add_argument(
- "--baudrate", type=int, default=1_000_000, help="Desired baudrate for the motor (default: 1_000_000)"
- )
- args = parser.parse_args()
-
- configure_motor(args.port, args.brand, args.model, args.id, args.baudrate)
diff --git a/lerobot/scripts/control_robot.py b/lerobot/scripts/control_robot.py
deleted file mode 100644
index 18787b037..000000000
--- a/lerobot/scripts/control_robot.py
+++ /dev/null
@@ -1,437 +0,0 @@
-# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-"""
-Utilities to control a robot.
-
-Useful to record a dataset, replay a recorded episode, run the policy on your robot
-and record an evaluation dataset, and to recalibrate your robot if needed.
-
-Examples of usage:
-
-- Recalibrate your robot:
-```bash
-python lerobot/scripts/control_robot.py \
- --robot.type=so100 \
- --control.type=calibrate
-```
-
-- Unlimited teleoperation at highest frequency (~200 Hz is expected), to exit with CTRL+C:
-```bash
-python lerobot/scripts/control_robot.py \
- --robot.type=so100 \
- --robot.cameras='{}' \
- --control.type=teleoperate
-
-# Add the cameras from the robot definition to visualize them:
-python lerobot/scripts/control_robot.py \
- --robot.type=so100 \
- --control.type=teleoperate
-```
-
-- Unlimited teleoperation at a limited frequency of 30 Hz, to simulate data recording frequency:
-```bash
-python lerobot/scripts/control_robot.py \
- --robot.type=so100 \
- --control.type=teleoperate \
- --control.fps=30
-```
-
-- Record one episode in order to test replay:
-```bash
-python lerobot/scripts/control_robot.py \
- --robot.type=so100 \
- --control.type=record \
- --control.fps=30 \
- --control.single_task="Grasp a lego block and put it in the bin." \
- --control.repo_id=$USER/koch_test \
- --control.num_episodes=1 \
- --control.push_to_hub=True
-```
-
-- Visualize dataset:
-```bash
-python lerobot/scripts/visualize_dataset.py \
- --repo-id $USER/koch_test \
- --episode-index 0
-```
-
-- Replay this test episode:
-```bash
-python lerobot/scripts/control_robot.py replay \
- --robot.type=so100 \
- --control.type=replay \
- --control.fps=30 \
- --control.repo_id=$USER/koch_test \
- --control.episode=0
-```
-
-- Record a full dataset in order to train a policy, with 2 seconds of warmup,
-30 seconds of recording for each episode, and 10 seconds to reset the environment in between episodes:
-```bash
-python lerobot/scripts/control_robot.py record \
- --robot.type=so100 \
- --control.type=record \
- --control.fps 30 \
- --control.repo_id=$USER/koch_pick_place_lego \
- --control.num_episodes=50 \
- --control.warmup_time_s=2 \
- --control.episode_time_s=30 \
- --control.reset_time_s=10
-```
-
-- For remote controlled robots like LeKiwi, run this script on the robot edge device (e.g. RaspBerryPi):
-```bash
-python lerobot/scripts/control_robot.py \
- --robot.type=lekiwi \
- --control.type=remote_robot
-```
-
-**NOTE**: You can use your keyboard to control data recording flow.
-- Tap right arrow key '->' to early exit while recording an episode and go to resseting the environment.
-- Tap right arrow key '->' to early exit while resetting the environment and got to recording the next episode.
-- Tap left arrow key '<-' to early exit and re-record the current episode.
-- Tap escape key 'esc' to stop the data recording.
-This might require a sudo permission to allow your terminal to monitor keyboard events.
-
-**NOTE**: You can resume/continue data recording by running the same data recording command and adding `--control.resume=true`.
-
-- Train on this dataset with the ACT policy:
-```bash
-python lerobot/scripts/train.py \
- --dataset.repo_id=${HF_USER}/koch_pick_place_lego \
- --policy.type=act \
- --output_dir=outputs/train/act_koch_pick_place_lego \
- --job_name=act_koch_pick_place_lego \
- --device=cuda \
- --wandb.enable=true
-```
-
-- Run the pretrained policy on the robot:
-```bash
-python lerobot/scripts/control_robot.py \
- --robot.type=so100 \
- --control.type=record \
- --control.fps=30 \
- --control.single_task="Grasp a lego block and put it in the bin." \
- --control.repo_id=$USER/eval_act_koch_pick_place_lego \
- --control.num_episodes=10 \
- --control.warmup_time_s=2 \
- --control.episode_time_s=30 \
- --control.reset_time_s=10 \
- --control.push_to_hub=true \
- --control.policy.path=outputs/train/act_koch_pick_place_lego/checkpoints/080000/pretrained_model
-```
-"""
-
-import logging
-import os
-import time
-from dataclasses import asdict
-from pprint import pformat
-
-import rerun as rr
-
-# from safetensors.torch import load_file, save_file
-from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
-from lerobot.common.policies.factory import make_policy
-from lerobot.common.robots.utils import Robot, make_robot_from_config
-from lerobot.common.utils.control_utils import (
- control_loop,
- init_keyboard_listener,
- is_headless,
- log_control_info,
- record_episode,
- reset_environment,
- sanity_check_dataset_name,
- sanity_check_dataset_robot_compatibility,
- stop_recording,
- warmup_record,
-)
-from lerobot.common.utils.robot_utils import busy_wait, safe_disconnect
-from lerobot.common.utils.utils import has_method, init_logging, log_say
-from lerobot.configs import parser
-from lerobot.configs.control import (
- CalibrateControlConfig,
- ControlConfig,
- ControlPipelineConfig,
- RecordControlConfig,
- RemoteRobotConfig,
- ReplayControlConfig,
- TeleoperateControlConfig,
-)
-
-########################################################################################
-# Control modes
-########################################################################################
-
-
-@safe_disconnect
-def calibrate(robot: Robot, cfg: CalibrateControlConfig):
- # TODO(aliberts): move this code in robots' classes
- if robot.robot_type.startswith("stretch"):
- if not robot.is_connected:
- robot.connect()
- if not robot.is_homed():
- robot.home()
- return
-
- arms = robot.available_arms if cfg.arms is None else cfg.arms
- unknown_arms = [arm_id for arm_id in arms if arm_id not in robot.available_arms]
- available_arms_str = " ".join(robot.available_arms)
- unknown_arms_str = " ".join(unknown_arms)
-
- if arms is None or len(arms) == 0:
- raise ValueError(
- "No arm provided. Use `--arms` as argument with one or more available arms.\n"
- f"For instance, to recalibrate all arms add: `--arms {available_arms_str}`"
- )
-
- if len(unknown_arms) > 0:
- raise ValueError(
- f"Unknown arms provided ('{unknown_arms_str}'). Available arms are `{available_arms_str}`."
- )
-
- for arm_id in arms:
- arm_calib_path = robot.calibration_dir / f"{arm_id}.json"
- if arm_calib_path.exists():
- print(f"Removing '{arm_calib_path}'")
- arm_calib_path.unlink()
- else:
- print(f"Calibration file not found '{arm_calib_path}'")
-
- if robot.is_connected:
- robot.disconnect()
-
- if robot.robot_type.startswith("lekiwi") and "main_follower" in arms:
- print("Calibrating only the lekiwi follower arm 'main_follower'...")
- robot.calibrate_follower()
- return
-
- if robot.robot_type.startswith("lekiwi") and "main_leader" in arms:
- print("Calibrating only the lekiwi leader arm 'main_leader'...")
- robot.calibrate_leader()
- return
-
- # Calling `connect` automatically runs calibration
- # when the calibration file is missing
- robot.connect()
- robot.disconnect()
- print("Calibration is done! You can now teleoperate and record datasets!")
-
-
-@safe_disconnect
-def teleoperate(robot: Robot, cfg: TeleoperateControlConfig):
- control_loop(
- robot,
- control_time_s=cfg.teleop_time_s,
- fps=cfg.fps,
- teleoperate=True,
- display_data=cfg.display_data,
- )
-
-
-@safe_disconnect
-def record(
- robot: Robot,
- cfg: RecordControlConfig,
-) -> LeRobotDataset:
- # TODO(rcadene): Add option to record logs
- if cfg.resume:
- dataset = LeRobotDataset(
- cfg.repo_id,
- root=cfg.root,
- )
- if len(robot.cameras) > 0:
- dataset.start_image_writer(
- num_processes=cfg.num_image_writer_processes,
- num_threads=cfg.num_image_writer_threads_per_camera * len(robot.cameras),
- )
- sanity_check_dataset_robot_compatibility(dataset, robot, cfg.fps, cfg.video)
- else:
- # Create empty dataset or load existing saved episodes
- sanity_check_dataset_name(cfg.repo_id, cfg.policy)
- dataset = LeRobotDataset.create(
- cfg.repo_id,
- cfg.fps,
- root=cfg.root,
- robot=robot,
- use_videos=cfg.video,
- image_writer_processes=cfg.num_image_writer_processes,
- image_writer_threads=cfg.num_image_writer_threads_per_camera * len(robot.cameras),
- )
-
- # Load pretrained policy
- policy = None if cfg.policy is None else make_policy(cfg.policy, ds_meta=dataset.meta)
-
- if not robot.is_connected:
- robot.connect()
-
- listener, events = init_keyboard_listener()
-
- # Execute a few seconds without recording to:
- # 1. teleoperate the robot to move it in starting position if no policy provided,
- # 2. give times to the robot devices to connect and start synchronizing,
- # 3. place the cameras windows on screen
- enable_teleoperation = policy is None
- log_say("Warmup record", cfg.play_sounds)
- warmup_record(robot, events, enable_teleoperation, cfg.warmup_time_s, cfg.display_data, cfg.fps)
-
- if has_method(robot, "teleop_safety_stop"):
- robot.teleop_safety_stop()
-
- recorded_episodes = 0
- while True:
- if recorded_episodes >= cfg.num_episodes:
- break
-
- log_say(f"Recording episode {dataset.num_episodes}", cfg.play_sounds)
- record_episode(
- robot=robot,
- dataset=dataset,
- events=events,
- episode_time_s=cfg.episode_time_s,
- display_data=cfg.display_data,
- policy=policy,
- fps=cfg.fps,
- single_task=cfg.single_task,
- )
-
- # Execute a few seconds without recording to give time to manually reset the environment
- # Current code logic doesn't allow to teleoperate during this time.
- # TODO(rcadene): add an option to enable teleoperation during reset
- # Skip reset for the last episode to be recorded
- if not events["stop_recording"] and (
- (recorded_episodes < cfg.num_episodes - 1) or events["rerecord_episode"]
- ):
- log_say("Reset the environment", cfg.play_sounds)
- reset_environment(robot, events, cfg.reset_time_s, cfg.fps)
-
- if events["rerecord_episode"]:
- log_say("Re-record episode", cfg.play_sounds)
- events["rerecord_episode"] = False
- events["exit_early"] = False
- dataset.clear_episode_buffer()
- continue
-
- dataset.save_episode()
- recorded_episodes += 1
-
- if events["stop_recording"]:
- break
-
- log_say("Stop recording", cfg.play_sounds, blocking=True)
- stop_recording(robot, listener, cfg.display_data)
-
- if cfg.push_to_hub:
- dataset.push_to_hub(tags=cfg.tags, private=cfg.private)
-
- log_say("Exiting", cfg.play_sounds)
- return dataset
-
-
-@safe_disconnect
-def replay(
- robot: Robot,
- cfg: ReplayControlConfig,
-):
- # TODO(rcadene, aliberts): refactor with control_loop, once `dataset` is an instance of LeRobotDataset
- # TODO(rcadene): Add option to record logs
-
- dataset = LeRobotDataset(cfg.repo_id, root=cfg.root, episodes=[cfg.episode])
- actions = dataset.hf_dataset.select_columns("action")
-
- if not robot.is_connected:
- robot.connect()
-
- log_say("Replaying episode", cfg.play_sounds, blocking=True)
- for idx in range(dataset.num_frames):
- start_episode_t = time.perf_counter()
-
- action = actions[idx]["action"]
- robot.send_action(action)
-
- dt_s = time.perf_counter() - start_episode_t
- busy_wait(1 / cfg.fps - dt_s)
-
- dt_s = time.perf_counter() - start_episode_t
- log_control_info(robot, dt_s, fps=cfg.fps)
-
-
-def _init_rerun(control_config: ControlConfig, session_name: str = "lerobot_control_loop") -> None:
- """Initializes the Rerun SDK for visualizing the control loop.
-
- Args:
- control_config: Configuration determining data display and robot type.
- session_name: Rerun session name. Defaults to "lerobot_control_loop".
-
- Raises:
- ValueError: If viewer IP is missing for non-remote configurations with display enabled.
- """
- if (control_config.display_data and not is_headless()) or (
- control_config.display_data and isinstance(control_config, RemoteRobotConfig)
- ):
- # Configure Rerun flush batch size default to 8KB if not set
- batch_size = os.getenv("RERUN_FLUSH_NUM_BYTES", "8000")
- os.environ["RERUN_FLUSH_NUM_BYTES"] = batch_size
-
- # Initialize Rerun based on configuration
- rr.init(session_name)
- if isinstance(control_config, RemoteRobotConfig):
- viewer_ip = control_config.viewer_ip
- viewer_port = control_config.viewer_port
- if not viewer_ip or not viewer_port:
- raise ValueError(
- "Viewer IP & Port are required for remote config. Set via config file/CLI or disable control_config.display_data."
- )
- logging.info(f"Connecting to viewer at {viewer_ip}:{viewer_port}")
- rr.connect_tcp(f"{viewer_ip}:{viewer_port}")
- else:
- # Get memory limit for rerun viewer parameters
- memory_limit = os.getenv("LEROBOT_RERUN_MEMORY_LIMIT", "10%")
- rr.spawn(memory_limit=memory_limit)
-
-
-@parser.wrap()
-def control_robot(cfg: ControlPipelineConfig):
- init_logging()
- logging.info(pformat(asdict(cfg)))
-
- robot = make_robot_from_config(cfg.robot)
-
- # TODO(Steven): Blueprint for fixed window size
-
- if isinstance(cfg.control, CalibrateControlConfig):
- calibrate(robot, cfg.control)
- elif isinstance(cfg.control, TeleoperateControlConfig):
- _init_rerun(control_config=cfg.control, session_name="lerobot_control_loop_teleop")
- teleoperate(robot, cfg.control)
- elif isinstance(cfg.control, RecordControlConfig):
- _init_rerun(control_config=cfg.control, session_name="lerobot_control_loop_record")
- record(robot, cfg.control)
- elif isinstance(cfg.control, ReplayControlConfig):
- replay(robot, cfg.control)
- elif isinstance(cfg.control, RemoteRobotConfig):
- from lerobot.common.robots.lekiwi.lekiwi_remote import run_lekiwi
-
- _init_rerun(control_config=cfg.control, session_name="lerobot_control_loop_remote")
- run_lekiwi(cfg.robot)
-
- if robot.is_connected:
- # Disconnect manually to avoid a "Core dump" during process
- # termination due to camera threads not properly exiting.
- robot.disconnect()
-
-
-if __name__ == "__main__":
- control_robot()
diff --git a/lerobot/scripts/control_sim_robot.py b/lerobot/scripts/control_sim_robot.py
deleted file mode 100644
index b0dc96304..000000000
--- a/lerobot/scripts/control_sim_robot.py
+++ /dev/null
@@ -1,561 +0,0 @@
-# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-"""
-Utilities to control a robot in simulation.
-
-Useful to record a dataset, replay a recorded episode and record an evaluation dataset.
-
-Examples of usage:
-
-
-- Unlimited teleoperation at a limited frequency of 30 Hz, to simulate data recording frequency.
- You can modify this value depending on how fast your simulation can run:
-```bash
-python lerobot/scripts/control_robot.py teleoperate \
- --fps 30 \
- --robot-path lerobot/configs/robot/your_robot_config.yaml \
- --sim-config lerobot/configs/env/your_sim_config.yaml
-```
-
-- Record one episode in order to test replay:
-```bash
-python lerobot/scripts/control_sim_robot.py record \
- --robot-path lerobot/configs/robot/your_robot_config.yaml \
- --sim-config lerobot/configs/env/your_sim_config.yaml \
- --fps 30 \
- --repo-id $USER/robot_sim_test \
- --num-episodes 1 \
- --run-compute-stats 0
-```
-
-Enable the --push-to-hub 1 to push the recorded dataset to the huggingface hub.
-
-- Visualize dataset:
-```bash
-python lerobot/scripts/visualize_dataset.py \
- --repo-id $USER/robot_sim_test \
- --episode-index 0
-```
-
-- Replay a sequence of test episodes:
-```bash
-python lerobot/scripts/control_sim_robot.py replay \
- --robot-path lerobot/configs/robot/your_robot_config.yaml \
- --sim-config lerobot/configs/env/your_sim_config.yaml \
- --fps 30 \
- --repo-id $USER/robot_sim_test \
- --episode 0
-```
-Note: The seed is saved, therefore, during replay we can load the same environment state as the one during collection.
-
-- Record a full dataset in order to train a policy,
-30 seconds of recording for each episode, and 10 seconds to reset the environment in between episodes:
-```bash
-python lerobot/scripts/control_sim_robot.py record \
- --robot-path lerobot/configs/robot/your_robot_config.yaml \
- --sim-config lerobot/configs/env/your_sim_config.yaml \
- --fps 30 \
- --repo-id $USER/robot_sim_test \
- --num-episodes 50 \
- --episode-time-s 30 \
-```
-
-**NOTE**: You can use your keyboard to control data recording flow.
-- Tap right arrow key '->' to early exit while recording an episode and go to resetting the environment.
-- Tap right arrow key '->' to early exit while resetting the environment and got to recording the next episode.
-- Tap left arrow key '<-' to early exit and re-record the current episode.
-- Tap escape key 'esc' to stop the data recording.
-This might require a sudo permission to allow your terminal to monitor keyboard events.
-
-**NOTE**: You can resume/continue data recording by running the same data recording command twice.
-"""
-
-import argparse
-import importlib
-import logging
-import time
-from pathlib import Path
-
-import cv2
-import gymnasium as gym
-import numpy as np
-import torch
-
-from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
-from lerobot.common.robots.utils import Robot, make_robot
-from lerobot.common.utils.control_utils import (
- init_keyboard_listener,
- init_policy,
- is_headless,
- log_control_info,
- predict_action,
- sanity_check_dataset_name,
- sanity_check_dataset_robot_compatibility,
- stop_recording,
-)
-from lerobot.common.utils.robot_utils import busy_wait
-from lerobot.common.utils.utils import init_hydra_config, init_logging, log_say
-
-raise NotImplementedError("This script is currently deactivated")
-
-DEFAULT_FEATURES = {
- "next.reward": {
- "dtype": "float32",
- "shape": (1,),
- "names": None,
- },
- "next.success": {
- "dtype": "bool",
- "shape": (1,),
- "names": None,
- },
- "seed": {
- "dtype": "int64",
- "shape": (1,),
- "names": None,
- },
- "timestamp": {
- "dtype": "float32",
- "shape": (1,),
- "names": None,
- },
-}
-
-
-########################################################################################
-# Utilities
-########################################################################################
-def none_or_int(value):
- if value == "None":
- return None
- return int(value)
-
-
-def init_sim_calibration(robot, cfg):
- # Constants necessary for transforming the joint pos of the real robot to the sim
- # depending on the robot description used in that sim.
- start_pos = np.array(robot.leader_arms.main.calibration["start_pos"])
- axis_directions = np.array(cfg.get("axis_directions", [1]))
- offsets = np.array(cfg.get("offsets", [0])) * np.pi
-
- return {"start_pos": start_pos, "axis_directions": axis_directions, "offsets": offsets}
-
-
-def real_positions_to_sim(real_positions, axis_directions, start_pos, offsets):
- """Counts - starting position -> radians -> align axes -> offset"""
- return axis_directions * (real_positions - start_pos) * 2.0 * np.pi / 4096 + offsets
-
-
-########################################################################################
-# Control modes
-########################################################################################
-
-
-def teleoperate(env, robot: Robot, process_action_fn, teleop_time_s=None):
- env = env()
- env.reset()
- start_teleop_t = time.perf_counter()
- while True:
- leader_pos = robot.leader_arms.main.read("Present_Position")
- action = process_action_fn(leader_pos)
- env.step(np.expand_dims(action, 0))
- if teleop_time_s is not None and time.perf_counter() - start_teleop_t > teleop_time_s:
- print("Teleoperation processes finished.")
- break
-
-
-def record(
- env,
- robot: Robot,
- process_action_from_leader,
- root: Path,
- repo_id: str,
- task: str,
- fps: int | None = None,
- tags: list[str] | None = None,
- pretrained_policy_name_or_path: str = None,
- policy_overrides: bool | None = None,
- episode_time_s: int = 30,
- num_episodes: int = 50,
- video: bool = True,
- push_to_hub: bool = True,
- num_image_writer_processes: int = 0,
- num_image_writer_threads_per_camera: int = 4,
- display_cameras: bool = False,
- play_sounds: bool = True,
- resume: bool = False,
- local_files_only: bool = False,
- run_compute_stats: bool = True,
-) -> LeRobotDataset:
- # Load pretrained policy
- policy = None
- if pretrained_policy_name_or_path is not None:
- policy, policy_fps, device, use_amp = init_policy(pretrained_policy_name_or_path, policy_overrides)
-
- if fps is None:
- fps = policy_fps
- logging.warning(f"No fps provided, so using the fps from policy config ({policy_fps}).")
-
- if policy is None and process_action_from_leader is None:
- raise ValueError("Either policy or process_action_fn has to be set to enable control in sim.")
-
- # initialize listener before sim env
- listener, events = init_keyboard_listener()
-
- # create sim env
- env = env()
-
- # Create empty dataset or load existing saved episodes
- num_cameras = sum([1 if "image" in key else 0 for key in env.observation_space])
-
- # get image keys
- image_keys = [key for key in env.observation_space if "image" in key]
- state_keys_dict = env_cfg.state_keys
-
- if resume:
- dataset = LeRobotDataset(
- repo_id,
- root=root,
- local_files_only=local_files_only,
- )
- dataset.start_image_writer(
- num_processes=num_image_writer_processes,
- num_threads=num_image_writer_threads_per_camera * num_cameras,
- )
- sanity_check_dataset_robot_compatibility(dataset, robot, fps, video)
- else:
- features = DEFAULT_FEATURES
- # add image keys to features
- for key in image_keys:
- shape = env.observation_space[key].shape
- if not key.startswith("observation.image."):
- key = "observation.image." + key
- features[key] = {"dtype": "video", "names": ["channels", "height", "width"], "shape": shape}
-
- for key, obs_key in state_keys_dict.items():
- features[key] = {
- "dtype": "float32",
- "names": None,
- "shape": env.observation_space[obs_key].shape,
- }
-
- features["action"] = {"dtype": "float32", "shape": env.action_space.shape, "names": None}
-
- # Create empty dataset or load existing saved episodes
- sanity_check_dataset_name(repo_id, policy)
- dataset = LeRobotDataset.create(
- repo_id,
- fps,
- root=root,
- features=features,
- use_videos=video,
- image_writer_processes=num_image_writer_processes,
- image_writer_threads=num_image_writer_threads_per_camera * num_cameras,
- )
-
- recorded_episodes = 0
- while True:
- log_say(f"Recording episode {dataset.num_episodes}", play_sounds)
-
- if events is None:
- events = {"exit_early": False}
-
- if episode_time_s is None:
- episode_time_s = float("inf")
-
- timestamp = 0
- start_episode_t = time.perf_counter()
-
- seed = np.random.randint(0, 1e5)
- observation, info = env.reset(seed=seed)
-
- while timestamp < episode_time_s:
- start_loop_t = time.perf_counter()
-
- if policy is not None:
- action = predict_action(observation, policy, device, use_amp)
- else:
- leader_pos = robot.leader_arms.main.read("Present_Position")
- action = process_action_from_leader(leader_pos)
-
- observation, reward, terminated, _, info = env.step(action)
-
- success = info.get("is_success", False)
- env_timestamp = info.get("timestamp", dataset.episode_buffer["size"] / fps)
-
- frame = {
- "action": torch.from_numpy(action),
- "next.reward": reward,
- "next.success": success,
- "seed": seed,
- "timestamp": env_timestamp,
- }
-
- for key in image_keys:
- if not key.startswith("observation.image"):
- frame["observation.image." + key] = observation[key]
- else:
- frame[key] = observation[key]
-
- for key, obs_key in state_keys_dict.items():
- frame[key] = torch.from_numpy(observation[obs_key])
-
- dataset.add_frame(frame)
-
- if display_cameras and not is_headless():
- for key in image_keys:
- cv2.imshow(key, cv2.cvtColor(observation[key], cv2.COLOR_RGB2BGR))
- cv2.waitKey(1)
-
- if fps is not None:
- dt_s = time.perf_counter() - start_loop_t
- busy_wait(1 / fps - dt_s)
-
- dt_s = time.perf_counter() - start_loop_t
- log_control_info(robot, dt_s, fps=fps)
-
- timestamp = time.perf_counter() - start_episode_t
- if events["exit_early"] or terminated:
- events["exit_early"] = False
- break
-
- if events["rerecord_episode"]:
- log_say("Re-record episode", play_sounds)
- events["rerecord_episode"] = False
- events["exit_early"] = False
- dataset.clear_episode_buffer()
- continue
-
- dataset.save_episode(task=task)
- recorded_episodes += 1
-
- if events["stop_recording"] or recorded_episodes >= num_episodes:
- break
- else:
- logging.info("Waiting for a few seconds before starting next episode recording...")
- busy_wait(3)
-
- log_say("Stop recording", play_sounds, blocking=True)
- stop_recording(robot, listener, display_cameras)
-
- if run_compute_stats:
- logging.info("Computing dataset statistics")
- dataset.consolidate(run_compute_stats)
-
- if push_to_hub:
- dataset.push_to_hub(tags=tags)
-
- log_say("Exiting", play_sounds)
- return dataset
-
-
-def replay(
- env, root: Path, repo_id: str, episode: int, fps: int | None = None, local_files_only: bool = True
-):
- env = env()
-
- local_dir = Path(root) / repo_id
- if not local_dir.exists():
- raise ValueError(local_dir)
-
- dataset = LeRobotDataset(repo_id, root=root, local_files_only=local_files_only)
- items = dataset.hf_dataset.select_columns("action")
- seeds = dataset.hf_dataset.select_columns("seed")["seed"]
-
- from_idx = dataset.episode_data_index["from"][episode].item()
- to_idx = dataset.episode_data_index["to"][episode].item()
- env.reset(seed=seeds[from_idx].item())
- logging.info("Replaying episode")
- log_say("Replaying episode", play_sounds=True)
- for idx in range(from_idx, to_idx):
- start_episode_t = time.perf_counter()
- action = items[idx]["action"]
- env.step(action.unsqueeze(0).numpy())
- dt_s = time.perf_counter() - start_episode_t
- busy_wait(1 / fps - dt_s)
-
-
-if __name__ == "__main__":
- parser = argparse.ArgumentParser()
- subparsers = parser.add_subparsers(dest="mode", required=True)
-
- # Set common options for all the subparsers
- base_parser = argparse.ArgumentParser(add_help=False)
- base_parser.add_argument(
- "--robot-path",
- type=str,
- default="lerobot/configs/robot/koch.yaml",
- help="Path to robot yaml file used to instantiate the robot using `make_robot` factory function.",
- )
-
- base_parser.add_argument(
- "--sim-config",
- help="Path to a yaml config you want to use for initializing a sim environment based on gym ",
- )
-
- parser_record = subparsers.add_parser("teleoperate", parents=[base_parser])
-
- parser_record = subparsers.add_parser("record", parents=[base_parser])
- parser_record.add_argument(
- "--fps", type=none_or_int, default=None, help="Frames per second (set to None to disable)"
- )
- parser_record.add_argument(
- "--root",
- type=Path,
- default=None,
- help="Root directory where the dataset will be stored locally at '{root}/{repo_id}' (e.g. 'data/hf_username/dataset_name').",
- )
- parser_record.add_argument(
- "--repo-id",
- type=str,
- default="lerobot/test",
- help="Dataset identifier. By convention it should match '{hf_username}/{dataset_name}' (e.g. `lerobot/test`).",
- )
- parser_record.add_argument(
- "--episode-time-s",
- type=int,
- default=60,
- help="Number of seconds for data recording for each episode.",
- )
- parser_record.add_argument(
- "--task",
- type=str,
- required=True,
- help="A description of the task preformed during recording that can be used as a language instruction.",
- )
- parser_record.add_argument("--num-episodes", type=int, default=50, help="Number of episodes to record.")
- parser_record.add_argument(
- "--run-compute-stats",
- type=int,
- default=1,
- help="By default, run the computation of the data statistics at the end of data collection. Compute intensive and not required to just replay an episode.",
- )
- parser_record.add_argument(
- "--push-to-hub",
- type=int,
- default=1,
- help="Upload dataset to Hugging Face hub.",
- )
- parser_record.add_argument(
- "--tags",
- type=str,
- nargs="*",
- help="Add tags to your dataset on the hub.",
- )
- parser_record.add_argument(
- "--num-image-writer-processes",
- type=int,
- default=0,
- help=(
- "Number of subprocesses handling the saving of frames as PNG. Set to 0 to use threads only; "
- "set to ≥1 to use subprocesses, each using threads to write images. The best number of processes "
- "and threads depends on your system. We recommend 4 threads per camera with 0 processes. "
- "If fps is unstable, adjust the thread count. If still unstable, try using 1 or more subprocesses."
- ),
- )
- parser_record.add_argument(
- "--num-image-writer-threads-per-camera",
- type=int,
- default=4,
- help=(
- "Number of threads writing the frames as png images on disk, per camera. "
- "Too much threads might cause unstable teleoperation fps due to main thread being blocked. "
- "Not enough threads might cause low camera fps."
- ),
- )
- parser_record.add_argument(
- "--display-cameras",
- type=int,
- default=0,
- help="Visualize image observations with opencv.",
- )
- parser_record.add_argument(
- "--resume",
- type=int,
- default=0,
- help="Resume recording on an existing dataset.",
- )
- parser_replay = subparsers.add_parser("replay", parents=[base_parser])
- parser_replay.add_argument(
- "--fps", type=none_or_int, default=None, help="Frames per second (set to None to disable)"
- )
- parser_replay.add_argument(
- "--root",
- type=Path,
- default=None,
- help="Root directory where the dataset will be stored locally (e.g. 'data/hf_username/dataset_name'). By default, stored in cache folder.",
- )
- parser_replay.add_argument(
- "--repo-id",
- type=str,
- default="lerobot/test",
- help="Dataset identifier. By convention it should match '{hf_username}/{dataset_name}' (e.g. `lerobot/test`).",
- )
- parser_replay.add_argument("--episode", type=int, default=0, help="Index of the episodes to replay.")
-
- args = parser.parse_args()
-
- init_logging()
-
- control_mode = args.mode
- robot_path = args.robot_path
- env_config_path = args.sim_config
- kwargs = vars(args)
- del kwargs["mode"]
- del kwargs["robot_path"]
- del kwargs["sim_config"]
-
- # make gym env
- env_cfg = init_hydra_config(env_config_path)
- importlib.import_module(f"gym_{env_cfg.env.type}")
-
- def env_constructor():
- return gym.make(env_cfg.env.handle, disable_env_checker=True, **env_cfg.env.gym)
-
- robot = None
- process_leader_actions_fn = None
-
- if control_mode in ["teleoperate", "record"]:
- # make robot
- robot_overrides = ["~cameras", "~follower_arms"]
- # TODO(rcadene): remove
- robot_cfg = init_hydra_config(robot_path, robot_overrides)
- robot = make_robot(robot_cfg)
- robot.connect()
-
- calib_kwgs = init_sim_calibration(robot, env_cfg.calibration)
-
- def process_leader_actions_fn(action):
- return real_positions_to_sim(action, **calib_kwgs)
-
- robot.leader_arms.main.calibration = None
-
- if control_mode == "teleoperate":
- teleoperate(env_constructor, robot, process_leader_actions_fn)
-
- elif control_mode == "record":
- record(env_constructor, robot, process_leader_actions_fn, **kwargs)
-
- elif control_mode == "replay":
- replay(env_constructor, **kwargs)
-
- else:
- raise ValueError(
- f"Invalid control mode: '{control_mode}', only valid modes are teleoperate, record and replay."
- )
-
- if robot and robot.is_connected:
- # Disconnect manually to avoid a "Core dump" during process
- # termination due to camera threads not properly exiting.
- robot.disconnect()
diff --git a/lerobot/scripts/eval.py b/lerobot/scripts/eval.py
index 9790f8b31..58275f666 100644
--- a/lerobot/scripts/eval.py
+++ b/lerobot/scripts/eval.py
@@ -94,8 +94,8 @@ def rollout(
data will probably need to be discarded (for environments that aren't the first one to be done).
The return dictionary contains:
- (optional) "observation": A a dictionary of (batch, sequence + 1, *) tensors mapped to observation
- keys. NOTE the that this has an extra sequence element relative to the other keys in the
+ (optional) "observation": A dictionary of (batch, sequence + 1, *) tensors mapped to observation
+ keys. NOTE that this has an extra sequence element relative to the other keys in the
dictionary. This is because an extra observation is included for after the environment is
terminated or truncated.
"action": A (batch, sequence, action_dim) tensor of actions applied based on the observations (not
diff --git a/lerobot/scripts/visualize_dataset_html.py b/lerobot/scripts/visualize_dataset_html.py
index 0fc21a8f1..d0c8f1ace 100644
--- a/lerobot/scripts/visualize_dataset_html.py
+++ b/lerobot/scripts/visualize_dataset_html.py
@@ -174,7 +174,10 @@ def run_server(
dataset.meta.get_video_file_path(episode_id, key) for key in dataset.meta.video_keys
]
videos_info = [
- {"url": url_for("static", filename=video_path), "filename": video_path.parent.name}
+ {
+ "url": url_for("static", filename=str(video_path).replace("\\", "/")),
+ "filename": video_path.parent.name,
+ }
for video_path in video_paths
]
tasks = dataset.meta.episodes[episode_id]["tasks"]
@@ -381,7 +384,7 @@ def visualize_dataset_html(
if isinstance(dataset, LeRobotDataset):
ln_videos_dir = static_dir / "videos"
if not ln_videos_dir.exists():
- ln_videos_dir.symlink_to((dataset.root / "videos").resolve())
+ ln_videos_dir.symlink_to((dataset.root / "videos").resolve().as_posix())
if serve:
run_server(dataset, episodes, host, port, static_dir, template_dir)
diff --git a/lerobot/setup_motors.py b/lerobot/setup_motors.py
new file mode 100644
index 000000000..d85e2810a
--- /dev/null
+++ b/lerobot/setup_motors.py
@@ -0,0 +1,66 @@
+# 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.
+
+"""
+Helper to set motor ids and baudrate.
+
+Example:
+
+```shell
+python -m lerobot.setup_motors \
+ --device.type=so100_leader \
+ --device.port=/dev/tty.usbmodem575E0031751
+```
+"""
+
+from dataclasses import dataclass
+
+import draccus
+
+from .common.robots import RobotConfig, koch_follower, make_robot_from_config, so100_follower # noqa: F401
+from .common.teleoperators import ( # noqa: F401
+ TeleoperatorConfig,
+ koch_leader,
+ make_teleoperator_from_config,
+ so100_leader,
+)
+
+COMPATIBLE_DEVICES = [
+ "koch_follower",
+ "koch_leader",
+ "so100_follower",
+ "so100_leader",
+]
+
+
+@dataclass
+class SetupConfig:
+ device: RobotConfig | TeleoperatorConfig
+
+
+@draccus.wrap()
+def setup_motors(cfg: SetupConfig):
+ if cfg.device.type not in COMPATIBLE_DEVICES:
+ raise NotImplementedError
+
+ if isinstance(cfg.device, RobotConfig):
+ device = make_robot_from_config(cfg.device)
+ else:
+ device = make_teleoperator_from_config(cfg.device)
+
+ device.setup_motors()
+
+
+if __name__ == "__main__":
+ setup_motors()
diff --git a/lerobot/teleoperate.py b/lerobot/teleoperate.py
new file mode 100644
index 000000000..f14f9a796
--- /dev/null
+++ b/lerobot/teleoperate.py
@@ -0,0 +1,130 @@
+# 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.
+
+"""
+Simple script to control a robot from teleoperation.
+
+Example:
+
+```shell
+python -m lerobot.teleoperate \
+ --robot.type=so100_follower \
+ --robot.port=/dev/tty.usbmodem58760431541 \
+ --robot.cameras="{laptop: {type: opencv, camera_index: 0}}" \
+ --robot.id=black \
+ --teleop.type=so100_leader \
+ --teleop.port=/dev/tty.usbmodem58760431551 \
+ --teleop.id=blue
+```
+"""
+
+import logging
+import time
+from dataclasses import asdict, dataclass
+from pprint import pformat
+
+import draccus
+import numpy as np
+import rerun as rr
+
+from lerobot.common.cameras import intel, opencv # noqa: F401
+from lerobot.common.robots import ( # noqa: F401
+ Robot,
+ RobotConfig,
+ koch_follower,
+ make_robot_from_config,
+ so100_follower,
+)
+from lerobot.common.teleoperators import (
+ Teleoperator,
+ TeleoperatorConfig,
+ make_teleoperator_from_config,
+)
+from lerobot.common.utils.utils import init_logging, move_cursor_up
+from lerobot.common.utils.visualization_utils import _init_rerun
+
+from .common.teleoperators import koch_leader, so100_leader # noqa: F401
+
+
+@dataclass
+class TeleoperateConfig:
+ teleop: TeleoperatorConfig
+ robot: RobotConfig
+ # Limit the maximum frames per second. By default, no limit.
+ fps: int | None = None
+ teleop_time_s: float | None = None
+ # Display all cameras on screen
+ display_data: bool = False
+
+
+def teleop_loop(
+ teleop: Teleoperator, robot: Robot, display_data: bool = False, duration: float | None = None
+):
+ display_len = max(len(key) for key in robot.action_features)
+ start = time.perf_counter()
+ while True:
+ loop_start = time.perf_counter()
+ action = teleop.get_action()
+ if display_data:
+ observation = robot.get_observation()
+ for obs, val in observation.items():
+ if isinstance(val, float):
+ rr.log(f"observation_{obs}", rr.Scalar(val))
+ elif isinstance(val, np.ndarray):
+ rr.log(f"observation_{obs}", rr.Image(val), static=True)
+ for act, val in action.items():
+ if isinstance(val, float):
+ rr.log(f"action_{act}", rr.Scalar(val))
+
+ robot.send_action(action)
+ loop_s = time.perf_counter() - loop_start
+
+ print("\n" + "-" * (display_len + 10))
+ print(f"{'NAME':<{display_len}} | {'NORM':>7}")
+ for motor, value in action.items():
+ print(f"{motor:<{display_len}} | {value:>7.2f}")
+ print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
+
+ if duration is not None and time.perf_counter() - start >= duration:
+ return
+
+ move_cursor_up(len(action) + 5)
+
+
+@draccus.wrap()
+def teleoperate(cfg: TeleoperateConfig):
+ init_logging()
+ logging.info(pformat(asdict(cfg)))
+ if cfg.display_data:
+ _init_rerun(session_name="teleoperation")
+
+ teleop = make_teleoperator_from_config(cfg.teleop)
+ robot = make_robot_from_config(cfg.robot)
+
+ teleop.connect()
+ robot.connect()
+
+ try:
+ teleop_loop(teleop, robot, display_data=cfg.display_data, duration=cfg.teleop_time_s)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ if cfg.display_data:
+ rr.rerun_shutdown()
+ teleop.disconnect()
+ robot.disconnect()
+
+
+if __name__ == "__main__":
+ teleoperate()
diff --git a/media/so101/follower_middle.webp b/media/so101/follower_middle.webp
new file mode 100644
index 000000000..7f22d2563
Binary files /dev/null and b/media/so101/follower_middle.webp differ
diff --git a/media/so101/follower_rest.webp b/media/so101/follower_rest.webp
new file mode 100644
index 000000000..4770b9ffb
Binary files /dev/null and b/media/so101/follower_rest.webp differ
diff --git a/media/so101/follower_rotated.webp b/media/so101/follower_rotated.webp
new file mode 100644
index 000000000..3ca3c98fd
Binary files /dev/null and b/media/so101/follower_rotated.webp differ
diff --git a/media/so101/follower_zero.webp b/media/so101/follower_zero.webp
new file mode 100644
index 000000000..7de5037a3
Binary files /dev/null and b/media/so101/follower_zero.webp differ
diff --git a/media/so101/leader_middle.webp b/media/so101/leader_middle.webp
new file mode 100644
index 000000000..502318b31
Binary files /dev/null and b/media/so101/leader_middle.webp differ
diff --git a/media/so101/leader_rest.webp b/media/so101/leader_rest.webp
new file mode 100644
index 000000000..8b996c66f
Binary files /dev/null and b/media/so101/leader_rest.webp differ
diff --git a/media/so101/leader_rotated.webp b/media/so101/leader_rotated.webp
new file mode 100644
index 000000000..324b5923c
Binary files /dev/null and b/media/so101/leader_rotated.webp differ
diff --git a/media/so101/leader_zero.webp b/media/so101/leader_zero.webp
new file mode 100644
index 000000000..218b43e46
Binary files /dev/null and b/media/so101/leader_zero.webp differ
diff --git a/media/so101/so101-leader.webp b/media/so101/so101-leader.webp
new file mode 100644
index 000000000..22ff3a4bc
Binary files /dev/null and b/media/so101/so101-leader.webp differ
diff --git a/media/so101/so101.webp b/media/so101/so101.webp
new file mode 100644
index 000000000..ce65e94bc
Binary files /dev/null and b/media/so101/so101.webp differ
diff --git a/pyproject.toml b/pyproject.toml
index 9a9ce7cb0..fbca62cce 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -49,7 +49,7 @@ dependencies = [
"datasets>=2.19.0",
"deepdiff>=7.0.1",
"diffusers>=0.27.2",
- "draccus>=0.10.0",
+ "draccus==0.10.0",
"einops>=0.8.0",
"flask>=3.0.3",
"gdown>=5.1.0",
@@ -62,14 +62,14 @@ dependencies = [
"omegaconf>=2.3.0",
"opencv-python-headless>=4.9.0",
"packaging>=24.2",
- "av>=12.0.5",
+ "av>=14.2.0",
"pymunk>=6.6.0",
"pynput>=1.7.7",
"pyzmq>=26.2.1",
"rerun-sdk>=0.21.0",
"termcolor>=2.4.0",
- "torch>=2.2.1",
- "torchcodec>=0.2.1; sys_platform != 'win32' and (sys_platform != 'linux' or (platform_machine != 'aarch64' and platform_machine != 'arm64' and platform_machine != 'armv7l')) and (sys_platform != 'darwin' or platform_machine != 'x86_64')",
+ "torch>=2.2.1,<2.7",
+ "torchcodec==0.2.1; sys_platform != 'win32' and (sys_platform != 'linux' or (platform_machine != 'aarch64' and platform_machine != 'arm64' and platform_machine != 'armv7l')) and (sys_platform != 'darwin' or platform_machine != 'x86_64')",
"torchvision>=0.21.0",
"wandb>=0.16.3",
"zarr>=2.17.0",
@@ -77,6 +77,7 @@ dependencies = [
[project.optional-dependencies]
aloha = ["gym-aloha>=0.1.1 ; python_version < '4.0'"]
+docs = ["hf-doc-builder @ git+https://github.com/huggingface/doc-builder.git@main", "watchdog >= 6.0.0"]
dev = ["pre-commit>=3.7.0", "debugpy>=1.8.1"]
dora = [
"gym-dora @ git+https://github.com/dora-rs/dora-lerobot.git#subdirectory=gym_dora ; python_version < '4.0'",
diff --git a/tests/conftest.py b/tests/conftest.py
index 4c8237098..23c5569b2 100644
--- a/tests/conftest.py
+++ b/tests/conftest.py
@@ -19,9 +19,8 @@ import traceback
import pytest
from serial import SerialException
-from lerobot import available_cameras, available_motors, available_robots
-from lerobot.common.robots.utils import make_robot
-from tests.utils import DEVICE, make_camera, make_motors_bus
+from lerobot import available_cameras
+from tests.utils import DEVICE, make_camera
# Import fixture modules as plugins
pytest_plugins = [
@@ -64,21 +63,11 @@ def _check_component_availability(component_type, available_components, make_com
return False
-@pytest.fixture
-def is_robot_available(robot_type):
- return _check_component_availability(robot_type, available_robots, make_robot)
-
-
@pytest.fixture
def is_camera_available(camera_type):
return _check_component_availability(camera_type, available_cameras, make_camera)
-@pytest.fixture
-def is_motor_available(motor_type):
- return _check_component_availability(motor_type, available_motors, make_motors_bus)
-
-
@pytest.fixture
def patch_builtins_input(monkeypatch):
def print_text(text=None):
diff --git a/tests/datasets/test_datasets.py b/tests/datasets/test_datasets.py
index 2b0fc5b12..55a417c30 100644
--- a/tests/datasets/test_datasets.py
+++ b/tests/datasets/test_datasets.py
@@ -41,7 +41,6 @@ from lerobot.common.datasets.utils import (
)
from lerobot.common.envs.factory import make_env_config
from lerobot.common.policies.factory import make_policy_config
-from lerobot.common.robots.utils import make_robot
from lerobot.configs.default import DatasetConfig
from lerobot.configs.train import TrainPipelineConfig
from tests.fixtures.constants import DUMMY_CHW, DUMMY_HWC, DUMMY_REPO_ID
@@ -70,9 +69,9 @@ def test_same_attributes_defined(tmp_path, lerobot_dataset_factory):
objects have the same sets of attributes defined.
"""
# Instantiate both ways
- robot = make_robot("koch", mock=True)
+ features = {"state": {"dtype": "float32", "shape": (1,), "names": None}}
root_create = tmp_path / "create"
- dataset_create = LeRobotDataset.create(repo_id=DUMMY_REPO_ID, fps=30, robot=robot, root=root_create)
+ dataset_create = LeRobotDataset.create(repo_id=DUMMY_REPO_ID, fps=30, features=features, root=root_create)
root_init = tmp_path / "init"
dataset_init = lerobot_dataset_factory(root=root_init)
@@ -100,22 +99,13 @@ def test_dataset_initialization(tmp_path, lerobot_dataset_factory):
assert dataset.num_frames == len(dataset)
-def test_add_frame_missing_task(tmp_path, empty_lerobot_dataset_factory):
- features = {"state": {"dtype": "float32", "shape": (1,), "names": None}}
- dataset = empty_lerobot_dataset_factory(root=tmp_path / "test", features=features)
- with pytest.raises(
- ValueError, match="Feature mismatch in `frame` dictionary:\nMissing features: {'task'}\n"
- ):
- dataset.add_frame({"state": torch.randn(1)})
-
-
def test_add_frame_missing_feature(tmp_path, empty_lerobot_dataset_factory):
features = {"state": {"dtype": "float32", "shape": (1,), "names": None}}
dataset = empty_lerobot_dataset_factory(root=tmp_path / "test", features=features)
with pytest.raises(
ValueError, match="Feature mismatch in `frame` dictionary:\nMissing features: {'state'}\n"
):
- dataset.add_frame({"task": "Dummy task"})
+ dataset.add_frame({"wrong_feature": torch.randn(1)}, task="Dummy task")
def test_add_frame_extra_feature(tmp_path, empty_lerobot_dataset_factory):
@@ -124,7 +114,7 @@ def test_add_frame_extra_feature(tmp_path, empty_lerobot_dataset_factory):
with pytest.raises(
ValueError, match="Feature mismatch in `frame` dictionary:\nExtra features: {'extra'}\n"
):
- dataset.add_frame({"state": torch.randn(1), "task": "Dummy task", "extra": "dummy_extra"})
+ dataset.add_frame({"state": torch.randn(1), "extra": "dummy_extra"}, task="Dummy task")
def test_add_frame_wrong_type(tmp_path, empty_lerobot_dataset_factory):
@@ -133,7 +123,7 @@ def test_add_frame_wrong_type(tmp_path, empty_lerobot_dataset_factory):
with pytest.raises(
ValueError, match="The feature 'state' of dtype 'float16' is not of the expected dtype 'float32'.\n"
):
- dataset.add_frame({"state": torch.randn(1, dtype=torch.float16), "task": "Dummy task"})
+ dataset.add_frame({"state": torch.randn(1, dtype=torch.float16)}, task="Dummy task")
def test_add_frame_wrong_shape(tmp_path, empty_lerobot_dataset_factory):
@@ -143,7 +133,7 @@ def test_add_frame_wrong_shape(tmp_path, empty_lerobot_dataset_factory):
ValueError,
match=re.escape("The feature 'state' of shape '(1,)' does not have the expected shape '(2,)'.\n"),
):
- dataset.add_frame({"state": torch.randn(1), "task": "Dummy task"})
+ dataset.add_frame({"state": torch.randn(1)}, task="Dummy task")
def test_add_frame_wrong_shape_python_float(tmp_path, empty_lerobot_dataset_factory):
@@ -155,7 +145,7 @@ def test_add_frame_wrong_shape_python_float(tmp_path, empty_lerobot_dataset_fact
"The feature 'state' is not a 'np.ndarray'. Expected type is 'float32', but type '
+