fix(cameras): correct validate_width_height logic

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Simon Alibert <simon.alibert@huggingface.co>
Co-authored-by: Simon Alibert <75076266+aliberts@users.noreply.github.com>

.gitattributes

@@ -11,10 +11,11 @@
 # 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.
-
 *.memmap filter=lfs diff=lfs merge=lfs -text
 *.stl filter=lfs diff=lfs merge=lfs -text
 *.safetensors filter=lfs diff=lfs merge=lfs -text
 *.mp4 filter=lfs diff=lfs merge=lfs -text
 *.arrow filter=lfs diff=lfs merge=lfs -text
 *.json !text !filter !merge !diff
+tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text
+tests/artifacts/cameras/*.bag filter=lfs diff=lfs merge=lfs -text

.github/workflows/build-docker-images.yml

@@ -40,24 +40,24 @@ jobs:
           git lfs install
 
       - name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@b5ca514318bd6ebac0fb2aedd5d36ec1b5c232a2 # v3.10.0
+        uses: docker/setup-buildx-action@v3
         with:
           cache-binary: false
 
       - name: Check out code
-        uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+        uses: actions/checkout@v4
         with:
           lfs: true
           persist-credentials: false
 
       - name: Login to DockerHub
-        uses: docker/login-action@74a5d142397b4f367a81961eba4e8cd7edddf772 # v3.4.0
+        uses: docker/login-action@v3
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_PASSWORD }}
 
       - name: Build and Push CPU
-        uses: docker/build-push-action@ca052bb54ab0790a636c9b5f226502c73d547a25 # v5.4.0
+        uses: docker/build-push-action@v5
         with:
           context: .
           file: ./docker/lerobot-cpu/Dockerfile
@@ -78,24 +78,24 @@ jobs:
           git lfs install
 
       - name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@b5ca514318bd6ebac0fb2aedd5d36ec1b5c232a2 # v3.10.0
+        uses: docker/setup-buildx-action@v3
         with:
           cache-binary: false
 
       - name: Check out code
-        uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+        uses: actions/checkout@v4
         with:
           lfs: true
           persist-credentials: false
 
       - name: Login to DockerHub
-        uses: docker/login-action@74a5d142397b4f367a81961eba4e8cd7edddf772 # v3.4.0
+        uses: docker/login-action@v3
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_PASSWORD }}
 
       - name: Build and Push GPU
-        uses: docker/build-push-action@ca052bb54ab0790a636c9b5f226502c73d547a25 # v5.4.0
+        uses: docker/build-push-action@v5
         with:
           context: .
           file: ./docker/lerobot-gpu/Dockerfile
@@ -110,23 +110,23 @@ jobs:
       group: aws-general-8-plus
     steps:
       - name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@b5ca514318bd6ebac0fb2aedd5d36ec1b5c232a2 # v3.10.0
+        uses: docker/setup-buildx-action@v3
         with:
           cache-binary: false
 
       - name: Check out code
-        uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+        uses: actions/checkout@v4
         with:
           persist-credentials: false
 
       - name: Login to DockerHub
-        uses: docker/login-action@74a5d142397b4f367a81961eba4e8cd7edddf772 # v3.4.0
+        uses: docker/login-action@v3
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_PASSWORD }}
 
       - name: Build and Push GPU dev
-        uses: docker/build-push-action@ca052bb54ab0790a636c9b5f226502c73d547a25 # v5.4.0
+        uses: docker/build-push-action@v5
         with:
           context: .
           file: ./docker/lerobot-gpu-dev/Dockerfile

.github/workflows/nightly-tests.yml

@@ -33,7 +33,7 @@ jobs:
     runs-on:
       group: aws-general-8-plus
     container:
-      image: huggingface/lerobot-cpu:latest  # zizmor: ignore[unpinned-images]
+      image: huggingface/lerobot-cpu:latest
       options: --shm-size "16gb"
       credentials:
         username: ${{ secrets.DOCKERHUB_USERNAME }}
@@ -60,7 +60,7 @@ jobs:
       CUDA_VISIBLE_DEVICES: "0"
       TEST_TYPE: "single_gpu"
     container:
-      image: huggingface/lerobot-gpu:latest  # zizmor: ignore[unpinned-images]
+      image: huggingface/lerobot-gpu:latest
       options: --gpus all --shm-size "16gb"
       credentials:
         username: ${{ secrets.DOCKERHUB_USERNAME }}

.github/workflows/quality.yml

@@ -33,12 +33,12 @@ jobs:
     runs-on: ubuntu-latest
     steps:
       - name: Checkout Repository
-        uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+        uses: actions/checkout@v4
         with:
           persist-credentials: false
 
       - name: Set up Python
-        uses: actions/setup-python@7f4fc3e22c37d6ff65e88745f38bd3157c663f7c # v4.9.1
+        uses: actions/setup-python@v4
         with:
           python-version: ${{ env.PYTHON_VERSION }}
 
@@ -64,9 +64,9 @@ jobs:
     runs-on: ubuntu-latest
     steps:
       - name: Checkout Repository
-        uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+        uses: actions/checkout@v4
         with:
           persist-credentials: false
 
       - name: typos-action
-        uses: crate-ci/typos@db35ee91e80fbb447f33b0e5fbddb24d2a1a884f # v1.29.10
+        uses: crate-ci/typos@v1.29.10

.github/workflows/test-docker-build.yml

@@ -35,7 +35,7 @@ jobs:
       matrix: ${{ steps.set-matrix.outputs.matrix }}
     steps:
       - name: Check out code
-        uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+        uses: actions/checkout@v4
         with:
           persist-credentials: false
 
@@ -64,17 +64,17 @@ jobs:
         docker-file: ${{ fromJson(needs.get_changed_files.outputs.matrix) }}
     steps:
       - name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@b5ca514318bd6ebac0fb2aedd5d36ec1b5c232a2 # v3.10.0
+        uses: docker/setup-buildx-action@v3
         with:
           cache-binary: false
 
       - name: Check out code
-        uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+        uses: actions/checkout@v4
         with:
           persist-credentials: false
 
       - name: Build Docker image
-        uses: docker/build-push-action@ca052bb54ab0790a636c9b5f226502c73d547a25 # v5.4.0
+        uses: docker/build-push-action@v5
         with:
           file: ${{ matrix.docker-file }}
           context: .

.github/workflows/test.yml

@@ -50,7 +50,7 @@ jobs:
     env:
       MUJOCO_GL: egl
     steps:
-      - uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+      - uses: actions/checkout@v4
         with:
           lfs: true  # Ensure LFS files are pulled
           persist-credentials: false
@@ -62,7 +62,7 @@ jobs:
           sudo apt-get install -y libegl1-mesa-dev ffmpeg portaudio19-dev
 
       - name: Install uv and python
-        uses: astral-sh/setup-uv@d4b2f3b6ecc6e67c4457f6d3e41ec42d3d0fcb86 # v5.4.2
+        uses: astral-sh/setup-uv@v5
         with:
           enable-cache: true
           version: ${{ env.UV_VERSION }}
@@ -85,7 +85,7 @@ jobs:
     env:
       MUJOCO_GL: egl
     steps:
-      - uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+      - uses: actions/checkout@v4
         with:
           lfs: true  # Ensure LFS files are pulled
           persist-credentials: false
@@ -94,7 +94,7 @@ jobs:
         run: sudo apt-get update && sudo apt-get install -y ffmpeg
 
       - name: Install uv and python
-        uses: astral-sh/setup-uv@d4b2f3b6ecc6e67c4457f6d3e41ec42d3d0fcb86 # v5.4.2
+        uses: astral-sh/setup-uv@v5
         with:
           enable-cache: true
           version: ${{ env.UV_VERSION }}
@@ -117,7 +117,7 @@ jobs:
     env:
       MUJOCO_GL: egl
     steps:
-      - uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+      - uses: actions/checkout@v4
         with:
           lfs: true  # Ensure LFS files are pulled
           persist-credentials: false
@@ -129,7 +129,7 @@ jobs:
           sudo apt-get install -y libegl1-mesa-dev ffmpeg portaudio19-dev
 
       - name: Install uv and python
-        uses: astral-sh/setup-uv@d4b2f3b6ecc6e67c4457f6d3e41ec42d3d0fcb86 # v5.4.2
+        uses: astral-sh/setup-uv@v5
         with:
           enable-cache: true
           version: ${{ env.UV_VERSION }}

.github/workflows/trufflehog.yml

@@ -24,12 +24,12 @@ jobs:
     runs-on: ubuntu-latest
     steps:
     - name: Checkout code
-      uses: actions/checkout@11bd71901bbe5b1630ceea73d27597364c9af683 # v4.2.2
+      uses: actions/checkout@v4
       with:
         fetch-depth: 0
         persist-credentials: false
 
     - name: Secret Scanning
-      uses: trufflesecurity/trufflehog@90694bf9af66e7536abc5824e7a87246dbf933cb # v3.88.35
+      uses: trufflesecurity/trufflehog@main
       with:
         extra_args: --only-verified

.gitignore

@@ -11,7 +11,7 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
-
+.dev
 # Logging
 logs
 tmp

.pre-commit-config.yaml

@@ -37,18 +37,18 @@ repos:
       - id: trailing-whitespace
 
   - repo: https://github.com/adhtruong/mirrors-typos
-    rev: v1.32.0
+    rev: v1.31.1
     hooks:
       - id: typos
         args: [--force-exclude]
 
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.20.0
+    rev: v3.19.1
     hooks:
     -   id: pyupgrade
 
   - repo: https://github.com/astral-sh/ruff-pre-commit
-    rev: v0.11.11
+    rev: v0.11.5
     hooks:
       - id: ruff
         args: [--fix]
@@ -57,12 +57,12 @@ repos:
 
   ##### Security #####
   - repo: https://github.com/gitleaks/gitleaks
-    rev: v8.26.0
+    rev: v8.24.3
     hooks:
       - id: gitleaks
 
   - repo: https://github.com/woodruffw/zizmor-pre-commit
-    rev: v1.8.0
+    rev: v1.5.2
     hooks:
       - id: zizmor
 

README.md

@@ -360,7 +360,7 @@ with profile(
 If you want, you can cite this work with:
 ```bibtex
 @misc{cadene2024lerobot,
-    author = {Cadene, Remi and Alibert, Simon and Soare, Alexander and Gallouedec, Quentin and Zouitine, Adil and Palma, Steven and Kooijmans, Pepijn and Aractingi, Michel and Shukor, Mustafa and Aubakirova, Dana and Russi, Martino and Capuano, Francesco and Pascale, Caroline and Choghari, Jade and Moss, Jess and Wolf, Thomas},
+    author = {Cadene, Remi and Alibert, Simon and Soare, Alexander and Gallouedec, Quentin and Zouitine, Adil and Wolf, Thomas},
     title = {LeRobot: State-of-the-art Machine Learning for Real-World Robotics in Pytorch},
     howpublished = "\url{https://github.com/huggingface/lerobot}",
     year = {2024}

docs/source/installation.mdx

@@ -55,7 +55,7 @@ conda install ffmpeg -c conda-forge
 
 Install 🤗 LeRobot:
 ```bash
-cd lerobot && pip install -e ".[feetech]"
+cd lerobot && pip install ".[feetech]"
 ```
 
 ## Troubleshooting

examples/11_use_moss.md

@@ -1,337 +0,0 @@
-This tutorial explains how to use [Moss v1](https://github.com/jess-moss/moss-robot-arms) with LeRobot.
-
-## Source the parts
-
-Follow this [README](https://github.com/jess-moss/moss-robot-arms). It contains the bill of materials with 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 already.
-
-**Important**: 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
-
-On your computer:
-
-1. [Install Miniconda](https://docs.anaconda.com/miniconda/#quick-command-line-install):
-```bash
-mkdir -p ~/miniconda3
-wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O ~/miniconda3/miniconda.sh
-bash ~/miniconda3/miniconda.sh -b -u -p ~/miniconda3
-rm ~/miniconda3/miniconda.sh
-~/miniconda3/bin/conda init bash
-```
-
-2. Restart shell or `source ~/.bashrc`
-
-3. Create and activate a fresh conda environment for lerobot
-```bash
-conda create -y -n lerobot python=3.10 && conda activate lerobot
-```
-
-4. Clone LeRobot:
-```bash
-git clone https://github.com/huggingface/lerobot.git ~/lerobot
-```
-
-5. Install ffmpeg in your environment:
-When using `miniconda`, install `ffmpeg` in your environment:
-```bash
-conda install ffmpeg -c conda-forge
-```
-
-6. Install LeRobot with dependencies for the feetech motors:
-```bash
-cd ~/lerobot && pip install -e ".[feetech]"
-```
-
-## Configure the motors
-
-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:
-```bash
-python lerobot/scripts/find_motors_bus_port.py
-```
-
-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 DynamixelMotorsBus and press Enter when done.
-
-[...Disconnect leader arm and press Enter...]
-
-The port of this DynamixelMotorsBus is /dev/tty.usbmodem575E0031751
-Reconnect the usb cable.
-```
-
-Example output when identifying the follower arm's port (e.g., `/dev/tty.usbmodem575E0032081`, or possibly `/dev/ttyACM1` on Linux):
-```
-Finding all available ports for the MotorBus.
-['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751']
-Remove the usb cable from your DynamixelMotorsBus and press Enter when done.
-
-[...Disconnect follower arm and press Enter...]
-
-The port of this DynamixelMotorsBus is /dev/tty.usbmodem575E0032081
-Reconnect the usb cable.
-```
-
-Troubleshooting: 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
-```
-
-#### Update config file
-
-IMPORTANTLY: Now that you have your ports, update the **port** default values of [`MossRobotConfig`](../lerobot/common/robot_devices/robots/configs.py). You will find something like:
-```python
-@RobotConfig.register_subclass("moss")
-@dataclass
-class MossRobotConfig(ManipulatorRobotConfig):
-    calibration_dir: str = ".cache/calibration/moss"
-    # `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",  <-- UPDATE HERE
-                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",  <-- UPDATE HERE
-                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"],
-                },
-            ),
-        }
-    )
-```
-
-**Configure your motors**
-Plug your first motor and run this script to set its ID to 1. It will also set its present position to 2048, so expect your motor to rotate:
-```bash
-python lerobot/scripts/configure_motor.py \
-  --port /dev/tty.usbmodem58760432961 \
-  --brand feetech \
-  --model sts3215 \
-  --baudrate 1000000 \
-  --ID 1
-```
-
-Note: These motors are currently limited. They can take values between 0 and 4096 only, which corresponds to a full turn. They can't turn more than that. 2048 is at the middle of this range, so we can take -2048 steps (180 degrees anticlockwise) and reach the maximum range, or take +2048 steps (180 degrees clockwise) and reach the maximum range. The configuration step also sets the homing offset to 0, so that if you misassembled the arm, you can always update the homing offset to account for a shift up to ± 2048 steps (± 180 degrees).
-
-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 the process for all your motors until ID 6. Do the same for the 6 motors of the leader arm.
-
-**Remove the gears of the 6 leader motors**
-Follow step 2 of the [assembly video](https://www.youtube.com/watch?v=DA91NJOtMic). You need to remove the gear for the motors of the leader arm. As a result, you will only use the position encoding of the motor and reduce friction to more easily operate the leader arm.
-
-**Add motor horn to the motors**
-Follow step 3 of the [assembly video](https://www.youtube.com/watch?v=DA91NJOtMic). For Moss v1, you need to align the holes on the motor horn to the motor spline to be approximately 3, 6, 9 and 12 o'clock.
-Try to avoid rotating the motor while doing so to keep position 2048 set during configuration. It is especially tricky for the leader motors as it is more sensible without the gears, but it's ok if it's a bit rotated.
-
-## 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 used to it, you can do it under 1 hour for the second arm.
-
-## Calibrate
-
-Next, you'll need to calibrate your Moss v1 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 Moss v1 robot to work on another.
-
-**Manual calibration of follower arm**
-/!\ Contrarily to step 6 of the [assembly video](https://www.youtube.com/watch?v=DA91NJOtMic) 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                                                                                                                                              |
-| ------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| <img src="../media/moss/follower_zero.webp?raw=true" alt="Moss v1 follower arm zero position" title="Moss v1 follower arm zero position" style="width:100%;"> | <img src="../media/moss/follower_rotated.webp?raw=true" alt="Moss v1 follower arm rotated position" title="Moss v1 follower arm rotated position" style="width:100%;"> | <img src="../media/moss/follower_rest.webp?raw=true" alt="Moss v1 follower arm rest position" title="Moss v1 follower arm rest position" style="width:100%;"> |
-
-Make sure both arms are connected and run this script to launch manual calibration:
-```bash
-python lerobot/scripts/control_robot.py \
-  --robot.type=moss \
-  --robot.cameras='{}' \
-  --control.type=calibrate \
-  --control.arms='["main_follower"]'
-```
-
-**Manual calibration of leader arm**
-Follow step 6 of the [assembly video](https://www.youtube.com/watch?v=DA91NJOtMic) 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                                                                                                                                        |
-| ------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| <img src="../media/moss/leader_zero.webp?raw=true" alt="Moss v1 leader arm zero position" title="Moss v1 leader arm zero position" style="width:100%;"> | <img src="../media/moss/leader_rotated.webp?raw=true" alt="Moss v1 leader arm rotated position" title="Moss v1 leader arm rotated position" style="width:100%;"> | <img src="../media/moss/leader_rest.webp?raw=true" alt="Moss v1 leader arm rest position" title="Moss v1 leader arm rest position" style="width:100%;"> |
-
-Run this script to launch manual calibration:
-```bash
-python lerobot/scripts/control_robot.py \
-  --robot.type=moss \
-  --robot.cameras='{}' \
-  --control.type=calibrate \
-  --control.arms='["main_leader"]'
-```
-
-## 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=moss \
-  --robot.cameras='{}' \
-  --control.type=teleoperate
-```
-
-
-**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=moss \
-  --control.type=teleoperate
-```
-
-## Record a dataset
-
-Once you're familiar with teleoperation, you can record your first dataset with Moss v1.
-
-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=moss \
-  --control.type=record \
-  --control.fps=30 \
-  --control.single_task="Grasp a lego block and put it in the bin." \
-  --control.repo_id=${HF_USER}/moss_test \
-  --control.tags='["moss","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`.
-
-## 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}/moss_test
-```
-
-If you didn't upload with `--control.push_to_hub=false`, you can also visualize it locally with:
-```bash
-python lerobot/scripts/visualize_dataset_html.py \
-  --repo-id ${HF_USER}/moss_test \
-  --local-files-only 1
-```
-
-## Replay an episode
-
-Now try to replay the first episode on your robot:
-```bash
-python lerobot/scripts/control_robot.py \
-  --robot.type=moss \
-  --control.type=replay \
-  --control.fps=30 \
-  --control.repo_id=${HF_USER}/moss_test \
-  --control.episode=0
-```
-
-## 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}/moss_test \
-  --policy.type=act \
-  --output_dir=outputs/train/act_moss_test \
-  --job_name=act_moss_test \
-  --policy.device=cuda \
-  --wandb.enable=true
-```
-
-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 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_moss_test/checkpoints`.
-
-## 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=moss \
-  --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_moss_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_moss_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_moss_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_moss_test`).
-2. The name of dataset begins by `eval` to reflect that you are running inference (e.g. `${HF_USER}/eval_act_moss_test`).
-
-## More
-
-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.
-
-If you have any question or need help, please reach out on Discord in the channel [`#moss-arm`](https://discord.com/channels/1216765309076115607/1275374638985252925).

examples/5_control_robot.md

@@ -83,7 +83,7 @@ python lerobot/scripts/configure_motor.py \
   --brand dynamixel \
   --model xl330-m288 \
   --baudrate 1000000 \
-  --ID 1
+  --id 1
 ```
 
 Then unplug your first motor and plug the second motor and set its ID to 2.
@@ -93,7 +93,7 @@ python lerobot/scripts/configure_motor.py \
   --brand dynamixel \
   --model xl330-m288 \
   --baudrate 1000000 \
-  --ID 2
+  --id 2
 ```
 
 Redo the process for all your motors until ID 6.

examples/robots/lekiwi_client_app.py

@@ -0,0 +1,98 @@
+# 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 lerobot.common.datasets.lerobot_dataset import LeRobotDataset
+from lerobot.common.robots.lekiwi.config_lekiwi import LeKiwiClientConfig
+from lerobot.common.robots.lekiwi.lekiwi_client import OBS_STATE, LeKiwiClient
+from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig
+from lerobot.common.teleoperators.so100 import SO100Leader, SO100LeaderConfig
+
+NB_CYCLES_CLIENT_CONNECTION = 250
+
+
+def main():
+    logging.info("Configuring Teleop Devices")
+    leader_arm_config = SO100LeaderConfig(port="/dev/tty.usbmodem58760434171")
+    leader_arm = SO100Leader(leader_arm_config)
+
+    keyboard_config = KeyboardTeleopConfig()
+    keyboard = KeyboardTeleop(keyboard_config)
+
+    logging.info("Configuring LeKiwi Client")
+    robot_config = LeKiwiClientConfig(remote_ip="192.0.2.42", id="lekiwi")
+    robot = LeKiwiClient(robot_config)
+
+    logging.info("Creating LeRobot Dataset")
+
+    # The observations that we get are expected to be in body frame (x,y,theta)
+    obs_dict = {f"{OBS_STATE}." + key: value for key, value in robot.state_feature.items()}
+    # The actions that we send are expected to be in wheel frame (motor encoders)
+    act_dict = {"action." + key: value for key, value in robot.action_feature.items()}
+
+    features_dict = {
+        **act_dict,
+        **obs_dict,
+        **robot.camera_features,
+    }
+    dataset = LeRobotDataset.create(
+        repo_id="user/lekiwi" + str(int(time.time())),
+        fps=10,
+        features=features_dict,
+    )
+
+    logging.info("Connecting Teleop Devices")
+    leader_arm.connect()
+    keyboard.connect()
+
+    logging.info("Connecting remote LeKiwi")
+    robot.connect()
+
+    if not robot.is_connected or not leader_arm.is_connected or not keyboard.is_connected:
+        logging.error("Failed to connect to all devices")
+        return
+
+    logging.info("Starting LeKiwi teleoperation")
+    i = 0
+    while i < NB_CYCLES_CLIENT_CONNECTION:
+        arm_action = leader_arm.get_action()
+        base_action = keyboard.get_action()
+        action = {**arm_action, **base_action} if len(base_action) > 0 else arm_action
+
+        action_sent = robot.send_action(action)
+        observation = robot.get_observation()
+
+        frame = {**action_sent, **observation}
+        frame.update({"task": "Dummy Example Task Dataset"})
+
+        logging.info("Saved a frame into the dataset")
+        dataset.add_frame(frame)
+        i += 1
+
+    logging.info("Disconnecting Teleop Devices and LeKiwi Client")
+    robot.disconnect()
+    leader_arm.disconnect()
+    keyboard.disconnect()
+
+    logging.info("Uploading dataset to the hub")
+    dataset.save_episode()
+    dataset.push_to_hub()
+
+    logging.info("Finished LeKiwi cleanly")
+
+
+if __name__ == "__main__":
+    main()

lerobot/__init__.py

@@ -168,7 +168,12 @@ available_datasets = sorted(
 )
 
 # lists all available policies from `lerobot/common/policies`
-available_policies = ["act", "diffusion", "tdmpc", "vqbet"]
+available_policies = [
+    "act",
+    "diffusion",
+    "tdmpc",
+    "vqbet",
+]
 
 # lists all available robots from `lerobot/common/robot_devices/robots`
 available_robots = [
@@ -177,7 +182,6 @@ available_robots = [
     "aloha",
     "so100",
     "so101",
-    "moss",
 ]
 
 # lists all available cameras from `lerobot/common/robot_devices/cameras`

lerobot/calibrate.py

@@ -0,0 +1,79 @@
+# 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 recalibrate your device (robot or teleoperator).
+
+Example:
+
+```shell
+python -m lerobot.calibrate \
+    --teleop.type=so100_leader \
+    --teleop.port=/dev/tty.usbmodem58760431551 \
+    --teleop.id=blue
+```
+"""
+
+import logging
+from dataclasses import asdict, dataclass
+from pprint import pformat
+
+import draccus
+
+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.utils import init_logging
+
+from .common.teleoperators import koch_leader, so100_leader  # noqa: F401
+
+
+@dataclass
+class CalibrateConfig:
+    teleop: TeleoperatorConfig | None = None
+    robot: RobotConfig | None = None
+
+    def __post_init__(self):
+        if bool(self.teleop) == bool(self.robot):
+            raise ValueError("Choose either a teleop or a robot.")
+
+        self.device = self.robot if self.robot else self.teleop
+
+
+@draccus.wrap()
+def calibrate(cfg: CalibrateConfig):
+    init_logging()
+    logging.info(pformat(asdict(cfg)))
+
+    if isinstance(cfg.device, RobotConfig):
+        device = make_robot_from_config(cfg.device)
+    elif isinstance(cfg.device, TeleoperatorConfig):
+        device = make_teleoperator_from_config(cfg.device)
+
+    device.connect(calibrate=False)
+    device.calibrate()
+    device.disconnect()
+
+
+if __name__ == "__main__":
+    calibrate()

lerobot/common/cameras/__init__.py

@@ -0,0 +1,17 @@
+# 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 .camera import Camera
+from .configs import CameraConfig
+from .utils import make_cameras_from_configs

lerobot/common/cameras/camera.py

@@ -0,0 +1,49 @@
+#!/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 abc
+
+import numpy as np
+
+from .configs import CameraConfig, ColorMode
+
+
+class Camera(abc.ABC):
+    def __init__(self, config: CameraConfig):
+        self.fps: int | None = config.fps
+        self.width: int | None = config.width
+        self.height: int | None = config.height
+
+    @property
+    @abc.abstractmethod
+    def is_connected(self) -> bool:
+        pass
+
+    @abc.abstractmethod
+    def connect(self, do_warmup_read: bool = True) -> None:
+        pass
+
+    @abc.abstractmethod
+    def read(self, color_mode: ColorMode | None = None) -> np.ndarray:
+        pass
+
+    @abc.abstractmethod
+    def async_read(self, timeout_ms: float = 2000) -> np.ndarray:
+        pass
+
+    @abc.abstractmethod
+    def disconnect(self) -> None:
+        pass

lerobot/common/cameras/configs.py

@@ -0,0 +1,44 @@
+#!/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 abc
+from dataclasses import dataclass
+from enum import Enum
+
+import draccus
+
+
+class ColorMode(Enum):
+    RGB = "rgb"
+    BGR = "bgr"
+
+
+class Cv2Rotation(Enum):
+    NO_ROTATION = 0
+    ROTATE_90 = 90
+    ROTATE_180 = 180
+    ROTATE_270 = -90
+
+
+@dataclass(kw_only=True)
+class CameraConfig(draccus.ChoiceRegistry, abc.ABC):
+    fps: int | None = None
+    width: int | None = None
+    height: int | None = None
+
+    @property
+    def type(self) -> str:
+        return self.get_choice_name(self.__class__)

lerobot/common/cameras/intel/__init__.py

@@ -0,0 +1,16 @@
+# 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 .camera_realsense import RealSenseCamera
+from .configuration_realsense import RealSenseCameraConfig

lerobot/common/cameras/intel/camera_realsense.py

@@ -0,0 +1,672 @@
+# 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.
+
+"""
+Provides the RealSenseCamera class for capturing frames from Intel RealSense cameras.
+"""
+
+import contextlib
+import logging
+import math
+import queue
+import time
+from threading import Event, Thread
+from typing import Any, Dict, List
+
+import cv2
+import numpy as np
+import pyrealsense2 as rs
+
+from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
+from lerobot.common.utils.utils import capture_timestamp_utc
+
+from ..camera import Camera
+from ..configs import ColorMode
+from ..utils import get_cv2_rotation
+from .configuration_realsense import RealSenseCameraConfig
+
+logger = logging.getLogger(__name__)
+
+
+class RealSenseCamera(Camera):
+    """
+    Manages interactions with Intel RealSense cameras for frame and depth recording.
+
+    This class provides an interface similar to `OpenCVCamera` but tailored for
+    RealSense devices, leveraging the `pyrealsense2` library. It uses the camera's
+    unique serial number for identification, offering more stability than device
+    indices, especially on Linux. It also supports capturing depth maps alongside
+    color frames.
+
+    Use the provided utility script to find available camera indices and default profiles:
+    ```bash
+    python -m lerobot.find_cameras
+    ```
+
+    A `RealSenseCamera` instance requires a configuration object specifying the
+    camera's serial number or a unique device name. If using the name, ensure only
+    one camera with that name is connected.
+
+    The camera's default settings (FPS, resolution, color mode) from the stream
+    profile are used unless overridden in the configuration.
+
+    Args:
+        config (RealSenseCameraConfig): Configuration object containing settings like
+            serial number or name, desired FPS, width, height, color mode, rotation,
+            and whether to capture depth.
+
+    Example:
+        ```python
+        from lerobot.common.cameras.intel.camera_realsense import RealSenseCamera
+        from lerobot.common.cameras.intel.configuration_realsense import RealSenseCameraConfig
+        from lerobot.common.cameras.configs import ColorMode
+
+        # Basic usage with serial number
+        config = RealSenseCameraConfig(serial_number="1234567890") # Replace with actual SN
+        camera = RealSenseCamera(config)
+        try:
+            camera.connect()
+            print(f"Connected to {camera}")
+            color_image = camera.read() # Synchronous read (color only)
+            print(f"Read frame shape: {color_image.shape}")
+            async_image = camera.async_read() # Asynchronous read
+            print(f"Async read frame shape: {async_image.shape}")
+        except Exception as e:
+            print(f"An error occurred: {e}")
+        finally:
+            camera.disconnect()
+            print(f"Disconnected from {camera}")
+
+        # Example with depth capture and custom settings
+        custom_config = RealSenseCameraConfig(
+            serial_number="1234567890", # Replace with actual SN
+            fps=30,
+            width=1280,
+            height=720,
+            color_mode=ColorMode.BGR, # Request BGR output
+            rotation=0,
+            use_depth=True
+        )
+        depth_camera = RealSenseCamera(custom_config)
+        try:
+            depth_camera.connect()
+            color_image, depth_map = depth_camera.read() # Returns tuple
+            print(f"Color shape: {color_image.shape}, Depth shape: {depth_map.shape}")
+        finally:
+            depth_camera.disconnect()
+
+        # Example using a unique camera name
+        name_config = RealSenseCameraConfig(name="Intel RealSense D435") # If unique
+        name_camera = RealSenseCamera(name_config)
+        # ... connect, read, disconnect ...
+        ```
+    """
+
+    def __init__(self, config: RealSenseCameraConfig):
+        """
+        Initializes the RealSenseCamera instance.
+
+        Args:
+            config: The configuration settings for the camera.
+        """
+
+        super().__init__(config)
+
+        self.config = config
+
+        if config.name is not None:  # NOTE(Steven): Do we want to continue supporting this?
+            self.serial_number = self._find_serial_number_from_name(config.name)
+        elif config.serial_number is not None:
+            self.serial_number = str(config.serial_number)
+        else:
+            raise ValueError("RealSenseCameraConfig must provide either 'serial_number' or 'name'.")
+
+        self.fps: int | None = config.fps
+        self.channels: int = config.channels
+        self.color_mode: ColorMode = config.color_mode
+        self.use_depth: bool = config.use_depth
+
+        self.rs_pipeline: rs.pipeline | None = None
+        self.rs_profile: rs.pipeline_profile | None = None
+
+        self.thread: Thread | None = None
+        self.stop_event: Event | None = None
+        self.frame_queue: queue.Queue = queue.Queue(maxsize=1)
+
+        self.logs: dict = {}  # For timestamping or other metadata
+
+        self.rotation: int | None = get_cv2_rotation(config.rotation)
+
+        if self.height and self.width:
+            if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
+                self.prerotated_width, self.prerotated_height = self.height, self.width
+            else:
+                self.prerotated_width, self.prerotated_height = self.width, self.height
+
+    def __str__(self) -> str:
+        """Returns a string representation of the camera instance."""
+        return f"{self.__class__.__name__}({self.serial_number})"
+
+    @property
+    def is_connected(self) -> bool:
+        """Checks if the camera pipeline is started and streams are active."""
+        return self.rs_pipeline is not None and self.rs_profile is not None
+
+    @staticmethod
+    def find_cameras(raise_when_empty: bool = True) -> List[Dict[str, Any]]:
+        """
+        Detects available Intel RealSense cameras connected to the system.
+
+        Args:
+            raise_when_empty (bool): If True, raises an OSError if no cameras are found.
+
+        Returns:
+            List[Dict[str, Any]]: A list of dictionaries,
+            where each dictionary contains 'type', 'id' (serial number), 'name',
+            firmware version, USB type, and other available specs, and the default profile properties (width, height, fps, format).
+
+        Raises:
+            OSError: If `raise_when_empty` is True and no cameras are detected,
+                     or if pyrealsense2 is not installed.
+            ImportError: If pyrealsense2 is not installed.
+        """
+        found_cameras_info = []
+        context = rs.context()
+        devices = context.query_devices()
+
+        if not devices:
+            logger.warning("No RealSense devices detected.")
+            if raise_when_empty:
+                raise OSError(
+                    "No RealSense devices detected. Ensure cameras are connected, "
+                    "library (`pyrealsense2`) is installed, and firmware is up-to-date."
+                )
+
+        for device in devices:
+            camera_info = {
+                "name": device.get_info(rs.camera_info.name),
+                "type": "RealSense",
+                "id": device.get_info(rs.camera_info.serial_number),
+                "firmware_version": device.get_info(rs.camera_info.firmware_version),
+                "usb_type_descriptor": device.get_info(rs.camera_info.usb_type_descriptor),
+                "physical_port": device.get_info(rs.camera_info.physical_port),
+                "product_id": device.get_info(rs.camera_info.product_id),
+                "product_line": device.get_info(rs.camera_info.product_line),
+            }
+
+            # Get stream profiles for each sensor
+            sensors = device.query_sensors()
+            for sensor in sensors:
+                profiles = sensor.get_stream_profiles()
+
+                for profile in profiles:
+                    if profile.is_video_stream_profile() and profile.is_default():
+                        vprofile = profile.as_video_stream_profile()
+                        stream_info = {
+                            "stream_type": vprofile.stream_name(),
+                            "format": vprofile.format().name,
+                            "width": vprofile.width(),
+                            "height": vprofile.height(),
+                            "fps": vprofile.fps(),
+                        }
+                        camera_info["default_stream_profile"] = stream_info
+
+            found_cameras_info.append(camera_info)
+            logger.debug(f"Found RealSense camera: {camera_info}")
+
+        logger.info(f"Detected RealSense cameras: {[cam['id'] for cam in found_cameras_info]}")
+        return found_cameras_info
+
+    def _find_serial_number_from_name(self, name: str) -> str:
+        """Finds the serial number for a given unique camera name."""
+        camera_infos = self.find_cameras(raise_when_empty=True)
+        found_devices = [cam for cam in camera_infos if str(cam["name"]) == name]
+
+        if not found_devices:
+            available_names = [cam["name"] for cam in camera_infos]
+            raise ValueError(
+                f"No RealSense camera found with name '{name}'. Available camera names: {available_names}"
+            )
+
+        if len(found_devices) > 1:
+            serial_numbers = [dev["serial_number"] for dev in found_devices]
+            raise ValueError(
+                f"Multiple RealSense cameras found with name '{name}'. "
+                f"Please use a unique serial number instead. Found SNs: {serial_numbers}"
+            )
+
+        serial_number = str(found_devices[0]["serial_number"])
+        logger.info(f"Found serial number '{serial_number}' for camera name '{name}'.")
+        return serial_number
+
+    def _configure_realsense_settings(self) -> rs.config:
+        """Creates and configures the RealSense pipeline configuration object."""
+        rs_config = rs.config()
+        rs.config.enable_device(rs_config, self.serial_number)
+
+        if self.width and self.height and self.fps:
+            logger.debug(
+                f"Requesting Color Stream: {self.prerotated_width}x{self.prerotated_height} @ {self.fps} FPS, Format: {rs.format.rgb8}"
+            )
+            rs_config.enable_stream(
+                rs.stream.color, self.prerotated_width, self.prerotated_height, rs.format.rgb8, self.fps
+            )
+            if self.use_depth:
+                logger.debug(
+                    f"Requesting Depth Stream: {self.prerotated_width}x{self.prerotated_height} @ {self.fps} FPS, Format: {rs.format.z16}"
+                )
+                rs_config.enable_stream(
+                    rs.stream.depth, self.prerotated_width, self.prerotated_height, rs.format.z16, self.fps
+                )
+        else:
+            logger.debug(f"Requesting Color Stream: Default settings, Format: {rs.stream.color}")
+            rs_config.enable_stream(rs.stream.color)
+            if self.use_depth:
+                logger.debug(f"Requesting Depth Stream: Default settings, Format: {rs.stream.depth}")
+                rs_config.enable_stream(rs.stream.depth)
+
+        return rs_config
+
+    def _validate_capture_settings(self) -> None:
+        """
+        Validates if the actual stream settings match the requested configuration.
+
+        This method compares the requested FPS, width, and height against the
+        actual settings obtained from the active RealSense profile after the
+        pipeline has started.
+
+        Raises:
+            RuntimeError: If the actual camera settings significantly deviate
+                          from the requested ones.
+            DeviceNotConnectedError: If the camera is not connected when attempting
+                                     to validate settings.
+        """
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"Cannot validate settings for {self} as it is not connected.")
+
+        self._validate_fps(self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile())
+        self._validate_width_and_height(self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile())
+
+        if self.use_depth:
+            self._validate_fps(self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile())
+            self._validate_width_and_height(
+                self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile()
+            )
+
+    def connect(self, do_warmup_read: bool = True):
+        """
+        Connects to the RealSense camera specified in the configuration.
+
+        Initializes the RealSense pipeline, configures the required streams (color
+        and optionally depth), starts the pipeline, and validates the actual stream settings.
+
+        Raises:
+            DeviceAlreadyConnectedError: If the camera is already connected.
+            ValueError: If the configuration is invalid (e.g., missing serial/name, name not unique).
+            ConnectionError: If the camera is found but fails to start the pipeline.
+            RuntimeError: If the pipeline starts but fails to apply requested settings.
+            OSError: If no RealSense devices are detected at all.
+        """
+        if self.is_connected:
+            raise DeviceAlreadyConnectedError(f"{self} is already connected.")
+
+        logger.debug(f"Attempting to connect to camera {self.serial_number}...")
+        self.rs_pipeline = rs.pipeline()
+        rs_config = self._configure_realsense_settings()
+
+        try:
+            self.rs_profile = self.rs_pipeline.start(rs_config)
+            logger.debug(f"Successfully started pipeline for camera {self.serial_number}.")
+        except RuntimeError as e:
+            self.rs_profile = None
+            self.rs_pipeline = None
+            raise ConnectionError(
+                f"Failed to open RealSense camera {self.serial_number}. Error: {e}. "
+                f"Run 'python -m find_cameras list-cameras' for details."
+            ) from e
+
+        logger.debug(f"Validating stream configuration for {self.serial_number}...")
+        self._validate_capture_settings()
+
+        if do_warmup_read:
+            logger.debug(f"Reading a warm-up frame for {self.serial_number}...")
+            self.read()  # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs
+
+        logger.info(f"Camera {self.serial_number} connected and configured successfully.")
+
+    def _validate_fps(self, stream) -> None:
+        """Validates and sets the internal FPS based on actual stream FPS."""
+        actual_fps = stream.fps()
+
+        if self.fps is None:
+            self.fps = actual_fps
+            logger.info(f"FPS not specified, using camera default: {self.fps} FPS.")
+            return
+
+        # Use math.isclose for robust float comparison
+        if not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
+            logger.warning(
+                f"Requested FPS {self.fps} for {self}, but camera reported {actual_fps}. "
+                "This might be due to camera limitations."
+            )
+            raise RuntimeError(
+                f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps}."
+            )
+        logger.debug(f"FPS set to {actual_fps} for {self}.")
+
+    def _validate_width_and_height(self, stream) -> None:
+        """Validates and sets the internal capture width and height based on actual stream width."""
+        actual_width = int(round(stream.width()))
+        actual_height = int(round(stream.height()))
+
+        if self.width is None or self.height is None:
+            if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
+                self.width, self.height = actual_height, actual_width
+                self.prerotated_width, self.prerotated_height = actual_width, actual_height
+            else:
+                self.width, self.height = actual_width, actual_height
+                self.prerotated_width, self.prerotated_height = actual_width, actual_height
+            logger.info(f"Capture width set to camera default: {self.width}.")
+            logger.info(f"Capture height set to camera default: {self.height}.")
+            return
+
+        if self.prerotated_width != actual_width:
+            logger.warning(
+                f"Requested capture width {self.prerotated_width} for {self}, but camera reported {actual_width}."
+            )
+            raise RuntimeError(
+                f"Failed to set requested capture width {self.prerotated_width} for {self}. Actual value: {actual_width}."
+            )
+        logger.debug(f"Capture width set to {actual_width} for {self}.")
+
+        if self.prerotated_height != actual_height:
+            logger.warning(
+                f"Requested capture height {self.prerotated_height} for {self}, but camera reported {actual_height}."
+            )
+            raise RuntimeError(
+                f"Failed to set requested capture height {self.prerotated_height} for {self}. Actual value: {actual_height}."
+            )
+        logger.debug(f"Capture height set to {actual_height} for {self}.")
+
+    def read_depth(self, timeout_ms: int = 5000) -> np.ndarray:
+        """
+        Reads a single frame (depth) synchronously from the camera.
+
+        This is a blocking call. It waits for a coherent set of frames (depth)
+        from the camera hardware via the RealSense pipeline.
+
+        Args:
+            timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms.
+
+        Returns:
+            np.ndarray: The depth map as a NumPy array (height, width)
+                  of type `np.uint16` (raw depth values in millimeters) and rotation.
+
+        Raises:
+            DeviceNotConnectedError: If the camera is not connected.
+            RuntimeError: If reading frames from the pipeline fails or frames are invalid.
+        """
+
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+
+        if not self.use_depth:
+            raise RuntimeError(
+                f"Failed to capture depth frame from {self}. '.read_depth()'. Depth stream is not enabled."
+            )
+
+        start_time = time.perf_counter()
+
+        ret, frame = self.rs_pipeline.try_wait_for_frames(
+            timeout_ms=timeout_ms
+        )  # NOTE(Steven): This read has a timeout
+
+        if not ret or frame is None:
+            raise RuntimeError(
+                f"Failed to capture frame from {self}. '.read_depth()' returned status={ret} and frame is None."
+            )
+
+        depth_frame = frame.get_depth_frame()
+        depth_map = np.asanyarray(depth_frame.get_data())
+
+        depth_map_processed = self._postprocess_image(depth_map)
+
+        read_duration_ms = (time.perf_counter() - start_time) * 1e3
+        logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
+
+        self.logs["timestamp_utc"] = capture_timestamp_utc()
+        return depth_map_processed
+
+    def read(self, color_mode: ColorMode | None = None, timeout_ms: int = 5000) -> np.ndarray:
+        """
+        Reads a single frame (color) synchronously from the camera.
+
+        This is a blocking call. It waits for a coherent set of frames (color)
+        from the camera hardware via the RealSense pipeline.
+
+        Args:
+            timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms.
+
+        Returns:
+            np.ndarray: The captured color frame as a NumPy array
+              (height, width, channels), processed according to `color_mode` and rotation.
+
+        Raises:
+            DeviceNotConnectedError: If the camera is not connected.
+            RuntimeError: If reading frames from the pipeline fails or frames are invalid.
+            ValueError: If an invalid `color_mode` is requested.
+        """
+
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+
+        start_time = time.perf_counter()
+
+        ret, frame = self.rs_pipeline.try_wait_for_frames(
+            timeout_ms=timeout_ms
+        )  # NOTE(Steven): This read has a timeout while opencv doesn't
+
+        if not ret or frame is None:
+            raise RuntimeError(
+                f"Failed to capture frame from {self}. '.read()' returned status={ret} and frame is None."
+            )
+
+        color_frame = frame.get_color_frame()
+        color_image_raw = np.asanyarray(color_frame.get_data())
+
+        color_image_processed = self._postprocess_image(color_image_raw, color_mode)
+
+        read_duration_ms = (time.perf_counter() - start_time) * 1e3
+        logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
+
+        self.logs["timestamp_utc"] = capture_timestamp_utc()
+        return color_image_processed
+
+    def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray:
+        """
+        Applies color conversion, dimension validation, and rotation to a raw color frame.
+
+        Args:
+            image (np.ndarray): The raw image frame (expected RGB format from RealSense).
+            color_mode (Optional[ColorMode]): The target color mode (RGB or BGR). If None,
+                                             uses the instance's default `self.color_mode`.
+
+        Returns:
+            np.ndarray: The processed image frame according to `self.color_mode` and `self.rotation`.
+
+        Raises:
+            ValueError: If the requested `color_mode` is invalid.
+            RuntimeError: If the raw frame dimensions do not match the configured
+                          `width` and `height`.
+        """
+
+        if color_mode and color_mode not in (ColorMode.RGB, ColorMode.BGR):
+            raise ValueError(
+                f"Invalid requested color mode '{color_mode}'. Expected {ColorMode.RGB} or {ColorMode.BGR}."
+            )
+
+        h, w, c = image.shape
+
+        if h != self.prerotated_height or w != self.prerotated_width:
+            raise RuntimeError(
+                f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.prerotated_height}x{self.prerotated_width}) for {self}."
+            )
+        if c != self.channels:
+            logger.warning(
+                f"Captured frame channels ({c}) do not match configured channels ({self.channels}) for {self}."
+            )
+
+        processed_image = image
+        if self.color_mode == ColorMode.BGR:
+            processed_image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
+            logger.debug(f"Converted frame from RGB to BGR for {self}.")
+
+        if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
+            processed_image = cv2.rotate(processed_image, self.rotation)
+            logger.debug(f"Rotated frame by {self.config.rotation} degrees for {self}.")
+
+        return processed_image
+
+    def _read_loop(self):
+        """
+        Internal loop run by the background thread for asynchronous reading.
+
+        Continuously reads frames (color and optional depth) using `read()`
+        and places the latest result (single image or tuple) into the `frame_queue`.
+        It overwrites any previous frame in the queue.
+        """
+        logger.debug(f"Starting read loop thread for {self}.")
+        while not self.stop_event.is_set():
+            try:
+                frame_data = self.read(timeout_ms=500)
+
+                with contextlib.suppress(queue.Empty):
+                    _ = self.frame_queue.get_nowait()
+                self.frame_queue.put(frame_data)
+                logger.debug(f"Frame data placed in queue for {self}.")
+
+            except DeviceNotConnectedError:
+                logger.error(f"Read loop for {self} stopped: Camera disconnected.")
+                break
+            except Exception as e:
+                logger.warning(f"Error reading frame in background thread for {self}: {e}")
+
+        logger.debug(f"Stopping read loop thread for {self}.")
+
+    def _ensure_read_thread_running(self):
+        """Starts or restarts the background read thread if it's not running."""
+        if self.thread is not None and self.thread.is_alive():
+            self.thread.join(timeout=0.1)
+        if self.stop_event is not None:
+            self.stop_event.set()
+
+        self.stop_event = Event()
+        self.thread = Thread(
+            target=self._read_loop, args=(), name=f"RealSenseReadLoop-{self}-{self.serial_number}"
+        )
+        self.thread.daemon = True
+        self.thread.start()
+        logger.debug(f"Read thread started for {self}.")
+
+    # NOTE(Steven): Missing implementation for depth for now
+    def async_read(self, timeout_ms: float = 2000) -> np.ndarray:
+        """
+        Reads the latest available frame data (color or color+depth) asynchronously.
+
+        This method retrieves the most recent frame captured by the background
+        read thread. It does not block waiting for the camera hardware directly,
+        only waits for a frame to appear in the internal queue up to the specified
+        timeout.
+
+        Args:
+            timeout_ms (float): Maximum time in milliseconds to wait for a frame
+                to become available in the queue. Defaults to 2000ms (2 seconds).
+
+        Returns:
+            np.ndarray:
+            The latest captured frame data (color image), processed according to configuration.
+
+        Raises:
+            DeviceNotConnectedError: If the camera is not connected.
+            TimeoutError: If no frame data becomes available within the specified timeout.
+            RuntimeError: If the background thread died unexpectedly or another queue error occurs.
+        """
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+
+        if self.thread is None or not self.thread.is_alive():
+            self._ensure_read_thread_running()
+
+        try:
+            return self.frame_queue.get(timeout=timeout_ms / 1000.0)
+        except queue.Empty as e:
+            thread_alive = self.thread is not None and self.thread.is_alive()
+            logger.error(
+                f"Timeout waiting for frame from {self} queue after {timeout_ms}ms. "
+                f"(Read thread alive: {thread_alive})"
+            )
+            raise TimeoutError(
+                f"Timed out waiting for frame from camera {self.serial_number} after {timeout_ms} ms. "
+                f"Read thread alive: {thread_alive}."
+            ) from e
+        except Exception as e:
+            logger.exception(f"Unexpected error getting frame data from queue for {self}: {e}")
+            raise RuntimeError(
+                f"Error getting frame data from queue for camera {self.serial_number}: {e}"
+            ) from e
+
+    def _shutdown_read_thread(self):
+        """Signals the background read thread to stop and waits for it to join."""
+        if self.stop_event is not None:
+            logger.debug(f"Signaling stop event for read thread of {self}.")
+            self.stop_event.set()
+
+        if self.thread is not None and self.thread.is_alive():
+            logger.debug(f"Waiting for read thread of {self} to join...")
+            self.thread.join(timeout=2.0)
+            if self.thread.is_alive():
+                logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.")
+            else:
+                logger.debug(f"Read thread for {self} joined successfully.")
+
+        self.thread = None
+        self.stop_event = None
+
+    def disconnect(self):
+        """
+        Disconnects from the camera, stops the pipeline, and cleans up resources.
+
+        Stops the background read thread (if running) and stops the RealSense pipeline.
+
+        Raises:
+            DeviceNotConnectedError: If the camera is already disconnected (pipeline not running).
+        """
+
+        if not self.is_connected and self.thread is None:
+            raise DeviceNotConnectedError(
+                f"Attempted to disconnect {self}, but it appears already disconnected."
+            )
+
+        logger.debug(f"Disconnecting from camera {self.serial_number}...")
+
+        if self.thread is not None:
+            self._shutdown_read_thread()
+
+        if self.rs_pipeline is not None:
+            logger.debug(f"Stopping RealSense pipeline object for {self}.")
+            self.rs_pipeline.stop()
+            self.rs_pipeline = None
+            self.rs_profile = None
+
+        logger.info(f"Camera {self.serial_number} disconnected successfully.")

lerobot/common/cameras/intel/configuration_realsense.py

@@ -0,0 +1,87 @@
+# 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
+
+from ..configs import CameraConfig, ColorMode, Cv2Rotation
+
+
+@CameraConfig.register_subclass("intelrealsense")
+@dataclass
+class RealSenseCameraConfig(CameraConfig):
+    """Configuration class for Intel RealSense cameras.
+
+    This class provides specialized configuration options for Intel RealSense cameras,
+    including support for depth sensing and device identification via serial number or name.
+
+    Example configurations for Intel RealSense D405:
+    ```python
+    # Basic configurations
+    RealSenseCameraConfig(128422271347, 30, 1280, 720)   # 1280x720 @ 30FPS
+    RealSenseCameraConfig(128422271347, 60, 640, 480)   # 640x480 @ 60FPS
+
+    # Advanced configurations
+    RealSenseCameraConfig(128422271347, 30, 640, 480, use_depth=True)  # With depth sensing
+    RealSenseCameraConfig(128422271347, 30, 640, 480, rotation=Cv2Rotation.ROTATE_90)     # With 90° rotation
+    ```
+
+    Attributes:
+        fps: Requested frames per second for the color stream.
+        width: Requested frame width in pixels for the color stream.
+        height: Requested frame height in pixels for the color stream.
+        name: Optional human-readable name to identify the camera.
+        serial_number: Optional unique serial number to identify the camera.
+                      Either name or serial_number must be provided.
+        color_mode: Color mode for image output (RGB or BGR). Defaults to RGB.
+        channels: Number of color channels (currently only 3 is supported).
+        use_depth: Whether to enable depth stream. Defaults to False.
+        rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation.
+
+    Note:
+        - Either name or serial_number must be specified, but not both.
+        - Depth stream configuration (if enabled) will use the same FPS as the color stream.
+        - The actual resolution and FPS may be adjusted by the camera to the nearest supported mode.
+        - Only 3-channel color output (RGB/BGR) is currently supported.
+    """
+
+    name: str | None = None
+    serial_number: int | None = None
+    color_mode: ColorMode = ColorMode.RGB
+    channels: int | None = 3
+    use_depth: bool = False
+    rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION  # NOTE(Steven): Check if draccus can parse to an enum
+
+    def __post_init__(self):
+        if self.color_mode not in (ColorMode.RGB, ColorMode.BGR):
+            raise ValueError(
+                f"`color_mode` is expected to be {ColorMode.RGB.value} or {ColorMode.BGR.value}, but {self.color_mode} is provided."
+            )
+
+        if self.rotation not in (
+            Cv2Rotation.NO_ROTATION,
+            Cv2Rotation.ROTATE_90,
+            Cv2Rotation.ROTATE_180,
+            Cv2Rotation.ROTATE_270,
+        ):
+            raise ValueError(
+                f"`rotation` is expected to be in {(Cv2Rotation.NO_ROTATION, Cv2Rotation.ROTATE_90, Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270)}, but {self.rotation} is provided."
+            )
+
+        if self.channels != 3:
+            raise NotImplementedError(f"Unsupported number of channels: {self.channels}")
+
+        if bool(self.name) and bool(self.serial_number):
+            raise ValueError(
+                f"One of them must be set: name or serial_number, but {self.name=} and {self.serial_number=} provided."
+            )

lerobot/common/cameras/opencv/__init__.py

@@ -0,0 +1,16 @@
+# 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 .camera_opencv import OpenCVCamera
+from .configuration_opencv import OpenCVCameraConfig

lerobot/common/cameras/opencv/camera_opencv.py

@@ -0,0 +1,555 @@
+# 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.
+
+"""
+Provides the OpenCVCamera class for capturing frames from cameras using OpenCV.
+"""
+
+import contextlib
+import logging
+import math
+import platform
+import queue
+import time
+from pathlib import Path
+from threading import Event, Thread
+from typing import Any, Dict, List
+
+import cv2
+import numpy as np
+
+from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
+from lerobot.common.utils.utils import capture_timestamp_utc
+
+from ..camera import Camera
+from ..utils import IndexOrPath, get_cv2_backend, get_cv2_rotation
+from .configuration_opencv import ColorMode, OpenCVCameraConfig
+
+# NOTE(Steven): The maximum opencv device index depends on your operating system. For instance,
+# if you have 3 cameras, they should be associated to index 0, 1, and 2. This is the case
+# on MacOS. However, on Ubuntu, the indices are different like 6, 16, 23.
+# When you change the USB port or reboot the computer, the operating system might
+# treat the same cameras as new devices. Thus we select a higher bound to search indices.
+MAX_OPENCV_INDEX = 60
+
+logger = logging.getLogger(__name__)
+
+
+class OpenCVCamera(Camera):
+    """
+    Manages camera interactions using OpenCV for efficient frame recording.
+
+    This class provides a high-level interface to connect to, configure, and read
+    frames from cameras compatible with OpenCV's VideoCapture. It supports both
+    synchronous and asynchronous frame reading.
+
+    An OpenCVCamera instance requires a camera index (e.g., 0) or a device path
+    (e.g., '/dev/video0' on Linux). Camera indices can be unstable across reboots
+    or port changes, especially on Linux. Use the provided utility script to find
+    available camera indices or paths:
+    ```bash
+    python -m lerobot.find_cameras
+    ```
+
+    The camera's default settings (FPS, resolution, color mode) are used unless
+    overridden in the configuration.
+
+    Args:
+        config (OpenCVCameraConfig): Configuration object containing settings like
+            camera index/path, desired FPS, width, height, color mode, and rotation.
+
+    Example:
+        ```python
+        from lerobot.common.cameras.opencv import OpenCVCamera
+        from lerobot.common.cameras.configuration_opencv import OpenCVCameraConfig, ColorMode
+
+        # Basic usage with camera index 0
+        config = OpenCVCameraConfig(index_or_path=0)
+        camera = OpenCVCamera(config)
+        try:
+            camera.connect()
+            print(f"Connected to {camera}")
+            color_image = camera.read() # Synchronous read
+            print(f"Read frame shape: {color_image.shape}")
+            async_image = camera.async_read() # Asynchronous read
+            print(f"Async read frame shape: {async_image.shape}")
+        except Exception as e:
+            print(f"An error occurred: {e}")
+        finally:
+            camera.disconnect()
+            print(f"Disconnected from {camera}")
+
+        # Example with custom settings
+        custom_config = OpenCVCameraConfig(
+            index_or_path='/dev/video0', # Or use an index
+            fps=30,
+            width=1280,
+            height=720,
+            color_mode=ColorMode.RGB,
+            rotation=90
+        )
+        custom_camera = OpenCVCamera(custom_config)
+        # ... connect, read, disconnect ...
+        ```
+    """
+
+    def __init__(self, config: OpenCVCameraConfig):
+        """
+        Initializes the OpenCVCamera instance.
+
+        Args:
+            config: The configuration settings for the camera.
+        """
+        super().__init__(config)
+
+        self.config = config
+        self.index_or_path: IndexOrPath = config.index_or_path
+
+        self.fps: int | None = config.fps
+        self.channels: int = config.channels
+        self.color_mode: ColorMode = config.color_mode
+
+        self.videocapture_camera: cv2.VideoCapture | None = None
+
+        self.thread: Thread | None = None
+        self.stop_event: Event | None = None
+        self.frame_queue: queue.Queue = queue.Queue(maxsize=1)
+
+        self.logs: dict = {}  # NOTE(Steven): Might be removed in the future
+
+        self.rotation: int | None = get_cv2_rotation(config.rotation)
+        self.backend: int = get_cv2_backend()  # NOTE(Steven): If we specify backend the opencv open fails
+
+        if self.height and self.width:
+            if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
+                self.prerotated_width, self.prerotated_height = self.height, self.width
+            else:
+                self.prerotated_width, self.prerotated_height = self.width, self.height
+
+    def __str__(self) -> str:
+        """Returns a string representation of the camera instance."""
+        return f"{self.__class__.__name__}({self.index_or_path})"
+
+    @property
+    def is_connected(self) -> bool:
+        """Checks if the camera is currently connected and opened."""
+        return isinstance(self.videocapture_camera, cv2.VideoCapture) and self.videocapture_camera.isOpened()
+
+    def _configure_capture_settings(self) -> None:
+        """
+        Applies the specified FPS, width, and height settings to the connected camera.
+
+        This method attempts to set the camera properties via OpenCV. It checks if
+        the camera successfully applied the settings and raises an error if not.
+
+        Args:
+            fps: The desired frames per second. If None, the setting is skipped.
+            width: The desired capture width. If None, the setting is skipped.
+            height: The desired capture height. If None, the setting is skipped.
+
+        Raises:
+            RuntimeError: If the camera fails to set any of the specified properties
+                          to the requested value.
+            DeviceNotConnectedError: If the camera is not connected when attempting
+                                     to configure settings.
+        """
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"Cannot configure settings for {self} as it is not connected.")
+
+        self._validate_fps()
+        self._validate_width_and_height()
+
+    def connect(self, do_warmup_read: bool = True):
+        """
+        Connects to the OpenCV camera specified in the configuration.
+
+        Initializes the OpenCV VideoCapture object, sets desired camera properties
+        (FPS, width, height), and performs initial checks.
+
+        Raises:
+            DeviceAlreadyConnectedError: If the camera is already connected.
+            ValueError: If the specified camera index/path is not found or accessible.
+            ConnectionError: If the camera is found but fails to open.
+            RuntimeError: If the camera opens but fails to apply requested FPS/resolution settings.
+        """
+        if self.is_connected:
+            raise DeviceAlreadyConnectedError(f"{self} is already connected.")
+
+        # Use 1 thread for OpenCV operations to avoid potential conflicts or
+        # blocking in multi-threaded applications, especially during data collection.
+        cv2.setNumThreads(1)
+
+        logger.debug(f"Attempting to connect to camera {self.index_or_path} using backend {self.backend}...")
+        self.videocapture_camera = cv2.VideoCapture(self.index_or_path)
+
+        if not self.videocapture_camera.isOpened():
+            self.videocapture_camera.release()
+            self.videocapture_camera = None
+            raise ConnectionError(
+                f"Failed to open OpenCV camera {self.index_or_path}."
+                f"Run 'python -m find_cameras list-cameras' for details."
+            )
+
+        logger.debug(f"Successfully opened camera {self.index_or_path}. Applying configuration...")
+        self._configure_capture_settings()
+
+        if do_warmup_read:
+            logger.debug(f"Reading a warm-up frame for {self.index_or_path}...")
+            self.read()  # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs
+
+        logger.debug(f"Camera {self.index_or_path} connected and configured successfully.")
+
+    def _validate_fps(self) -> None:
+        """Validates and sets the camera's frames per second (FPS)."""
+
+        if self.fps is None:
+            self.fps = self.videocapture_camera.get(cv2.CAP_PROP_FPS)
+            logger.info(f"FPS set to camera default: {self.fps}.")
+            return
+
+        success = self.videocapture_camera.set(cv2.CAP_PROP_FPS, float(self.fps))
+        actual_fps = self.videocapture_camera.get(cv2.CAP_PROP_FPS)
+        # Use math.isclose for robust float comparison
+        if not success or not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
+            logger.warning(
+                f"Requested FPS {self.fps} for {self}, but camera reported {actual_fps} (set success: {success}). "
+                "This might be due to camera limitations."
+            )
+            raise RuntimeError(
+                f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps}."
+            )
+        logger.debug(f"FPS set to {actual_fps} for {self}.")
+
+    def _validate_width_and_height(self) -> None:
+        """Validates and sets the camera's frame capture width and height."""
+
+        default_width = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_WIDTH)))
+        default_height = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_HEIGHT)))
+
+        if self.width is None or self.height is None:
+            if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
+                self.width, self.height = default_height, default_width
+                self.prerotated_width, self.prerotated_height = default_width, default_height
+            else:
+                self.width, self.height = default_width, default_height
+                self.prerotated_width, self.prerotated_height = default_width, default_height
+            logger.info(f"Capture width set to camera default: {self.width}.")
+            logger.info(f"Capture height set to camera default: {self.height}.")
+            return
+
+        success = self.videocapture_camera.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.prerotated_width))
+        actual_width = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_WIDTH)))
+        if not success or self.prerotated_width != actual_width:
+            logger.warning(
+                f"Requested capture width {self.prerotated_width} for {self}, but camera reported {actual_width} (set success: {success})."
+            )
+            raise RuntimeError(
+                f"Failed to set requested capture width {self.prerotated_width} for {self}. Actual value: {actual_width}."
+            )
+        logger.debug(f"Capture width set to {actual_width} for {self}.")
+
+        success = self.videocapture_camera.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.prerotated_height))
+        actual_height = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_HEIGHT)))
+        if not success or self.prerotated_height != actual_height:
+            logger.warning(
+                f"Requested capture height {self.prerotated_height} for {self}, but camera reported {actual_height} (set success: {success})."
+            )
+            raise RuntimeError(
+                f"Failed to set requested capture height {self.prerotated_height} for {self}. Actual value: {actual_height}."
+            )
+        logger.debug(f"Capture height set to {actual_height} for {self}.")
+
+    @staticmethod
+    def find_cameras(
+        max_index_search_range=MAX_OPENCV_INDEX, raise_when_empty: bool = True
+    ) -> List[Dict[str, Any]]:
+        """
+        Detects available OpenCV cameras connected to the system.
+
+        On Linux, it scans '/dev/video*' paths. On other systems (like macOS, Windows),
+        it checks indices from 0 up to `max_index_search_range`.
+
+        Args:
+            max_index_search_range (int): The maximum index to check on non-Linux systems.
+            raise_when_empty (bool): If True, raises an OSError if no cameras are found.
+
+        Returns:
+            List[Dict[str, Any]]: A list of dictionaries,
+            where each dictionary contains 'type', 'id' (port index or path),
+            and the default profile properties (width, height, fps, format).
+        """
+        found_cameras_info = []
+
+        if platform.system() == "Linux":
+            logger.info("Linux detected. Scanning '/dev/video*' device paths...")
+            possible_paths = sorted(Path("/dev").glob("video*"), key=lambda p: p.name)
+            targets_to_scan = [str(p) for p in possible_paths]
+            logger.debug(f"Found potential paths: {targets_to_scan}")
+        else:
+            logger.info(
+                f"{platform.system()} system detected. Scanning indices from 0 to {max_index_search_range}..."
+            )
+            targets_to_scan = list(range(max_index_search_range))
+
+        for target in targets_to_scan:
+            camera = cv2.VideoCapture(target)
+            if camera.isOpened():
+                default_width = int(camera.get(cv2.CAP_PROP_FRAME_WIDTH))
+                default_height = int(camera.get(cv2.CAP_PROP_FRAME_HEIGHT))
+                default_fps = camera.get(cv2.CAP_PROP_FPS)
+                default_format = camera.get(cv2.CAP_PROP_FORMAT)
+                camera_info = {
+                    "name": f"OpenCV Camera @ {target}",
+                    "type": "OpenCV",
+                    "id": target,
+                    "backend_api": camera.getBackendName(),
+                    "default_stream_profile": {
+                        "format": default_format,
+                        "width": default_width,
+                        "height": default_height,
+                        "fps": default_fps,
+                    },
+                }
+
+                found_cameras_info.append(camera_info)
+                logger.debug(f"Found OpenCV camera:: {camera_info}")
+                camera.release()
+
+        if not found_cameras_info:
+            logger.warning("No OpenCV devices detected.")
+            if raise_when_empty:
+                raise OSError("No OpenCV devices detected. Ensure cameras are connected.")
+
+        logger.info(f"Detected OpenCV cameras: {[cam['id'] for cam in found_cameras_info]}")
+        return found_cameras_info
+
+    def read(self, color_mode: ColorMode | None = None) -> np.ndarray:
+        """
+        Reads a single frame synchronously from the camera.
+
+        This is a blocking call. It waits for the next available frame from the
+        camera hardware via OpenCV.
+
+        Args:
+            color_mode (Optional[ColorMode]): If specified, overrides the default
+                color mode (`self.color_mode`) for this read operation (e.g.,
+                request RGB even if default is BGR).
+
+        Returns:
+            np.ndarray: The captured frame as a NumPy array in the format
+                       (height, width, channels), using the specified or default
+                       color mode and applying any configured rotation.
+
+        Raises:
+            DeviceNotConnectedError: If the camera is not connected.
+            RuntimeError: If reading the frame from the camera fails or if the
+                          received frame dimensions don't match expectations before rotation.
+            ValueError: If an invalid `color_mode` is requested.
+        """
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+
+        start_time = time.perf_counter()
+
+        # NOTE(Steven): Are we okay with this blocking an undefined amount of time?
+        ret, frame = self.videocapture_camera.read()
+
+        if not ret or frame is None:
+            raise RuntimeError(
+                f"Failed to capture frame from {self}. '.read()' returned status={ret} and frame is None."
+            )
+
+        # Post-process the frame (color conversion, dimension check, rotation)
+        processed_frame = self._postprocess_image(frame, color_mode)
+
+        read_duration_ms = (time.perf_counter() - start_time) * 1e3
+        logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
+
+        self.logs["timestamp_utc"] = capture_timestamp_utc()
+        return processed_frame
+
+    def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray:
+        """
+        Applies color conversion, dimension validation, and rotation to a raw frame.
+
+        Args:
+            image (np.ndarray): The raw image frame (expected BGR format from OpenCV).
+            color_mode (Optional[ColorMode]): The target color mode (RGB or BGR). If None,
+                                             uses the instance's default `self.color_mode`.
+
+        Returns:
+            np.ndarray: The processed image frame.
+
+        Raises:
+            ValueError: If the requested `color_mode` is invalid.
+            RuntimeError: If the raw frame dimensions do not match the configured
+                          `width` and `height`.
+        """
+        requested_color_mode = self.color_mode if color_mode is None else color_mode
+
+        if requested_color_mode not in (ColorMode.RGB, ColorMode.BGR):
+            raise ValueError(
+                f"Invalid requested color mode '{requested_color_mode}'. Expected {ColorMode.RGB} or {ColorMode.BGR}."
+            )
+
+        h, w, c = image.shape
+
+        if h != self.prerotated_height or w != self.prerotated_width:
+            raise RuntimeError(
+                f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.prerotated_height}x{self.prerotated_width}) for {self}."
+            )
+        if c != self.channels:
+            logger.warning(
+                f"Captured frame channels ({c}) do not match configured channels ({self.channels}) for {self}."
+            )
+
+        processed_image = image
+        if requested_color_mode == ColorMode.RGB:
+            processed_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+            logger.debug(f"Converted frame from BGR to RGB for {self}.")
+
+        if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
+            processed_image = cv2.rotate(processed_image, self.rotation)
+            logger.debug(f"Rotated frame by {self.config.rotation} degrees for {self}.")
+
+        return processed_image
+
+    def _read_loop(self):
+        """
+        Internal loop run by the background thread for asynchronous reading.
+
+        Continuously reads frames from the camera using the synchronous `read()`
+        method and places the latest frame into the `frame_queue`. It overwrites
+        any previous frame in the queue.
+        """
+        logger.debug(f"Starting read loop thread for {self}.")
+        while not self.stop_event.is_set():
+            try:
+                color_image = self.read()
+
+                with contextlib.suppress(queue.Empty):
+                    _ = self.frame_queue.get_nowait()
+                self.frame_queue.put(color_image)
+                logger.debug(f"Frame placed in queue for {self}.")
+
+            except DeviceNotConnectedError:
+                logger.error(f"Read loop for {self} stopped: Camera disconnected.")
+                break
+            except Exception as e:
+                logger.warning(f"Error reading frame in background thread for {self}: {e}")
+
+        logger.debug(f"Stopping read loop thread for {self}.")
+
+    def _ensure_read_thread_running(self):
+        """Starts or restarts the background read thread if it's not running."""
+        if self.thread is not None and self.thread.is_alive():
+            self.thread.join(timeout=0.1)
+        if self.stop_event is not None:
+            self.stop_event.set()
+
+        self.stop_event = Event()
+        self.thread = Thread(
+            target=self._read_loop, args=(), name=f"OpenCVCameraReadLoop-{self}-{self.index_or_path}"
+        )
+        self.thread.daemon = True
+        self.thread.start()
+        logger.debug(f"Read thread started for {self}.")
+
+    def async_read(self, timeout_ms: float = 2000) -> np.ndarray:
+        """
+        Reads the latest available frame asynchronously.
+
+        This method retrieves the most recent frame captured by the background
+        read thread. It does not block waiting for the camera hardware directly,
+        only waits for a frame to appear in the internal queue up to the specified
+        timeout.
+
+        Args:
+            timeout_ms (float): Maximum time in milliseconds to wait for a frame
+                to become available in the queue. Defaults to 2000ms (2 seconds).
+
+        Returns:
+            np.ndarray: The latest captured frame as a NumPy array in the format
+                       (height, width, channels), processed according to configuration.
+
+        Raises:
+            DeviceNotConnectedError: If the camera is not connected.
+            TimeoutError: If no frame becomes available within the specified timeout.
+            RuntimeError: If an unexpected error occurs while retrieving from the queue.
+        """
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+
+        if self.thread is None or not self.thread.is_alive():
+            self._ensure_read_thread_running()
+
+        try:
+            return self.frame_queue.get(timeout=timeout_ms / 1000.0)
+        except queue.Empty as e:
+            thread_alive = self.thread is not None and self.thread.is_alive()
+            logger.error(
+                f"Timeout waiting for frame from {self} queue after {timeout_ms}ms. "
+                f"(Read thread alive: {thread_alive})"
+            )
+            raise TimeoutError(
+                f"Timed out waiting for frame from camera {self.index_or_path} after {timeout_ms} ms. "
+                f"Read thread alive: {thread_alive}."
+            ) from e
+        except Exception as e:
+            logger.exception(f"Unexpected error getting frame from queue for {self}: {e}")
+            raise RuntimeError(f"Error getting frame from queue for camera {self.index_or_path}: {e}") from e
+
+    def _shutdown_read_thread(self):
+        """Signals the background read thread to stop and waits for it to join."""
+        if self.stop_event is not None:
+            logger.debug(f"Signaling stop event for read thread of {self}.")
+            self.stop_event.set()
+
+        if self.thread is not None and self.thread.is_alive():
+            logger.debug(f"Waiting for read thread of {self} to join...")
+            self.thread.join(timeout=2.0)
+            if self.thread.is_alive():
+                logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.")
+            else:
+                logger.debug(f"Read thread for {self} joined successfully.")
+
+        self.thread = None
+        self.stop_event = None
+
+    def disconnect(self):
+        """
+        Disconnects from the camera and cleans up resources.
+
+        Stops the background read thread (if running) and releases the OpenCV
+        VideoCapture object.
+
+        Raises:
+            DeviceNotConnectedError: If the camera is already disconnected.
+        """
+        if not self.is_connected and self.thread is None:
+            raise DeviceNotConnectedError(
+                f"Attempted to disconnect {self}, but it appears already disconnected."
+            )
+
+        logger.debug(f"Disconnecting from camera {self.index_or_path}...")
+
+        if self.thread is not None:
+            self._shutdown_read_thread()
+
+        if self.videocapture_camera is not None:
+            logger.debug(f"Releasing OpenCV VideoCapture object for {self}.")
+            self.videocapture_camera.release()
+            self.videocapture_camera = None
+
+        logger.info(f"Camera {self.index_or_path} disconnected successfully.")

lerobot/common/cameras/opencv/configuration_opencv.py

@@ -0,0 +1,76 @@
+# 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
+from pathlib import Path
+
+from ..configs import CameraConfig, ColorMode, Cv2Rotation
+
+
+@CameraConfig.register_subclass("opencv")
+@dataclass
+class OpenCVCameraConfig(CameraConfig):
+    """Configuration class for OpenCV-based camera devices or video files.
+
+    This class provides configuration options for cameras accessed through OpenCV,
+    supporting both physical camera devices and video files. It includes settings
+    for resolution, frame rate, color mode, and image rotation.
+
+    Example configurations:
+    ```python
+    # Basic configurations
+    OpenCVCameraConfig(0, 30, 1280, 720)   # 1280x720 @ 30FPS
+    OpenCVCameraConfig(/dev/video4, 60, 640, 480)   # 640x480 @ 60FPS
+
+    # Advanced configurations
+    OpenCVCameraConfig(128422271347, 30, 640, 480, rotation=Cv2Rotation.ROTATE_90)     # With 90° rotation
+    ```
+
+    Attributes:
+        index_or_path: Either an integer representing the camera device index,
+                      or a Path object pointing to a video file.
+        fps: Requested frames per second for the color stream.
+        width: Requested frame width in pixels for the color stream.
+        height: Requested frame height in pixels for the color stream.
+        color_mode: Color mode for image output (RGB or BGR). Defaults to RGB.
+        channels: Number of color channels (currently only 3 is supported).
+        rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation.
+
+    Note:
+        - Only 3-channel color output (RGB/BGR) is currently supported.
+    """
+
+    index_or_path: int | Path
+    color_mode: ColorMode = ColorMode.RGB
+    channels: int = 3  # NOTE(Steven): Why is this a config?
+    rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION
+
+    def __post_init__(self):
+        if self.color_mode not in (ColorMode.RGB, ColorMode.BGR):
+            raise ValueError(
+                f"`color_mode` is expected to be {ColorMode.RGB.value} or {ColorMode.BGR.value}, but {self.color_mode} is provided."
+            )
+
+        if self.rotation not in (
+            Cv2Rotation.NO_ROTATION,
+            Cv2Rotation.ROTATE_90,
+            Cv2Rotation.ROTATE_180,
+            Cv2Rotation.ROTATE_270,
+        ):
+            raise ValueError(
+                f"`rotation` is expected to be in {(Cv2Rotation.NO_ROTATION, Cv2Rotation.ROTATE_90, Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270)}, but {self.rotation} is provided."
+            )
+
+        if self.channels != 3:
+            raise NotImplementedError(f"Unsupported number of channels: {self.channels}")

lerobot/common/cameras/utils.py

@@ -0,0 +1,73 @@
+#!/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 platform
+from pathlib import Path
+from typing import TypeAlias
+
+import numpy as np
+from PIL import Image
+
+from .camera import Camera
+from .configs import CameraConfig, Cv2Rotation
+
+IndexOrPath: TypeAlias = int | Path
+
+
+def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[str, Camera]:
+    cameras = {}
+
+    for key, cfg in camera_configs.items():
+        if cfg.type == "opencv":
+            from .opencv import OpenCVCamera
+
+            cameras[key] = OpenCVCamera(cfg)
+
+        elif cfg.type == "intelrealsense":
+            from .intel.camera_realsense import RealSenseCamera
+
+            cameras[key] = RealSenseCamera(cfg)
+        else:
+            raise ValueError(f"The motor type '{cfg.type}' is not valid.")
+
+    return cameras
+
+
+def get_cv2_rotation(rotation: Cv2Rotation) -> int:
+    import cv2
+
+    return {
+        Cv2Rotation.ROTATE_270: cv2.ROTATE_90_COUNTERCLOCKWISE,
+        Cv2Rotation.ROTATE_90: cv2.ROTATE_90_CLOCKWISE,
+        Cv2Rotation.ROTATE_180: cv2.ROTATE_180,
+    }.get(rotation)
+
+
+def get_cv2_backend() -> int:
+    import cv2
+
+    return {
+        "Linux": cv2.CAP_DSHOW,
+        "Windows": cv2.CAP_AVFOUNDATION,
+        "Darwin": cv2.CAP_ANY,
+    }.get(platform.system(), cv2.CAP_V4L2)
+
+
+def save_image(img_array: np.ndarray, camera_index: int, frame_index: int, images_dir: Path):
+    img = Image.fromarray(img_array)
+    path = images_dir / f"camera_{camera_index:02d}_frame_{frame_index:06d}.png"
+    path.parent.mkdir(parents=True, exist_ok=True)
+    img.save(str(path), quality=100)

lerobot/common/constants.py

@@ -17,12 +17,15 @@ from pathlib import Path
 
 from huggingface_hub.constants import HF_HOME
 
-OBS_ENV = "observation.environment_state"
-OBS_ROBOT = "observation.state"
+OBS_ENV_STATE = "observation.environment_state"
+OBS_STATE = "observation.state"
 OBS_IMAGE = "observation.image"
 OBS_IMAGES = "observation.images"
 ACTION = "action"
 
+ROBOTS = "robots"
+TELEOPERATORS = "teleoperators"
+
 # files & directories
 CHECKPOINTS_DIR = "checkpoints"
 LAST_CHECKPOINT_LINK = "last"
@@ -34,12 +37,16 @@ OPTIMIZER_STATE = "optimizer_state.safetensors"
 OPTIMIZER_PARAM_GROUPS = "optimizer_param_groups.json"
 SCHEDULER_STATE = "scheduler_state.json"
 
-# cache dir
-default_cache_path = Path(HF_HOME) / "lerobot"
-HF_LEROBOT_HOME = Path(os.getenv("HF_LEROBOT_HOME", default_cache_path)).expanduser()
-
 if "LEROBOT_HOME" in os.environ:
     raise ValueError(
         f"You have a 'LEROBOT_HOME' environment variable set to '{os.getenv('LEROBOT_HOME')}'.\n"
         "'LEROBOT_HOME' is deprecated, please use 'HF_LEROBOT_HOME' instead."
     )
+
+# cache dir
+default_cache_path = Path(HF_HOME) / "lerobot"
+HF_LEROBOT_HOME = Path(os.getenv("HF_LEROBOT_HOME", default_cache_path)).expanduser()
+
+# calibration dir
+default_calibration_path = HF_LEROBOT_HOME / "calibration"
+HF_LEROBOT_CALIBRATION = Path(os.getenv("HF_LEROBOT_CALIBRATION", default_calibration_path)).expanduser()

lerobot/common/datasets/image_writer.py

@@ -106,7 +106,7 @@ def worker_process(queue: queue.Queue, num_threads: int):
 class AsyncImageWriter:
     """
     This class abstract away the initialisation of processes or/and threads to
-    save images on disk asynchronously, which is critical to control a robot and record data
+    save images on disk asynchrounously, which is critical to control a robot and record data
     at a high frame rate.
 
     When `num_processes=0`, it creates a threads pool of size `num_threads`.

lerobot/common/datasets/lerobot_dataset.py

@@ -48,7 +48,6 @@ from lerobot.common.datasets.utils import (
     embed_images,
     get_delta_indices,
     get_episode_data_index,
-    get_features_from_robot,
     get_hf_features_from_features,
     get_safe_version,
     hf_transform_to_torch,
@@ -72,7 +71,6 @@ from lerobot.common.datasets.video_utils import (
     get_safe_default_codec,
     get_video_info,
 )
-from lerobot.common.robot_devices.robots.utils import Robot
 
 CODEBASE_VERSION = "v2.1"
 
@@ -304,10 +302,9 @@ class LeRobotDatasetMetadata:
         cls,
         repo_id: str,
         fps: int,
-        root: str | Path | None = None,
-        robot: Robot | None = None,
+        features: dict,
         robot_type: str | None = None,
-        features: dict | None = None,
+        root: str | Path | None = None,
         use_videos: bool = True,
     ) -> "LeRobotDatasetMetadata":
         """Creates metadata for a LeRobotDataset."""
@@ -317,33 +314,27 @@ class LeRobotDatasetMetadata:
 
         obj.root.mkdir(parents=True, exist_ok=False)
 
-        if robot is not None:
-            features = get_features_from_robot(robot, use_videos)
-            robot_type = robot.robot_type
-            if not all(cam.fps == fps for cam in robot.cameras.values()):
-                logging.warning(
-                    f"Some cameras in your {robot.robot_type} robot don't have an fps matching the fps of your dataset."
-                    "In this case, frames from lower fps cameras will be repeated to fill in the blanks."
-                )
-        elif features is None:
-            raise ValueError(
-                "Dataset features must either come from a Robot or explicitly passed upon creation."
-            )
-        else:
-            # TODO(aliberts, rcadene): implement sanity check for features
-            features = {**features, **DEFAULT_FEATURES}
+        # if robot is not None:
+        #     features = get_features_from_robot(robot, use_videos)
+        #     robot_type = robot.robot_type
+        #     if not all(cam.fps == fps for cam in robot.cameras.values()):
+        #         logging.warning(
+        #             f"Some cameras in your {robot.robot_type} robot don't have an fps matching the fps of your dataset."
+        #             "In this case, frames from lower fps cameras will be repeated to fill in the blanks."
+        #         )
 
-            # check if none of the features contains a "/" in their names,
-            # as this would break the dict flattening in the stats computation, which uses '/' as separator
-            for key in features:
-                if "/" in key:
-                    raise ValueError(f"Feature names should not contain '/'. Found '/' in feature '{key}'.")
+        # TODO(aliberts, rcadene): implement sanity check for features
+        features = {**features, **DEFAULT_FEATURES}
 
-            features = {**features, **DEFAULT_FEATURES}
+        # check if none of the features contains a "/" in their names,
+        # as this would break the dict flattening in the stats computation, which uses '/' as separator
+        for key in features:
+            if "/" in key:
+                raise ValueError(f"Feature names should not contain '/'. Found '/' in feature '{key}'.")
 
         obj.tasks, obj.task_to_task_index = {}, {}
         obj.episodes_stats, obj.stats, obj.episodes = {}, {}, {}
-        obj.info = create_empty_dataset_info(CODEBASE_VERSION, fps, robot_type, features, use_videos)
+        obj.info = create_empty_dataset_info(CODEBASE_VERSION, fps, features, use_videos, robot_type)
         if len(obj.video_keys) > 0 and not use_videos:
             raise ValueError()
         write_json(obj.info, obj.root / INFO_PATH)
@@ -785,7 +776,7 @@ class LeRobotDataset(torch.utils.data.Dataset):
         else:
             self.image_writer.save_image(image=image, fpath=fpath)
 
-    def add_frame(self, frame: dict) -> None:
+    def add_frame(self, frame: dict, task: str, timestamp: float | None = None) -> None:
         """
         This function only adds the frame to the episode_buffer. Apart from images — which are written in a
         temporary directory — nothing is written to disk. To save those frames, the 'save_episode()' method
@@ -803,17 +794,14 @@ class LeRobotDataset(torch.utils.data.Dataset):
 
         # Automatically add frame_index and timestamp to episode buffer
         frame_index = self.episode_buffer["size"]
-        timestamp = frame.pop("timestamp") if "timestamp" in frame else frame_index / self.fps
+        if timestamp is None:
+            timestamp = frame_index / self.fps
         self.episode_buffer["frame_index"].append(frame_index)
         self.episode_buffer["timestamp"].append(timestamp)
+        self.episode_buffer["task"].append(task)
 
         # Add frame features to episode_buffer
         for key in frame:
-            if key == "task":
-                # Note: we associate the task in natural language to its task index during `save_episode`
-                self.episode_buffer["task"].append(frame["task"])
-                continue
-
             if key not in self.features:
                 raise ValueError(
                     f"An element of the frame is not in the features. '{key}' not in '{self.features.keys()}'."
@@ -944,7 +932,7 @@ class LeRobotDataset(torch.utils.data.Dataset):
     def stop_image_writer(self) -> None:
         """
         Whenever wrapping this dataset inside a parallelized DataLoader, this needs to be called first to
-        remove the image_writer in order for the LeRobotDataset object to be picklable and parallelized.
+        remove the image_writer in order for the LeRobotDataset object to be pickleable and parallelized.
         """
         if self.image_writer is not None:
             self.image_writer.stop()
@@ -989,10 +977,9 @@ class LeRobotDataset(torch.utils.data.Dataset):
         cls,
         repo_id: str,
         fps: int,
+        features: dict,
         root: str | Path | None = None,
-        robot: Robot | None = None,
         robot_type: str | None = None,
-        features: dict | None = None,
         use_videos: bool = True,
         tolerance_s: float = 1e-4,
         image_writer_processes: int = 0,
@@ -1004,10 +991,9 @@ class LeRobotDataset(torch.utils.data.Dataset):
         obj.meta = LeRobotDatasetMetadata.create(
             repo_id=repo_id,
             fps=fps,
-            root=root,
-            robot=robot,
             robot_type=robot_type,
             features=features,
+            root=root,
             use_videos=use_videos,
         )
         obj.repo_id = obj.meta.repo_id

lerobot/common/datasets/utils.py

@@ -40,7 +40,7 @@ from lerobot.common.datasets.backward_compatibility import (
     BackwardCompatibilityError,
     ForwardCompatibilityError,
 )
-from lerobot.common.robot_devices.robots.utils import Robot
+from lerobot.common.robots import Robot
 from lerobot.common.utils.utils import is_valid_numpy_dtype_string
 from lerobot.configs.types import DictLike, FeatureType, PolicyFeature
 
@@ -387,6 +387,52 @@ def get_hf_features_from_features(features: dict) -> datasets.Features:
     return datasets.Features(hf_features)
 
 
+def _validate_feature_names(features: dict[str, dict]) -> None:
+    invalid_features = {name: ft for name, ft in features.items() if "/" in name}
+    if invalid_features:
+        raise ValueError(f"Feature names should not contain '/'. Found '/' in '{invalid_features}'.")
+
+
+def hw_to_dataset_features(
+    hw_features: dict[str, type | tuple], prefix: str, use_video: bool = True
+) -> dict[str, dict]:
+    features = {}
+    joint_fts = {key: ftype for key, ftype in hw_features.items() if ftype is float}
+    cam_fts = {key: shape for key, shape in hw_features.items() if isinstance(shape, tuple)}
+
+    if joint_fts:
+        features[f"{prefix}.joints"] = {
+            "dtype": "float32",
+            "shape": (len(joint_fts),),
+            "names": list(joint_fts),
+        }
+
+    for key, shape in cam_fts.items():
+        features[f"{prefix}.cameras.{key}"] = {
+            "dtype": "video" if use_video else "image",
+            "shape": shape,
+            "names": ["height", "width", "channels"],
+        }
+
+    _validate_feature_names(features)
+    return features
+
+
+def build_dataset_frame(
+    ds_features: dict[str, dict], values: dict[str, Any], prefix: str
+) -> dict[str, np.ndarray]:
+    frame = {}
+    for key, ft in ds_features.items():
+        if key in DEFAULT_FEATURES or not key.startswith(prefix):
+            continue
+        elif ft["dtype"] == "float32" and len(ft["shape"]) == 1:
+            frame[key] = np.array([values[name] for name in ft["names"]], dtype=np.float32)
+        elif ft["dtype"] in ["image", "video"]:
+            frame[key] = values[key.removeprefix(f"{prefix}.cameras.")]
+
+    return frame
+
+
 def get_features_from_robot(robot: Robot, use_videos: bool = True) -> dict:
     camera_ft = {}
     if robot.cameras:
@@ -431,9 +477,9 @@ def dataset_to_policy_features(features: dict[str, dict]) -> dict[str, PolicyFea
 def create_empty_dataset_info(
     codebase_version: str,
     fps: int,
-    robot_type: str,
     features: dict,
     use_videos: bool,
+    robot_type: str | None = None,
 ) -> dict:
     return {
         "codebase_version": codebase_version,
@@ -699,16 +745,12 @@ class IterableNamespace(SimpleNamespace):
 
 
 def validate_frame(frame: dict, features: dict):
-    optional_features = {"timestamp"}
-    expected_features = (set(features) - set(DEFAULT_FEATURES.keys())) | {"task"}
-    actual_features = set(frame.keys())
+    expected_features = set(features) - set(DEFAULT_FEATURES)
+    actual_features = set(frame)
 
-    error_message = validate_features_presence(actual_features, expected_features, optional_features)
+    error_message = validate_features_presence(actual_features, expected_features)
 
-    if "task" in frame:
-        error_message += validate_feature_string("task", frame["task"])
-
-    common_features = actual_features & (expected_features | optional_features)
+    common_features = actual_features & expected_features
     for name in common_features - {"task"}:
         error_message += validate_feature_dtype_and_shape(name, features[name], frame[name])
 
@@ -716,12 +758,10 @@ def validate_frame(frame: dict, features: dict):
         raise ValueError(error_message)
 
 
-def validate_features_presence(
-    actual_features: set[str], expected_features: set[str], optional_features: set[str]
-):
+def validate_features_presence(actual_features: set[str], expected_features: set[str]):
     error_message = ""
     missing_features = expected_features - actual_features
-    extra_features = actual_features - (expected_features | optional_features)
+    extra_features = actual_features - expected_features
 
     if missing_features or extra_features:
         error_message += "Feature mismatch in `frame` dictionary:\n"

lerobot/common/datasets/v2/batch_convert_dataset_v1_to_v2.py

@@ -27,7 +27,7 @@ from textwrap import dedent
 
 from lerobot import available_datasets
 from lerobot.common.datasets.v2.convert_dataset_v1_to_v2 import convert_dataset
-from lerobot.common.robot_devices.robots.configs import AlohaRobotConfig
+from lerobot.common.robots.aloha.configuration_aloha import AlohaRobotConfig
 
 LOCAL_DIR = Path("data/")
 

lerobot/common/datasets/v2/convert_dataset_v1_to_v2.py

@@ -141,8 +141,8 @@ from lerobot.common.datasets.video_utils import (
     get_image_pixel_channels,
     get_video_info,
 )
-from lerobot.common.robot_devices.robots.configs import RobotConfig
-from lerobot.common.robot_devices.robots.utils import make_robot_config
+from lerobot.common.robots import RobotConfig
+from lerobot.common.robots.utils import make_robot_config
 
 V16 = "v1.6"
 V20 = "v2.0"

lerobot/common/datasets/video_utils.py

@@ -101,7 +101,7 @@ def decode_video_frames_torchvision(
     keyframes_only = False
     torchvision.set_video_backend(backend)
     if backend == "pyav":
-        keyframes_only = True  # pyav doesn't support accurate seek
+        keyframes_only = True  # pyav doesnt support accuracte seek
 
     # set a video stream reader
     # TODO(rcadene): also load audio stream at the same time

lerobot/common/envs/configs.py

@@ -17,7 +17,7 @@ from dataclasses import dataclass, field
 
 import draccus
 
-from lerobot.common.constants import ACTION, OBS_ENV, OBS_IMAGE, OBS_IMAGES, OBS_ROBOT
+from lerobot.common.constants import ACTION, OBS_ENV_STATE, OBS_IMAGE, OBS_IMAGES, OBS_STATE
 from lerobot.configs.types import FeatureType, PolicyFeature
 
 
@@ -53,7 +53,7 @@ class AlohaEnv(EnvConfig):
     features_map: dict[str, str] = field(
         default_factory=lambda: {
             "action": ACTION,
-            "agent_pos": OBS_ROBOT,
+            "agent_pos": OBS_STATE,
             "top": f"{OBS_IMAGE}.top",
             "pixels/top": f"{OBS_IMAGES}.top",
         }
@@ -94,8 +94,8 @@ class PushtEnv(EnvConfig):
     features_map: dict[str, str] = field(
         default_factory=lambda: {
             "action": ACTION,
-            "agent_pos": OBS_ROBOT,
-            "environment_state": OBS_ENV,
+            "agent_pos": OBS_STATE,
+            "environment_state": OBS_ENV_STATE,
             "pixels": OBS_IMAGE,
         }
     )
@@ -136,7 +136,7 @@ class XarmEnv(EnvConfig):
     features_map: dict[str, str] = field(
         default_factory=lambda: {
             "action": ACTION,
-            "agent_pos": OBS_ROBOT,
+            "agent_pos": OBS_STATE,
             "pixels": OBS_IMAGE,
         }
     )

lerobot/common/errors.py

@@ -0,0 +1,43 @@
+# 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.
+
+
+class DeviceNotConnectedError(ConnectionError):
+    """Exception raised when the device is not connected."""
+
+    def __init__(self, message="This device is not connected. Try calling `connect()` first."):
+        self.message = message
+        super().__init__(self.message)
+
+
+class DeviceAlreadyConnectedError(ConnectionError):
+    """Exception raised when the device is already connected."""
+
+    def __init__(
+        self,
+        message="This device is already connected. Try not calling `connect()` twice.",
+    ):
+        self.message = message
+        super().__init__(self.message)
+
+
+class InvalidActionError(ValueError):
+    """Exception raised when an action is already invalid."""
+
+    def __init__(
+        self,
+        message="The action is invalid. Check the value follows what it is expected from the action space.",
+    ):
+        self.message = message
+        super().__init__(self.message)

lerobot/common/motors/__init__.py

@@ -0,0 +1 @@
+from .motors_bus import Motor, MotorCalibration, MotorNormMode, MotorsBus

lerobot/common/motors/configs.py

@@ -39,24 +39,3 @@ class FeetechMotorsBusConfig(MotorsBusConfig):
     port: str
     motors: dict[str, tuple[int, str]]
     mock: bool = False
-
-
-@MotorsBusConfig.register_subclass("realman")
-@dataclass
-class RealmanMotorsBusConfig(MotorsBusConfig):
-    ip: str
-    port: int
-    motors: dict[str, tuple[int, str]]
-    init_joint: dict[str, list]
-
-
-@MotorsBusConfig.register_subclass("realman_dual")
-@dataclass
-class RealmanDualMotorsBusConfig(MotorsBusConfig):
-    left_ip: str
-    right_ip: str
-    left_port: int
-    right_port: int
-    motors: dict[str, tuple[int, str]]
-    init_joint: dict[str, list]
-    axis: dict[str, int]

lerobot/common/motors/dynamixel/__init__.py

@@ -0,0 +1,2 @@
+from .dynamixel import DriveMode, DynamixelMotorsBus, OperatingMode, TorqueMode
+from .tables import *

lerobot/common/motors/dynamixel/dynamixel.py

@@ -0,0 +1,259 @@
+# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# TODO(aliberts): Should we implement FastSyncRead/Write?
+# https://github.com/ROBOTIS-GIT/DynamixelSDK/pull/643
+# https://github.com/ROBOTIS-GIT/DynamixelSDK/releases/tag/3.8.2
+# https://emanual.robotis.com/docs/en/dxl/protocol2/#fast-sync-read-0x8a
+# -> Need to check compatibility across models
+
+import logging
+from copy import deepcopy
+from enum import Enum
+
+from lerobot.common.utils.encoding_utils import decode_twos_complement, encode_twos_complement
+
+from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value, get_address
+from .tables import (
+    AVAILABLE_BAUDRATES,
+    MODEL_BAUDRATE_TABLE,
+    MODEL_CONTROL_TABLE,
+    MODEL_ENCODING_TABLE,
+    MODEL_NUMBER_TABLE,
+    MODEL_RESOLUTION,
+)
+
+PROTOCOL_VERSION = 2.0
+DEFAULT_BAUDRATE = 1_000_000
+DEFAULT_TIMEOUT_MS = 1000
+
+NORMALIZED_DATA = ["Goal_Position", "Present_Position"]
+CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"]
+
+logger = logging.getLogger(__name__)
+
+
+class OperatingMode(Enum):
+    # DYNAMIXEL only controls current(torque) regardless of speed and position. This mode is ideal for a
+    # gripper or a system that only uses current(torque) control or a system that has additional
+    # velocity/position controllers.
+    CURRENT = 0
+
+    # This mode controls velocity. This mode is identical to the Wheel Mode(endless) from existing DYNAMIXEL.
+    # This mode is ideal for wheel-type robots.
+    VELOCITY = 1
+
+    # This mode controls position. This mode is identical to the Joint Mode from existing DYNAMIXEL. Operating
+    # position range is limited by the Max Position Limit(48) and the Min Position Limit(52). This mode is
+    # ideal for articulated robots that each joint rotates less than 360 degrees.
+    POSITION = 3
+
+    # This mode controls position. This mode is identical to the Multi-turn Position Control from existing
+    # DYNAMIXEL. 512 turns are supported(-256[rev] ~ 256[rev]). This mode is ideal for multi-turn wrists or
+    # conveyer systems or a system that requires an additional reduction gear. Note that Max Position
+    # Limit(48), Min Position Limit(52) are not used on Extended Position Control Mode.
+    EXTENDED_POSITION = 4
+
+    # This mode controls both position and current(torque). Up to 512 turns are supported (-256[rev] ~
+    # 256[rev]). This mode is ideal for a system that requires both position and current control such as
+    # articulated robots or grippers.
+    CURRENT_POSITION = 5
+
+    # This mode directly controls PWM output. (Voltage Control Mode)
+    PWM = 16
+
+
+class DriveMode(Enum):
+    NON_INVERTED = 0
+    INVERTED = 1
+
+
+class TorqueMode(Enum):
+    ENABLED = 1
+    DISABLED = 0
+
+
+def _split_into_byte_chunks(value: int, length: int) -> list[int]:
+    import dynamixel_sdk as dxl
+
+    if length == 1:
+        data = [value]
+    elif length == 2:
+        data = [dxl.DXL_LOBYTE(value), dxl.DXL_HIBYTE(value)]
+    elif length == 4:
+        data = [
+            dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
+            dxl.DXL_HIBYTE(dxl.DXL_LOWORD(value)),
+            dxl.DXL_LOBYTE(dxl.DXL_HIWORD(value)),
+            dxl.DXL_HIBYTE(dxl.DXL_HIWORD(value)),
+        ]
+    return data
+
+
+class DynamixelMotorsBus(MotorsBus):
+    """
+    The Dynamixel implementation for a MotorsBus. It relies on the python dynamixel sdk to communicate with
+    the motors. For more info, see the Dynamixel SDK Documentation:
+    https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/sample_code/python_read_write_protocol_2_0/#python-read-write-protocol-20
+    """
+
+    available_baudrates = deepcopy(AVAILABLE_BAUDRATES)
+    default_baudrate = DEFAULT_BAUDRATE
+    default_timeout = DEFAULT_TIMEOUT_MS
+    model_baudrate_table = deepcopy(MODEL_BAUDRATE_TABLE)
+    model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
+    model_encoding_table = deepcopy(MODEL_ENCODING_TABLE)
+    model_number_table = deepcopy(MODEL_NUMBER_TABLE)
+    model_resolution_table = deepcopy(MODEL_RESOLUTION)
+    normalized_data = deepcopy(NORMALIZED_DATA)
+
+    def __init__(
+        self,
+        port: str,
+        motors: dict[str, Motor],
+        calibration: dict[str, MotorCalibration] | None = None,
+    ):
+        super().__init__(port, motors, calibration)
+        import dynamixel_sdk as dxl
+
+        self.port_handler = dxl.PortHandler(self.port)
+        self.packet_handler = dxl.PacketHandler(PROTOCOL_VERSION)
+        self.sync_reader = dxl.GroupSyncRead(self.port_handler, self.packet_handler, 0, 0)
+        self.sync_writer = dxl.GroupSyncWrite(self.port_handler, self.packet_handler, 0, 0)
+        self._comm_success = dxl.COMM_SUCCESS
+        self._no_error = 0x00
+
+    def _assert_protocol_is_compatible(self, instruction_name: str) -> None:
+        pass
+
+    def _handshake(self) -> None:
+        self._assert_motors_exist()
+
+    def _find_single_motor(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
+        model = self.motors[motor].model
+        search_baudrates = (
+            [initial_baudrate] if initial_baudrate is not None else self.model_baudrate_table[model]
+        )
+
+        for baudrate in search_baudrates:
+            self.set_baudrate(baudrate)
+            id_model = self.broadcast_ping()
+            if id_model:
+                found_id, found_model = next(iter(id_model.items()))
+                expected_model_nb = self.model_number_table[model]
+                if found_model != expected_model_nb:
+                    raise RuntimeError(
+                        f"Found one motor on {baudrate=} with id={found_id} but it has a "
+                        f"model number '{found_model}' different than the one expected: '{expected_model_nb}'. "
+                        f"Make sure you are connected only connected to the '{motor}' motor (model '{model}')."
+                    )
+                return baudrate, found_id
+
+        raise RuntimeError(f"Motor '{motor}' (model '{model}') was not found. Make sure it is connected.")
+
+    def configure_motors(self) -> None:
+        # By default, Dynamixel motors have a 500µs delay response time (corresponding to a value of 250 on
+        # the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0).
+        for motor in self.motors:
+            self.write("Return_Delay_Time", motor, 0)
+
+    def read_calibration(self) -> dict[str, MotorCalibration]:
+        offsets = self.sync_read("Homing_Offset", normalize=False)
+        mins = self.sync_read("Min_Position_Limit", normalize=False)
+        maxes = self.sync_read("Max_Position_Limit", normalize=False)
+        drive_modes = self.sync_read("Drive_Mode", normalize=False)
+
+        calibration = {}
+        for motor, m in self.motors.items():
+            calibration[motor] = MotorCalibration(
+                id=m.id,
+                drive_mode=drive_modes[motor],
+                homing_offset=offsets[motor],
+                range_min=mins[motor],
+                range_max=maxes[motor],
+            )
+
+        return calibration
+
+    def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
+        for motor, calibration in calibration_dict.items():
+            self.write("Homing_Offset", motor, calibration.homing_offset)
+            self.write("Min_Position_Limit", motor, calibration.range_min)
+            self.write("Max_Position_Limit", motor, calibration.range_max)
+
+        self.calibration = calibration_dict
+
+    def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
+        for motor in self._get_motors_list(motors):
+            self.write("Torque_Enable", motor, TorqueMode.DISABLED.value, num_retry=num_retry)
+
+    def _disable_torque(self, motor_id: int, model: str, num_retry: int = 0) -> None:
+        addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable")
+        self._write(addr, length, motor_id, TorqueMode.DISABLED.value, num_retry=num_retry)
+
+    def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
+        for motor in self._get_motors_list(motors):
+            self.write("Torque_Enable", motor, TorqueMode.ENABLED.value, num_retry=num_retry)
+
+    def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
+        for id_ in ids_values:
+            model = self._id_to_model(id_)
+            encoding_table = self.model_encoding_table.get(model)
+            if encoding_table and data_name in encoding_table:
+                n_bytes = encoding_table[data_name]
+                ids_values[id_] = encode_twos_complement(ids_values[id_], n_bytes)
+
+        return ids_values
+
+    def _decode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
+        for id_ in ids_values:
+            model = self._id_to_model(id_)
+            encoding_table = self.model_encoding_table.get(model)
+            if encoding_table and data_name in encoding_table:
+                n_bytes = encoding_table[data_name]
+                ids_values[id_] = decode_twos_complement(ids_values[id_], n_bytes)
+
+        return ids_values
+
+    def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]:
+        """
+        On Dynamixel Motors:
+        Present_Position = Actual_Position + Homing_Offset
+        """
+        half_turn_homings = {}
+        for motor, pos in positions.items():
+            model = self._get_motor_model(motor)
+            max_res = self.model_resolution_table[model] - 1
+            half_turn_homings[motor] = int(max_res / 2) - pos
+
+        return half_turn_homings
+
+    def _split_into_byte_chunks(self, value: int, length: int) -> list[int]:
+        return _split_into_byte_chunks(value, length)
+
+    def broadcast_ping(self, num_retry: int = 0, raise_on_error: bool = False) -> dict[int, int] | None:
+        for n_try in range(1 + num_retry):
+            data_list, comm = self.packet_handler.broadcastPing(self.port_handler)
+            if self._is_comm_success(comm):
+                break
+            logger.debug(f"Broadcast ping failed on port '{self.port}' ({n_try=})")
+            logger.debug(self.packet_handler.getTxRxResult(comm))
+
+        if not self._is_comm_success(comm):
+            if raise_on_error:
+                raise ConnectionError(self.packet_handler.getTxRxResult(comm))
+
+            return
+
+        return {id_: data[0] for id_, data in data_list.items()}

lerobot/common/motors/dynamixel/tables.py

@@ -0,0 +1,197 @@
+# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# TODO(Steven): Consider doing the following:
+# from enum import Enum
+# class MyControlTableKey(Enum):
+#   ID = "ID"
+#   GOAL_SPEED = "Goal_Speed"
+#   ...
+#
+# MY_CONTROL_TABLE ={
+#   MyControlTableKey.ID.value: (5,1)
+#   MyControlTableKey.GOAL_SPEED.value: (46, 2)
+#   ...
+# }
+# This allows me do to:
+# bus.write(MyControlTableKey.GOAL_SPEED, ...)
+# Instead of:
+# bus.write("Goal_Speed", ...)
+# This is important for two reasons:
+# 1. The linter will tell me if I'm trying to use an invalid key, instead of me realizing when I get the RunTimeError
+# 2. We can change the value of the MyControlTableKey enums without impacting the client code
+
+
+# {data_name: (address, size_byte)}
+# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table
+X_SERIES_CONTROL_TABLE = {
+    "Model_Number": (0, 2),
+    "Model_Information": (2, 4),
+    "Firmware_Version": (6, 1),
+    "ID": (7, 1),
+    "Baud_Rate": (8, 1),
+    "Return_Delay_Time": (9, 1),
+    "Drive_Mode": (10, 1),
+    "Operating_Mode": (11, 1),
+    "Secondary_ID": (12, 1),
+    "Protocol_Type": (13, 1),
+    "Homing_Offset": (20, 4),
+    "Moving_Threshold": (24, 4),
+    "Temperature_Limit": (31, 1),
+    "Max_Voltage_Limit": (32, 2),
+    "Min_Voltage_Limit": (34, 2),
+    "PWM_Limit": (36, 2),
+    "Current_Limit": (38, 2),
+    "Acceleration_Limit": (40, 4),
+    "Velocity_Limit": (44, 4),
+    "Max_Position_Limit": (48, 4),
+    "Min_Position_Limit": (52, 4),
+    "Shutdown": (63, 1),
+    "Torque_Enable": (64, 1),
+    "LED": (65, 1),
+    "Status_Return_Level": (68, 1),
+    "Registered_Instruction": (69, 1),
+    "Hardware_Error_Status": (70, 1),
+    "Velocity_I_Gain": (76, 2),
+    "Velocity_P_Gain": (78, 2),
+    "Position_D_Gain": (80, 2),
+    "Position_I_Gain": (82, 2),
+    "Position_P_Gain": (84, 2),
+    "Feedforward_2nd_Gain": (88, 2),
+    "Feedforward_1st_Gain": (90, 2),
+    "Bus_Watchdog": (98, 1),
+    "Goal_PWM": (100, 2),
+    "Goal_Current": (102, 2),
+    "Goal_Velocity": (104, 4),
+    "Profile_Acceleration": (108, 4),
+    "Profile_Velocity": (112, 4),
+    "Goal_Position": (116, 4),
+    "Realtime_Tick": (120, 2),
+    "Moving": (122, 1),
+    "Moving_Status": (123, 1),
+    "Present_PWM": (124, 2),
+    "Present_Current": (126, 2),
+    "Present_Velocity": (128, 4),
+    "Present_Position": (132, 4),
+    "Velocity_Trajectory": (136, 4),
+    "Position_Trajectory": (140, 4),
+    "Present_Input_Voltage": (144, 2),
+    "Present_Temperature": (146, 1),
+}
+
+# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#baud-rate8
+X_SERIES_BAUDRATE_TABLE = {
+    9_600: 0,
+    57_600: 1,
+    115_200: 2,
+    1_000_000: 3,
+    2_000_000: 4,
+    3_000_000: 5,
+    4_000_000: 6,
+}
+
+# {data_name: size_byte}
+X_SERIES_ENCODINGS_TABLE = {
+    "Homing_Offset": X_SERIES_CONTROL_TABLE["Homing_Offset"][1],
+    "Goal_PWM": X_SERIES_CONTROL_TABLE["Goal_PWM"][1],
+    "Goal_Current": X_SERIES_CONTROL_TABLE["Goal_Current"][1],
+    "Goal_Velocity": X_SERIES_CONTROL_TABLE["Goal_Velocity"][1],
+    "Present_PWM": X_SERIES_CONTROL_TABLE["Present_PWM"][1],
+    "Present_Current": X_SERIES_CONTROL_TABLE["Present_Current"][1],
+    "Present_Velocity": X_SERIES_CONTROL_TABLE["Present_Velocity"][1],
+}
+
+MODEL_ENCODING_TABLE = {
+    "x_series": X_SERIES_ENCODINGS_TABLE,
+    "xl330-m077": X_SERIES_ENCODINGS_TABLE,
+    "xl330-m288": X_SERIES_ENCODINGS_TABLE,
+    "xl430-w250": X_SERIES_ENCODINGS_TABLE,
+    "xm430-w350": X_SERIES_ENCODINGS_TABLE,
+    "xm540-w270": X_SERIES_ENCODINGS_TABLE,
+    "xc430-w150": X_SERIES_ENCODINGS_TABLE,
+}
+
+# {model: model_resolution}
+# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#specifications
+MODEL_RESOLUTION = {
+    "x_series": 4096,
+    "xl330-m077": 4096,
+    "xl330-m288": 4096,
+    "xl430-w250": 4096,
+    "xm430-w350": 4096,
+    "xm540-w270": 4096,
+    "xc430-w150": 4096,
+}
+
+# {model: model_number}
+# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table-of-eeprom-area
+MODEL_NUMBER_TABLE = {
+    "xl330-m077": 1190,
+    "xl330-m288": 1200,
+    "xl430-w250": 1060,
+    "xm430-w350": 1020,
+    "xm540-w270": 1120,
+    "xc430-w150": 1070,
+}
+
+# {model: available_operating_modes}
+# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#operating-mode11
+MODEL_OPERATING_MODES = {
+    "xl330-m077": [0, 1, 3, 4, 5, 16],
+    "xl330-m288": [0, 1, 3, 4, 5, 16],
+    "xl430-w250": [1, 3, 4, 16],
+    "xm430-w350": [0, 1, 3, 4, 5, 16],
+    "xm540-w270": [0, 1, 3, 4, 5, 16],
+    "xc430-w150": [1, 3, 4, 16],
+}
+
+MODEL_CONTROL_TABLE = {
+    "x_series": X_SERIES_CONTROL_TABLE,
+    "xl330-m077": X_SERIES_CONTROL_TABLE,
+    "xl330-m288": X_SERIES_CONTROL_TABLE,
+    "xl430-w250": X_SERIES_CONTROL_TABLE,
+    "xm430-w350": X_SERIES_CONTROL_TABLE,
+    "xm540-w270": X_SERIES_CONTROL_TABLE,
+    "xc430-w150": X_SERIES_CONTROL_TABLE,
+}
+
+MODEL_BAUDRATE_TABLE = {
+    "x_series": X_SERIES_BAUDRATE_TABLE,
+    "xl330-m077": X_SERIES_BAUDRATE_TABLE,
+    "xl330-m288": X_SERIES_BAUDRATE_TABLE,
+    "xl430-w250": X_SERIES_BAUDRATE_TABLE,
+    "xm430-w350": X_SERIES_BAUDRATE_TABLE,
+    "xm540-w270": X_SERIES_BAUDRATE_TABLE,
+    "xc430-w150": X_SERIES_BAUDRATE_TABLE,
+}
+
+AVAILABLE_BAUDRATES = [
+    9_600,
+    19_200,
+    38_400,
+    57_600,
+    115_200,
+    230_400,
+    460_800,
+    500_000,
+    576_000,
+    921_600,
+    1_000_000,
+    1_152_000,
+    2_000_000,
+    2_500_000,
+    3_000_000,
+    3_500_000,
+    4_000_000,
+]

lerobot/common/motors/feetech/__init__.py

@@ -0,0 +1,2 @@
+from .feetech import DriveMode, FeetechMotorsBus, OperatingMode, TorqueMode
+from .tables import *

lerobot/common/motors/feetech/feetech.py

@@ -0,0 +1,441 @@
+# 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
+from copy import deepcopy
+from enum import Enum
+from pprint import pformat
+
+from lerobot.common.utils.encoding_utils import decode_sign_magnitude, encode_sign_magnitude
+
+from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value, get_address
+from .tables import (
+    FIRMWARE_MAJOR_VERSION,
+    FIRMWARE_MINOR_VERSION,
+    MODEL_BAUDRATE_TABLE,
+    MODEL_CONTROL_TABLE,
+    MODEL_ENCODING_TABLE,
+    MODEL_NUMBER,
+    MODEL_NUMBER_TABLE,
+    MODEL_PROTOCOL,
+    MODEL_RESOLUTION,
+    SCAN_BAUDRATES,
+)
+
+DEFAULT_PROTOCOL_VERSION = 0
+DEFAULT_BAUDRATE = 1_000_000
+DEFAULT_TIMEOUT_MS = 1000
+
+NORMALIZED_DATA = ["Goal_Position", "Present_Position"]
+
+logger = logging.getLogger(__name__)
+
+
+class OperatingMode(Enum):
+    # position servo mode
+    POSITION = 0
+    # The motor is in constant speed mode, which is controlled by parameter 0x2e, and the highest bit 15 is
+    # the direction bit
+    VELOCITY = 1
+    # PWM open-loop speed regulation mode, with parameter 0x2c running time parameter control, bit11 as
+    # direction bit
+    PWM = 2
+    # In step servo mode, the number of step progress is represented by parameter 0x2a, and the highest bit 15
+    # is the direction bit
+    STEP = 3
+
+
+class DriveMode(Enum):
+    NON_INVERTED = 0
+    INVERTED = 1
+
+
+class TorqueMode(Enum):
+    ENABLED = 1
+    DISABLED = 0
+
+
+def _split_into_byte_chunks(value: int, length: int) -> list[int]:
+    import scservo_sdk as scs
+
+    if length == 1:
+        data = [value]
+    elif length == 2:
+        data = [scs.SCS_LOBYTE(value), scs.SCS_HIBYTE(value)]
+    elif length == 4:
+        data = [
+            scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
+            scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
+            scs.SCS_LOBYTE(scs.SCS_HIWORD(value)),
+            scs.SCS_HIBYTE(scs.SCS_HIWORD(value)),
+        ]
+    return data
+
+
+def patch_setPacketTimeout(self, packet_length):  # noqa: N802
+    """
+    HACK: This patches the PortHandler behavior to set the correct packet timeouts.
+
+    It fixes https://gitee.com/ftservo/SCServoSDK/issues/IBY2S6
+    The bug is fixed on the official Feetech SDK repo (https://gitee.com/ftservo/FTServo_Python)
+    but because that version is not published on PyPI, we rely on the (unofficial) on that is, which needs
+    patching.
+    """
+    self.packet_start_time = self.getCurrentTime()
+    self.packet_timeout = (self.tx_time_per_byte * packet_length) + (self.tx_time_per_byte * 3.0) + 50
+
+
+class FeetechMotorsBus(MotorsBus):
+    """
+    The FeetechMotorsBus class allows to efficiently read and write to the attached motors. It relies on the
+    python feetech sdk to communicate with the motors, which is itself based on the dynamixel sdk.
+    """
+
+    available_baudrates = deepcopy(SCAN_BAUDRATES)
+    default_baudrate = DEFAULT_BAUDRATE
+    default_timeout = DEFAULT_TIMEOUT_MS
+    model_baudrate_table = deepcopy(MODEL_BAUDRATE_TABLE)
+    model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
+    model_encoding_table = deepcopy(MODEL_ENCODING_TABLE)
+    model_number_table = deepcopy(MODEL_NUMBER_TABLE)
+    model_resolution_table = deepcopy(MODEL_RESOLUTION)
+    normalized_data = deepcopy(NORMALIZED_DATA)
+
+    def __init__(
+        self,
+        port: str,
+        motors: dict[str, Motor],
+        calibration: dict[str, MotorCalibration] | None = None,
+        protocol_version: int = DEFAULT_PROTOCOL_VERSION,
+    ):
+        super().__init__(port, motors, calibration)
+        self.protocol_version = protocol_version
+        self._assert_same_protocol()
+        import scservo_sdk as scs
+
+        self.port_handler = scs.PortHandler(self.port)
+        # HACK: monkeypatch
+        self.port_handler.setPacketTimeout = patch_setPacketTimeout.__get__(
+            self.port_handler, scs.PortHandler
+        )
+        self.packet_handler = scs.PacketHandler(protocol_version)
+        self.sync_reader = scs.GroupSyncRead(self.port_handler, self.packet_handler, 0, 0)
+        self.sync_writer = scs.GroupSyncWrite(self.port_handler, self.packet_handler, 0, 0)
+        self._comm_success = scs.COMM_SUCCESS
+        self._no_error = 0x00
+
+        if any(MODEL_PROTOCOL[model] != self.protocol_version for model in self.models):
+            raise ValueError(f"Some motors are incompatible with protocol_version={self.protocol_version}")
+
+    def _assert_same_protocol(self) -> None:
+        if any(MODEL_PROTOCOL[model] != self.protocol_version for model in self.models):
+            raise RuntimeError("Some motors use an incompatible protocol.")
+
+    def _assert_protocol_is_compatible(self, instruction_name: str) -> None:
+        if instruction_name == "sync_read" and self.protocol_version == 1:
+            raise NotImplementedError(
+                "'Sync Read' is not available with Feetech motors using Protocol 1. Use 'Read' sequentially instead."
+            )
+        if instruction_name == "broadcast_ping" and self.protocol_version == 1:
+            raise NotImplementedError(
+                "'Broadcast Ping' is not available with Feetech motors using Protocol 1. Use 'Ping' sequentially instead."
+            )
+
+    def _assert_same_firmware(self) -> None:
+        firmware_versions = self._read_firmware_version(self.ids)
+        if len(set(firmware_versions.values())) != 1:
+            raise RuntimeError(
+                "Some Motors use different firmware versions. Update their firmware first using Feetech's software. "
+                "Visit https://www.feetechrc.com/software."
+            )
+
+    def _handshake(self) -> None:
+        self._assert_motors_exist()
+        self._assert_same_firmware()
+
+    def _find_single_motor(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
+        if self.protocol_version == 0:
+            return self._find_single_motor_p0(motor, initial_baudrate)
+        else:
+            return self._find_single_motor_p1(motor, initial_baudrate)
+
+    def _find_single_motor_p0(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
+        model = self.motors[motor].model
+        search_baudrates = (
+            [initial_baudrate] if initial_baudrate is not None else self.model_baudrate_table[model]
+        )
+        expected_model_nb = self.model_number_table[model]
+
+        for baudrate in search_baudrates:
+            self.set_baudrate(baudrate)
+            id_model = self.broadcast_ping()
+            if id_model:
+                found_id, found_model = next(iter(id_model.items()))
+                if found_model != expected_model_nb:
+                    raise RuntimeError(
+                        f"Found one motor on {baudrate=} with id={found_id} but it has a "
+                        f"model number '{found_model}' different than the one expected: '{expected_model_nb}'. "
+                        f"Make sure you are connected only connected to the '{motor}' motor (model '{model}')."
+                    )
+                return baudrate, found_id
+
+        raise RuntimeError(f"Motor '{motor}' (model '{model}') was not found. Make sure it is connected.")
+
+    def _find_single_motor_p1(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
+        import scservo_sdk as scs
+
+        model = self.motors[motor].model
+        search_baudrates = (
+            [initial_baudrate] if initial_baudrate is not None else self.model_baudrate_table[model]
+        )
+        expected_model_nb = self.model_number_table[model]
+
+        for baudrate in search_baudrates:
+            self.set_baudrate(baudrate)
+            for id_ in range(scs.MAX_ID + 1):
+                found_model = self.ping(id_)
+                if found_model is not None:
+                    if found_model != expected_model_nb:
+                        raise RuntimeError(
+                            f"Found one motor on {baudrate=} with id={id_} but it has a "
+                            f"model number '{found_model}' different than the one expected: '{expected_model_nb}'. "
+                            f"Make sure you are connected only connected to the '{motor}' motor (model '{model}')."
+                        )
+                    return baudrate, id_
+
+        raise RuntimeError(f"Motor '{motor}' (model '{model}') was not found. Make sure it is connected.")
+
+    def configure_motors(self) -> None:
+        for motor in self.motors:
+            # By default, Feetech motors have a 500µs delay response time (corresponding to a value of 250 on
+            # the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0).
+            self.write("Return_Delay_Time", motor, 0)
+            # Set 'Maximum_Acceleration' to 254 to speedup acceleration and deceleration of the motors.
+            # Note: this address is not in the official STS3215 Memory Table
+            self.write("Maximum_Acceleration", motor, 254)
+            self.write("Acceleration", motor, 254)
+
+    def read_calibration(self) -> dict[str, MotorCalibration]:
+        if self.protocol_version == 0:
+            offsets = self.sync_read("Homing_Offset", normalize=False)
+            mins = self.sync_read("Min_Position_Limit", normalize=False)
+            maxes = self.sync_read("Max_Position_Limit", normalize=False)
+            drive_modes = dict.fromkeys(self.motors, 0)
+        else:
+            offsets, mins, maxes, drive_modes = {}, {}, {}, {}
+            for motor in self.motors:
+                offsets[motor] = 0
+                mins[motor] = self.read("Min_Position_Limit", motor, normalize=False)
+                maxes[motor] = self.read("Max_Position_Limit", motor, normalize=False)
+                drive_modes[motor] = 0
+
+        # TODO(aliberts): add set/get_drive_mode?
+
+        calibration = {}
+        for motor, m in self.motors.items():
+            calibration[motor] = MotorCalibration(
+                id=m.id,
+                drive_mode=drive_modes[motor],
+                homing_offset=offsets[motor],
+                range_min=mins[motor],
+                range_max=maxes[motor],
+            )
+
+        return calibration
+
+    def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
+        for motor, calibration in calibration_dict.items():
+            if self.protocol_version == 0:
+                self.write("Homing_Offset", motor, calibration.homing_offset)
+            self.write("Min_Position_Limit", motor, calibration.range_min)
+            self.write("Max_Position_Limit", motor, calibration.range_max)
+
+        self.calibration = calibration_dict
+
+    def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]:
+        """
+        On Feetech Motors:
+        Present_Position = Actual_Position - Homing_Offset
+        """
+        half_turn_homings = {}
+        for motor, pos in positions.items():
+            model = self._get_motor_model(motor)
+            max_res = self.model_resolution_table[model] - 1
+            half_turn_homings[motor] = pos - int(max_res / 2)
+
+        return half_turn_homings
+
+    def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
+        for motor in self._get_motors_list(motors):
+            self.write("Torque_Enable", motor, TorqueMode.DISABLED.value, num_retry=num_retry)
+            self.write("Lock", motor, 0, num_retry=num_retry)
+
+    def _disable_torque(self, motor_id: int, model: str, num_retry: int = 0) -> None:
+        addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable")
+        self._write(addr, length, motor_id, TorqueMode.DISABLED.value, num_retry=num_retry)
+        addr, length = get_address(self.model_ctrl_table, model, "Lock")
+        self._write(addr, length, motor_id, 0, num_retry=num_retry)
+
+    def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
+        for motor in self._get_motors_list(motors):
+            self.write("Torque_Enable", motor, TorqueMode.ENABLED.value, num_retry=num_retry)
+            self.write("Lock", motor, 1, num_retry=num_retry)
+
+    def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
+        for id_ in ids_values:
+            model = self._id_to_model(id_)
+            encoding_table = self.model_encoding_table.get(model)
+            if encoding_table and data_name in encoding_table:
+                sign_bit = encoding_table[data_name]
+                ids_values[id_] = encode_sign_magnitude(ids_values[id_], sign_bit)
+
+        return ids_values
+
+    def _decode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
+        for id_ in ids_values:
+            model = self._id_to_model(id_)
+            encoding_table = self.model_encoding_table.get(model)
+            if encoding_table and data_name in encoding_table:
+                sign_bit = encoding_table[data_name]
+                ids_values[id_] = decode_sign_magnitude(ids_values[id_], sign_bit)
+
+        return ids_values
+
+    def _split_into_byte_chunks(self, value: int, length: int) -> list[int]:
+        return _split_into_byte_chunks(value, length)
+
+    def _broadcast_ping(self) -> tuple[dict[int, int], int]:
+        import scservo_sdk as scs
+
+        data_list = {}
+
+        status_length = 6
+
+        rx_length = 0
+        wait_length = status_length * scs.MAX_ID
+
+        txpacket = [0] * 6
+
+        tx_time_per_byte = (1000.0 / self.port_handler.getBaudRate()) * 10.0
+
+        txpacket[scs.PKT_ID] = scs.BROADCAST_ID
+        txpacket[scs.PKT_LENGTH] = 2
+        txpacket[scs.PKT_INSTRUCTION] = scs.INST_PING
+
+        result = self.packet_handler.txPacket(self.port_handler, txpacket)
+        if result != scs.COMM_SUCCESS:
+            self.port_handler.is_using = False
+            return data_list, result
+
+        # set rx timeout
+        self.port_handler.setPacketTimeoutMillis((wait_length * tx_time_per_byte) + (3.0 * scs.MAX_ID) + 16.0)
+
+        rxpacket = []
+        while True:
+            rxpacket += self.port_handler.readPort(wait_length - rx_length)
+            rx_length = len(rxpacket)
+
+            if self.port_handler.isPacketTimeout():  # or rx_length >= wait_length
+                break
+
+        self.port_handler.is_using = False
+
+        if rx_length == 0:
+            return data_list, scs.COMM_RX_TIMEOUT
+
+        while True:
+            if rx_length < status_length:
+                return data_list, scs.COMM_RX_CORRUPT
+
+            # find packet header
+            for idx in range(0, (rx_length - 1)):
+                if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF):
+                    break
+
+            if idx == 0:  # found at the beginning of the packet
+                # calculate checksum
+                checksum = 0
+                for idx in range(2, status_length - 1):  # except header & checksum
+                    checksum += rxpacket[idx]
+
+                checksum = ~checksum & 0xFF
+                if rxpacket[status_length - 1] == checksum:
+                    result = scs.COMM_SUCCESS
+                    data_list[rxpacket[scs.PKT_ID]] = rxpacket[scs.PKT_ERROR]
+
+                    del rxpacket[0:status_length]
+                    rx_length = rx_length - status_length
+
+                    if rx_length == 0:
+                        return data_list, result
+                else:
+                    result = scs.COMM_RX_CORRUPT
+                    # remove header (0xFF 0xFF)
+                    del rxpacket[0:2]
+                    rx_length = rx_length - 2
+            else:
+                # remove unnecessary packets
+                del rxpacket[0:idx]
+                rx_length = rx_length - idx
+
+    def broadcast_ping(self, num_retry: int = 0, raise_on_error: bool = False) -> dict[int, int] | None:
+        self._assert_protocol_is_compatible("broadcast_ping")
+        for n_try in range(1 + num_retry):
+            ids_status, comm = self._broadcast_ping()
+            if self._is_comm_success(comm):
+                break
+            logger.debug(f"Broadcast ping failed on port '{self.port}' ({n_try=})")
+            logger.debug(self.packet_handler.getTxRxResult(comm))
+
+        if not self._is_comm_success(comm):
+            if raise_on_error:
+                raise ConnectionError(self.packet_handler.getTxRxResult(comm))
+            return
+
+        ids_errors = {id_: status for id_, status in ids_status.items() if self._is_error(status)}
+        if ids_errors:
+            display_dict = {id_: self.packet_handler.getRxPacketError(err) for id_, err in ids_errors.items()}
+            logger.error(f"Some motors found returned an error status:\n{pformat(display_dict, indent=4)}")
+
+        return self._read_model_number(list(ids_status), raise_on_error)
+
+    def _read_firmware_version(self, motor_ids: list[int], raise_on_error: bool = False) -> dict[int, str]:
+        firmware_versions = {}
+        for id_ in motor_ids:
+            firm_ver_major, comm, error = self._read(
+                *FIRMWARE_MAJOR_VERSION, id_, raise_on_error=raise_on_error
+            )
+            if not self._is_comm_success(comm) or self._is_error(error):
+                return
+
+            firm_ver_minor, comm, error = self._read(
+                *FIRMWARE_MINOR_VERSION, id_, raise_on_error=raise_on_error
+            )
+            if not self._is_comm_success(comm) or self._is_error(error):
+                return
+
+            firmware_versions[id_] = f"{firm_ver_major}.{firm_ver_minor}"
+
+        return firmware_versions
+
+    def _read_model_number(self, motor_ids: list[int], raise_on_error: bool = False) -> dict[int, int]:
+        model_numbers = {}
+        for id_ in motor_ids:
+            model_nb, comm, error = self._read(*MODEL_NUMBER, id_, raise_on_error=raise_on_error)
+            if not self._is_comm_success(comm) or self._is_error(error):
+                return
+
+            model_numbers[id_] = model_nb
+
+        return model_numbers

lerobot/common/motors/feetech/tables.py

@@ -0,0 +1,251 @@
+# 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.
+
+FIRMWARE_MAJOR_VERSION = (0, 1)
+FIRMWARE_MINOR_VERSION = (1, 1)
+MODEL_NUMBER = (3, 2)
+
+# TODO(Steven): Consider doing the following:
+# from enum import Enum
+# class MyControlTableKey(Enum):
+#   ID = "ID"
+#   GOAL_SPEED = "Goal_Speed"
+#   ...
+#
+# MY_CONTROL_TABLE ={
+#   MyControlTableKey.ID.value: (5,1)
+#   MyControlTableKey.GOAL_SPEED.value: (46, 2)
+#   ...
+# }
+# This allows me do to:
+# bus.write(MyControlTableKey.GOAL_SPEED, ...)
+# Instead of:
+# bus.write("Goal_Speed", ...)
+# This is important for two reasons:
+# 1. The linter will tell me if I'm trying to use an invalid key, instead of me realizing when I get the RunTimeError
+# 2. We can change the value of the MyControlTableKey enums without impacting the client code
+
+# data_name: (address, size_byte)
+# http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SMS-STS-emanual-229f4476422d4059abfb1cb0
+STS_SMS_SERIES_CONTROL_TABLE = {
+    # EPROM
+    "Firmware_Major_Version": FIRMWARE_MAJOR_VERSION,  # read-only
+    "Firmware_Minor_Version": FIRMWARE_MINOR_VERSION,  # read-only
+    "Model_Number": MODEL_NUMBER,  # read-only
+    "ID": (5, 1),
+    "Baud_Rate": (6, 1),
+    "Return_Delay_Time": (7, 1),
+    "Response_Status_Level": (8, 1),
+    "Min_Position_Limit": (9, 2),
+    "Max_Position_Limit": (11, 2),
+    "Max_Temperature_Limit": (13, 1),
+    "Max_Voltage_Limit": (14, 1),
+    "Min_Voltage_Limit": (15, 1),
+    "Max_Torque_Limit": (16, 2),
+    "Phase": (18, 1),
+    "Unloading_Condition": (19, 1),
+    "LED_Alarm_Condition": (20, 1),
+    "P_Coefficient": (21, 1),
+    "D_Coefficient": (22, 1),
+    "I_Coefficient": (23, 1),
+    "Minimum_Startup_Force": (24, 2),
+    "CW_Dead_Zone": (26, 1),
+    "CCW_Dead_Zone": (27, 1),
+    "Protection_Current": (28, 2),
+    "Angular_Resolution": (30, 1),
+    "Homing_Offset": (31, 2),
+    "Operating_Mode": (33, 1),
+    "Protective_Torque": (34, 1),
+    "Protection_Time": (35, 1),
+    "Overload_Torque": (36, 1),
+    "Velocity_closed_loop_P_proportional_coefficient": (37, 1),
+    "Over_Current_Protection_Time": (38, 1),
+    "Velocity_closed_loop_I_integral_coefficient": (39, 1),
+    # SRAM
+    "Torque_Enable": (40, 1),
+    "Acceleration": (41, 1),
+    "Goal_Position": (42, 2),
+    "Goal_Time": (44, 2),
+    "Goal_Velocity": (46, 2),
+    "Torque_Limit": (48, 2),
+    "Lock": (55, 1),
+    "Present_Position": (56, 2),  # read-only
+    "Present_Velocity": (58, 2),  # read-only
+    "Present_Load": (60, 2),  # read-only
+    "Present_Voltage": (62, 1),  # read-only
+    "Present_Temperature": (63, 1),  # read-only
+    "Status": (65, 1),  # read-only
+    "Moving": (66, 1),  # read-only
+    "Present_Current": (69, 2),  # read-only
+    "Goal_Position_2": (71, 2),  # read-only
+    # Factory
+    "Moving_Velocity": (80, 1),
+    "Moving_Velocity_Threshold": (80, 1),
+    "DTs": (81, 1),  # (ms)
+    "Velocity_Unit_factor": (82, 1),
+    "Hts": (83, 1),  # (ns) valid for firmware >= 2.54, other versions keep 0
+    "Maximum_Velocity_Limit": (84, 1),
+    "Maximum_Acceleration": (85, 1),
+    "Acceleration_Multiplier ": (86, 1),  # Acceleration multiplier in effect when acceleration is 0
+}
+
+# http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SCSCL-emanual-cbcc8ab2e3384282a01d4bf3
+SCS_SERIES_CONTROL_TABLE = {
+    # EPROM
+    "Firmware_Major_Version": FIRMWARE_MAJOR_VERSION,  # read-only
+    "Firmware_Minor_Version": FIRMWARE_MINOR_VERSION,  # read-only
+    "Model_Number": MODEL_NUMBER,  # read-only
+    "ID": (5, 1),
+    "Baud_Rate": (6, 1),
+    "Return_Delay_Time": (7, 1),
+    "Response_Status_Level": (8, 1),
+    "Min_Position_Limit": (9, 2),
+    "Max_Position_Limit": (11, 2),
+    "Max_Temperature_Limit": (13, 1),
+    "Max_Voltage_Limit": (14, 1),
+    "Min_Voltage_Limit": (15, 1),
+    "Max_Torque_Limit": (16, 2),
+    "Phase": (18, 1),
+    "Unloading_Condition": (19, 1),
+    "LED_Alarm_Condition": (20, 1),
+    "P_Coefficient": (21, 1),
+    "D_Coefficient": (22, 1),
+    "I_Coefficient": (23, 1),
+    "Minimum_Startup_Force": (24, 2),
+    "CW_Dead_Zone": (26, 1),
+    "CCW_Dead_Zone": (27, 1),
+    "Protective_Torque": (37, 1),
+    "Protection_Time": (38, 1),
+    # SRAM
+    "Torque_Enable": (40, 1),
+    "Acceleration": (41, 1),
+    "Goal_Position": (42, 2),
+    "Running_Time": (44, 2),
+    "Goal_Velocity": (46, 2),
+    "Lock": (48, 1),
+    "Present_Position": (56, 2),  # read-only
+    "Present_Velocity": (58, 2),  # read-only
+    "Present_Load": (60, 2),  # read-only
+    "Present_Voltage": (62, 1),  # read-only
+    "Present_Temperature": (63, 1),  # read-only
+    "Sync_Write_Flag": (64, 1),  # read-only
+    "Status": (65, 1),  # read-only
+    "Moving": (66, 1),  # read-only
+    # Factory
+    "PWM_Maximum_Step": (78, 1),
+    "Moving_Velocity_Threshold*50": (79, 1),
+    "DTs": (80, 1),  # (ms)
+    "Minimum_Velocity_Limit*50": (81, 1),
+    "Maximum_Velocity_Limit*50": (82, 1),
+    "Acceleration_2": (83, 1),  # don't know what that is
+}
+
+STS_SMS_SERIES_BAUDRATE_TABLE = {
+    1_000_000: 0,
+    500_000: 1,
+    250_000: 2,
+    128_000: 3,
+    115_200: 4,
+    57_600: 5,
+    38_400: 6,
+    19_200: 7,
+}
+
+SCS_SERIES_BAUDRATE_TABLE = {
+    1_000_000: 0,
+    500_000: 1,
+    250_000: 2,
+    128_000: 3,
+    115_200: 4,
+    57_600: 5,
+    38_400: 6,
+    19_200: 7,
+}
+
+MODEL_CONTROL_TABLE = {
+    "sts_series": STS_SMS_SERIES_CONTROL_TABLE,
+    "scs_series": SCS_SERIES_CONTROL_TABLE,
+    "sms_series": STS_SMS_SERIES_CONTROL_TABLE,
+    "sts3215": STS_SMS_SERIES_CONTROL_TABLE,
+    "sts3250": STS_SMS_SERIES_CONTROL_TABLE,
+    "scs0009": SCS_SERIES_CONTROL_TABLE,
+    "sm8512bl": STS_SMS_SERIES_CONTROL_TABLE,
+}
+
+MODEL_RESOLUTION = {
+    "sts_series": 4096,
+    "sms_series": 4096,
+    "scs_series": 1024,
+    "sts3215": 4096,
+    "sts3250": 4096,
+    "sm8512bl": 65536,
+    "scs0009": 1024,
+}
+
+MODEL_BAUDRATE_TABLE = {
+    "sts_series": STS_SMS_SERIES_BAUDRATE_TABLE,
+    "sms_series": STS_SMS_SERIES_BAUDRATE_TABLE,
+    "scs_series": SCS_SERIES_BAUDRATE_TABLE,
+    "sm8512bl": STS_SMS_SERIES_BAUDRATE_TABLE,
+    "sts3215": STS_SMS_SERIES_BAUDRATE_TABLE,
+    "sts3250": STS_SMS_SERIES_BAUDRATE_TABLE,
+    "scs0009": SCS_SERIES_BAUDRATE_TABLE,
+}
+
+# Sign-Magnitude encoding bits
+STS_SMS_SERIES_ENCODINGS_TABLE = {
+    "Homing_Offset": 11,
+    "Goal_Velocity": 15,
+}
+
+MODEL_ENCODING_TABLE = {
+    "sts_series": STS_SMS_SERIES_ENCODINGS_TABLE,
+    "sms_series": STS_SMS_SERIES_ENCODINGS_TABLE,
+    "scs_series": {},
+    "sts3215": STS_SMS_SERIES_ENCODINGS_TABLE,
+    "sts3250": STS_SMS_SERIES_ENCODINGS_TABLE,
+    "sm8512bl": STS_SMS_SERIES_ENCODINGS_TABLE,
+    "scs0009": {},
+}
+
+SCAN_BAUDRATES = [
+    4_800,
+    9_600,
+    14_400,
+    19_200,
+    38_400,
+    57_600,
+    115_200,
+    128_000,
+    250_000,
+    500_000,
+    1_000_000,
+]
+
+MODEL_NUMBER_TABLE = {
+    "sts3215": 777,
+    "sts3250": 2825,
+    "sm8512bl": 11272,
+    "scs0009": 1284,
+}
+
+MODEL_PROTOCOL = {
+    "sts_series": 0,
+    "sms_series": 0,
+    "scs_series": 1,
+    "sts3215": 0,
+    "sts3250": 0,
+    "sm8512bl": 0,
+    "scs0009": 1,
+}

lerobot/common/motors/utils.py

@@ -12,22 +12,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from typing import Protocol
-
-from lerobot.common.robot_devices.motors.configs import (
-    DynamixelMotorsBusConfig,
-    FeetechMotorsBusConfig,
-    MotorsBusConfig,
-)
-
-
-class MotorsBus(Protocol):
-    def motor_names(self): ...
-    def set_calibration(self): ...
-    def apply_calibration(self): ...
-    def revert_calibration(self): ...
-    def read(self): ...
-    def write(self): ...
+from .configs import MotorsBusConfig
+from .motors_bus import MotorsBus
 
 
 def make_motors_buses_from_configs(motors_bus_configs: dict[str, MotorsBusConfig]) -> list[MotorsBus]:
@@ -35,24 +21,15 @@ def make_motors_buses_from_configs(motors_bus_configs: dict[str, MotorsBusConfig
 
     for key, cfg in motors_bus_configs.items():
         if cfg.type == "dynamixel":
-            from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus
+            from .dynamixel import DynamixelMotorsBus
 
             motors_buses[key] = DynamixelMotorsBus(cfg)
 
         elif cfg.type == "feetech":
-            from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus
+            from lerobot.common.motors.feetech.feetech import FeetechMotorsBus
 
             motors_buses[key] = FeetechMotorsBus(cfg)
 
-        elif cfg.type == "realman":
-            from lerobot.common.robot_devices.motors.realman import RealmanMotorsBus
-
-            motors_buses[key] = RealmanMotorsBus(cfg)
-
-        elif cfg.type == "realman_dual":
-            from lerobot.common.robot_devices.motors.realman_dual import RealmanDualMotorsBus
-
-            motors_buses[key] = RealmanDualMotorsBus(cfg)
         else:
             raise ValueError(f"The motor type '{cfg.type}' is not valid.")
 
@@ -61,20 +38,19 @@ def make_motors_buses_from_configs(motors_bus_configs: dict[str, MotorsBusConfig
 
 def make_motors_bus(motor_type: str, **kwargs) -> MotorsBus:
     if motor_type == "dynamixel":
-        from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus
+        from .configs import DynamixelMotorsBusConfig
+        from .dynamixel import DynamixelMotorsBus
 
         config = DynamixelMotorsBusConfig(**kwargs)
         return DynamixelMotorsBus(config)
 
     elif motor_type == "feetech":
-        from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus
+        from feetech import FeetechMotorsBus
+
+        from .configs import FeetechMotorsBusConfig
 
         config = FeetechMotorsBusConfig(**kwargs)
         return FeetechMotorsBus(config)
 
     else:
         raise ValueError(f"The motor type '{motor_type}' is not valid.")
-
-
-def get_motor_names(arm: dict[str, MotorsBus]) -> list:
-    return [f"{arm}_{motor}" for arm, bus in arm.items() for motor in bus.motors]

lerobot/common/policies/__init__.py

@@ -15,6 +15,5 @@
 from .act.configuration_act import ACTConfig as ACTConfig
 from .diffusion.configuration_diffusion import DiffusionConfig as DiffusionConfig
 from .pi0.configuration_pi0 import PI0Config as PI0Config
-from .smolvla.configuration_smolvla import SmolVLAConfig as SmolVLAConfig
 from .tdmpc.configuration_tdmpc import TDMPCConfig as TDMPCConfig
 from .vqbet.configuration_vqbet import VQBeTConfig as VQBeTConfig

lerobot/common/policies/diffusion/modeling_diffusion.py

@@ -33,7 +33,7 @@ from diffusers.schedulers.scheduling_ddim import DDIMScheduler
 from diffusers.schedulers.scheduling_ddpm import DDPMScheduler
 from torch import Tensor, nn
 
-from lerobot.common.constants import OBS_ENV, OBS_ROBOT
+from lerobot.common.constants import OBS_ENV_STATE, OBS_STATE
 from lerobot.common.policies.diffusion.configuration_diffusion import DiffusionConfig
 from lerobot.common.policies.normalize import Normalize, Unnormalize
 from lerobot.common.policies.pretrained import PreTrainedPolicy
@@ -238,8 +238,8 @@ class DiffusionModel(nn.Module):
 
     def _prepare_global_conditioning(self, batch: dict[str, Tensor]) -> Tensor:
         """Encode image features and concatenate them all together along with the state vector."""
-        batch_size, n_obs_steps = batch[OBS_ROBOT].shape[:2]
-        global_cond_feats = [batch[OBS_ROBOT]]
+        batch_size, n_obs_steps = batch[OBS_STATE].shape[:2]
+        global_cond_feats = [batch[OBS_STATE]]
         # Extract image features.
         if self.config.image_features:
             if self.config.use_separate_rgb_encoder_per_camera:
@@ -269,7 +269,7 @@ class DiffusionModel(nn.Module):
             global_cond_feats.append(img_features)
 
         if self.config.env_state_feature:
-            global_cond_feats.append(batch[OBS_ENV])
+            global_cond_feats.append(batch[OBS_ENV_STATE])
 
         # Concatenate features then flatten to (B, global_cond_dim).
         return torch.cat(global_cond_feats, dim=-1).flatten(start_dim=1)

lerobot/common/policies/factory.py

@@ -27,7 +27,6 @@ from lerobot.common.policies.diffusion.configuration_diffusion import DiffusionC
 from lerobot.common.policies.pi0.configuration_pi0 import PI0Config
 from lerobot.common.policies.pi0fast.configuration_pi0fast import PI0FASTConfig
 from lerobot.common.policies.pretrained import PreTrainedPolicy
-from lerobot.common.policies.smolvla.configuration_smolvla import SmolVLAConfig
 from lerobot.common.policies.tdmpc.configuration_tdmpc import TDMPCConfig
 from lerobot.common.policies.vqbet.configuration_vqbet import VQBeTConfig
 from lerobot.configs.policies import PreTrainedConfig
@@ -60,10 +59,6 @@ def get_policy_class(name: str) -> PreTrainedPolicy:
         from lerobot.common.policies.pi0fast.modeling_pi0fast import PI0FASTPolicy
 
         return PI0FASTPolicy
-    elif name == "smolvla":
-        from lerobot.common.policies.smolvla.modeling_smolvla import SmolVLAPolicy
-
-        return SmolVLAPolicy
     else:
         raise NotImplementedError(f"Policy with name {name} is not implemented.")
 
@@ -81,8 +76,6 @@ def make_policy_config(policy_type: str, **kwargs) -> PreTrainedConfig:
         return PI0Config(**kwargs)
     elif policy_type == "pi0fast":
         return PI0FASTConfig(**kwargs)
-    elif policy_type == "smolvla":
-        return SmolVLAConfig(**kwargs)
     else:
         raise ValueError(f"Policy type '{policy_type}' is not available.")
 

lerobot/common/policies/pi0/modeling_pi0.py

@@ -57,7 +57,7 @@ import torch.nn.functional as F  # noqa: N812
 from torch import Tensor, nn
 from transformers import AutoTokenizer
 
-from lerobot.common.constants import ACTION, OBS_ROBOT
+from lerobot.common.constants import ACTION, OBS_STATE
 from lerobot.common.policies.normalize import Normalize, Unnormalize
 from lerobot.common.policies.pi0.configuration_pi0 import PI0Config
 from lerobot.common.policies.pi0.paligemma_with_expert import (
@@ -271,7 +271,7 @@ class PI0Policy(PreTrainedPolicy):
         self.eval()
 
         if self.config.adapt_to_pi_aloha:
-            batch[OBS_ROBOT] = self._pi_aloha_decode_state(batch[OBS_ROBOT])
+            batch[OBS_STATE] = self._pi_aloha_decode_state(batch[OBS_STATE])
 
         batch = self.normalize_inputs(batch)
 
@@ -303,7 +303,7 @@ class PI0Policy(PreTrainedPolicy):
     def forward(self, batch: dict[str, Tensor], noise=None, time=None) -> tuple[Tensor, dict[str, Tensor]]:
         """Do a full training forward pass to compute the loss"""
         if self.config.adapt_to_pi_aloha:
-            batch[OBS_ROBOT] = self._pi_aloha_decode_state(batch[OBS_ROBOT])
+            batch[OBS_STATE] = self._pi_aloha_decode_state(batch[OBS_STATE])
             batch[ACTION] = self._pi_aloha_encode_actions_inv(batch[ACTION])
 
         batch = self.normalize_inputs(batch)
@@ -357,7 +357,7 @@ class PI0Policy(PreTrainedPolicy):
             if self.config.resize_imgs_with_padding is not None:
                 img = resize_with_pad(img, *self.config.resize_imgs_with_padding, pad_value=0)
 
-            # Normalize from range [0,1] to [-1,1] as expected by siglip
+            # Normalize from range [0,1] to [-1,1] as expacted by siglip
             img = img * 2.0 - 1.0
 
             bsize = img.shape[0]
@@ -380,7 +380,7 @@ class PI0Policy(PreTrainedPolicy):
 
     def prepare_language(self, batch) -> tuple[Tensor, Tensor]:
         """Tokenize the text input"""
-        device = batch[OBS_ROBOT].device
+        device = batch[OBS_STATE].device
         tasks = batch["task"]
 
         # PaliGemma prompt has to end with a new line
@@ -427,7 +427,7 @@ class PI0Policy(PreTrainedPolicy):
 
     def prepare_state(self, batch):
         """Pad state"""
-        state = pad_vector(batch[OBS_ROBOT], self.config.max_state_dim)
+        state = pad_vector(batch[OBS_STATE], self.config.max_state_dim)
         return state
 
     def prepare_action(self, batch):

lerobot/common/policies/pi0fast/modeling_pi0fast.py

@@ -516,7 +516,7 @@ class PI0FAST(nn.Module):
                         interpolate_like_pi=self.config.interpolate_like_pi,
                     )
 
-                # Normalize from range [0,1] to [-1,1] as expected by siglip
+                # Normalize from range [0,1] to [-1,1] as expacted by siglip
                 img = img * 2.0 - 1.0
 
                 bsize = img.shape[0]

lerobot/common/policies/smolvla/configuration_smolvla.py

@@ -1,154 +0,0 @@
-# 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.optim.optimizers import AdamWConfig
-from lerobot.common.optim.schedulers import (
-    CosineDecayWithWarmupSchedulerConfig,
-)
-from lerobot.configs.policies import PreTrainedConfig
-from lerobot.configs.types import FeatureType, NormalizationMode, PolicyFeature
-
-
-@PreTrainedConfig.register_subclass("smolvla")
-@dataclass
-class SmolVLAConfig(PreTrainedConfig):
-    # Input / output structure.
-    n_obs_steps: int = 1
-    chunk_size: int = 50
-    n_action_steps: int = 50
-
-    normalization_mapping: dict[str, NormalizationMode] = field(
-        default_factory=lambda: {
-            "VISUAL": NormalizationMode.IDENTITY,
-            "STATE": NormalizationMode.MEAN_STD,
-            "ACTION": NormalizationMode.MEAN_STD,
-        }
-    )
-
-    # Shorter state and action vectors will be padded
-    max_state_dim: int = 32
-    max_action_dim: int = 32
-
-    # Image preprocessing
-    resize_imgs_with_padding: tuple[int, int] = (512, 512)
-
-    # Add empty images. Used by smolvla_aloha_sim which adds the empty
-    # left and right wrist cameras in addition to the top camera.
-    empty_cameras: int = 0
-
-    # Converts the joint and gripper values from the standard Aloha space to
-    # the space used by the pi internal runtime which was used to train the base model.
-    adapt_to_pi_aloha: bool = False
-
-    # Converts joint dimensions to deltas with respect to the current state before passing to the model.
-    # Gripper dimensions will remain in absolute values.
-    use_delta_joint_actions_aloha: bool = False
-
-    # Tokenizer
-    tokenizer_max_length: int = 48
-
-    # Decoding
-    num_steps: int = 10
-
-    # Attention utils
-    use_cache: bool = True
-
-    # Finetuning settings
-    freeze_vision_encoder: bool = True
-    train_expert_only: bool = True
-    train_state_proj: bool = True
-
-    # Training presets
-    optimizer_lr: float = 1e-4
-    optimizer_betas: tuple[float, float] = (0.9, 0.95)
-    optimizer_eps: float = 1e-8
-    optimizer_weight_decay: float = 1e-10
-    optimizer_grad_clip_norm: float = 10
-
-    scheduler_warmup_steps: int = 1_000
-    scheduler_decay_steps: int = 30_000
-    scheduler_decay_lr: float = 2.5e-6
-
-    vlm_model_name: str = "HuggingFaceTB/SmolVLM2-500M-Video-Instruct"  # Select the VLM backbone.
-    load_vlm_weights: bool = False  # Set to True in case of training the expert from scratch. True when init from pretrained SmolVLA weights
-
-    add_image_special_tokens: bool = False  # Whether to use special image tokens around image features.
-
-    attention_mode: str = "cross_attn"
-
-    prefix_length: int = -1
-
-    pad_language_to: str = "longest"  # "max_length"
-
-    num_expert_layers: int = -1  # Less or equal to 0 is the default where the action expert has the same number of layers of VLM. Otherwise the expert have less layers.
-    num_vlm_layers: int = 16  # Number of layers used in the VLM (first num_vlm_layers layers)
-    self_attn_every_n_layers: int = 2  # Interleave SA layers each self_attn_every_n_layers
-    expert_width_multiplier: float = 0.75  # The action expert hidden size (wrt to the VLM)
-
-    min_period: float = 4e-3  # sensitivity range for the timestep used in sine-cosine positional encoding
-    max_period: float = 4.0
-
-    def __post_init__(self):
-        super().__post_init__()
-
-        """Input validation (not exhaustive)."""
-        if self.n_action_steps > self.chunk_size:
-            raise ValueError(
-                f"The chunk size is the upper bound for the number of action steps per model invocation. Got "
-                f"{self.n_action_steps} for `n_action_steps` and {self.chunk_size} for `chunk_size`."
-            )
-        if self.use_delta_joint_actions_aloha:
-            raise NotImplementedError(
-                "`use_delta_joint_actions_aloha` is used by smolvla for aloha real models. It is not ported yet in LeRobot."
-            )
-
-    def validate_features(self) -> None:
-        for i in range(self.empty_cameras):
-            key = f"observation.images.empty_camera_{i}"
-            empty_camera = PolicyFeature(
-                type=FeatureType.VISUAL,
-                shape=(3, 480, 640),
-            )
-            self.input_features[key] = empty_camera
-
-    def get_optimizer_preset(self) -> AdamWConfig:
-        return AdamWConfig(
-            lr=self.optimizer_lr,
-            betas=self.optimizer_betas,
-            eps=self.optimizer_eps,
-            weight_decay=self.optimizer_weight_decay,
-            grad_clip_norm=self.optimizer_grad_clip_norm,
-        )
-
-    def get_scheduler_preset(self):
-        return CosineDecayWithWarmupSchedulerConfig(
-            peak_lr=self.optimizer_lr,
-            decay_lr=self.scheduler_decay_lr,
-            num_warmup_steps=self.scheduler_warmup_steps,
-            num_decay_steps=self.scheduler_decay_steps,
-        )
-
-    @property
-    def observation_delta_indices(self) -> list:
-        return [0]
-
-    @property
-    def action_delta_indices(self) -> list:
-        return list(range(self.chunk_size))
-
-    @property
-    def reward_delta_indices(self) -> None:
-        return None

lerobot/common/policies/smolvla/modeling_smolvla.py

@@ -1,801 +0,0 @@
-#!/usr/bin/env python
-
-# Copyright 2025 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.
-
-"""
-SmolVLA:
-
-[Paper](https://huggingface.co/papers/2506.01844)
-
-Designed by Hugging Face.
-
-Install smolvla extra dependencies:
-```bash
-pip install -e ".[smolvla]"
-```
-
-Example of finetuning the smolvla pretrained model (`smolvla_base`):
-```bash
-python lerobot/scripts/train.py \
---policy.path=lerobot/smolvla_base \
---dataset.repo_id=danaaubakirova/svla_so100_task1_v3 \
---batch_size=64 \
---steps=200000
-```
-
-Example of finetuning a smolVLA. SmolVLA is composed of a pretrained VLM,
-and an action expert.
-```bash
-python lerobot/scripts/train.py \
---policy.type=smolvla \
---dataset.repo_id=danaaubakirova/svla_so100_task1_v3 \
---batch_size=64 \
---steps=200000
-```
-
-Example of using the smolvla pretrained model outside LeRobot training framework:
-```python
-policy = SmolVLAPolicy.from_pretrained("lerobot/smolvla_base")
-```
-
-"""
-
-import math
-from collections import deque
-
-import torch
-import torch.nn.functional as F  # noqa: N812
-from torch import Tensor, nn
-from transformers import AutoProcessor
-
-from lerobot.common.constants import ACTION, OBS_ROBOT
-from lerobot.common.policies.normalize import (
-    Normalize,
-    Unnormalize,
-)
-from lerobot.common.policies.pretrained import PreTrainedPolicy
-from lerobot.common.policies.smolvla.configuration_smolvla import SmolVLAConfig
-from lerobot.common.policies.smolvla.smolvlm_with_expert import SmolVLMWithExpertModel
-from lerobot.common.policies.utils import (
-    populate_queues,
-)
-from lerobot.common.utils.utils import get_safe_dtype
-
-
-def create_sinusoidal_pos_embedding(
-    time: torch.tensor, dimension: int, min_period: float, max_period: float, device="cpu"
-) -> Tensor:
-    """Computes sine-cosine positional embedding vectors for scalar positions."""
-    if dimension % 2 != 0:
-        raise ValueError(f"dimension ({dimension}) must be divisible by 2")
-
-    if time.ndim != 1:
-        raise ValueError("The time tensor is expected to be of shape `(batch_size, )`.")
-
-    dtype = get_safe_dtype(torch.float64, device.type)
-    fraction = torch.linspace(0.0, 1.0, dimension // 2, dtype=dtype, device=device)
-    period = min_period * (max_period / min_period) ** fraction
-
-    # Compute the outer product
-    scaling_factor = 1.0 / period * 2 * math.pi
-    sin_input = scaling_factor[None, :] * time[:, None]
-    pos_emb = torch.cat([torch.sin(sin_input), torch.cos(sin_input)], dim=1)
-    return pos_emb
-
-
-def sample_beta(alpha, beta, bsize, device):
-    gamma1 = torch.empty((bsize,), device=device).uniform_(0, 1).pow(1 / alpha)
-    gamma2 = torch.empty((bsize,), device=device).uniform_(0, 1).pow(1 / beta)
-    return gamma1 / (gamma1 + gamma2)
-
-
-def make_att_2d_masks(pad_masks, att_masks):
-    """Copied from big_vision.
-
-    Tokens can attend to valid inputs tokens which have a cumulative mask_ar
-    smaller or equal to theirs. This way `mask_ar` int[B, N] can be used to
-    setup several types of attention, for example:
-
-      [[1 1 1 1 1 1]]: pure causal attention.
-
-      [[0 0 0 1 1 1]]: prefix-lm attention. The first 3 tokens can attend between
-          themselves and the last 3 tokens have a causal attention. The first
-          entry could also be a 1 without changing behaviour.
-
-      [[1 0 1 0 1 0 0 1 0 0]]: causal attention between 4 blocks. Tokens of a
-          block can attend all previous blocks and all tokens on the same block.
-
-    Args:
-      input_mask: bool[B, N] true if its part of the input, false if padding.
-      mask_ar: int32[B, N] mask that's 1 where previous tokens cannot depend on
-        it and 0 where it shares the same attention mask as the previous token.
-    """
-    if att_masks.ndim != 2:
-        raise ValueError(att_masks.ndim)
-    if pad_masks.ndim != 2:
-        raise ValueError(pad_masks.ndim)
-
-    cumsum = torch.cumsum(att_masks, dim=1)
-    att_2d_masks = cumsum[:, None, :] <= cumsum[:, :, None]
-    pad_2d_masks = pad_masks[:, None, :] * pad_masks[:, :, None]
-    att_2d_masks = att_2d_masks & pad_2d_masks
-    return att_2d_masks
-
-
-def resize_with_pad(img, width, height, pad_value=-1):
-    # assume no-op when width height fits already
-    if img.ndim != 4:
-        raise ValueError(f"(b,c,h,w) expected, but {img.shape}")
-
-    cur_height, cur_width = img.shape[2:]
-
-    ratio = max(cur_width / width, cur_height / height)
-    resized_height = int(cur_height / ratio)
-    resized_width = int(cur_width / ratio)
-    resized_img = F.interpolate(
-        img, size=(resized_height, resized_width), mode="bilinear", align_corners=False
-    )
-
-    pad_height = max(0, int(height - resized_height))
-    pad_width = max(0, int(width - resized_width))
-
-    # pad on left and top of image
-    padded_img = F.pad(resized_img, (pad_width, 0, pad_height, 0), value=pad_value)
-    return padded_img
-
-
-def pad_vector(vector, new_dim):
-    """Can be (batch_size x sequence_length x features_dimension)
-    or (batch_size x features_dimension)
-    """
-    if vector.shape[-1] == new_dim:
-        return vector
-    shape = list(vector.shape)
-    current_dim = shape[-1]
-    shape[-1] = new_dim
-    new_vector = torch.zeros(*shape, dtype=vector.dtype, device=vector.device)
-    new_vector[..., :current_dim] = vector
-    return new_vector
-
-
-def normalize(x, min_val, max_val):
-    return (x - min_val) / (max_val - min_val)
-
-
-def unnormalize(x, min_val, max_val):
-    return x * (max_val - min_val) + min_val
-
-
-def safe_arcsin(value):
-    # This ensures that the input stays within
-    # [−1,1] to avoid invalid values for arcsin
-    return torch.arcsin(torch.clamp(value, -1.0, 1.0))
-
-
-def aloha_gripper_to_angular(value):
-    # Aloha transforms the gripper positions into a linear space. The following code
-    # reverses this transformation to be consistent with smolvla which is pretrained in
-    # angular space.
-    #
-    # These values are coming from the Aloha code:
-    # PUPPET_GRIPPER_POSITION_OPEN, PUPPET_GRIPPER_POSITION_CLOSED
-    value = unnormalize(value, min_val=0.01844, max_val=0.05800)
-
-    # This is the inverse of the angular to linear transformation inside the Interbotix code.
-    def linear_to_radian(linear_position, arm_length, horn_radius):
-        value = (horn_radius**2 + linear_position**2 - arm_length**2) / (2 * horn_radius * linear_position)
-        return safe_arcsin(value)
-
-    # The constants are taken from the Interbotix code.
-    value = linear_to_radian(value, arm_length=0.036, horn_radius=0.022)
-
-    # Normalize to [0, 1].
-    # The values 0.4 and 1.5 were measured on an actual Trossen robot.
-    return normalize(value, min_val=0.4, max_val=1.5)
-
-
-def aloha_gripper_from_angular(value):
-    # Convert from the gripper position used by smolvla to the gripper position that is used by Aloha.
-    # Note that the units are still angular but the range is different.
-
-    # The values 0.4 and 1.5 were measured on an actual Trossen robot.
-    value = unnormalize(value, min_val=0.4, max_val=1.5)
-
-    # These values are coming from the Aloha code:
-    # PUPPET_GRIPPER_JOINT_OPEN, PUPPET_GRIPPER_JOINT_CLOSE
-    return normalize(value, min_val=-0.6213, max_val=1.4910)
-
-
-def aloha_gripper_from_angular_inv(value):
-    # Directly inverts the gripper_from_angular function.
-    value = unnormalize(value, min_val=-0.6213, max_val=1.4910)
-    return normalize(value, min_val=0.4, max_val=1.5)
-
-
-class SmolVLAPolicy(PreTrainedPolicy):
-    """Wrapper class around VLAFlowMatching model to train and run inference within LeRobot."""
-
-    config_class = SmolVLAConfig
-    name = "smolvla"
-
-    def __init__(
-        self,
-        config: SmolVLAConfig,
-        dataset_stats: dict[str, dict[str, Tensor]] | None = None,
-    ):
-        """
-        Args:
-            config: Policy configuration class instance or None, in which case the default instantiation of
-                    the configuration class is used.
-            dataset_stats: Dataset statistics to be used for normalization. If not passed here, it is expected
-                that they will be passed with a call to `load_state_dict` before the policy is used.
-        """
-
-        super().__init__(config)
-        config.validate_features()
-        self.config = config
-        self.normalize_inputs = Normalize(config.input_features, config.normalization_mapping, dataset_stats)
-        self.normalize_targets = Normalize(
-            config.output_features, config.normalization_mapping, dataset_stats
-        )
-        self.unnormalize_outputs = Unnormalize(
-            config.output_features, config.normalization_mapping, dataset_stats
-        )
-
-        self.language_tokenizer = AutoProcessor.from_pretrained(self.config.vlm_model_name).tokenizer
-        self.model = VLAFlowMatching(config)
-        self.reset()
-
-    def reset(self):
-        """This should be called whenever the environment is reset."""
-        self._queues = {
-            ACTION: deque(maxlen=self.config.n_action_steps),
-        }
-
-    def get_optim_params(self) -> dict:
-        return self.parameters()
-
-    @torch.no_grad
-    def select_action(self, batch: dict[str, Tensor], noise: Tensor | None = None) -> Tensor:
-        """Select a single action given environment observations.
-
-        This method wraps `select_actions` in order to return one action at a time for execution in the
-        environment. It works by managing the actions in a queue and only calling `select_actions` when the
-        queue is empty.
-        """
-        self.eval()
-
-        if self.config.adapt_to_pi_aloha:
-            batch[OBS_ROBOT] = self._pi_aloha_decode_state(batch[OBS_ROBOT])
-
-        batch = self.normalize_inputs(batch)
-
-        self._queues = populate_queues(self._queues, batch, exclude_keys=[ACTION])
-        # Action queue logic for n_action_steps > 1. When the action_queue is depleted, populate it by
-        # querying the policy.
-        if len(self._queues[ACTION]) == 0:
-            for k in batch:
-                if k in self._queues:
-                    batch[k] = torch.stack(list(self._queues[k]), dim=1)
-            images, img_masks = self.prepare_images(batch)
-            state = self.prepare_state(batch)
-            lang_tokens, lang_masks = self.prepare_language(batch)
-
-            actions = self.model.sample_actions(
-                images, img_masks, lang_tokens, lang_masks, state, noise=noise
-            )
-            # Unpad actions
-            original_action_dim = self.config.action_feature.shape[0]
-            actions = actions[:, :, :original_action_dim]
-
-            actions = self.unnormalize_outputs({"action": actions})["action"]
-
-            if self.config.adapt_to_pi_aloha:
-                actions = self._pi_aloha_encode_actions(actions)
-
-            # `self.model.forward` returns a (batch_size, n_action_steps, action_dim) tensor, but the queue
-            # effectively has shape (n_action_steps, batch_size, *), hence the transpose.
-            self._queues[ACTION].extend(actions.transpose(0, 1)[: self.config.n_action_steps])
-        return self._queues[ACTION].popleft()
-
-    def forward(self, batch: dict[str, Tensor], noise=None, time=None) -> dict[str, Tensor]:
-        """Do a full training forward pass to compute the loss"""
-        if self.config.adapt_to_pi_aloha:
-            batch[OBS_ROBOT] = self._pi_aloha_decode_state(batch[OBS_ROBOT])
-            batch[ACTION] = self._pi_aloha_encode_actions_inv(batch[ACTION])
-        batch = self.normalize_inputs(batch)
-        batch = self.normalize_targets(batch)
-        images, img_masks = self.prepare_images(batch)
-        state = self.prepare_state(batch)
-        lang_tokens, lang_masks = self.prepare_language(batch)
-        actions = self.prepare_action(batch)
-        actions_is_pad = batch.get("actions_id_pad")
-        loss_dict = {}
-        losses = self.model.forward(images, img_masks, lang_tokens, lang_masks, state, actions, noise, time)
-        loss_dict["losses_after_forward"] = losses.clone()
-
-        if actions_is_pad is not None:
-            in_episode_bound = ~actions_is_pad
-            losses = losses * in_episode_bound.unsqueeze(-1)
-            loss_dict["losses_after_in_ep_bound"] = losses.clone()
-
-        # Remove padding
-        losses = losses[:, :, : self.config.max_action_dim]
-        loss_dict["losses_after_rm_padding"] = losses.clone()
-
-        # For backward pass
-        loss = losses.mean()
-        # For backward pass
-        loss_dict["loss"] = loss
-        return loss, loss_dict
-
-    def prepare_images(self, batch):
-        """Apply SmolVLA preprocessing to the images, like resizing to 224x224 and padding to keep aspect ratio, and
-        convert pixel range from [0.0, 1.0] to [-1.0, 1.0] as requested by SigLIP.
-        """
-        images = []
-        img_masks = []
-        present_img_keys = [key for key in self.config.image_features if key in batch]
-        missing_img_keys = [key for key in self.config.image_features if key not in batch]
-
-        if len(present_img_keys) == 0:
-            raise ValueError(
-                f"All image features are missing from the batch. At least one expected. (batch: {batch.keys()}) (image_features:{self.config.image_features})"
-            )
-        # Preprocess image features present in the batch
-        for key in present_img_keys:
-            img = batch[key][:, -1, :, :, :] if batch[key].ndim == 5 else batch[key]
-            if self.config.resize_imgs_with_padding is not None:
-                img = resize_with_pad(img, *self.config.resize_imgs_with_padding, pad_value=0)
-
-            # Normalize from range [0,1] to [-1,1] as expacted by siglip
-            img = img * 2.0 - 1.0
-
-            bsize = img.shape[0]
-            device = img.device
-            if f"{key}_padding_mask" in batch:
-                mask = batch[f"{key}_padding_mask"].bool()
-            else:
-                mask = torch.ones(bsize, dtype=torch.bool, device=device)
-            images.append(img)
-            img_masks.append(mask)
-
-        # Create image features not present in the batch
-        # as fully 0 padded images.
-        for num_empty_cameras in range(len(missing_img_keys)):
-            if num_empty_cameras >= self.config.empty_cameras:
-                break
-            img = torch.ones_like(img) * -1
-            mask = torch.zeros_like(mask)
-            images.append(img)
-            img_masks.append(mask)
-        return images, img_masks
-
-    def prepare_language(self, batch) -> tuple[Tensor, Tensor]:
-        """Tokenize the text input"""
-        device = batch[OBS_ROBOT].device
-        tasks = batch["task"]
-        if len(tasks) == 1:
-            tasks = [tasks[0] for _ in range(batch[OBS_ROBOT].shape[0])]
-
-        tasks = [task if task.endswith("\n") else f"{task}\n" for task in tasks]
-        tokenized_prompt = self.language_tokenizer.__call__(
-            tasks,
-            padding=self.config.pad_language_to,
-            padding_side="right",
-            max_length=self.config.tokenizer_max_length,
-            return_tensors="pt",
-        )
-        lang_tokens = tokenized_prompt["input_ids"].to(device=device)
-        lang_masks = tokenized_prompt["attention_mask"].to(device=device, dtype=torch.bool)
-
-        return lang_tokens, lang_masks
-
-    def _pi_aloha_decode_state(self, state):
-        # Flip the joints.
-        for motor_idx in [1, 2, 8, 9]:
-            state[:, motor_idx] *= -1
-        # Reverse the gripper transformation that is being applied by the Aloha runtime.
-        for motor_idx in [6, 13]:
-            state[:, motor_idx] = aloha_gripper_to_angular(state[:, motor_idx])
-        return state
-
-    def _pi_aloha_encode_actions(self, actions):
-        # Flip the joints.
-        for motor_idx in [1, 2, 8, 9]:
-            actions[:, :, motor_idx] *= -1
-        # Reverse the gripper transformation that is being applied by the Aloha runtime.
-        for motor_idx in [6, 13]:
-            actions[:, :, motor_idx] = aloha_gripper_from_angular(actions[:, :, motor_idx])
-        return actions
-
-    def _pi_aloha_encode_actions_inv(self, actions):
-        # Flip the joints again.
-        for motor_idx in [1, 2, 8, 9]:
-            actions[:, :, motor_idx] *= -1
-        # Reverse the gripper transformation that is being applied by the Aloha runtime.
-        for motor_idx in [6, 13]:
-            actions[:, :, motor_idx] = aloha_gripper_from_angular_inv(actions[:, :, motor_idx])
-        return actions
-
-    def prepare_state(self, batch):
-        """Pad state"""
-        state = batch[OBS_ROBOT][:, -1, :] if batch[OBS_ROBOT].ndim > 2 else batch[OBS_ROBOT]
-        state = pad_vector(state, self.config.max_state_dim)
-        return state
-
-    def prepare_action(self, batch):
-        """Pad action"""
-        actions = pad_vector(batch[ACTION], self.config.max_action_dim)
-        return actions
-
-
-def pad_tensor(tensor, max_len, pad_value=0):
-    """
-    Efficiently pads a tensor along sequence dimension to match max_len.
-
-    Args:
-        tensor (torch.Tensor): Shape (B, L, ...) or (B, L).
-        max_len (int): Fixed sequence length.
-        pad_value (int/float): Value for padding.
-
-    Returns:
-        torch.Tensor: Shape (B, max_len, ...) or (B, max_len).
-    """
-    b, d = tensor.shape[:2]
-
-    # Create a padded tensor of max_len and copy the existing values
-    padded_tensor = torch.full(
-        (b, max_len, *tensor.shape[2:]), pad_value, dtype=tensor.dtype, device=tensor.device
-    )
-    padded_tensor[:, :d] = tensor  # Efficient in-place copy
-
-    return padded_tensor
-
-
-class VLAFlowMatching(nn.Module):
-    """
-    SmolVLA
-
-    [Paper]()
-
-    Designed by Hugging Face.
-    ┌──────────────────────────────┐
-    │                 actions      │
-    │                    ▲         │
-    │ ┌─────────┐      ┌─|────┐    │
-    │ |         │────► │      │    │
-    │ |         │ kv   │      │    │
-    │ |         │────► │Action│    │
-    │ |   VLM   │cache │Expert│    |
-    │ │         │────► |      │    │
-    │ │         │      │      │    │
-    │ └▲──▲───▲─┘      └───▲──┘    |
-    │  │  |   |            │       |
-    │  |  |   |          noise     │
-    │  │  │ state                  │
-    │  │ language tokens           │
-    │  image(s)                    │
-    └──────────────────────────────┘
-    """
-
-    def __init__(self, config):
-        super().__init__()
-        self.config = config
-
-        self.vlm_with_expert = SmolVLMWithExpertModel(
-            model_id=self.config.vlm_model_name,
-            freeze_vision_encoder=self.config.freeze_vision_encoder,
-            train_expert_only=self.config.train_expert_only,
-            load_vlm_weights=self.config.load_vlm_weights,
-            attention_mode=self.config.attention_mode,
-            num_expert_layers=self.config.num_expert_layers,
-            num_vlm_layers=self.config.num_vlm_layers,
-            self_attn_every_n_layers=self.config.self_attn_every_n_layers,
-            expert_width_multiplier=self.config.expert_width_multiplier,
-        )
-        self.state_proj = nn.Linear(
-            self.config.max_state_dim, self.vlm_with_expert.config.text_config.hidden_size
-        )
-        self.action_in_proj = nn.Linear(self.config.max_action_dim, self.vlm_with_expert.expert_hidden_size)
-        self.action_out_proj = nn.Linear(self.vlm_with_expert.expert_hidden_size, self.config.max_action_dim)
-
-        self.action_time_mlp_in = nn.Linear(
-            self.vlm_with_expert.expert_hidden_size * 2, self.vlm_with_expert.expert_hidden_size
-        )
-        self.action_time_mlp_out = nn.Linear(
-            self.vlm_with_expert.expert_hidden_size, self.vlm_with_expert.expert_hidden_size
-        )
-
-        self.set_requires_grad()
-        self.fake_image_token = self.vlm_with_expert.processor.tokenizer.fake_image_token_id
-        self.global_image_token = self.vlm_with_expert.processor.tokenizer.global_image_token_id
-        self.global_image_start_token = torch.tensor(
-            [self.fake_image_token, self.global_image_token], dtype=torch.long
-        )
-
-        self.add_image_special_tokens = self.config.add_image_special_tokens
-        self.image_end_token = torch.tensor([self.fake_image_token], dtype=torch.long)
-        self.prefix_length = self.config.prefix_length
-
-    def set_requires_grad(self):
-        for params in self.state_proj.parameters():
-            params.requires_grad = self.config.train_state_proj
-
-    def sample_noise(self, shape, device):
-        noise = torch.normal(
-            mean=0.0,
-            std=1.0,
-            size=shape,
-            dtype=torch.float32,
-            device=device,
-        )
-        return noise
-
-    def sample_time(self, bsize, device):
-        time_beta = sample_beta(1.5, 1.0, bsize, device)
-        time = time_beta * 0.999 + 0.001
-        return time.to(dtype=torch.float32, device=device)
-
-    def embed_prefix(
-        self, images, img_masks, lang_tokens, lang_masks, state: torch.Tensor = None
-    ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]:
-        """Embed images with SigLIP and language tokens with embedding layer to prepare
-        for SmolVLM transformer processing.
-        """
-        embs = []
-        pad_masks = []
-        att_masks = []
-        for _img_idx, (
-            img,
-            img_mask,
-        ) in enumerate(zip(images, img_masks, strict=False)):
-            if self.add_image_special_tokens:
-                image_start_token = (
-                    self.vlm_with_expert.embed_language_tokens(
-                        self.global_image_start_token.to(device=self.vlm_with_expert.vlm.device)
-                    )
-                    .unsqueeze(0)
-                    .expand(img.shape[0], -1, -1)
-                )
-                image_start_mask = torch.ones_like(
-                    image_start_token[:, :, 0], dtype=torch.bool, device=image_start_token.device
-                )
-                att_masks += [0] * (image_start_mask.shape[-1])
-                embs.append(image_start_token)
-                pad_masks.append(image_start_mask)
-
-            img_emb = self.vlm_with_expert.embed_image(img)
-            img_emb = img_emb
-
-            # Normalize image embeddings
-            img_emb_dim = img_emb.shape[-1]
-            img_emb = img_emb * torch.tensor(img_emb_dim**0.5, dtype=img_emb.dtype, device=img_emb.device)
-
-            bsize, num_img_embs = img_emb.shape[:2]
-            img_mask = img_mask[:, None].expand(bsize, num_img_embs)
-
-            embs.append(img_emb)
-            pad_masks.append(img_mask)
-
-            att_masks += [0] * (num_img_embs)
-            if self.add_image_special_tokens:
-                image_end_token = (
-                    self.vlm_with_expert.embed_language_tokens(
-                        self.image_end_token.to(device=self.vlm_with_expert.vlm.device)
-                    )
-                    .unsqueeze(0)
-                    .expand(img.shape[0], -1, -1)
-                )
-                image_end_mask = torch.ones_like(
-                    image_end_token[:, :, 0], dtype=torch.bool, device=image_end_token.device
-                )
-                embs.append(image_end_token)
-                pad_masks.append(image_end_mask)
-                att_masks += [0] * (image_end_mask.shape[1])
-        lang_emb = self.vlm_with_expert.embed_language_tokens(lang_tokens)
-        # Normalize language embeddings
-        lang_emb_dim = lang_emb.shape[-1]
-        lang_emb = lang_emb * math.sqrt(lang_emb_dim)
-
-        embs.append(lang_emb)
-        pad_masks.append(lang_masks)
-
-        num_lang_embs = lang_emb.shape[1]
-        att_masks += [0] * num_lang_embs
-
-        state_emb = self.state_proj(state)
-        state_emb = state_emb[:, None, :] if state_emb.ndim == 2 else state_emb
-        embs.append(state_emb)
-        bsize = state_emb.shape[0]
-        device = state_emb.device
-
-        states_seq_len = state_emb.shape[1]
-        state_mask = torch.ones(bsize, states_seq_len, dtype=torch.bool, device=device)
-        pad_masks.append(state_mask)
-
-        # Set attention masks so that image and language inputs do not attend to state or actions
-        att_masks += [1] * (states_seq_len)
-        embs = torch.cat(embs, dim=1)
-        pad_masks = torch.cat(pad_masks, dim=1)
-        att_masks = torch.tensor(att_masks, dtype=torch.bool, device=pad_masks.device)
-        att_masks = att_masks[None, :]
-
-        seq_len = pad_masks.shape[1]
-        if seq_len < self.prefix_length:
-            embs = pad_tensor(embs, self.prefix_length, pad_value=0)
-            pad_masks = pad_tensor(pad_masks, self.prefix_length, pad_value=0)
-            att_masks = pad_tensor(att_masks, self.prefix_length, pad_value=0)
-
-        att_masks = att_masks.expand(bsize, -1)
-
-        return embs, pad_masks, att_masks
-
-    def embed_suffix(self, noisy_actions, timestep):
-        """Embed state, noisy_actions, timestep to prepare for Expert Gemma processing."""
-        embs = []
-        pad_masks = []
-        att_masks = []
-
-        # Fuse timestep + action information using an MLP
-        action_emb = self.action_in_proj(noisy_actions)
-        device = action_emb.device
-        bsize = action_emb.shape[0]
-        dtype = action_emb.dtype
-        # Embed timestep using sine-cosine positional encoding with sensitivity in the range [0, 1]
-        time_emb = create_sinusoidal_pos_embedding(
-            timestep,
-            self.vlm_with_expert.expert_hidden_size,
-            self.config.min_period,
-            self.config.max_period,
-            device=device,
-        )
-        time_emb = time_emb.type(dtype=dtype)
-
-        time_emb = time_emb[:, None, :].expand_as(action_emb)
-        action_time_emb = torch.cat([action_emb, time_emb], dim=2)
-
-        action_time_emb = self.action_time_mlp_in(action_time_emb)
-        action_time_emb = F.silu(action_time_emb)  # swish == silu
-        action_time_emb = self.action_time_mlp_out(action_time_emb)
-
-        # Add to input tokens
-        embs.append(action_time_emb)
-
-        bsize, action_time_dim = action_time_emb.shape[:2]
-        action_time_mask = torch.ones(bsize, action_time_dim, dtype=torch.bool, device=device)
-        pad_masks.append(action_time_mask)
-
-        # Set attention masks so that image, language and state inputs do not attend to action tokens
-        att_masks += [1] * self.config.chunk_size
-        embs = torch.cat(embs, dim=1)
-        pad_masks = torch.cat(pad_masks, dim=1)
-        att_masks = torch.tensor(att_masks, dtype=embs.dtype, device=embs.device)
-        att_masks = att_masks[None, :].expand(bsize, len(att_masks))
-        return embs, pad_masks, att_masks
-
-    def forward(
-        self, images, img_masks, lang_tokens, lang_masks, state, actions, noise=None, time=None
-    ) -> Tensor:
-        """Do a full training forward pass and compute the loss (batch_size x num_steps x num_motors)"""
-        if noise is None:
-            noise = self.sample_noise(actions.shape, actions.device)
-
-        if time is None:
-            time = self.sample_time(actions.shape[0], actions.device)
-
-        time_expanded = time[:, None, None]
-        x_t = time_expanded * noise + (1 - time_expanded) * actions
-        u_t = noise - actions
-        prefix_embs, prefix_pad_masks, prefix_att_masks = self.embed_prefix(
-            images, img_masks, lang_tokens, lang_masks, state=state
-        )
-        suffix_embs, suffix_pad_masks, suffix_att_masks = self.embed_suffix(x_t, time)
-
-        pad_masks = torch.cat([prefix_pad_masks, suffix_pad_masks], dim=1)
-        att_masks = torch.cat([prefix_att_masks, suffix_att_masks], dim=1)
-
-        att_2d_masks = make_att_2d_masks(pad_masks, att_masks)
-        position_ids = torch.cumsum(pad_masks, dim=1) - 1
-        (_, suffix_out), _ = self.vlm_with_expert.forward(
-            attention_mask=att_2d_masks,
-            position_ids=position_ids,
-            past_key_values=None,
-            inputs_embeds=[prefix_embs, suffix_embs],
-            use_cache=False,
-            fill_kv_cache=False,
-        )
-        suffix_out = suffix_out[:, -self.config.chunk_size :]
-        # Original openpi code, upcast attention output
-        suffix_out = suffix_out.to(dtype=torch.float32)
-        v_t = self.action_out_proj(suffix_out)
-        losses = F.mse_loss(u_t, v_t, reduction="none")
-        return losses
-
-    def sample_actions(self, images, img_masks, lang_tokens, lang_masks, state, noise=None) -> Tensor:
-        """Do a full inference forward and compute the action (batch_size x num_steps x num_motors)"""
-        bsize = state.shape[0]
-        device = state.device
-
-        if noise is None:
-            actions_shape = (bsize, self.config.chunk_size, self.config.max_action_dim)
-            noise = self.sample_noise(actions_shape, device)
-
-        prefix_embs, prefix_pad_masks, prefix_att_masks = self.embed_prefix(
-            images, img_masks, lang_tokens, lang_masks, state=state
-        )
-        prefix_att_2d_masks = make_att_2d_masks(prefix_pad_masks, prefix_att_masks)
-        prefix_position_ids = torch.cumsum(prefix_pad_masks, dim=1) - 1
-        # Compute image and language key value cache
-        _, past_key_values = self.vlm_with_expert.forward(
-            attention_mask=prefix_att_2d_masks,
-            position_ids=prefix_position_ids,
-            past_key_values=None,
-            inputs_embeds=[prefix_embs, None],
-            use_cache=self.config.use_cache,
-            fill_kv_cache=True,
-        )
-        dt = -1.0 / self.config.num_steps
-        dt = torch.tensor(dt, dtype=torch.float32, device=device)
-
-        x_t = noise
-        time = torch.tensor(1.0, dtype=torch.float32, device=device)
-        while time >= -dt / 2:
-            expanded_time = time.expand(bsize)
-            v_t = self.denoise_step(
-                prefix_pad_masks,
-                past_key_values,
-                x_t,
-                expanded_time,
-            )
-            # Euler step
-            x_t += dt * v_t
-            time += dt
-        return x_t
-
-    def denoise_step(
-        self,
-        prefix_pad_masks,
-        past_key_values,
-        x_t,
-        timestep,
-    ):
-        """Apply one denoising step of the noise `x_t` at a given timestep."""
-        suffix_embs, suffix_pad_masks, suffix_att_masks = self.embed_suffix(x_t, timestep)
-
-        suffix_len = suffix_pad_masks.shape[1]
-        batch_size = prefix_pad_masks.shape[0]
-        prefix_len = prefix_pad_masks.shape[1]
-        prefix_pad_2d_masks = prefix_pad_masks[:, None, :].expand(batch_size, suffix_len, prefix_len)
-
-        suffix_att_2d_masks = make_att_2d_masks(suffix_pad_masks, suffix_att_masks)
-
-        full_att_2d_masks = torch.cat([prefix_pad_2d_masks, suffix_att_2d_masks], dim=2)
-        prefix_offsets = torch.sum(prefix_pad_masks, dim=-1)[:, None]
-        position_ids = prefix_offsets + torch.cumsum(suffix_pad_masks, dim=1) - 1
-
-        outputs_embeds, _ = self.vlm_with_expert.forward(
-            attention_mask=full_att_2d_masks,
-            position_ids=position_ids,
-            past_key_values=past_key_values,
-            inputs_embeds=[None, suffix_embs],
-            use_cache=self.config.use_cache,
-            fill_kv_cache=False,
-        )
-        suffix_out = outputs_embeds[1]
-        suffix_out = suffix_out[:, -self.config.chunk_size :]
-        suffix_out = suffix_out.to(dtype=torch.float32)
-        v_t = self.action_out_proj(suffix_out)
-        return v_t

lerobot/common/policies/smolvla/smolvlm_with_expert.py

@@ -1,550 +0,0 @@
-# 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 copy
-from typing import List, Optional
-
-import torch
-from torch import nn
-from transformers import (
-    AutoConfig,
-    AutoModel,
-    AutoModelForImageTextToText,
-    AutoProcessor,
-    SmolVLMForConditionalGeneration,
-)
-
-
-def apply_rope(x, positions, max_wavelength=10_000):
-    """
-    Applies RoPE positions [B, L] to x [B, L, H, D].
-    """
-    d_half = x.shape[-1] // 2
-    device = x.device
-    dtype = x.dtype
-    x = x.to(torch.float32)
-
-    freq_exponents = (2.0 / x.shape[-1]) * torch.arange(d_half, dtype=torch.float32, device=device)
-    timescale = max_wavelength**freq_exponents
-    radians = positions[..., None].to(torch.float32) / timescale[None, None, :].to(torch.float32)
-
-    radians = radians[..., None, :]
-
-    sin = torch.sin(radians)  # .to(dtype=dtype)
-    cos = torch.cos(radians)  # .to(dtype=dtype)
-
-    x1, x2 = x.split(d_half, dim=-1)
-    res = torch.empty_like(x)
-    res[..., :d_half] = x1 * cos - x2 * sin
-    res[..., d_half:] = x2 * cos + x1 * sin
-
-    return res.to(dtype)
-
-
-def get_intermediate_size(hidden_dim, ffn_dim_multiplier=4, multiple_of=256):
-    hidden_dim = int(2 * hidden_dim / 3)
-    hidden_dim = int(ffn_dim_multiplier * hidden_dim)
-    hidden_dim = multiple_of * ((hidden_dim + multiple_of - 1) // multiple_of)
-    return hidden_dim
-
-
-class SmolVLMWithExpertModel(nn.Module):
-    def __init__(
-        self,
-        model_id: str = "HuggingFaceTB/SmolVLM2-500M-Video-Instruct",
-        load_vlm_weights: bool = True,
-        train_expert_only: bool = True,
-        freeze_vision_encoder: bool = False,
-        attention_mode: str = "self_attn",
-        num_expert_layers: int = -1,
-        num_vlm_layers: int = -1,
-        self_attn_every_n_layers: int = -1,
-        expert_width_multiplier: float = 0.5,
-    ):
-        super().__init__()
-        if load_vlm_weights:
-            print(f"Loading  {model_id} weights ...")
-            self.vlm = AutoModelForImageTextToText.from_pretrained(
-                model_id,
-                device_map="auto",
-                torch_dtype="bfloat16",
-                low_cpu_mem_usage=True,
-            )
-            config = self.vlm.config
-        else:
-            config = AutoConfig.from_pretrained(model_id)
-            self.vlm = SmolVLMForConditionalGeneration(config=config)
-        self.processor = AutoProcessor.from_pretrained(model_id)
-        if num_vlm_layers > 0:
-            print(f"Reducing the number of VLM layers to {num_vlm_layers} ...")
-            self.get_vlm_model().text_model.layers = self.get_vlm_model().text_model.layers[:num_vlm_layers]
-        self.num_vlm_layers = len(self.get_vlm_model().text_model.layers)
-        self.config = config
-        # Smaller lm expert
-        lm_expert_config = copy.deepcopy(config.text_config)
-        hidden_size = lm_expert_config.hidden_size
-        lm_expert_config.hidden_size = int(hidden_size * expert_width_multiplier)  # hidden_size // 2
-        lm_expert_config.intermediate_size = get_intermediate_size(int(hidden_size * expert_width_multiplier))
-        lm_expert_config.num_hidden_layers = self.num_vlm_layers
-        if num_expert_layers > 0:
-            assert len(self.get_vlm_model().text_model.layers) % num_expert_layers == 0, (
-                f"Number of layers in the VLM {len(self.get_vlm_model().text_model.layers)} are not multiple of num_expert_layers {num_expert_layers}"
-            )
-            lm_expert_config.num_hidden_layers = num_expert_layers
-        self.lm_expert = AutoModel.from_config(lm_expert_config)
-
-        self.num_expert_layers = len(self.lm_expert.layers)
-        self.self_attn_every_n_layers = self_attn_every_n_layers
-        if "cross" in attention_mode:
-            # Reshape qkv projections to have the same input dimension as the vlm
-            for layer_idx in range(len(self.lm_expert.layers)):
-                if self.self_attn_every_n_layers > 0 and layer_idx % self.self_attn_every_n_layers == 0:
-                    continue
-                self.lm_expert.layers[layer_idx].self_attn.k_proj = nn.Linear(
-                    config.text_config.num_key_value_heads * config.text_config.head_dim,
-                    lm_expert_config.num_key_value_heads * lm_expert_config.head_dim,
-                    bias=lm_expert_config.attention_bias,
-                )
-                self.lm_expert.layers[layer_idx].self_attn.v_proj = nn.Linear(
-                    config.text_config.num_key_value_heads * config.text_config.head_dim,
-                    lm_expert_config.num_key_value_heads * lm_expert_config.head_dim,
-                    bias=lm_expert_config.attention_bias,
-                )
-        # Remove unused embed_tokens
-        self.lm_expert.embed_tokens = None
-
-        self.num_attention_heads = self.config.text_config.num_attention_heads
-        self.num_key_value_heads = self.config.text_config.num_key_value_heads
-
-        self.freeze_vision_encoder = freeze_vision_encoder
-        self.train_expert_only = train_expert_only
-        self.attention_mode = attention_mode
-        self.expert_hidden_size = lm_expert_config.hidden_size
-        self.set_requires_grad()
-
-    def get_vlm_model(self):
-        return self.vlm.model
-
-    def set_requires_grad(self):
-        if self.freeze_vision_encoder:
-            self.get_vlm_model().vision_model.eval()
-            for params in self.get_vlm_model().vision_model.parameters():
-                params.requires_grad = False
-        if self.train_expert_only:
-            self.vlm.eval()
-            for params in self.vlm.parameters():
-                params.requires_grad = False
-        else:
-            # To avoid unused params issue with distributed training
-            last_layers = [self.num_vlm_layers - 1]
-            if (
-                self.num_vlm_layers != self.num_expert_layers
-                and self.num_vlm_layers % self.num_expert_layers == 0
-            ):
-                last_layers.append(self.num_vlm_layers - 2)
-            frozen_layers = [
-                "lm_head",
-                "text_model.model.norm.weight",
-            ]
-            for layer in last_layers:
-                frozen_layers.append(f"text_model.model.layers.{layer}.")
-
-            for name, params in self.vlm.named_parameters():
-                if any(k in name for k in frozen_layers):
-                    params.requires_grad = False
-        # To avoid unused params issue with distributed training
-        for name, params in self.lm_expert.named_parameters():
-            if "lm_head" in name:
-                params.requires_grad = False
-
-    def train(self, mode: bool = True):
-        super().train(mode)
-
-        if self.freeze_vision_encoder:
-            self.get_vlm_model().vision_model.eval()
-
-        if self.train_expert_only:
-            self.vlm.eval()
-
-    def embed_image(self, image: torch.Tensor):
-        patch_attention_mask = None
-        # Get sequence from the vision encoder
-        image_hidden_states = (
-            self.get_vlm_model()
-            .vision_model(
-                pixel_values=image.to(dtype=self.get_vlm_model().vision_model.dtype),
-                patch_attention_mask=patch_attention_mask,
-            )
-            .last_hidden_state
-        )
-        # Modality projection & resampling
-        image_hidden_states = self.get_vlm_model().connector(image_hidden_states)
-        return image_hidden_states
-
-    def embed_language_tokens(self, tokens: torch.Tensor):
-        return self.get_vlm_model().text_model.get_input_embeddings()(tokens)
-
-    def forward_attn_layer(
-        self,
-        model_layers,
-        inputs_embeds,
-        layer_idx,
-        position_ids,
-        attention_mask,
-        batch_size,
-        head_dim,
-        use_cache: bool = True,
-        fill_kv_cache: bool = True,
-        past_key_values=None,
-    ) -> list[torch.Tensor]:
-        query_states = []
-        key_states = []
-        value_states = []
-        for i, hidden_states in enumerate(inputs_embeds):
-            layer = model_layers[i][layer_idx]
-            if hidden_states is None or layer is None:
-                continue
-            hidden_states = layer.input_layernorm(hidden_states)
-
-            input_shape = hidden_states.shape[:-1]
-            hidden_shape = (*input_shape, -1, layer.self_attn.head_dim)
-
-            hidden_states = hidden_states.to(dtype=layer.self_attn.q_proj.weight.dtype)
-            query_state = layer.self_attn.q_proj(hidden_states).view(hidden_shape)
-            key_state = layer.self_attn.k_proj(hidden_states).view(hidden_shape)
-            value_state = layer.self_attn.v_proj(hidden_states).view(hidden_shape)
-
-            query_states.append(query_state)
-            key_states.append(key_state)
-            value_states.append(value_state)
-
-        # B,L,H,D with L sequence length, H number of heads, D head dim
-        # concatenate on the number of embeddings/tokens
-        query_states = torch.cat(query_states, dim=1)
-        key_states = torch.cat(key_states, dim=1)
-        value_states = torch.cat(value_states, dim=1)
-        seq_len = query_states.shape[1]
-        if seq_len < position_ids.shape[1]:
-            _position_ids = position_ids[:, :seq_len]
-            _attention_mask = attention_mask[:, :seq_len, :seq_len]
-        else:
-            _position_ids = position_ids
-            _attention_mask = attention_mask
-
-        attention_mask_ = _attention_mask
-        position_ids_ = _position_ids
-
-        query_states = apply_rope(query_states, position_ids_)
-        key_states = apply_rope(key_states, position_ids_)
-
-        if use_cache and past_key_values is None:
-            past_key_values = {}
-
-        if use_cache:
-            if fill_kv_cache:
-                past_key_values[layer_idx] = {
-                    "key_states": key_states,
-                    "value_states": value_states,
-                }
-            else:
-                # TODO here, some optimization can be done - similar to a `StaticCache` we can declare the `max_len` before.
-                # so we create an empty cache, with just one cuda malloc, and if (in autoregressive case) we reach
-                # the max len, then we (for instance) double the cache size. This implementation already exists
-                # in `transformers`. (molbap)
-                key_states = torch.cat([past_key_values[layer_idx]["key_states"], key_states], dim=1)
-                value_states = torch.cat([past_key_values[layer_idx]["value_states"], value_states], dim=1)
-
-        attention_interface = self.get_attention_interface()
-
-        att_output = attention_interface(
-            attention_mask_, batch_size, head_dim, query_states, key_states, value_states
-        )
-        return [att_output], past_key_values
-
-    def forward_cross_attn_layer(
-        self,
-        model_layers,
-        inputs_embeds,
-        layer_idx,
-        position_ids,
-        attention_mask,
-        batch_size,
-        head_dim,
-        use_cache: bool = True,
-        fill_kv_cache: bool = True,
-        past_key_values=None,
-    ) -> list[torch.Tensor]:
-        attention_interface = self.get_attention_interface()
-
-        att_outputs = []
-        assert len(inputs_embeds) == 2 or (use_cache and past_key_values is not None and not fill_kv_cache), (
-            f"Both len(inputs_embeds) == {len(inputs_embeds)} and past_key_values is {past_key_values}"
-        )
-
-        if len(inputs_embeds) == 2 and not past_key_values:
-            # Prefix attention
-            seq_len = inputs_embeds[0].shape[1]
-            position_id, expert_position_id = position_ids[:, :seq_len], position_ids[:, seq_len:]
-            prefix_attention_mask = attention_mask[:, :seq_len, :seq_len]
-
-            layer = model_layers[0][layer_idx]
-
-            hidden_states = layer.input_layernorm(inputs_embeds[0])
-
-            input_shape = hidden_states.shape[:-1]
-            hidden_shape = (*input_shape, -1, layer.self_attn.head_dim)
-
-            hidden_states = hidden_states.to(dtype=layer.self_attn.q_proj.weight.dtype)
-            query_state = layer.self_attn.q_proj(hidden_states).view(hidden_shape)
-            key_state = layer.self_attn.k_proj(hidden_states).view(hidden_shape)
-            value_states = layer.self_attn.v_proj(hidden_states).view(hidden_shape)
-
-            # B,L,H,D with L sequence length, H number of heads, D head dim
-            query_states = apply_rope(query_state, position_id)
-            key_states = apply_rope(key_state, position_id)
-
-            att_output = attention_interface(
-                prefix_attention_mask, batch_size, head_dim, query_states, key_states, value_states
-            )
-            att_outputs.append(att_output)
-        else:
-            expert_position_id = position_ids
-
-        if use_cache and past_key_values is None:
-            past_key_values = {}
-
-        if use_cache:
-            if fill_kv_cache:
-                past_key_values[layer_idx] = {
-                    "key_states": key_states,
-                    "value_states": value_states,
-                }
-            else:
-                # TODO here, some optimization can be done - similar to a `StaticCache` we can declare the `max_len` before.
-                # so we create an empty cache, with just one cuda malloc, and if (in autoregressive case) we reach
-                # the max len, then we (for instance) double the cache size. This implementation already exists
-                # in `transformers`. (molbap)
-                key_states = past_key_values[layer_idx]["key_states"]
-                value_states = past_key_values[layer_idx]["value_states"]
-
-        # Expert
-        expert_layer = model_layers[1][layer_idx]
-        if expert_layer is not None:
-            expert_hidden_states = expert_layer.input_layernorm(inputs_embeds[1])
-
-            expert_input_shape = expert_hidden_states.shape[:-1]
-            expert_hidden_shape = (*expert_input_shape, -1, expert_layer.self_attn.head_dim)
-
-            expert_hidden_states = expert_hidden_states.to(dtype=expert_layer.self_attn.q_proj.weight.dtype)
-            expert_query_state = expert_layer.self_attn.q_proj(expert_hidden_states).view(expert_hidden_shape)
-
-            _key_states = key_states.to(dtype=expert_layer.self_attn.k_proj.weight.dtype).view(
-                *key_states.shape[:2], -1
-            )
-            expert_key_states = expert_layer.self_attn.k_proj(_key_states).view(
-                *_key_states.shape[:-1], -1, expert_layer.self_attn.head_dim
-            )  # k_proj should have same dim as kv
-
-            _value_states = value_states.to(dtype=expert_layer.self_attn.v_proj.weight.dtype).view(
-                *value_states.shape[:2], -1
-            )
-            expert_value_states = expert_layer.self_attn.v_proj(_value_states).view(
-                *_value_states.shape[:-1], -1, expert_layer.self_attn.head_dim
-            )
-
-            expert_position_id = (
-                expert_position_id - torch.min(expert_position_id, dim=1, keepdim=True).values
-            )  # start from 0
-            expert_attention_mask = attention_mask[
-                :, -inputs_embeds[1].shape[1] :, : expert_key_states.shape[1] :
-            ]  # take into account kv
-
-            expert_query_states = apply_rope(expert_query_state, expert_position_id)
-
-            att_output = attention_interface(
-                expert_attention_mask,
-                batch_size,
-                head_dim,
-                expert_query_states,
-                expert_key_states,
-                expert_value_states,
-            )
-            att_outputs.append(att_output)
-        else:
-            att_outputs.append(None)
-
-        # att_output = att_output.to(dtype=models[i].dtype)
-        return att_outputs, past_key_values
-
-    def get_model_layers(self, models: list) -> list:
-        vlm_layers = []
-        expert_layers = []
-        multiple_of = self.num_vlm_layers // self.num_expert_layers
-        for i in range(self.num_vlm_layers):
-            if multiple_of > 0 and i > 0 and i % multiple_of != 0:
-                expert_layer = None
-            else:
-                expert_layer_index = i // multiple_of if multiple_of > 0 else i
-                expert_layer = models[1].layers[expert_layer_index]
-            vlm_layers.append(models[0].layers[i])
-            expert_layers.append(expert_layer)
-        return [vlm_layers, expert_layers]
-
-    def forward(
-        self,
-        attention_mask: Optional[torch.Tensor] = None,
-        position_ids: Optional[torch.LongTensor] = None,
-        past_key_values: Optional[List[torch.FloatTensor]] = None,
-        inputs_embeds: List[torch.FloatTensor] = None,
-        use_cache: Optional[bool] = None,
-        fill_kv_cache: Optional[bool] = None,
-    ):
-        models = [self.get_vlm_model().text_model, self.lm_expert]
-        model_layers = self.get_model_layers(models)
-        for hidden_states in inputs_embeds:
-            # TODO this is very inefficient
-            # dtype is always the same, batch size too (if > 1 len)
-            # device could be trickier in multi gpu edge cases but that's it
-            if hidden_states is None:
-                continue
-            batch_size = hidden_states.shape[0]
-
-        # RMSNorm
-        num_layers = self.num_vlm_layers
-        head_dim = self.vlm.config.text_config.head_dim
-        for layer_idx in range(num_layers):
-            if (
-                fill_kv_cache
-                or "cross" not in self.attention_mode
-                or (self.self_attn_every_n_layers > 0 and layer_idx % self.self_attn_every_n_layers == 0)
-            ):
-                att_outputs, past_key_values = self.forward_attn_layer(
-                    model_layers,
-                    inputs_embeds,
-                    layer_idx,
-                    position_ids,
-                    attention_mask,
-                    batch_size,
-                    head_dim,
-                    use_cache=use_cache,
-                    fill_kv_cache=fill_kv_cache,
-                    past_key_values=past_key_values,
-                )
-            else:
-                att_outputs, past_key_values = self.forward_cross_attn_layer(
-                    model_layers,
-                    inputs_embeds,
-                    layer_idx,
-                    position_ids,
-                    attention_mask,
-                    batch_size,
-                    head_dim,
-                    use_cache=use_cache,
-                    fill_kv_cache=fill_kv_cache,
-                    past_key_values=past_key_values,
-                )
-            outputs_embeds = []
-            start = 0
-            for i, hidden_states in enumerate(inputs_embeds):
-                layer = model_layers[i][layer_idx]
-                att_output = (
-                    att_outputs[i] if i < len(att_outputs) else att_outputs[0]
-                )  # in case of self_attn
-                if hidden_states is not None:
-                    if layer is None:
-                        outputs_embeds.append(hidden_states)
-                        continue
-                    end = start + hidden_states.shape[1]
-
-                    if att_output.dtype != layer.self_attn.o_proj.weight.dtype:
-                        att_output = att_output.to(layer.self_attn.o_proj.weight.dtype)
-                    att_out = att_output[:, start:end]
-                    out_emb = layer.self_attn.o_proj(att_out)
-
-                    out_emb += hidden_states
-                    after_first_residual = out_emb.clone()
-
-                    out_emb = layer.post_attention_layernorm(out_emb)
-                    out_emb = layer.mlp(out_emb)
-
-                    out_emb += after_first_residual
-
-                    outputs_embeds.append(out_emb)
-
-                    start = end if len(att_outputs) == 1 else 0
-                else:
-                    outputs_embeds.append(None)
-
-            inputs_embeds = outputs_embeds
-
-        # final norm
-        outputs_embeds = []
-        for i, hidden_states in enumerate(inputs_embeds):
-            if hidden_states is not None:
-                out_emb = models[i].norm(hidden_states)
-                outputs_embeds.append(out_emb)
-            else:
-                outputs_embeds.append(None)
-        return outputs_embeds, past_key_values
-
-    def get_attention_interface(self):
-        attention_interface = self.eager_attention_forward
-        return attention_interface
-
-    def eager_attention_forward(
-        self, attention_mask, batch_size, head_dim, query_states, key_states, value_states
-    ):
-        num_att_heads = self.num_attention_heads
-        num_key_value_heads = self.num_key_value_heads
-        num_key_value_groups = num_att_heads // num_key_value_heads
-
-        sequence_length = key_states.shape[1]
-
-        key_states = key_states[:, :, :, None, :].expand(
-            batch_size, sequence_length, num_key_value_heads, num_key_value_groups, head_dim
-        )
-        key_states = key_states.reshape(
-            batch_size, sequence_length, num_key_value_heads * num_key_value_groups, head_dim
-        )
-
-        value_states = value_states[:, :, :, None, :].expand(
-            batch_size, sequence_length, num_key_value_heads, num_key_value_groups, head_dim
-        )
-        value_states = value_states.reshape(
-            batch_size, sequence_length, num_key_value_heads * num_key_value_groups, head_dim
-        )
-
-        # Attention here is upcasted to float32 to match the original eager implementation.
-        query_states = query_states.to(dtype=torch.float32)
-        key_states = key_states.to(dtype=torch.float32)
-
-        query_states = query_states.transpose(1, 2)
-        key_states = key_states.transpose(1, 2)
-
-        att_weights = torch.matmul(query_states, key_states.transpose(2, 3))
-        att_weights *= head_dim**-0.5
-
-        att_weights = att_weights.to(dtype=torch.float32)
-        big_neg = torch.finfo(att_weights.dtype).min  # -2.3819763e38  # See gemma/modules.py
-        masked_att_weights = torch.where(attention_mask[:, None, :, :], att_weights, big_neg)
-        probs = nn.functional.softmax(masked_att_weights, dim=-1)
-        probs = probs.to(dtype=value_states.dtype)
-
-        att_output = torch.matmul(probs, value_states.permute(0, 2, 1, 3))
-
-        att_output = att_output.permute(0, 2, 1, 3)
-        # we use -1 because sequence length can change
-        att_output = att_output.reshape(batch_size, -1, num_key_value_heads * num_key_value_groups * head_dim)
-
-        return att_output

lerobot/common/policies/tdmpc/modeling_tdmpc.py

@@ -35,7 +35,7 @@ import torch.nn as nn
 import torch.nn.functional as F  # noqa: N812
 from torch import Tensor
 
-from lerobot.common.constants import OBS_ENV, OBS_ROBOT
+from lerobot.common.constants import OBS_ENV_STATE, OBS_STATE
 from lerobot.common.policies.normalize import Normalize, Unnormalize
 from lerobot.common.policies.pretrained import PreTrainedPolicy
 from lerobot.common.policies.tdmpc.configuration_tdmpc import TDMPCConfig
@@ -753,9 +753,9 @@ class TDMPCObservationEncoder(nn.Module):
                 )
             )
         if self.config.env_state_feature:
-            feat.append(self.env_state_enc_layers(obs_dict[OBS_ENV]))
+            feat.append(self.env_state_enc_layers(obs_dict[OBS_ENV_STATE]))
         if self.config.robot_state_feature:
-            feat.append(self.state_enc_layers(obs_dict[OBS_ROBOT]))
+            feat.append(self.state_enc_layers(obs_dict[OBS_STATE]))
         return torch.stack(feat, dim=0).mean(0)
 
 

lerobot/common/robot_devices/cameras/configs.py

@@ -1,114 +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 abc
-from dataclasses import dataclass
-
-import draccus
-
-
-@dataclass
-class CameraConfig(draccus.ChoiceRegistry, abc.ABC):
-    @property
-    def type(self) -> str:
-        return self.get_choice_name(self.__class__)
-
-
-@CameraConfig.register_subclass("opencv")
-@dataclass
-class OpenCVCameraConfig(CameraConfig):
-    """
-    Example of tested options for Intel Real Sense D405:
-
-    ```python
-    OpenCVCameraConfig(0, 30, 640, 480)
-    OpenCVCameraConfig(0, 60, 640, 480)
-    OpenCVCameraConfig(0, 90, 640, 480)
-    OpenCVCameraConfig(0, 30, 1280, 720)
-    ```
-    """
-
-    camera_index: int
-    fps: int | None = None
-    width: int | None = None
-    height: int | None = None
-    color_mode: str = "rgb"
-    channels: int | None = None
-    rotation: int | None = None
-    mock: bool = False
-
-    def __post_init__(self):
-        if self.color_mode not in ["rgb", "bgr"]:
-            raise ValueError(
-                f"`color_mode` is expected to be 'rgb' or 'bgr', but {self.color_mode} is provided."
-            )
-
-        self.channels = 3
-
-        if self.rotation not in [-90, None, 90, 180]:
-            raise ValueError(f"`rotation` must be in [-90, None, 90, 180] (got {self.rotation})")
-
-
-@CameraConfig.register_subclass("intelrealsense")
-@dataclass
-class IntelRealSenseCameraConfig(CameraConfig):
-    """
-    Example of tested options for Intel Real Sense D405:
-
-    ```python
-    IntelRealSenseCameraConfig(128422271347, 30, 640, 480)
-    IntelRealSenseCameraConfig(128422271347, 60, 640, 480)
-    IntelRealSenseCameraConfig(128422271347, 90, 640, 480)
-    IntelRealSenseCameraConfig(128422271347, 30, 1280, 720)
-    IntelRealSenseCameraConfig(128422271347, 30, 640, 480, use_depth=True)
-    IntelRealSenseCameraConfig(128422271347, 30, 640, 480, rotation=90)
-    ```
-    """
-
-    name: str | None = None
-    serial_number: int | None = None
-    fps: int | None = None
-    width: int | None = None
-    height: int | None = None
-    color_mode: str = "rgb"
-    channels: int | None = None
-    use_depth: bool = False
-    force_hardware_reset: bool = True
-    rotation: int | None = None
-    mock: bool = False
-
-    def __post_init__(self):
-        # bool is stronger than is None, since it works with empty strings
-        if bool(self.name) and bool(self.serial_number):
-            raise ValueError(
-                f"One of them must be set: name or serial_number, but {self.name=} and {self.serial_number=} provided."
-            )
-
-        if self.color_mode not in ["rgb", "bgr"]:
-            raise ValueError(
-                f"`color_mode` is expected to be 'rgb' or 'bgr', but {self.color_mode} is provided."
-            )
-
-        self.channels = 3
-
-        at_least_one_is_not_none = self.fps is not None or self.width is not None or self.height is not None
-        at_least_one_is_none = self.fps is None or self.width is None or self.height is None
-        if at_least_one_is_not_none and at_least_one_is_none:
-            raise ValueError(
-                "For `fps`, `width` and `height`, either all of them need to be set, or none of them, "
-                f"but {self.fps=}, {self.width=}, {self.height=} were provided."
-            )
-
-        if self.rotation not in [-90, None, 90, 180]:
-            raise ValueError(f"`rotation` must be in [-90, None, 90, 180] (got {self.rotation})")

lerobot/common/robot_devices/cameras/intelrealsense.py

@@ -1,538 +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 file contains utilities for recording frames from Intel Realsense cameras.
-"""
-
-import argparse
-import concurrent.futures
-import logging
-import math
-import shutil
-import threading
-import time
-import traceback
-from collections import Counter
-from pathlib import Path
-from threading import Thread
-
-import numpy as np
-from PIL import Image
-
-from lerobot.common.robot_devices.cameras.configs import IntelRealSenseCameraConfig
-from lerobot.common.robot_devices.utils import (
-    RobotDeviceAlreadyConnectedError,
-    RobotDeviceNotConnectedError,
-    busy_wait,
-)
-from lerobot.common.utils.utils import capture_timestamp_utc
-
-SERIAL_NUMBER_INDEX = 1
-
-
-def find_cameras(raise_when_empty=True, mock=False) -> list[dict]:
-    """
-    Find the names and the serial numbers of the Intel RealSense cameras
-    connected to the computer.
-    """
-    if mock:
-        import tests.cameras.mock_pyrealsense2 as rs
-    else:
-        import pyrealsense2 as rs
-
-    cameras = []
-    for device in rs.context().query_devices():
-        serial_number = int(device.get_info(rs.camera_info(SERIAL_NUMBER_INDEX)))
-        name = device.get_info(rs.camera_info.name)
-        cameras.append(
-            {
-                "serial_number": serial_number,
-                "name": name,
-            }
-        )
-
-    if raise_when_empty and len(cameras) == 0:
-        raise OSError(
-            "Not a single camera was detected. Try re-plugging, or re-installing `librealsense` and its python wrapper `pyrealsense2`, or updating the firmware."
-        )
-
-    return cameras
-
-
-def save_image(img_array, serial_number, frame_index, images_dir):
-    try:
-        img = Image.fromarray(img_array)
-        path = images_dir / f"camera_{serial_number}_frame_{frame_index:06d}.png"
-        path.parent.mkdir(parents=True, exist_ok=True)
-        img.save(str(path), quality=100)
-        logging.info(f"Saved image: {path}")
-    except Exception as e:
-        logging.error(f"Failed to save image for camera {serial_number} frame {frame_index}: {e}")
-
-
-def save_images_from_cameras(
-    images_dir: Path,
-    serial_numbers: list[int] | None = None,
-    fps=None,
-    width=None,
-    height=None,
-    record_time_s=2,
-    mock=False,
-):
-    """
-    Initializes all the cameras and saves images to the directory. Useful to visually identify the camera
-    associated to a given serial number.
-    """
-    if serial_numbers is None or len(serial_numbers) == 0:
-        camera_infos = find_cameras(mock=mock)
-        serial_numbers = [cam["serial_number"] for cam in camera_infos]
-
-    if mock:
-        import tests.cameras.mock_cv2 as cv2
-    else:
-        import cv2
-
-    print("Connecting cameras")
-    cameras = []
-    for cam_sn in serial_numbers:
-        print(f"{cam_sn=}")
-        config = IntelRealSenseCameraConfig(
-            serial_number=cam_sn, fps=fps, width=width, height=height, mock=mock
-        )
-        camera = IntelRealSenseCamera(config)
-        camera.connect()
-        print(
-            f"IntelRealSenseCamera({camera.serial_number}, fps={camera.fps}, width={camera.capture_width}, height={camera.capture_height}, color_mode={camera.color_mode})"
-        )
-        cameras.append(camera)
-
-    images_dir = Path(images_dir)
-    if images_dir.exists():
-        shutil.rmtree(
-            images_dir,
-        )
-    images_dir.mkdir(parents=True, exist_ok=True)
-
-    print(f"Saving images to {images_dir}")
-    frame_index = 0
-    start_time = time.perf_counter()
-    try:
-        with concurrent.futures.ThreadPoolExecutor(max_workers=1) as executor:
-            while True:
-                now = time.perf_counter()
-
-                for camera in cameras:
-                    # If we use async_read when fps is None, the loop will go full speed, and we will end up
-                    # saving the same images from the cameras multiple times until the RAM/disk is full.
-                    image = camera.read() if fps is None else camera.async_read()
-                    if image is None:
-                        print("No Frame")
-
-                    bgr_converted_image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
-
-                    executor.submit(
-                        save_image,
-                        bgr_converted_image,
-                        camera.serial_number,
-                        frame_index,
-                        images_dir,
-                    )
-
-                if fps is not None:
-                    dt_s = time.perf_counter() - now
-                    busy_wait(1 / fps - dt_s)
-
-                if time.perf_counter() - start_time > record_time_s:
-                    break
-
-                print(f"Frame: {frame_index:04d}\tLatency (ms): {(time.perf_counter() - now) * 1000:.2f}")
-
-                frame_index += 1
-    finally:
-        print(f"Images have been saved to {images_dir}")
-        for camera in cameras:
-            camera.disconnect()
-
-
-class IntelRealSenseCamera:
-    """
-    The IntelRealSenseCamera class is similar to OpenCVCamera class but adds additional features for Intel Real Sense cameras:
-    - is instantiated with the serial number of the camera - won't randomly change as it can be the case of OpenCVCamera for Linux,
-    - can also be instantiated with the camera's name — if it's unique — using IntelRealSenseCamera.init_from_name(),
-    - depth map can be returned.
-
-    To find the camera indices of your cameras, you can run our utility script that will save a few frames for each camera:
-    ```bash
-    python lerobot/common/robot_devices/cameras/intelrealsense.py --images-dir outputs/images_from_intelrealsense_cameras
-    ```
-
-    When an IntelRealSenseCamera is instantiated, if no specific config is provided, the default fps, width, height and color_mode
-    of the given camera will be used.
-
-    Example of instantiating with a serial number:
-    ```python
-    from lerobot.common.robot_devices.cameras.configs import IntelRealSenseCameraConfig
-
-    config = IntelRealSenseCameraConfig(serial_number=128422271347)
-    camera = IntelRealSenseCamera(config)
-    camera.connect()
-    color_image = camera.read()
-    # when done using the camera, consider disconnecting
-    camera.disconnect()
-    ```
-
-    Example of instantiating with a name if it's unique:
-    ```
-    config = IntelRealSenseCameraConfig(name="Intel RealSense D405")
-    ```
-
-    Example of changing default fps, width, height and color_mode:
-    ```python
-    config = IntelRealSenseCameraConfig(serial_number=128422271347, fps=30, width=1280, height=720)
-    config = IntelRealSenseCameraConfig(serial_number=128422271347, fps=90, width=640, height=480)
-    config = IntelRealSenseCameraConfig(serial_number=128422271347, fps=90, width=640, height=480, color_mode="bgr")
-    # Note: might error out upon `camera.connect()` if these settings are not compatible with the camera
-    ```
-
-    Example of returning depth:
-    ```python
-    config = IntelRealSenseCameraConfig(serial_number=128422271347, use_depth=True)
-    camera = IntelRealSenseCamera(config)
-    camera.connect()
-    color_image, depth_map = camera.read()
-    ```
-    """
-
-    def __init__(
-        self,
-        config: IntelRealSenseCameraConfig,
-    ):
-        self.config = config
-        if config.name is not None:
-            self.serial_number = self.find_serial_number_from_name(config.name)
-        else:
-            self.serial_number = config.serial_number
-
-        # Store the raw (capture) resolution from the config.
-        self.capture_width = config.width
-        self.capture_height = config.height
-
-        # If rotated by ±90, swap width and height.
-        if config.rotation in [-90, 90]:
-            self.width = config.height
-            self.height = config.width
-        else:
-            self.width = config.width
-            self.height = config.height
-
-        self.fps = config.fps
-        self.channels = config.channels
-        self.color_mode = config.color_mode
-        self.use_depth = config.use_depth
-        self.force_hardware_reset = config.force_hardware_reset
-        self.mock = config.mock
-
-        self.camera = None
-        self.is_connected = False
-        self.thread = None
-        self.stop_event = None
-        self.color_image = None
-        self.depth_map = None
-        self.logs = {}
-
-        if self.mock:
-            import tests.cameras.mock_cv2 as cv2
-        else:
-            import cv2
-
-        self.rotation = None
-        if config.rotation == -90:
-            self.rotation = cv2.ROTATE_90_COUNTERCLOCKWISE
-        elif config.rotation == 90:
-            self.rotation = cv2.ROTATE_90_CLOCKWISE
-        elif config.rotation == 180:
-            self.rotation = cv2.ROTATE_180
-
-    def find_serial_number_from_name(self, name):
-        camera_infos = find_cameras()
-        camera_names = [cam["name"] for cam in camera_infos]
-        this_name_count = Counter(camera_names)[name]
-        if this_name_count > 1:
-            # TODO(aliberts): Test this with multiple identical cameras (Aloha)
-            raise ValueError(
-                f"Multiple {name} cameras have been detected. Please use their serial number to instantiate them."
-            )
-
-        name_to_serial_dict = {cam["name"]: cam["serial_number"] for cam in camera_infos}
-        cam_sn = name_to_serial_dict[name]
-
-        return cam_sn
-
-    def connect(self):
-        if self.is_connected:
-            raise RobotDeviceAlreadyConnectedError(
-                f"IntelRealSenseCamera({self.serial_number}) is already connected."
-            )
-
-        if self.mock:
-            import tests.cameras.mock_pyrealsense2 as rs
-        else:
-            import pyrealsense2 as rs
-
-        config = rs.config()
-        config.enable_device(str(self.serial_number))
-
-        if self.fps and self.capture_width and self.capture_height:
-            # TODO(rcadene): can we set rgb8 directly?
-            config.enable_stream(
-                rs.stream.color, self.capture_width, self.capture_height, rs.format.rgb8, self.fps
-            )
-        else:
-            config.enable_stream(rs.stream.color)
-
-        if self.use_depth:
-            if self.fps and self.capture_width and self.capture_height:
-                config.enable_stream(
-                    rs.stream.depth, self.capture_width, self.capture_height, rs.format.z16, self.fps
-                )
-            else:
-                config.enable_stream(rs.stream.depth)
-
-        self.camera = rs.pipeline()
-        try:
-            profile = self.camera.start(config)
-            is_camera_open = True
-        except RuntimeError:
-            is_camera_open = False
-            traceback.print_exc()
-
-        # If the camera doesn't work, display the camera indices corresponding to
-        # valid cameras.
-        if not is_camera_open:
-            # Verify that the provided `serial_number` is valid before printing the traceback
-            camera_infos = find_cameras()
-            serial_numbers = [cam["serial_number"] for cam in camera_infos]
-            if self.serial_number not in serial_numbers:
-                raise ValueError(
-                    f"`serial_number` is expected to be one of these available cameras {serial_numbers}, but {self.serial_number} is provided instead. "
-                    "To find the serial number you should use, run `python lerobot/common/robot_devices/cameras/intelrealsense.py`."
-                )
-
-            raise OSError(f"Can't access IntelRealSenseCamera({self.serial_number}).")
-
-        color_stream = profile.get_stream(rs.stream.color)
-        color_profile = color_stream.as_video_stream_profile()
-        actual_fps = color_profile.fps()
-        actual_width = color_profile.width()
-        actual_height = color_profile.height()
-
-        # Using `math.isclose` since actual fps can be a float (e.g. 29.9 instead of 30)
-        if self.fps is not None and not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
-            # Using `OSError` since it's a broad that encompasses issues related to device communication
-            raise OSError(
-                f"Can't set {self.fps=} for IntelRealSenseCamera({self.serial_number}). Actual value is {actual_fps}."
-            )
-        if self.capture_width is not None and self.capture_width != actual_width:
-            raise OSError(
-                f"Can't set {self.capture_width=} for IntelRealSenseCamera({self.serial_number}). Actual value is {actual_width}."
-            )
-        if self.capture_height is not None and self.capture_height != actual_height:
-            raise OSError(
-                f"Can't set {self.capture_height=} for IntelRealSenseCamera({self.serial_number}). Actual value is {actual_height}."
-            )
-
-        self.fps = round(actual_fps)
-        self.capture_width = round(actual_width)
-        self.capture_height = round(actual_height)
-
-        self.is_connected = True
-
-    def read(self, temporary_color: str | None = None) -> np.ndarray | tuple[np.ndarray, np.ndarray]:
-        """Read a frame from the camera returned in the format height x width x channels (e.g. 480 x 640 x 3)
-        of type `np.uint8`, contrarily to the pytorch format which is float channel first.
-
-        When `use_depth=True`, returns a tuple `(color_image, depth_map)` with a depth map in the format
-        height x width (e.g. 480 x 640) of type np.uint16.
-
-        Note: Reading a frame is done every `camera.fps` times per second, and it is blocking.
-        If you are reading data from other sensors, we advise to use `camera.async_read()` which is non blocking version of `camera.read()`.
-        """
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"IntelRealSenseCamera({self.serial_number}) is not connected. Try running `camera.connect()` first."
-            )
-
-        if self.mock:
-            import tests.cameras.mock_cv2 as cv2
-        else:
-            import cv2
-
-        start_time = time.perf_counter()
-
-        frame = self.camera.wait_for_frames(timeout_ms=5000)
-
-        color_frame = frame.get_color_frame()
-
-        if not color_frame:
-            raise OSError(f"Can't capture color image from IntelRealSenseCamera({self.serial_number}).")
-
-        color_image = np.asanyarray(color_frame.get_data())
-
-        requested_color_mode = self.color_mode if temporary_color is None else temporary_color
-        if requested_color_mode not in ["rgb", "bgr"]:
-            raise ValueError(
-                f"Expected color values are 'rgb' or 'bgr', but {requested_color_mode} is provided."
-            )
-
-        # IntelRealSense uses RGB format as default (red, green, blue).
-        if requested_color_mode == "bgr":
-            color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
-
-        h, w, _ = color_image.shape
-        if h != self.capture_height or w != self.capture_width:
-            raise OSError(
-                f"Can't capture color image with expected height and width ({self.height} x {self.width}). ({h} x {w}) returned instead."
-            )
-
-        if self.rotation is not None:
-            color_image = cv2.rotate(color_image, self.rotation)
-
-        # log the number of seconds it took to read the image
-        self.logs["delta_timestamp_s"] = time.perf_counter() - start_time
-
-        # log the utc time at which the image was received
-        self.logs["timestamp_utc"] = capture_timestamp_utc()
-
-        if self.use_depth:
-            depth_frame = frame.get_depth_frame()
-            if not depth_frame:
-                raise OSError(f"Can't capture depth image from IntelRealSenseCamera({self.serial_number}).")
-
-            depth_map = np.asanyarray(depth_frame.get_data())
-
-            h, w = depth_map.shape
-            if h != self.capture_height or w != self.capture_width:
-                raise OSError(
-                    f"Can't capture depth map with expected height and width ({self.height} x {self.width}). ({h} x {w}) returned instead."
-                )
-
-            if self.rotation is not None:
-                depth_map = cv2.rotate(depth_map, self.rotation)
-
-            return color_image, depth_map
-        else:
-            return color_image
-
-    def read_loop(self):
-        while not self.stop_event.is_set():
-            if self.use_depth:
-                self.color_image, self.depth_map = self.read()
-            else:
-                self.color_image = self.read()
-
-    def async_read(self):
-        """Access the latest color image"""
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"IntelRealSenseCamera({self.serial_number}) is not connected. Try running `camera.connect()` first."
-            )
-
-        if self.thread is None:
-            self.stop_event = threading.Event()
-            self.thread = Thread(target=self.read_loop, args=())
-            self.thread.daemon = True
-            self.thread.start()
-
-        num_tries = 0
-        while self.color_image is None:
-            # TODO(rcadene, aliberts): intelrealsense has diverged compared to opencv over here
-            num_tries += 1
-            time.sleep(1 / self.fps)
-            if num_tries > self.fps and (self.thread.ident is None or not self.thread.is_alive()):
-                raise Exception(
-                    "The thread responsible for `self.async_read()` took too much time to start. There might be an issue. Verify that `self.thread.start()` has been called."
-                )
-
-        if self.use_depth:
-            return self.color_image, self.depth_map
-        else:
-            return self.color_image
-
-    def disconnect(self):
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"IntelRealSenseCamera({self.serial_number}) is not connected. Try running `camera.connect()` first."
-            )
-
-        if self.thread is not None and self.thread.is_alive():
-            # wait for the thread to finish
-            self.stop_event.set()
-            self.thread.join()
-            self.thread = None
-            self.stop_event = None
-
-        self.camera.stop()
-        self.camera = None
-
-        self.is_connected = False
-
-    def __del__(self):
-        if getattr(self, "is_connected", False):
-            self.disconnect()
-
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser(
-        description="Save a few frames using `IntelRealSenseCamera` for all cameras connected to the computer, or a selected subset."
-    )
-    parser.add_argument(
-        "--serial-numbers",
-        type=int,
-        nargs="*",
-        default=None,
-        help="List of serial numbers used to instantiate the `IntelRealSenseCamera`. If not provided, find and use all available camera indices.",
-    )
-    parser.add_argument(
-        "--fps",
-        type=int,
-        default=30,
-        help="Set the number of frames recorded per seconds for all cameras. If not provided, use the default fps of each camera.",
-    )
-    parser.add_argument(
-        "--width",
-        type=int,
-        default=640,
-        help="Set the width for all cameras. If not provided, use the default width of each camera.",
-    )
-    parser.add_argument(
-        "--height",
-        type=int,
-        default=480,
-        help="Set the height for all cameras. If not provided, use the default height of each camera.",
-    )
-    parser.add_argument(
-        "--images-dir",
-        type=Path,
-        default="outputs/images_from_intelrealsense_cameras",
-        help="Set directory to save a few frames for each camera.",
-    )
-    parser.add_argument(
-        "--record-time-s",
-        type=float,
-        default=2.0,
-        help="Set the number of seconds used to record the frames. By default, 2 seconds.",
-    )
-    args = parser.parse_args()
-    save_images_from_cameras(**vars(args))

lerobot/common/robot_devices/cameras/opencv.py

@@ -1,518 +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 file contains utilities for recording frames from cameras. For more info look at `OpenCVCamera` docstring.
-"""
-
-import argparse
-import concurrent.futures
-import math
-import platform
-import shutil
-import threading
-import time
-from pathlib import Path
-from threading import Thread
-
-import numpy as np
-from PIL import Image
-
-from lerobot.common.robot_devices.cameras.configs import OpenCVCameraConfig
-from lerobot.common.robot_devices.utils import (
-    RobotDeviceAlreadyConnectedError,
-    RobotDeviceNotConnectedError,
-    busy_wait,
-)
-from lerobot.common.utils.utils import capture_timestamp_utc
-
-# The maximum opencv device index depends on your operating system. For instance,
-# if you have 3 cameras, they should be associated to index 0, 1, and 2. This is the case
-# on MacOS. However, on Ubuntu, the indices are different like 6, 16, 23.
-# When you change the USB port or reboot the computer, the operating system might
-# treat the same cameras as new devices. Thus we select a higher bound to search indices.
-MAX_OPENCV_INDEX = 60
-
-
-def find_cameras(raise_when_empty=False, max_index_search_range=MAX_OPENCV_INDEX, mock=False) -> list[dict]:
-    cameras = []
-    if platform.system() == "Linux":
-        print("Linux detected. Finding available camera indices through scanning '/dev/video*' ports")
-        possible_ports = [str(port) for port in Path("/dev").glob("video*")]
-        ports = _find_cameras(possible_ports, mock=mock)
-        for port in ports:
-            cameras.append(
-                {
-                    "port": port,
-                    "index": int(port.removeprefix("/dev/video")),
-                }
-            )
-    else:
-        print(
-            "Mac or Windows detected. Finding available camera indices through "
-            f"scanning all indices from 0 to {MAX_OPENCV_INDEX}"
-        )
-        possible_indices = range(max_index_search_range)
-        indices = _find_cameras(possible_indices, mock=mock)
-        for index in indices:
-            cameras.append(
-                {
-                    "port": None,
-                    "index": index,
-                }
-            )
-
-    return cameras
-
-
-def _find_cameras(
-    possible_camera_ids: list[int | str], raise_when_empty=False, mock=False
-) -> list[int | str]:
-    if mock:
-        import tests.cameras.mock_cv2 as cv2
-    else:
-        import cv2
-
-    camera_ids = []
-    for camera_idx in possible_camera_ids:
-        camera = cv2.VideoCapture(camera_idx)
-        is_open = camera.isOpened()
-        camera.release()
-
-        if is_open:
-            print(f"Camera found at index {camera_idx}")
-            camera_ids.append(camera_idx)
-
-    if raise_when_empty and len(camera_ids) == 0:
-        raise OSError(
-            "Not a single camera was detected. Try re-plugging, or re-installing `opencv2`, "
-            "or your camera driver, or make sure your camera is compatible with opencv2."
-        )
-
-    return camera_ids
-
-
-def is_valid_unix_path(path: str) -> bool:
-    """Note: if 'path' points to a symlink, this will return True only if the target exists"""
-    p = Path(path)
-    return p.is_absolute() and p.exists()
-
-
-def get_camera_index_from_unix_port(port: Path) -> int:
-    return int(str(port.resolve()).removeprefix("/dev/video"))
-
-
-def save_image(img_array, camera_index, frame_index, images_dir):
-    img = Image.fromarray(img_array)
-    path = images_dir / f"camera_{camera_index:02d}_frame_{frame_index:06d}.png"
-    path.parent.mkdir(parents=True, exist_ok=True)
-    img.save(str(path), quality=100)
-
-
-def save_images_from_cameras(
-    images_dir: Path,
-    camera_ids: list | None = None,
-    fps=None,
-    width=None,
-    height=None,
-    record_time_s=2,
-    mock=False,
-):
-    """
-    Initializes all the cameras and saves images to the directory. Useful to visually identify the camera
-    associated to a given camera index.
-    """
-    if camera_ids is None or len(camera_ids) == 0:
-        camera_infos = find_cameras(mock=mock)
-        camera_ids = [cam["index"] for cam in camera_infos]
-
-    print("Connecting cameras")
-    cameras = []
-    for cam_idx in camera_ids:
-        config = OpenCVCameraConfig(camera_index=cam_idx, fps=fps, width=width, height=height, mock=mock)
-        camera = OpenCVCamera(config)
-        camera.connect()
-        print(
-            f"OpenCVCamera({camera.camera_index}, fps={camera.fps}, width={camera.capture_width}, "
-            f"height={camera.capture_height}, color_mode={camera.color_mode})"
-        )
-        cameras.append(camera)
-
-    images_dir = Path(images_dir)
-    if images_dir.exists():
-        shutil.rmtree(
-            images_dir,
-        )
-    images_dir.mkdir(parents=True, exist_ok=True)
-
-    print(f"Saving images to {images_dir}")
-    frame_index = 0
-    start_time = time.perf_counter()
-    with concurrent.futures.ThreadPoolExecutor(max_workers=1) as executor:
-        while True:
-            now = time.perf_counter()
-
-            for camera in cameras:
-                # If we use async_read when fps is None, the loop will go full speed, and we will endup
-                # saving the same images from the cameras multiple times until the RAM/disk is full.
-                image = camera.read() if fps is None else camera.async_read()
-
-                executor.submit(
-                    save_image,
-                    image,
-                    camera.camera_index,
-                    frame_index,
-                    images_dir,
-                )
-
-            if fps is not None:
-                dt_s = time.perf_counter() - now
-                busy_wait(1 / fps - dt_s)
-
-            print(f"Frame: {frame_index:04d}\tLatency (ms): {(time.perf_counter() - now) * 1000:.2f}")
-
-            if time.perf_counter() - start_time > record_time_s:
-                break
-
-            frame_index += 1
-
-    print(f"Images have been saved to {images_dir}")
-
-
-class OpenCVCamera:
-    """
-    The OpenCVCamera class allows to efficiently record images from cameras. It relies on opencv2 to communicate
-    with the cameras. Most cameras are compatible. For more info, see the [Video I/O with OpenCV Overview](https://docs.opencv.org/4.x/d0/da7/videoio_overview.html).
-
-    An OpenCVCamera instance requires 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 expected to be 0, but it might also be very different, and the camera index
-    might change if you reboot your computer or re-plug your camera. This behavior depends on your operation system.
-
-    To find the camera indices of your cameras, you can run our utility script that will be save a few frames for each camera:
-    ```bash
-    python lerobot/common/robot_devices/cameras/opencv.py --images-dir outputs/images_from_opencv_cameras
-    ```
-
-    When an OpenCVCamera is instantiated, if no specific config is provided, the default fps, width, height and color_mode
-    of the given camera will be used.
-
-    Example of usage:
-    ```python
-    from lerobot.common.robot_devices.cameras.configs import OpenCVCameraConfig
-
-    config = OpenCVCameraConfig(camera_index=0)
-    camera = OpenCVCamera(config)
-    camera.connect()
-    color_image = camera.read()
-    # when done using the camera, consider disconnecting
-    camera.disconnect()
-    ```
-
-    Example of changing default fps, width, height and color_mode:
-    ```python
-    config = OpenCVCameraConfig(camera_index=0, fps=30, width=1280, height=720)
-    config = OpenCVCameraConfig(camera_index=0, fps=90, width=640, height=480)
-    config = OpenCVCameraConfig(camera_index=0, fps=90, width=640, height=480, color_mode="bgr")
-    # Note: might error out open `camera.connect()` if these settings are not compatible with the camera
-    ```
-    """
-
-    def __init__(self, config: OpenCVCameraConfig):
-        self.config = config
-        self.camera_index = config.camera_index
-        self.port = None
-
-        # Linux uses ports for connecting to cameras
-        if platform.system() == "Linux":
-            if isinstance(self.camera_index, int):
-                self.port = Path(f"/dev/video{self.camera_index}")
-            elif isinstance(self.camera_index, str) and is_valid_unix_path(self.camera_index):
-                self.port = Path(self.camera_index)
-                # Retrieve the camera index from a potentially symlinked path
-                self.camera_index = get_camera_index_from_unix_port(self.port)
-            else:
-                raise ValueError(f"Please check the provided camera_index: {self.camera_index}")
-
-        # Store the raw (capture) resolution from the config.
-        self.capture_width = config.width
-        self.capture_height = config.height
-
-        # If rotated by ±90, swap width and height.
-        if config.rotation in [-90, 90]:
-            self.width = config.height
-            self.height = config.width
-        else:
-            self.width = config.width
-            self.height = config.height
-
-        self.fps = config.fps
-        self.channels = config.channels
-        self.color_mode = config.color_mode
-        self.mock = config.mock
-
-        self.camera = None
-        self.is_connected = False
-        self.thread = None
-        self.stop_event = None
-        self.color_image = None
-        self.logs = {}
-
-        if self.mock:
-            import tests.cameras.mock_cv2 as cv2
-        else:
-            import cv2
-
-        self.rotation = None
-        if config.rotation == -90:
-            self.rotation = cv2.ROTATE_90_COUNTERCLOCKWISE
-        elif config.rotation == 90:
-            self.rotation = cv2.ROTATE_90_CLOCKWISE
-        elif config.rotation == 180:
-            self.rotation = cv2.ROTATE_180
-
-    def connect(self):
-        if self.is_connected:
-            raise RobotDeviceAlreadyConnectedError(f"OpenCVCamera({self.camera_index}) is already connected.")
-
-        if self.mock:
-            import tests.cameras.mock_cv2 as cv2
-        else:
-            import cv2
-
-            # Use 1 thread to avoid blocking the main thread. Especially useful during data collection
-            # when other threads are used to save the images.
-            cv2.setNumThreads(1)
-
-        backend = (
-            cv2.CAP_V4L2
-            if platform.system() == "Linux"
-            else cv2.CAP_DSHOW
-            if platform.system() == "Windows"
-            else cv2.CAP_AVFOUNDATION
-            if platform.system() == "Darwin"
-            else cv2.CAP_ANY
-        )
-
-        camera_idx = f"/dev/video{self.camera_index}" if platform.system() == "Linux" else self.camera_index
-        # First create a temporary camera trying to access `camera_index`,
-        # and verify it is a valid camera by calling `isOpened`.
-        tmp_camera = cv2.VideoCapture(camera_idx, backend)
-        is_camera_open = tmp_camera.isOpened()
-        # Release camera to make it accessible for `find_camera_indices`
-        tmp_camera.release()
-        del tmp_camera
-
-        # If the camera doesn't work, display the camera indices corresponding to
-        # valid cameras.
-        if not is_camera_open:
-            # Verify that the provided `camera_index` is valid before printing the traceback
-            cameras_info = find_cameras()
-            available_cam_ids = [cam["index"] for cam in cameras_info]
-            if self.camera_index not in available_cam_ids:
-                raise ValueError(
-                    f"`camera_index` is expected to be one of these available cameras {available_cam_ids}, but {self.camera_index} is provided instead. "
-                    "To find the camera index you should use, run `python lerobot/common/robot_devices/cameras/opencv.py`."
-                )
-
-            raise OSError(f"Can't access OpenCVCamera({camera_idx}).")
-
-        # Secondly, create the camera that will be used downstream.
-        # Note: For some unknown reason, calling `isOpened` blocks the camera which then
-        # needs to be re-created.
-        self.camera = cv2.VideoCapture(camera_idx, backend)
-
-        if self.fps is not None:
-            self.camera.set(cv2.CAP_PROP_FPS, self.fps)
-        if self.capture_width is not None:
-            self.camera.set(cv2.CAP_PROP_FRAME_WIDTH, self.capture_width)
-        if self.capture_height is not None:
-            self.camera.set(cv2.CAP_PROP_FRAME_HEIGHT, self.capture_height)
-
-        actual_fps = self.camera.get(cv2.CAP_PROP_FPS)
-        actual_width = self.camera.get(cv2.CAP_PROP_FRAME_WIDTH)
-        actual_height = self.camera.get(cv2.CAP_PROP_FRAME_HEIGHT)
-
-        # Using `math.isclose` since actual fps can be a float (e.g. 29.9 instead of 30)
-        if self.fps is not None and not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
-            # Using `OSError` since it's a broad that encompasses issues related to device communication
-            raise OSError(
-                f"Can't set {self.fps=} for OpenCVCamera({self.camera_index}). Actual value is {actual_fps}."
-            )
-        if self.capture_width is not None and not math.isclose(
-            self.capture_width, actual_width, rel_tol=1e-3
-        ):
-            raise OSError(
-                f"Can't set {self.capture_width=} for OpenCVCamera({self.camera_index}). Actual value is {actual_width}."
-            )
-        if self.capture_height is not None and not math.isclose(
-            self.capture_height, actual_height, rel_tol=1e-3
-        ):
-            raise OSError(
-                f"Can't set {self.capture_height=} for OpenCVCamera({self.camera_index}). Actual value is {actual_height}."
-            )
-
-        self.fps = round(actual_fps)
-        self.capture_width = round(actual_width)
-        self.capture_height = round(actual_height)
-        self.is_connected = True
-
-    def read(self, temporary_color_mode: str | None = None) -> np.ndarray:
-        """Read a frame from the camera returned in the format (height, width, channels)
-        (e.g. 480 x 640 x 3), contrarily to the pytorch format which is channel first.
-
-        Note: Reading a frame is done every `camera.fps` times per second, and it is blocking.
-        If you are reading data from other sensors, we advise to use `camera.async_read()` which is non blocking version of `camera.read()`.
-        """
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"OpenCVCamera({self.camera_index}) is not connected. Try running `camera.connect()` first."
-            )
-
-        start_time = time.perf_counter()
-
-        ret, color_image = self.camera.read()
-
-        if not ret:
-            raise OSError(f"Can't capture color image from camera {self.camera_index}.")
-
-        requested_color_mode = self.color_mode if temporary_color_mode is None else temporary_color_mode
-
-        if requested_color_mode not in ["rgb", "bgr"]:
-            raise ValueError(
-                f"Expected color values are 'rgb' or 'bgr', but {requested_color_mode} is provided."
-            )
-
-        # OpenCV uses BGR format as default (blue, green, red) for all operations, including displaying images.
-        # However, Deep Learning framework such as LeRobot uses RGB format as default to train neural networks,
-        # so we convert the image color from BGR to RGB.
-        if requested_color_mode == "rgb":
-            if self.mock:
-                import tests.cameras.mock_cv2 as cv2
-            else:
-                import cv2
-
-            color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
-
-        h, w, _ = color_image.shape
-        if h != self.capture_height or w != self.capture_width:
-            raise OSError(
-                f"Can't capture color image with expected height and width ({self.height} x {self.width}). ({h} x {w}) returned instead."
-            )
-
-        if self.rotation is not None:
-            color_image = cv2.rotate(color_image, self.rotation)
-
-        # log the number of seconds it took to read the image
-        self.logs["delta_timestamp_s"] = time.perf_counter() - start_time
-
-        # log the utc time at which the image was received
-        self.logs["timestamp_utc"] = capture_timestamp_utc()
-
-        self.color_image = color_image
-
-        return color_image
-
-    def read_loop(self):
-        while not self.stop_event.is_set():
-            try:
-                self.color_image = self.read()
-            except Exception as e:
-                print(f"Error reading in thread: {e}")
-
-    def async_read(self):
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"OpenCVCamera({self.camera_index}) is not connected. Try running `camera.connect()` first."
-            )
-
-        if self.thread is None:
-            self.stop_event = threading.Event()
-            self.thread = Thread(target=self.read_loop, args=())
-            self.thread.daemon = True
-            self.thread.start()
-
-        num_tries = 0
-        while True:
-            if self.color_image is not None:
-                return self.color_image
-
-            time.sleep(1 / self.fps)
-            num_tries += 1
-            if num_tries > self.fps * 2:
-                raise TimeoutError("Timed out waiting for async_read() to start.")
-
-    def disconnect(self):
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"OpenCVCamera({self.camera_index}) is not connected. Try running `camera.connect()` first."
-            )
-
-        if self.thread is not None:
-            self.stop_event.set()
-            self.thread.join()  # wait for the thread to finish
-            self.thread = None
-            self.stop_event = None
-
-        self.camera.release()
-        self.camera = None
-        self.is_connected = False
-
-    def __del__(self):
-        if getattr(self, "is_connected", False):
-            self.disconnect()
-
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser(
-        description="Save a few frames using `OpenCVCamera` for all cameras connected to the computer, or a selected subset."
-    )
-    parser.add_argument(
-        "--camera-ids",
-        type=int,
-        nargs="*",
-        default=None,
-        help="List of camera indices used to instantiate the `OpenCVCamera`. If not provided, find and use all available camera indices.",
-    )
-    parser.add_argument(
-        "--fps",
-        type=int,
-        default=None,
-        help="Set the number of frames recorded per seconds for all cameras. If not provided, use the default fps of each camera.",
-    )
-    parser.add_argument(
-        "--width",
-        type=int,
-        default=None,
-        help="Set the width for all cameras. If not provided, use the default width of each camera.",
-    )
-    parser.add_argument(
-        "--height",
-        type=int,
-        default=None,
-        help="Set the height for all cameras. If not provided, use the default height of each camera.",
-    )
-    parser.add_argument(
-        "--images-dir",
-        type=Path,
-        default="outputs/images_from_opencv_cameras",
-        help="Set directory to save a few frames for each camera.",
-    )
-    parser.add_argument(
-        "--record-time-s",
-        type=float,
-        default=4.0,
-        help="Set the number of seconds used to record the frames. By default, 2 seconds.",
-    )
-    args = parser.parse_args()
-    save_images_from_cameras(**vars(args))

lerobot/common/robot_devices/cameras/utils.py

@@ -1,67 +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.
-
-from typing import Protocol
-
-import numpy as np
-
-from lerobot.common.robot_devices.cameras.configs import (
-    CameraConfig,
-    IntelRealSenseCameraConfig,
-    OpenCVCameraConfig,
-)
-
-
-# Defines a camera type
-class Camera(Protocol):
-    def connect(self): ...
-    def read(self, temporary_color: str | None = None) -> np.ndarray: ...
-    def async_read(self) -> np.ndarray: ...
-    def disconnect(self): ...
-
-
-def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> list[Camera]:
-    cameras = {}
-
-    for key, cfg in camera_configs.items():
-        if cfg.type == "opencv":
-            from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera
-
-            cameras[key] = OpenCVCamera(cfg)
-
-        elif cfg.type == "intelrealsense":
-            from lerobot.common.robot_devices.cameras.intelrealsense import IntelRealSenseCamera
-
-            cameras[key] = IntelRealSenseCamera(cfg)
-        else:
-            raise ValueError(f"The camera type '{cfg.type}' is not valid.")
-
-    return cameras
-
-
-def make_camera(camera_type, **kwargs) -> Camera:
-    if camera_type == "opencv":
-        from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera
-
-        config = OpenCVCameraConfig(**kwargs)
-        return OpenCVCamera(config)
-
-    elif camera_type == "intelrealsense":
-        from lerobot.common.robot_devices.cameras.intelrealsense import IntelRealSenseCamera
-
-        config = IntelRealSenseCameraConfig(**kwargs)
-        return IntelRealSenseCamera(config)
-
-    else:
-        raise ValueError(f"The camera type '{camera_type}' is not valid.")

lerobot/common/robot_devices/motors/dynamixel.py

@@ -1,873 +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 enum
-import logging
-import math
-import time
-import traceback
-from copy import deepcopy
-
-import numpy as np
-import tqdm
-
-from lerobot.common.robot_devices.motors.configs import DynamixelMotorsBusConfig
-from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
-from lerobot.common.utils.utils import capture_timestamp_utc
-
-PROTOCOL_VERSION = 2.0
-BAUDRATE = 1_000_000
-TIMEOUT_MS = 1000
-
-MAX_ID_RANGE = 252
-
-# The following bounds define the lower and upper joints range (after calibration).
-# For joints in degree (i.e. revolute joints), their nominal range is [-180, 180] degrees
-# which corresponds to a half rotation on the left and half rotation on the right.
-# Some joints might require higher range, so we allow up to [-270, 270] degrees until
-# an error is raised.
-LOWER_BOUND_DEGREE = -270
-UPPER_BOUND_DEGREE = 270
-# For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper),
-# their nominal range is [0, 100] %. For instance, for Aloha gripper, 0% is fully
-# closed, and 100% is fully open. To account for slight calibration issue, we allow up to
-# [-10, 110] until an error is raised.
-LOWER_BOUND_LINEAR = -10
-UPPER_BOUND_LINEAR = 110
-
-HALF_TURN_DEGREE = 180
-
-# https://emanual.robotis.com/docs/en/dxl/x/xl330-m077
-# https://emanual.robotis.com/docs/en/dxl/x/xl330-m288
-# https://emanual.robotis.com/docs/en/dxl/x/xl430-w250
-# https://emanual.robotis.com/docs/en/dxl/x/xm430-w350
-# https://emanual.robotis.com/docs/en/dxl/x/xm540-w270
-# https://emanual.robotis.com/docs/en/dxl/x/xc430-w150
-
-# data_name: (address, size_byte)
-X_SERIES_CONTROL_TABLE = {
-    "Model_Number": (0, 2),
-    "Model_Information": (2, 4),
-    "Firmware_Version": (6, 1),
-    "ID": (7, 1),
-    "Baud_Rate": (8, 1),
-    "Return_Delay_Time": (9, 1),
-    "Drive_Mode": (10, 1),
-    "Operating_Mode": (11, 1),
-    "Secondary_ID": (12, 1),
-    "Protocol_Type": (13, 1),
-    "Homing_Offset": (20, 4),
-    "Moving_Threshold": (24, 4),
-    "Temperature_Limit": (31, 1),
-    "Max_Voltage_Limit": (32, 2),
-    "Min_Voltage_Limit": (34, 2),
-    "PWM_Limit": (36, 2),
-    "Current_Limit": (38, 2),
-    "Acceleration_Limit": (40, 4),
-    "Velocity_Limit": (44, 4),
-    "Max_Position_Limit": (48, 4),
-    "Min_Position_Limit": (52, 4),
-    "Shutdown": (63, 1),
-    "Torque_Enable": (64, 1),
-    "LED": (65, 1),
-    "Status_Return_Level": (68, 1),
-    "Registered_Instruction": (69, 1),
-    "Hardware_Error_Status": (70, 1),
-    "Velocity_I_Gain": (76, 2),
-    "Velocity_P_Gain": (78, 2),
-    "Position_D_Gain": (80, 2),
-    "Position_I_Gain": (82, 2),
-    "Position_P_Gain": (84, 2),
-    "Feedforward_2nd_Gain": (88, 2),
-    "Feedforward_1st_Gain": (90, 2),
-    "Bus_Watchdog": (98, 1),
-    "Goal_PWM": (100, 2),
-    "Goal_Current": (102, 2),
-    "Goal_Velocity": (104, 4),
-    "Profile_Acceleration": (108, 4),
-    "Profile_Velocity": (112, 4),
-    "Goal_Position": (116, 4),
-    "Realtime_Tick": (120, 2),
-    "Moving": (122, 1),
-    "Moving_Status": (123, 1),
-    "Present_PWM": (124, 2),
-    "Present_Current": (126, 2),
-    "Present_Velocity": (128, 4),
-    "Present_Position": (132, 4),
-    "Velocity_Trajectory": (136, 4),
-    "Position_Trajectory": (140, 4),
-    "Present_Input_Voltage": (144, 2),
-    "Present_Temperature": (146, 1),
-}
-
-X_SERIES_BAUDRATE_TABLE = {
-    0: 9_600,
-    1: 57_600,
-    2: 115_200,
-    3: 1_000_000,
-    4: 2_000_000,
-    5: 3_000_000,
-    6: 4_000_000,
-}
-
-CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"]
-CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"]
-
-MODEL_CONTROL_TABLE = {
-    "x_series": X_SERIES_CONTROL_TABLE,
-    "xl330-m077": X_SERIES_CONTROL_TABLE,
-    "xl330-m288": X_SERIES_CONTROL_TABLE,
-    "xl430-w250": X_SERIES_CONTROL_TABLE,
-    "xm430-w350": X_SERIES_CONTROL_TABLE,
-    "xm540-w270": X_SERIES_CONTROL_TABLE,
-    "xc430-w150": X_SERIES_CONTROL_TABLE,
-}
-
-MODEL_RESOLUTION = {
-    "x_series": 4096,
-    "xl330-m077": 4096,
-    "xl330-m288": 4096,
-    "xl430-w250": 4096,
-    "xm430-w350": 4096,
-    "xm540-w270": 4096,
-    "xc430-w150": 4096,
-}
-
-MODEL_BAUDRATE_TABLE = {
-    "x_series": X_SERIES_BAUDRATE_TABLE,
-    "xl330-m077": X_SERIES_BAUDRATE_TABLE,
-    "xl330-m288": X_SERIES_BAUDRATE_TABLE,
-    "xl430-w250": X_SERIES_BAUDRATE_TABLE,
-    "xm430-w350": X_SERIES_BAUDRATE_TABLE,
-    "xm540-w270": X_SERIES_BAUDRATE_TABLE,
-    "xc430-w150": X_SERIES_BAUDRATE_TABLE,
-}
-
-NUM_READ_RETRY = 10
-NUM_WRITE_RETRY = 10
-
-
-def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray:
-    """This function converts the degree range to the step range for indicating motors rotation.
-    It assumes a motor achieves a full rotation by going from -180 degree position to +180.
-    The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
-    """
-    resolutions = [MODEL_RESOLUTION[model] for model in models]
-    steps = degrees / 180 * np.array(resolutions) / 2
-    steps = steps.astype(int)
-    return steps
-
-
-def convert_to_bytes(value, bytes, mock=False):
-    if mock:
-        return value
-
-    import dynamixel_sdk as dxl
-
-    # Note: No need to convert back into unsigned int, since this byte preprocessing
-    # already handles it for us.
-    if bytes == 1:
-        data = [
-            dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
-        ]
-    elif bytes == 2:
-        data = [
-            dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
-            dxl.DXL_HIBYTE(dxl.DXL_LOWORD(value)),
-        ]
-    elif bytes == 4:
-        data = [
-            dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
-            dxl.DXL_HIBYTE(dxl.DXL_LOWORD(value)),
-            dxl.DXL_LOBYTE(dxl.DXL_HIWORD(value)),
-            dxl.DXL_HIBYTE(dxl.DXL_HIWORD(value)),
-        ]
-    else:
-        raise NotImplementedError(
-            f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but "
-            f"{bytes} is provided instead."
-        )
-    return data
-
-
-def get_group_sync_key(data_name, motor_names):
-    group_key = f"{data_name}_" + "_".join(motor_names)
-    return group_key
-
-
-def get_result_name(fn_name, data_name, motor_names):
-    group_key = get_group_sync_key(data_name, motor_names)
-    rslt_name = f"{fn_name}_{group_key}"
-    return rslt_name
-
-
-def get_queue_name(fn_name, data_name, motor_names):
-    group_key = get_group_sync_key(data_name, motor_names)
-    queue_name = f"{fn_name}_{group_key}"
-    return queue_name
-
-
-def get_log_name(var_name, fn_name, data_name, motor_names):
-    group_key = get_group_sync_key(data_name, motor_names)
-    log_name = f"{var_name}_{fn_name}_{group_key}"
-    return log_name
-
-
-def assert_same_address(model_ctrl_table, motor_models, data_name):
-    all_addr = []
-    all_bytes = []
-    for model in motor_models:
-        addr, bytes = model_ctrl_table[model][data_name]
-        all_addr.append(addr)
-        all_bytes.append(bytes)
-
-    if len(set(all_addr)) != 1:
-        raise NotImplementedError(
-            f"At least two motor models use a different address for `data_name`='{data_name}' ({list(zip(motor_models, all_addr, strict=False))}). Contact a LeRobot maintainer."
-        )
-
-    if len(set(all_bytes)) != 1:
-        raise NotImplementedError(
-            f"At least two motor models use a different bytes representation for `data_name`='{data_name}' ({list(zip(motor_models, all_bytes, strict=False))}). Contact a LeRobot maintainer."
-        )
-
-
-class TorqueMode(enum.Enum):
-    ENABLED = 1
-    DISABLED = 0
-
-
-class DriveMode(enum.Enum):
-    NON_INVERTED = 0
-    INVERTED = 1
-
-
-class CalibrationMode(enum.Enum):
-    # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180]
-    DEGREE = 0
-    # Joints with linear motions (like gripper of Aloha) are expressed in nominal range of [0, 100]
-    LINEAR = 1
-
-
-class JointOutOfRangeError(Exception):
-    def __init__(self, message="Joint is out of range"):
-        self.message = message
-        super().__init__(self.message)
-
-
-class DynamixelMotorsBus:
-    """
-    The DynamixelMotorsBus class allows to efficiently read and write to the attached motors. It relies on
-    the python dynamixel sdk to communicate with the motors. For more info, see the [Dynamixel SDK Documentation](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/sample_code/python_read_write_protocol_2_0/#python-read-write-protocol-20).
-
-    A DynamixelMotorsBus instance requires a port (e.g. `DynamixelMotorsBus(port="/dev/tty.usbmodem575E0031751"`)).
-    To find the port, you can run our utility script:
-    ```bash
-    python lerobot/scripts/find_motors_bus_port.py
-    >>> Finding all available ports for the MotorBus.
-    >>> ['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751']
-    >>> Remove the usb cable from your DynamixelMotorsBus and press Enter when done.
-    >>> The port of this DynamixelMotorsBus is /dev/tty.usbmodem575E0031751.
-    >>> Reconnect the usb cable.
-    ```
-
-    Example of usage for 1 motor connected to the bus:
-    ```python
-    motor_name = "gripper"
-    motor_index = 6
-    motor_model = "xl330-m288"
-
-    config = DynamixelMotorsBusConfig(
-        port="/dev/tty.usbmodem575E0031751",
-        motors={motor_name: (motor_index, motor_model)},
-    )
-    motors_bus = DynamixelMotorsBus(config)
-    motors_bus.connect()
-
-    position = motors_bus.read("Present_Position")
-
-    # move from a few motor steps as an example
-    few_steps = 30
-    motors_bus.write("Goal_Position", position + few_steps)
-
-    # when done, consider disconnecting
-    motors_bus.disconnect()
-    ```
-    """
-
-    def __init__(
-        self,
-        config: DynamixelMotorsBusConfig,
-    ):
-        self.port = config.port
-        self.motors = config.motors
-        self.mock = config.mock
-
-        self.model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
-        self.model_resolution = deepcopy(MODEL_RESOLUTION)
-
-        self.port_handler = None
-        self.packet_handler = None
-        self.calibration = None
-        self.is_connected = False
-        self.group_readers = {}
-        self.group_writers = {}
-        self.logs = {}
-
-    def connect(self):
-        if self.is_connected:
-            raise RobotDeviceAlreadyConnectedError(
-                f"DynamixelMotorsBus({self.port}) is already connected. Do not call `motors_bus.connect()` twice."
-            )
-
-        if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
-        else:
-            import dynamixel_sdk as dxl
-
-        self.port_handler = dxl.PortHandler(self.port)
-        self.packet_handler = dxl.PacketHandler(PROTOCOL_VERSION)
-
-        try:
-            if not self.port_handler.openPort():
-                raise OSError(f"Failed to open port '{self.port}'.")
-        except Exception:
-            traceback.print_exc()
-            print(
-                "\nTry running `python lerobot/scripts/find_motors_bus_port.py` to make sure you are using the correct port.\n"
-            )
-            raise
-
-        # Allow to read and write
-        self.is_connected = True
-
-        self.port_handler.setPacketTimeoutMillis(TIMEOUT_MS)
-
-    def reconnect(self):
-        if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
-        else:
-            import dynamixel_sdk as dxl
-
-        self.port_handler = dxl.PortHandler(self.port)
-        self.packet_handler = dxl.PacketHandler(PROTOCOL_VERSION)
-
-        if not self.port_handler.openPort():
-            raise OSError(f"Failed to open port '{self.port}'.")
-
-        self.is_connected = True
-
-    def are_motors_configured(self):
-        # Only check the motor indices and not baudrate, since if the motor baudrates are incorrect,
-        # a ConnectionError will be raised anyway.
-        try:
-            return (self.motor_indices == self.read("ID")).all()
-        except ConnectionError as e:
-            print(e)
-            return False
-
-    def find_motor_indices(self, possible_ids=None, num_retry=2):
-        if possible_ids is None:
-            possible_ids = range(MAX_ID_RANGE)
-
-        indices = []
-        for idx in tqdm.tqdm(possible_ids):
-            try:
-                present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID", num_retry=num_retry)[0]
-            except ConnectionError:
-                continue
-
-            if idx != present_idx:
-                # sanity check
-                raise OSError(
-                    "Motor index used to communicate through the bus is not the same as the one present in the motor memory. The motor memory might be damaged."
-                )
-            indices.append(idx)
-
-        return indices
-
-    def set_bus_baudrate(self, baudrate):
-        present_bus_baudrate = self.port_handler.getBaudRate()
-        if present_bus_baudrate != baudrate:
-            print(f"Setting bus baud rate to {baudrate}. Previously {present_bus_baudrate}.")
-            self.port_handler.setBaudRate(baudrate)
-
-            if self.port_handler.getBaudRate() != baudrate:
-                raise OSError("Failed to write bus baud rate.")
-
-    @property
-    def motor_names(self) -> list[str]:
-        return list(self.motors.keys())
-
-    @property
-    def motor_models(self) -> list[str]:
-        return [model for _, model in self.motors.values()]
-
-    @property
-    def motor_indices(self) -> list[int]:
-        return [idx for idx, _ in self.motors.values()]
-
-    def set_calibration(self, calibration: dict[str, list]):
-        self.calibration = calibration
-
-    def apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """This function applies the calibration, automatically detects out of range errors for motors values and attempts to correct.
-
-        For more info, see docstring of `apply_calibration` and `autocorrect_calibration`.
-        """
-        try:
-            values = self.apply_calibration(values, motor_names)
-        except JointOutOfRangeError as e:
-            print(e)
-            self.autocorrect_calibration(values, motor_names)
-            values = self.apply_calibration(values, motor_names)
-        return values
-
-    def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """Convert from unsigned int32 joint position range [0, 2**32[ to the universal float32 nominal degree range ]-180.0, 180.0[ with
-        a "zero position" at 0 degree.
-
-        Note: We say "nominal degree range" since the motors can take values outside this range. For instance, 190 degrees, if the motor
-        rotate more than a half a turn from the zero position. However, most motors can't rotate more than 180 degrees and will stay in this range.
-
-        Joints values are original in [0, 2**32[ (unsigned int32). Each motor are expected to complete a full rotation
-        when given a goal position that is + or - their resolution. For instance, dynamixel xl330-m077 have a resolution of 4096, and
-        at any position in their original range, let's say the position 56734, they complete a full rotation clockwise by moving to 60830,
-        or anticlockwise by moving to 52638. The position in the original range is arbitrary and might change a lot between each motor.
-        To harmonize between motors of the same model, different robots, or even models of different brands, we propose to work
-        in the centered nominal degree range ]-180, 180[.
-        """
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        # Convert from unsigned int32 original range [0, 2**32] to signed float32 range
-        values = values.astype(np.float32)
-
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                drive_mode = self.calibration["drive_mode"][calib_idx]
-                homing_offset = self.calibration["homing_offset"][calib_idx]
-                _, model = self.motors[name]
-                resolution = self.model_resolution[model]
-
-                # Update direction of rotation of the motor to match between leader and follower.
-                # In fact, the motor of the leader for a given joint can be assembled in an
-                # opposite direction in term of rotation than the motor of the follower on the same joint.
-                if drive_mode:
-                    values[i] *= -1
-
-                # Convert from range [-2**31, 2**31] to
-                # nominal range [-resolution//2, resolution//2] (e.g. [-2048, 2048])
-                values[i] += homing_offset
-
-                # Convert from range [-resolution//2, resolution//2] to
-                # universal float32 centered degree range [-180, 180]
-                # (e.g. 2048 / (4096 // 2) * 180 = 180)
-                values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE
-
-                if (values[i] < LOWER_BOUND_DEGREE) or (values[i] > UPPER_BOUND_DEGREE):
-                    raise JointOutOfRangeError(
-                        f"Wrong motor position range detected for {name}. "
-                        f"Expected to be in nominal range of [-{HALF_TURN_DEGREE}, {HALF_TURN_DEGREE}] degrees (a full rotation), "
-                        f"with a maximum range of [{LOWER_BOUND_DEGREE}, {UPPER_BOUND_DEGREE}] degrees to account for joints that can rotate a bit more, "
-                        f"but present value is {values[i]} degree. "
-                        "This might be due to a cable connection issue creating an artificial 360 degrees jump in motor values. "
-                        "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
-                    )
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Rescale the present position to a nominal range [0, 100] %,
-                # useful for joints with linear motions like Aloha gripper
-                values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100
-
-                if (values[i] < LOWER_BOUND_LINEAR) or (values[i] > UPPER_BOUND_LINEAR):
-                    raise JointOutOfRangeError(
-                        f"Wrong motor position range detected for {name}. "
-                        f"Expected to be in nominal range of [0, 100] % (a full linear translation), "
-                        f"with a maximum range of [{LOWER_BOUND_LINEAR}, {UPPER_BOUND_LINEAR}] % to account for some imprecision during calibration, "
-                        f"but present value is {values[i]} %. "
-                        "This might be due to a cable connection issue creating an artificial jump in motor values. "
-                        "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
-                    )
-
-        return values
-
-    def autocorrect_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """This function automatically detects issues with values of motors after calibration, and correct for these issues.
-
-        Some motors might have values outside of expected maximum bounds after calibration.
-        For instance, for a joint in degree, its value can be outside [-270, 270] degrees, which is totally unexpected given
-        a nominal range of [-180, 180] degrees, which represents half a turn to the left or right starting from zero position.
-
-        Known issues:
-        #1: Motor value randomly shifts of a full turn, caused by hardware/connection errors.
-        #2: Motor internal homing offset is shifted by a full turn, caused by using default calibration (e.g Aloha).
-        #3: motor internal homing offset is shifted by less or more than a full turn, caused by using default calibration
-            or by human error during manual calibration.
-
-        Issues #1 and #2 can be solved by shifting the calibration homing offset by a full turn.
-        Issue #3 will be visually detected by user and potentially captured by the safety feature `max_relative_target`,
-        that will slow down the motor, raise an error asking to recalibrate. Manual recalibrating will solve the issue.
-
-        Note: A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096.
-        """
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        # Convert from unsigned int32 original range [0, 2**32] to signed float32 range
-        values = values.astype(np.float32)
-
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                drive_mode = self.calibration["drive_mode"][calib_idx]
-                homing_offset = self.calibration["homing_offset"][calib_idx]
-                _, model = self.motors[name]
-                resolution = self.model_resolution[model]
-
-                # Update direction of rotation of the motor to match between leader and follower.
-                # In fact, the motor of the leader for a given joint can be assembled in an
-                # opposite direction in term of rotation than the motor of the follower on the same joint.
-                if drive_mode:
-                    values[i] *= -1
-
-                # Convert from initial range to range [-180, 180] degrees
-                calib_val = (values[i] + homing_offset) / (resolution // 2) * HALF_TURN_DEGREE
-                in_range = (calib_val > LOWER_BOUND_DEGREE) and (calib_val < UPPER_BOUND_DEGREE)
-
-                # Solve this inequality to find the factor to shift the range into [-180, 180] degrees
-                # values[i] = (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE
-                # - HALF_TURN_DEGREE <= (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE <= HALF_TURN_DEGREE
-                # (- (resolution // 2) - values[i] - homing_offset) / resolution <= factor <= ((resolution // 2) - values[i] - homing_offset) / resolution
-                low_factor = (-(resolution // 2) - values[i] - homing_offset) / resolution
-                upp_factor = ((resolution // 2) - values[i] - homing_offset) / resolution
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Convert from initial range to range [0, 100] in %
-                calib_val = (values[i] - start_pos) / (end_pos - start_pos) * 100
-                in_range = (calib_val > LOWER_BOUND_LINEAR) and (calib_val < UPPER_BOUND_LINEAR)
-
-                # Solve this inequality to find the factor to shift the range into [0, 100] %
-                # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos + resolution * factor - start_pos - resolution * factor) * 100
-                # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100
-                # 0 <= (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 <= 100
-                # (start_pos - values[i]) / resolution <= factor <= (end_pos - values[i]) / resolution
-                low_factor = (start_pos - values[i]) / resolution
-                upp_factor = (end_pos - values[i]) / resolution
-
-            if not in_range:
-                # Get first integer between the two bounds
-                if low_factor < upp_factor:
-                    factor = math.ceil(low_factor)
-
-                    if factor > upp_factor:
-                        raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]")
-                else:
-                    factor = math.ceil(upp_factor)
-
-                    if factor > low_factor:
-                        raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]")
-
-                if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                    out_of_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees"
-                    in_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees"
-                elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                    out_of_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %"
-                    in_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %"
-
-                logging.warning(
-                    f"Auto-correct calibration of motor '{name}' by shifting value by {abs(factor)} full turns, "
-                    f"from '{out_of_range_str}' to '{in_range_str}'."
-                )
-
-                # A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096.
-                self.calibration["homing_offset"][calib_idx] += resolution * factor
-
-    def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """Inverse of `apply_calibration`."""
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                drive_mode = self.calibration["drive_mode"][calib_idx]
-                homing_offset = self.calibration["homing_offset"][calib_idx]
-                _, model = self.motors[name]
-                resolution = self.model_resolution[model]
-
-                # Convert from nominal 0-centered degree range [-180, 180] to
-                # 0-centered resolution range (e.g. [-2048, 2048] for resolution=4096)
-                values[i] = values[i] / HALF_TURN_DEGREE * (resolution // 2)
-
-                # Subtract the homing offsets to come back to actual motor range of values
-                # which can be arbitrary.
-                values[i] -= homing_offset
-
-                # Remove drive mode, which is the rotation direction of the motor, to come back to
-                # actual motor rotation direction which can be arbitrary.
-                if drive_mode:
-                    values[i] *= -1
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Convert from nominal lnear range of [0, 100] % to
-                # actual motor range of values which can be arbitrary.
-                values[i] = values[i] / 100 * (end_pos - start_pos) + start_pos
-
-        values = np.round(values).astype(np.int32)
-        return values
-
-    def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY):
-        if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
-        else:
-            import dynamixel_sdk as dxl
-
-        return_list = True
-        if not isinstance(motor_ids, list):
-            return_list = False
-            motor_ids = [motor_ids]
-
-        assert_same_address(self.model_ctrl_table, self.motor_models, data_name)
-        addr, bytes = self.model_ctrl_table[motor_models[0]][data_name]
-        group = dxl.GroupSyncRead(self.port_handler, self.packet_handler, addr, bytes)
-        for idx in motor_ids:
-            group.addParam(idx)
-
-        for _ in range(num_retry):
-            comm = group.txRxPacket()
-            if comm == dxl.COMM_SUCCESS:
-                break
-
-        if comm != dxl.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Read failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-        values = []
-        for idx in motor_ids:
-            value = group.getData(idx, addr, bytes)
-            values.append(value)
-
-        if return_list:
-            return values
-        else:
-            return values[0]
-
-    def read(self, data_name, motor_names: str | list[str] | None = None):
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`."
-            )
-
-        start_time = time.perf_counter()
-
-        if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
-        else:
-            import dynamixel_sdk as dxl
-
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        if isinstance(motor_names, str):
-            motor_names = [motor_names]
-
-        motor_ids = []
-        models = []
-        for name in motor_names:
-            motor_idx, model = self.motors[name]
-            motor_ids.append(motor_idx)
-            models.append(model)
-
-        assert_same_address(self.model_ctrl_table, models, data_name)
-        addr, bytes = self.model_ctrl_table[model][data_name]
-        group_key = get_group_sync_key(data_name, motor_names)
-
-        if data_name not in self.group_readers:
-            # create new group reader
-            self.group_readers[group_key] = dxl.GroupSyncRead(
-                self.port_handler, self.packet_handler, addr, bytes
-            )
-            for idx in motor_ids:
-                self.group_readers[group_key].addParam(idx)
-
-        for _ in range(NUM_READ_RETRY):
-            comm = self.group_readers[group_key].txRxPacket()
-            if comm == dxl.COMM_SUCCESS:
-                break
-
-        if comm != dxl.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Read failed due to communication error on port {self.port} for group_key {group_key}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-        values = []
-        for idx in motor_ids:
-            value = self.group_readers[group_key].getData(idx, addr, bytes)
-            values.append(value)
-
-        values = np.array(values)
-
-        # Convert to signed int to use range [-2048, 2048] for our motor positions.
-        if data_name in CONVERT_UINT32_TO_INT32_REQUIRED:
-            values = values.astype(np.int32)
-
-        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
-            values = self.apply_calibration_autocorrect(values, motor_names)
-
-        # log the number of seconds it took to read the data from the motors
-        delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names)
-        self.logs[delta_ts_name] = time.perf_counter() - start_time
-
-        # log the utc time at which the data was received
-        ts_utc_name = get_log_name("timestamp_utc", "read", data_name, motor_names)
-        self.logs[ts_utc_name] = capture_timestamp_utc()
-
-        return values
-
-    def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY):
-        if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
-        else:
-            import dynamixel_sdk as dxl
-
-        if not isinstance(motor_ids, list):
-            motor_ids = [motor_ids]
-        if not isinstance(values, list):
-            values = [values]
-
-        assert_same_address(self.model_ctrl_table, motor_models, data_name)
-        addr, bytes = self.model_ctrl_table[motor_models[0]][data_name]
-        group = dxl.GroupSyncWrite(self.port_handler, self.packet_handler, addr, bytes)
-        for idx, value in zip(motor_ids, values, strict=True):
-            data = convert_to_bytes(value, bytes, self.mock)
-            group.addParam(idx, data)
-
-        for _ in range(num_retry):
-            comm = group.txPacket()
-            if comm == dxl.COMM_SUCCESS:
-                break
-
-        if comm != dxl.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Write failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-    def write(self, data_name, values: int | float | np.ndarray, motor_names: str | list[str] | None = None):
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`."
-            )
-
-        start_time = time.perf_counter()
-
-        if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
-        else:
-            import dynamixel_sdk as dxl
-
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        if isinstance(motor_names, str):
-            motor_names = [motor_names]
-
-        if isinstance(values, (int, float, np.integer)):
-            values = [int(values)] * len(motor_names)
-
-        values = np.array(values)
-
-        motor_ids = []
-        models = []
-        for name in motor_names:
-            motor_idx, model = self.motors[name]
-            motor_ids.append(motor_idx)
-            models.append(model)
-
-        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
-            values = self.revert_calibration(values, motor_names)
-
-        values = values.tolist()
-
-        assert_same_address(self.model_ctrl_table, models, data_name)
-        addr, bytes = self.model_ctrl_table[model][data_name]
-        group_key = get_group_sync_key(data_name, motor_names)
-
-        init_group = data_name not in self.group_readers
-        if init_group:
-            self.group_writers[group_key] = dxl.GroupSyncWrite(
-                self.port_handler, self.packet_handler, addr, bytes
-            )
-
-        for idx, value in zip(motor_ids, values, strict=True):
-            data = convert_to_bytes(value, bytes, self.mock)
-            if init_group:
-                self.group_writers[group_key].addParam(idx, data)
-            else:
-                self.group_writers[group_key].changeParam(idx, data)
-
-        comm = self.group_writers[group_key].txPacket()
-        if comm != dxl.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Write failed due to communication error on port {self.port} for group_key {group_key}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-        # log the number of seconds it took to write the data to the motors
-        delta_ts_name = get_log_name("delta_timestamp_s", "write", data_name, motor_names)
-        self.logs[delta_ts_name] = time.perf_counter() - start_time
-
-        # TODO(rcadene): should we log the time before sending the write command?
-        # log the utc time when the write has been completed
-        ts_utc_name = get_log_name("timestamp_utc", "write", data_name, motor_names)
-        self.logs[ts_utc_name] = capture_timestamp_utc()
-
-    def disconnect(self):
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"DynamixelMotorsBus({self.port}) is not connected. Try running `motors_bus.connect()` first."
-            )
-
-        if self.port_handler is not None:
-            self.port_handler.closePort()
-            self.port_handler = None
-
-        self.packet_handler = None
-        self.group_readers = {}
-        self.group_writers = {}
-        self.is_connected = False
-
-    def __del__(self):
-        if getattr(self, "is_connected", False):
-            self.disconnect()

lerobot/common/robot_devices/motors/feetech.py

@@ -1,898 +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 enum
-import logging
-import math
-import time
-import traceback
-from copy import deepcopy
-
-import numpy as np
-import tqdm
-
-from lerobot.common.robot_devices.motors.configs import FeetechMotorsBusConfig
-from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
-from lerobot.common.utils.utils import capture_timestamp_utc
-
-PROTOCOL_VERSION = 0
-BAUDRATE = 1_000_000
-TIMEOUT_MS = 1000
-
-MAX_ID_RANGE = 252
-
-# The following bounds define the lower and upper joints range (after calibration).
-# For joints in degree (i.e. revolute joints), their nominal range is [-180, 180] degrees
-# which corresponds to a half rotation on the left and half rotation on the right.
-# Some joints might require higher range, so we allow up to [-270, 270] degrees until
-# an error is raised.
-LOWER_BOUND_DEGREE = -270
-UPPER_BOUND_DEGREE = 270
-# For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper),
-# their nominal range is [0, 100] %. For instance, for Aloha gripper, 0% is fully
-# closed, and 100% is fully open. To account for slight calibration issue, we allow up to
-# [-10, 110] until an error is raised.
-LOWER_BOUND_LINEAR = -10
-UPPER_BOUND_LINEAR = 110
-
-HALF_TURN_DEGREE = 180
-
-
-# See this link for STS3215 Memory Table:
-# https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true
-# data_name: (address, size_byte)
-SCS_SERIES_CONTROL_TABLE = {
-    "Model": (3, 2),
-    "ID": (5, 1),
-    "Baud_Rate": (6, 1),
-    "Return_Delay": (7, 1),
-    "Response_Status_Level": (8, 1),
-    "Min_Angle_Limit": (9, 2),
-    "Max_Angle_Limit": (11, 2),
-    "Max_Temperature_Limit": (13, 1),
-    "Max_Voltage_Limit": (14, 1),
-    "Min_Voltage_Limit": (15, 1),
-    "Max_Torque_Limit": (16, 2),
-    "Phase": (18, 1),
-    "Unloading_Condition": (19, 1),
-    "LED_Alarm_Condition": (20, 1),
-    "P_Coefficient": (21, 1),
-    "D_Coefficient": (22, 1),
-    "I_Coefficient": (23, 1),
-    "Minimum_Startup_Force": (24, 2),
-    "CW_Dead_Zone": (26, 1),
-    "CCW_Dead_Zone": (27, 1),
-    "Protection_Current": (28, 2),
-    "Angular_Resolution": (30, 1),
-    "Offset": (31, 2),
-    "Mode": (33, 1),
-    "Protective_Torque": (34, 1),
-    "Protection_Time": (35, 1),
-    "Overload_Torque": (36, 1),
-    "Speed_closed_loop_P_proportional_coefficient": (37, 1),
-    "Over_Current_Protection_Time": (38, 1),
-    "Velocity_closed_loop_I_integral_coefficient": (39, 1),
-    "Torque_Enable": (40, 1),
-    "Acceleration": (41, 1),
-    "Goal_Position": (42, 2),
-    "Goal_Time": (44, 2),
-    "Goal_Speed": (46, 2),
-    "Torque_Limit": (48, 2),
-    "Lock": (55, 1),
-    "Present_Position": (56, 2),
-    "Present_Speed": (58, 2),
-    "Present_Load": (60, 2),
-    "Present_Voltage": (62, 1),
-    "Present_Temperature": (63, 1),
-    "Status": (65, 1),
-    "Moving": (66, 1),
-    "Present_Current": (69, 2),
-    # Not in the Memory Table
-    "Maximum_Acceleration": (85, 2),
-}
-
-SCS_SERIES_BAUDRATE_TABLE = {
-    0: 1_000_000,
-    1: 500_000,
-    2: 250_000,
-    3: 128_000,
-    4: 115_200,
-    5: 57_600,
-    6: 38_400,
-    7: 19_200,
-}
-
-CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"]
-CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"]
-
-
-MODEL_CONTROL_TABLE = {
-    "scs_series": SCS_SERIES_CONTROL_TABLE,
-    "sts3215": SCS_SERIES_CONTROL_TABLE,
-}
-
-MODEL_RESOLUTION = {
-    "scs_series": 4096,
-    "sts3215": 4096,
-}
-
-MODEL_BAUDRATE_TABLE = {
-    "scs_series": SCS_SERIES_BAUDRATE_TABLE,
-    "sts3215": SCS_SERIES_BAUDRATE_TABLE,
-}
-
-# High number of retries is needed for feetech compared to dynamixel motors.
-NUM_READ_RETRY = 20
-NUM_WRITE_RETRY = 20
-
-
-def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray:
-    """This function converts the degree range to the step range for indicating motors rotation.
-    It assumes a motor achieves a full rotation by going from -180 degree position to +180.
-    The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
-    """
-    resolutions = [MODEL_RESOLUTION[model] for model in models]
-    steps = degrees / 180 * np.array(resolutions) / 2
-    steps = steps.astype(int)
-    return steps
-
-
-def convert_to_bytes(value, bytes, mock=False):
-    if mock:
-        return value
-
-    import scservo_sdk as scs
-
-    # Note: No need to convert back into unsigned int, since this byte preprocessing
-    # already handles it for us.
-    if bytes == 1:
-        data = [
-            scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
-        ]
-    elif bytes == 2:
-        data = [
-            scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
-            scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
-        ]
-    elif bytes == 4:
-        data = [
-            scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
-            scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
-            scs.SCS_LOBYTE(scs.SCS_HIWORD(value)),
-            scs.SCS_HIBYTE(scs.SCS_HIWORD(value)),
-        ]
-    else:
-        raise NotImplementedError(
-            f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but "
-            f"{bytes} is provided instead."
-        )
-    return data
-
-
-def get_group_sync_key(data_name, motor_names):
-    group_key = f"{data_name}_" + "_".join(motor_names)
-    return group_key
-
-
-def get_result_name(fn_name, data_name, motor_names):
-    group_key = get_group_sync_key(data_name, motor_names)
-    rslt_name = f"{fn_name}_{group_key}"
-    return rslt_name
-
-
-def get_queue_name(fn_name, data_name, motor_names):
-    group_key = get_group_sync_key(data_name, motor_names)
-    queue_name = f"{fn_name}_{group_key}"
-    return queue_name
-
-
-def get_log_name(var_name, fn_name, data_name, motor_names):
-    group_key = get_group_sync_key(data_name, motor_names)
-    log_name = f"{var_name}_{fn_name}_{group_key}"
-    return log_name
-
-
-def assert_same_address(model_ctrl_table, motor_models, data_name):
-    all_addr = []
-    all_bytes = []
-    for model in motor_models:
-        addr, bytes = model_ctrl_table[model][data_name]
-        all_addr.append(addr)
-        all_bytes.append(bytes)
-
-    if len(set(all_addr)) != 1:
-        raise NotImplementedError(
-            f"At least two motor models use a different address for `data_name`='{data_name}' ({list(zip(motor_models, all_addr, strict=False))}). Contact a LeRobot maintainer."
-        )
-
-    if len(set(all_bytes)) != 1:
-        raise NotImplementedError(
-            f"At least two motor models use a different bytes representation for `data_name`='{data_name}' ({list(zip(motor_models, all_bytes, strict=False))}). Contact a LeRobot maintainer."
-        )
-
-
-class TorqueMode(enum.Enum):
-    ENABLED = 1
-    DISABLED = 0
-
-
-class DriveMode(enum.Enum):
-    NON_INVERTED = 0
-    INVERTED = 1
-
-
-class CalibrationMode(enum.Enum):
-    # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180]
-    DEGREE = 0
-    # Joints with linear motions (like gripper of Aloha) are expressed in nominal range of [0, 100]
-    LINEAR = 1
-
-
-class JointOutOfRangeError(Exception):
-    def __init__(self, message="Joint is out of range"):
-        self.message = message
-        super().__init__(self.message)
-
-
-class FeetechMotorsBus:
-    """
-    The FeetechMotorsBus class allows to efficiently read and write to the attached motors. It relies on
-    the python feetech sdk to communicate with the motors. For more info, see the [feetech SDK Documentation](https://emanual.robotis.com/docs/en/software/feetech/feetech_sdk/sample_code/python_read_write_protocol_2_0/#python-read-write-protocol-20).
-
-    A FeetechMotorsBus instance requires a port (e.g. `FeetechMotorsBus(port="/dev/tty.usbmodem575E0031751"`)).
-    To find the port, you can run our utility script:
-    ```bash
-    python lerobot/scripts/find_motors_bus_port.py
-    >>> Finding all available ports for the MotorsBus.
-    >>> ['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751']
-    >>> Remove the usb cable from your FeetechMotorsBus and press Enter when done.
-    >>> The port of this FeetechMotorsBus is /dev/tty.usbmodem575E0031751.
-    >>> Reconnect the usb cable.
-    ```
-
-    Example of usage for 1 motor connected to the bus:
-    ```python
-    motor_name = "gripper"
-    motor_index = 6
-    motor_model = "sts3215"
-
-    config = FeetechMotorsBusConfig(
-        port="/dev/tty.usbmodem575E0031751",
-        motors={motor_name: (motor_index, motor_model)},
-    )
-    motors_bus = FeetechMotorsBus(config)
-    motors_bus.connect()
-
-    position = motors_bus.read("Present_Position")
-
-    # move from a few motor steps as an example
-    few_steps = 30
-    motors_bus.write("Goal_Position", position + few_steps)
-
-    # when done, consider disconnecting
-    motors_bus.disconnect()
-    ```
-    """
-
-    def __init__(
-        self,
-        config: FeetechMotorsBusConfig,
-    ):
-        self.port = config.port
-        self.motors = config.motors
-        self.mock = config.mock
-
-        self.model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
-        self.model_resolution = deepcopy(MODEL_RESOLUTION)
-
-        self.port_handler = None
-        self.packet_handler = None
-        self.calibration = None
-        self.is_connected = False
-        self.group_readers = {}
-        self.group_writers = {}
-        self.logs = {}
-
-        self.track_positions = {}
-
-    def connect(self):
-        if self.is_connected:
-            raise RobotDeviceAlreadyConnectedError(
-                f"FeetechMotorsBus({self.port}) is already connected. Do not call `motors_bus.connect()` twice."
-            )
-
-        if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
-        else:
-            import scservo_sdk as scs
-
-        self.port_handler = scs.PortHandler(self.port)
-        self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION)
-
-        try:
-            if not self.port_handler.openPort():
-                raise OSError(f"Failed to open port '{self.port}'.")
-        except Exception:
-            traceback.print_exc()
-            print(
-                "\nTry running `python lerobot/scripts/find_motors_bus_port.py` to make sure you are using the correct port.\n"
-            )
-            raise
-
-        # Allow to read and write
-        self.is_connected = True
-
-        self.port_handler.setPacketTimeoutMillis(TIMEOUT_MS)
-
-    def reconnect(self):
-        if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
-        else:
-            import scservo_sdk as scs
-
-        self.port_handler = scs.PortHandler(self.port)
-        self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION)
-
-        if not self.port_handler.openPort():
-            raise OSError(f"Failed to open port '{self.port}'.")
-
-        self.is_connected = True
-
-    def are_motors_configured(self):
-        # Only check the motor indices and not baudrate, since if the motor baudrates are incorrect,
-        # a ConnectionError will be raised anyway.
-        try:
-            return (self.motor_indices == self.read("ID")).all()
-        except ConnectionError as e:
-            print(e)
-            return False
-
-    def find_motor_indices(self, possible_ids=None, num_retry=2):
-        if possible_ids is None:
-            possible_ids = range(MAX_ID_RANGE)
-
-        indices = []
-        for idx in tqdm.tqdm(possible_ids):
-            try:
-                present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID", num_retry=num_retry)[0]
-            except ConnectionError:
-                continue
-
-            if idx != present_idx:
-                # sanity check
-                raise OSError(
-                    "Motor index used to communicate through the bus is not the same as the one present in the motor memory. The motor memory might be damaged."
-                )
-            indices.append(idx)
-
-        return indices
-
-    def set_bus_baudrate(self, baudrate):
-        present_bus_baudrate = self.port_handler.getBaudRate()
-        if present_bus_baudrate != baudrate:
-            print(f"Setting bus baud rate to {baudrate}. Previously {present_bus_baudrate}.")
-            self.port_handler.setBaudRate(baudrate)
-
-            if self.port_handler.getBaudRate() != baudrate:
-                raise OSError("Failed to write bus baud rate.")
-
-    @property
-    def motor_names(self) -> list[str]:
-        return list(self.motors.keys())
-
-    @property
-    def motor_models(self) -> list[str]:
-        return [model for _, model in self.motors.values()]
-
-    @property
-    def motor_indices(self) -> list[int]:
-        return [idx for idx, _ in self.motors.values()]
-
-    def set_calibration(self, calibration: dict[str, list]):
-        self.calibration = calibration
-
-    def apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """This function apply the calibration, automatically detects out of range errors for motors values and attempt to correct.
-
-        For more info, see docstring of `apply_calibration` and `autocorrect_calibration`.
-        """
-        try:
-            values = self.apply_calibration(values, motor_names)
-        except JointOutOfRangeError as e:
-            print(e)
-            self.autocorrect_calibration(values, motor_names)
-            values = self.apply_calibration(values, motor_names)
-        return values
-
-    def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """Convert from unsigned int32 joint position range [0, 2**32[ to the universal float32 nominal degree range ]-180.0, 180.0[ with
-        a "zero position" at 0 degree.
-
-        Note: We say "nominal degree range" since the motors can take values outside this range. For instance, 190 degrees, if the motor
-        rotate more than a half a turn from the zero position. However, most motors can't rotate more than 180 degrees and will stay in this range.
-
-        Joints values are original in [0, 2**32[ (unsigned int32). Each motor are expected to complete a full rotation
-        when given a goal position that is + or - their resolution. For instance, feetech xl330-m077 have a resolution of 4096, and
-        at any position in their original range, let's say the position 56734, they complete a full rotation clockwise by moving to 60830,
-        or anticlockwise by moving to 52638. The position in the original range is arbitrary and might change a lot between each motor.
-        To harmonize between motors of the same model, different robots, or even models of different brands, we propose to work
-        in the centered nominal degree range ]-180, 180[.
-        """
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        # Convert from unsigned int32 original range [0, 2**32] to signed float32 range
-        values = values.astype(np.float32)
-
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                drive_mode = self.calibration["drive_mode"][calib_idx]
-                homing_offset = self.calibration["homing_offset"][calib_idx]
-                _, model = self.motors[name]
-                resolution = self.model_resolution[model]
-
-                # Update direction of rotation of the motor to match between leader and follower.
-                # In fact, the motor of the leader for a given joint can be assembled in an
-                # opposite direction in term of rotation than the motor of the follower on the same joint.
-                if drive_mode:
-                    values[i] *= -1
-
-                # Convert from range [-2**31, 2**31[ to
-                # nominal range ]-resolution, resolution[ (e.g. ]-2048, 2048[)
-                values[i] += homing_offset
-
-                # Convert from range ]-resolution, resolution[ to
-                # universal float32 centered degree range ]-180, 180[
-                values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE
-
-                if (values[i] < LOWER_BOUND_DEGREE) or (values[i] > UPPER_BOUND_DEGREE):
-                    raise JointOutOfRangeError(
-                        f"Wrong motor position range detected for {name}. "
-                        f"Expected to be in nominal range of [-{HALF_TURN_DEGREE}, {HALF_TURN_DEGREE}] degrees (a full rotation), "
-                        f"with a maximum range of [{LOWER_BOUND_DEGREE}, {UPPER_BOUND_DEGREE}] degrees to account for joints that can rotate a bit more, "
-                        f"but present value is {values[i]} degree. "
-                        "This might be due to a cable connection issue creating an artificial 360 degrees jump in motor values. "
-                        "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
-                    )
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Rescale the present position to a nominal range [0, 100] %,
-                # useful for joints with linear motions like Aloha gripper
-                values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100
-
-                if (values[i] < LOWER_BOUND_LINEAR) or (values[i] > UPPER_BOUND_LINEAR):
-                    raise JointOutOfRangeError(
-                        f"Wrong motor position range detected for {name}. "
-                        f"Expected to be in nominal range of [0, 100] % (a full linear translation), "
-                        f"with a maximum range of [{LOWER_BOUND_LINEAR}, {UPPER_BOUND_LINEAR}] % to account for some imprecision during calibration, "
-                        f"but present value is {values[i]} %. "
-                        "This might be due to a cable connection issue creating an artificial jump in motor values. "
-                        "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
-                    )
-
-        return values
-
-    def autocorrect_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """This function automatically detects issues with values of motors after calibration, and correct for these issues.
-
-        Some motors might have values outside of expected maximum bounds after calibration.
-        For instance, for a joint in degree, its value can be outside [-270, 270] degrees, which is totally unexpected given
-        a nominal range of [-180, 180] degrees, which represents half a turn to the left or right starting from zero position.
-
-        Known issues:
-        #1: Motor value randomly shifts of a full turn, caused by hardware/connection errors.
-        #2: Motor internal homing offset is shifted of a full turn, caused by using default calibration (e.g Aloha).
-        #3: motor internal homing offset is shifted of less or more than a full turn, caused by using default calibration
-            or by human error during manual calibration.
-
-        Issues #1 and #2 can be solved by shifting the calibration homing offset by a full turn.
-        Issue #3 will be visually detected by user and potentially captured by the safety feature `max_relative_target`,
-        that will slow down the motor, raise an error asking to recalibrate. Manual recalibrating will solve the issue.
-
-        Note: A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096.
-        """
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        # Convert from unsigned int32 original range [0, 2**32] to signed float32 range
-        values = values.astype(np.float32)
-
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                drive_mode = self.calibration["drive_mode"][calib_idx]
-                homing_offset = self.calibration["homing_offset"][calib_idx]
-                _, model = self.motors[name]
-                resolution = self.model_resolution[model]
-
-                if drive_mode:
-                    values[i] *= -1
-
-                # Convert from initial range to range [-180, 180] degrees
-                calib_val = (values[i] + homing_offset) / (resolution // 2) * HALF_TURN_DEGREE
-                in_range = (calib_val > LOWER_BOUND_DEGREE) and (calib_val < UPPER_BOUND_DEGREE)
-
-                # Solve this inequality to find the factor to shift the range into [-180, 180] degrees
-                # values[i] = (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE
-                # - HALF_TURN_DEGREE <= (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE <= HALF_TURN_DEGREE
-                # (- HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset) / resolution <= factor <= (HALF_TURN_DEGREE / 180 * (resolution // 2) - values[i] - homing_offset) / resolution
-                low_factor = (
-                    -HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset
-                ) / resolution
-                upp_factor = (
-                    HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset
-                ) / resolution
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Convert from initial range to range [0, 100] in %
-                calib_val = (values[i] - start_pos) / (end_pos - start_pos) * 100
-                in_range = (calib_val > LOWER_BOUND_LINEAR) and (calib_val < UPPER_BOUND_LINEAR)
-
-                # Solve this inequality to find the factor to shift the range into [0, 100] %
-                # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos + resolution * factor - start_pos - resolution * factor) * 100
-                # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100
-                # 0 <= (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 <= 100
-                # (start_pos - values[i]) / resolution <= factor <= (end_pos - values[i]) / resolution
-                low_factor = (start_pos - values[i]) / resolution
-                upp_factor = (end_pos - values[i]) / resolution
-
-            if not in_range:
-                # Get first integer between the two bounds
-                if low_factor < upp_factor:
-                    factor = math.ceil(low_factor)
-
-                    if factor > upp_factor:
-                        raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]")
-                else:
-                    factor = math.ceil(upp_factor)
-
-                    if factor > low_factor:
-                        raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]")
-
-                if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                    out_of_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees"
-                    in_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees"
-                elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                    out_of_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %"
-                    in_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %"
-
-                logging.warning(
-                    f"Auto-correct calibration of motor '{name}' by shifting value by {abs(factor)} full turns, "
-                    f"from '{out_of_range_str}' to '{in_range_str}'."
-                )
-
-                # A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096.
-                self.calibration["homing_offset"][calib_idx] += resolution * factor
-
-    def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """Inverse of `apply_calibration`."""
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                drive_mode = self.calibration["drive_mode"][calib_idx]
-                homing_offset = self.calibration["homing_offset"][calib_idx]
-                _, model = self.motors[name]
-                resolution = self.model_resolution[model]
-
-                # Convert from nominal 0-centered degree range [-180, 180] to
-                # 0-centered resolution range (e.g. [-2048, 2048] for resolution=4096)
-                values[i] = values[i] / HALF_TURN_DEGREE * (resolution // 2)
-
-                # Subtract the homing offsets to come back to actual motor range of values
-                # which can be arbitrary.
-                values[i] -= homing_offset
-
-                # Remove drive mode, which is the rotation direction of the motor, to come back to
-                # actual motor rotation direction which can be arbitrary.
-                if drive_mode:
-                    values[i] *= -1
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Convert from nominal lnear range of [0, 100] % to
-                # actual motor range of values which can be arbitrary.
-                values[i] = values[i] / 100 * (end_pos - start_pos) + start_pos
-
-        values = np.round(values).astype(np.int32)
-        return values
-
-    def avoid_rotation_reset(self, values, motor_names, data_name):
-        if data_name not in self.track_positions:
-            self.track_positions[data_name] = {
-                "prev": [None] * len(self.motor_names),
-                # Assume False at initialization
-                "below_zero": [False] * len(self.motor_names),
-                "above_max": [False] * len(self.motor_names),
-            }
-
-        track = self.track_positions[data_name]
-
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        for i, name in enumerate(motor_names):
-            idx = self.motor_names.index(name)
-
-            if track["prev"][idx] is None:
-                track["prev"][idx] = values[i]
-                continue
-
-            # Detect a full rotation occurred
-            if abs(track["prev"][idx] - values[i]) > 2048:
-                # Position went below 0 and got reset to 4095
-                if track["prev"][idx] < values[i]:
-                    # So we set negative value by adding a full rotation
-                    values[i] -= 4096
-
-                # Position went above 4095 and got reset to 0
-                elif track["prev"][idx] > values[i]:
-                    # So we add a full rotation
-                    values[i] += 4096
-
-            track["prev"][idx] = values[i]
-
-        return values
-
-    def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY):
-        if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
-        else:
-            import scservo_sdk as scs
-
-        return_list = True
-        if not isinstance(motor_ids, list):
-            return_list = False
-            motor_ids = [motor_ids]
-
-        assert_same_address(self.model_ctrl_table, self.motor_models, data_name)
-        addr, bytes = self.model_ctrl_table[motor_models[0]][data_name]
-        group = scs.GroupSyncRead(self.port_handler, self.packet_handler, addr, bytes)
-        for idx in motor_ids:
-            group.addParam(idx)
-
-        for _ in range(num_retry):
-            comm = group.txRxPacket()
-            if comm == scs.COMM_SUCCESS:
-                break
-
-        if comm != scs.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Read failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-        values = []
-        for idx in motor_ids:
-            value = group.getData(idx, addr, bytes)
-            values.append(value)
-
-        if return_list:
-            return values
-        else:
-            return values[0]
-
-    def read(self, data_name, motor_names: str | list[str] | None = None):
-        if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
-        else:
-            import scservo_sdk as scs
-
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`."
-            )
-
-        start_time = time.perf_counter()
-
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        if isinstance(motor_names, str):
-            motor_names = [motor_names]
-
-        motor_ids = []
-        models = []
-        for name in motor_names:
-            motor_idx, model = self.motors[name]
-            motor_ids.append(motor_idx)
-            models.append(model)
-
-        assert_same_address(self.model_ctrl_table, models, data_name)
-        addr, bytes = self.model_ctrl_table[model][data_name]
-        group_key = get_group_sync_key(data_name, motor_names)
-
-        if data_name not in self.group_readers:
-            # Very Important to flush the buffer!
-            self.port_handler.ser.reset_output_buffer()
-            self.port_handler.ser.reset_input_buffer()
-
-            # create new group reader
-            self.group_readers[group_key] = scs.GroupSyncRead(
-                self.port_handler, self.packet_handler, addr, bytes
-            )
-            for idx in motor_ids:
-                self.group_readers[group_key].addParam(idx)
-
-        for _ in range(NUM_READ_RETRY):
-            comm = self.group_readers[group_key].txRxPacket()
-            if comm == scs.COMM_SUCCESS:
-                break
-
-        if comm != scs.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Read failed due to communication error on port {self.port} for group_key {group_key}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-        values = []
-        for idx in motor_ids:
-            value = self.group_readers[group_key].getData(idx, addr, bytes)
-            values.append(value)
-
-        values = np.array(values)
-
-        # Convert to signed int to use range [-2048, 2048] for our motor positions.
-        if data_name in CONVERT_UINT32_TO_INT32_REQUIRED:
-            values = values.astype(np.int32)
-
-        if data_name in CALIBRATION_REQUIRED:
-            values = self.avoid_rotation_reset(values, motor_names, data_name)
-
-        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
-            values = self.apply_calibration_autocorrect(values, motor_names)
-
-        # log the number of seconds it took to read the data from the motors
-        delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names)
-        self.logs[delta_ts_name] = time.perf_counter() - start_time
-
-        # log the utc time at which the data was received
-        ts_utc_name = get_log_name("timestamp_utc", "read", data_name, motor_names)
-        self.logs[ts_utc_name] = capture_timestamp_utc()
-
-        return values
-
-    def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY):
-        if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
-        else:
-            import scservo_sdk as scs
-
-        if not isinstance(motor_ids, list):
-            motor_ids = [motor_ids]
-        if not isinstance(values, list):
-            values = [values]
-
-        assert_same_address(self.model_ctrl_table, motor_models, data_name)
-        addr, bytes = self.model_ctrl_table[motor_models[0]][data_name]
-        group = scs.GroupSyncWrite(self.port_handler, self.packet_handler, addr, bytes)
-        for idx, value in zip(motor_ids, values, strict=True):
-            data = convert_to_bytes(value, bytes, self.mock)
-            group.addParam(idx, data)
-
-        for _ in range(num_retry):
-            comm = group.txPacket()
-            if comm == scs.COMM_SUCCESS:
-                break
-
-        if comm != scs.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Write failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-    def write(self, data_name, values: int | float | np.ndarray, motor_names: str | list[str] | None = None):
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`."
-            )
-
-        start_time = time.perf_counter()
-
-        if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
-        else:
-            import scservo_sdk as scs
-
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        if isinstance(motor_names, str):
-            motor_names = [motor_names]
-
-        if isinstance(values, (int, float, np.integer)):
-            values = [int(values)] * len(motor_names)
-
-        values = np.array(values)
-
-        motor_ids = []
-        models = []
-        for name in motor_names:
-            motor_idx, model = self.motors[name]
-            motor_ids.append(motor_idx)
-            models.append(model)
-
-        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
-            values = self.revert_calibration(values, motor_names)
-
-        values = values.tolist()
-
-        assert_same_address(self.model_ctrl_table, models, data_name)
-        addr, bytes = self.model_ctrl_table[model][data_name]
-        group_key = get_group_sync_key(data_name, motor_names)
-
-        init_group = data_name not in self.group_readers
-        if init_group:
-            self.group_writers[group_key] = scs.GroupSyncWrite(
-                self.port_handler, self.packet_handler, addr, bytes
-            )
-
-        for idx, value in zip(motor_ids, values, strict=True):
-            data = convert_to_bytes(value, bytes, self.mock)
-            if init_group:
-                self.group_writers[group_key].addParam(idx, data)
-            else:
-                self.group_writers[group_key].changeParam(idx, data)
-
-        comm = self.group_writers[group_key].txPacket()
-        if comm != scs.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Write failed due to communication error on port {self.port} for group_key {group_key}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-        # log the number of seconds it took to write the data to the motors
-        delta_ts_name = get_log_name("delta_timestamp_s", "write", data_name, motor_names)
-        self.logs[delta_ts_name] = time.perf_counter() - start_time
-
-        # TODO(rcadene): should we log the time before sending the write command?
-        # log the utc time when the write has been completed
-        ts_utc_name = get_log_name("timestamp_utc", "write", data_name, motor_names)
-        self.logs[ts_utc_name] = capture_timestamp_utc()
-
-    def disconnect(self):
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                f"FeetechMotorsBus({self.port}) is not connected. Try running `motors_bus.connect()` first."
-            )
-
-        if self.port_handler is not None:
-            self.port_handler.closePort()
-            self.port_handler = None
-
-        self.packet_handler = None
-        self.group_readers = {}
-        self.group_writers = {}
-        self.is_connected = False
-
-    def __del__(self):
-        if getattr(self, "is_connected", False):
-            self.disconnect()

lerobot/common/robot_devices/motors/realman.py

@@ -1,150 +0,0 @@
-import time
-from typing import Dict
-from lerobot.common.robot_devices.motors.configs import RealmanMotorsBusConfig
-from Robotic_Arm.rm_robot_interface import *
-
-
-class RealmanMotorsBus:
-    """
-        对Realman SDK的二次封装
-    """
-    def __init__(self, 
-                 config: RealmanMotorsBusConfig):
-        self.rmarm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
-        self.handle = self.rmarm.rm_create_robot_arm(config.ip, config.port)
-        self.motors = config.motors
-        self.init_joint_position = config.init_joint['joint'] # [6 joints + 1 gripper]
-        self.safe_disable_position = config.init_joint['joint']
-        self.rmarm.rm_movej(self.init_joint_position[:-1], 5, 0, 0, 1)
-        time.sleep(3)
-        ret = self.rmarm.rm_get_current_arm_state()
-        self.init_pose = ret[1]['pose']
-
-    @property
-    def motor_names(self) -> list[str]:
-        return list(self.motors.keys())
-
-    @property
-    def motor_models(self) -> list[str]:
-        return [model for _, model in self.motors.values()]
-
-    @property
-    def motor_indices(self) -> list[int]:
-        return [idx for idx, _ in self.motors.values()]
-
-
-    def connect(self, enable=True) -> bool:
-        '''
-        使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
-        '''
-        enable_flag = False
-        loop_flag = False
-        # 设置超时时间（秒）
-        timeout = 5
-        # 记录进入循环前的时间
-        start_time = time.time()
-        elapsed_time_flag = False
-        
-        while not loop_flag:
-            elapsed_time = time.time() - start_time
-            print("--------------------")
-            
-            if enable:
-                # 获取机械臂状态
-                ret = self.rmarm.rm_get_current_arm_state()
-                if ret[0] == 0:  # 成功获取状态
-                    enable_flag = True
-            else:
-                enable_flag = False
-                # 断开所有连接，销毁线程
-                RoboticArm.rm_destory()
-            print("使能状态:", enable_flag)
-            print("--------------------")
-            if(enable_flag == enable):
-                loop_flag = True
-                enable_flag = True
-            else: 
-                loop_flag = False
-                enable_flag = False
-            # 检查是否超过超时时间
-            if elapsed_time > timeout:
-                print("超时....")
-                elapsed_time_flag = True
-                enable_flag = True
-                break
-            time.sleep(1)
-
-        resp = enable_flag
-        print(f"Returning response: {resp}")
-        return resp
-    
-    def motor_names(self):
-        return
-
-    def set_calibration(self):
-        return
-    
-    def revert_calibration(self):
-        return
-
-    def apply_calibration(self):
-        """
-            移动到初始位置
-        """
-        self.write(target_joint=self.init_joint_position)
-
-    def write(self, target_joint:list):
-        # self.rmarm.rm_movej(target_joint[:-1], 50, 0, 0, 1)
-        self.rmarm.rm_movej_follow(target_joint[:-1])
-        self.rmarm.rm_set_gripper_position(target_joint[-1], block=False, timeout=2)
-    
-    def write_endpose(self, target_endpose: list, gripper: int):
-        self.rmarm.rm_movej_p(target_endpose, 50, 0, 0, 1)
-        self.rmarm.rm_set_gripper_position(gripper, block=False, timeout=2)
-
-    def write_joint_slow(self, target_joint: list):
-        self.rmarm.rm_movej(target_joint, 5, 0, 0, 0)
-
-    def write_joint_canfd(self, target_joint: list):
-        self.rmarm.rm_movej_canfd(target_joint, False)
-
-    def write_endpose_canfd(self, target_pose: list):
-        self.rmarm.rm_movep_canfd(target_pose, False)
-
-    def write_gripper(self, gripper: int):
-        self.rmarm.rm_set_gripper_position(gripper, False, 2)
-
-    def read(self) -> Dict:
-        """
-            - 机械臂关节消息,单位1度;[-1, 1]
-            - 机械臂夹爪消息,[-1, 1]
-        """
-        joint_msg = self.rmarm.rm_get_current_arm_state()[1]
-        joint_state = joint_msg['joint']
-
-        gripper_msg = self.rmarm.rm_get_gripper_state()[1]
-        gripper_state = gripper_msg['actpos']
-        
-        return {
-            "joint_1": joint_state[0]/180,
-            "joint_2": joint_state[1]/180,
-            "joint_3": joint_state[2]/180,
-            "joint_4": joint_state[3]/180,
-            "joint_5": joint_state[4]/180,
-            "joint_6": joint_state[5]/180,
-            "gripper": (gripper_state-500)/500
-        }
-    
-    def read_current_arm_joint_state(self):
-        return self.rmarm.rm_get_current_arm_state()[1]['joint']
-    
-    def read_current_arm_endpose_state(self):
-        return self.rmarm.rm_get_current_arm_state()[1]['pose']
-
-    def safe_disconnect(self):
-        """ 
-            Move to safe disconnect position
-        """
-        self.write(target_joint=self.safe_disable_position)
-        # 断开所有连接，销毁线程
-        RoboticArm.rm_destory()

lerobot/common/robot_devices/motors/realman_dual.py

@@ -1,351 +0,0 @@
-import time
-import threading
-from typing import Dict
-from dataclasses import dataclass
-from contextlib import contextmanager
-from lerobot.common.robot_devices.motors.configs import RealmanDualMotorsBusConfig
-from Robotic_Arm.rm_robot_interface import *
-
-
-def compare_joint_difference(master_joints, follow_joints, threshold=30.0):
-    """
-    比较主臂和从臂关节数据的差异
-    
-    Args:
-        master_joints (list): 主臂关节数据 [joint1, joint2, ..., joint6]
-        follow_joints (list): 从臂关节数据 [joint1, joint2, ..., joint6]
-        threshold (float): 差异阈值（度），默认5度
-    
-    Returns:
-        bool: True表示差异在阈值内，False表示超过阈值
-    """
-    # 检查数据长度
-    if len(master_joints) != len(follow_joints):
-        return False
-    
-    # 计算每个关节的绝对差异
-    for i in range(len(master_joints)):
-        diff = abs(master_joints[i] - follow_joints[i])
-        if diff > threshold:
-            return False
-    
-    return True
-
-
-@dataclass
-class ArmState:
-    """机械臂状态数据类"""
-    joint_positions: list
-    gripper_position: int
-    pose: list
-
-
-class RealmanDualMotorsBus:
-    """
-        对Realman SDK的二次封装
-    """
-    def __init__(self, config: RealmanDualMotorsBusConfig):
-        self.config = config
-        # import pdb; pdb.set_trace()
-        self._initialize_arms()
-        self._initialize_parameters()
-        self._initialize_positions()
-        self._initialize_threading()
-        
-    def _initialize_arms(self):
-        """初始化机械臂连接"""
-        self.left_rmarm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
-        self.right_rmarm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
-        self.handle_left = self.left_rmarm.rm_create_robot_arm(
-            self.config.left_ip, self.config.left_port
-        )
-        self.handle_right = self.right_rmarm.rm_create_robot_arm(
-            self.config.right_ip, self.config.right_port
-        )
-        
-    def _initialize_parameters(self):
-        """初始化参数"""
-        self.motors = self.config.motors
-        self.axis = self.config.axis
-        self.joint_count = sum(self.axis.values())
-        self.left_offset = self.axis['left_joint']
-        
-    def _initialize_positions(self):
-        """初始化位置"""
-        self.init_joint_position = self.config.init_joint['joint']
-        self.safe_disable_position = self.config.init_joint['joint']
-        
-        # 移动到初始位置
-        self._move_to_initial_position()
-        
-        # 获取初始姿态
-        time.sleep(3)
-        self.init_pose = self._get_initial_pose()
-        
-    def _initialize_threading(self):
-        """初始化线程控制"""
-        self.left_slow_busy = False
-        self.right_slow_busy = False
-        self.gripper_busy = False
-        self._thread_lock = threading.Lock()
-
-        # 添加读取相关的线程控制
-        self._state_cache = {"joint": {}, "pose": {}}
-        self._cache_lock = threading.Lock()
-        self._keep_reading = True
-        
-        # 启动后台读取线程
-        self._start_background_readers()
-
-    def _start_background_readers(self):
-        """启动后台读取线程"""
-        # 读取线程
-        threading.Thread(
-            target=self._read_task,
-            daemon=True,
-            name="arm_reader"
-        ).start()
-
-    @property
-    def motor_names(self) -> list[str]:
-        return list(self.motors.keys())
-
-    @property
-    def motor_models(self) -> list[str]:
-        return [model for _, model in self.motors.values()]
-
-    @property
-    def motor_indices(self) -> list[int]:
-        return [idx for idx, _ in self.motors.values()]
-
-    @contextmanager
-    def _timeout_context(self, timeout: float = 5.0):
-        """超时上下文管理器"""
-        start_time = time.time()
-        try:
-            yield lambda: time.time() - start_time < timeout
-        except Exception as e:
-            raise TimeoutError(f"操作超时: {e}")
-
-    def _read_task(self):
-        """左臂后台读取任务 - 模仿_left_slow_task的风格"""
-        while self._keep_reading:
-            try:
-                left_state = self._read_arm_state(self.left_rmarm, "left")
-                with self._cache_lock:
-                    self._state_cache["joint"].update(left_state["joint"])
-                    self._state_cache["pose"].update(left_state["pose"])
-            except Exception as e:
-                print(f"左臂读取失败: {e}")
-
-            try:
-                right_state = self._read_arm_state(self.right_rmarm, "right")
-                with self._cache_lock:
-                    self._state_cache["joint"].update(right_state["joint"])
-                    self._state_cache["pose"].update(right_state["pose"])
-            except Exception as e:
-                print(f"右臂读取失败: {e}")
-    
-    def _read_arm_state(self, arm: RoboticArm, prefix: str) -> dict:
-        """读取单臂状态 - 保持原有逻辑"""
-        joint_msg = arm.rm_get_current_arm_state()[1]
-        gripper_msg = arm.rm_get_gripper_state()[1]
-        
-        joint_state = joint_msg['joint']
-        gripper_state = gripper_msg['actpos']
-        pose_state = joint_msg['pose']
-        
-        joint_state_dict = {}
-        for i in range(len(joint_state)):
-            joint_state_dict[f"{prefix}_joint_{i+1}"] = joint_state[i]
-        joint_state_dict[f"{prefix}_gripper"] = gripper_state
-
-        pose_state_dict = {
-            f"{prefix}_x": pose_state[0],
-            f"{prefix}_y": pose_state[1],
-            f"{prefix}_z": pose_state[2],
-            f"{prefix}_rx": pose_state[3],
-            f"{prefix}_ry": pose_state[4],
-            f"{prefix}_rz": pose_state[5],
-        }
-
-        return {"joint": joint_state_dict, 'pose': pose_state_dict}
-
-    def _move_to_initial_position(self):
-        """移动到初始位置"""
-        left_joints = self.init_joint_position[:self.left_offset]
-        right_joints = self.init_joint_position[self.left_offset+1:-1]
-        
-        self.left_rmarm.rm_movej(left_joints, 5, 0, 0, 1)
-        self.right_rmarm.rm_movej(right_joints, 5, 0, 0, 1)
-        
-    def _get_initial_pose(self) -> list:
-        """获取初始姿态"""
-        left_ret = self.left_rmarm.rm_get_current_arm_state()
-        right_ret = self.right_rmarm.rm_get_current_arm_state()
-        return left_ret[1]['pose'] + right_ret[1]['pose']
-
-    def _validate_joint_count(self, joints: list, expected_count: int):
-        """验证关节数量"""
-        if len(joints) != expected_count:
-            raise ValueError(f"关节数量不匹配: 期望 {expected_count}, 实际 {len(joints)}")
-
-    def _execute_slow_movement(self, arm: str, joint_data: list):
-        """执行慢速运动"""
-        busy_flag = f"{arm}_slow_busy"
-        
-        if not getattr(self, busy_flag):
-            setattr(self, busy_flag, True)
-            
-            target_method = getattr(self, f"_{arm}_slow_task")
-            threading.Thread(
-                target=target_method,
-                args=(joint_data.copy(),),
-                daemon=True
-            ).start()
-
-    def _left_slow_task(self, joint_data: list):
-        """左臂慢速任务"""
-        try:
-            self.write_left_joint_slow(joint_data)
-        finally:
-            self.left_slow_busy = False
-
-    def _right_slow_task(self, joint_data: list):
-        """右臂慢速任务"""
-        try:
-            self.write_right_joint_slow(joint_data)
-        finally:
-            self.right_slow_busy = False
-
-    def _execute_arm_action(self, arm: str, action: dict, master_joint: list, follow_joint: list):
-        """执行单臂动作"""
-        controller_status = action['master_controller_status'][arm]
-        
-        if controller_status['infrared'] == 1:
-            if compare_joint_difference(master_joint, follow_joint):
-                if arm == 'left':
-                    self.write_left_joint_canfd(master_joint)
-                else:
-                    self.write_right_joint_canfd(master_joint)
-            else:
-                self._execute_slow_movement(arm, master_joint)
-
-    def write_endpose(self, target_endpose: list):
-        assert target_endpose == 12, "the length of target pose is not equal 12"
-        self.left_rmarm.rm_movej_p(target_endpose[:6], 50, 0, 0, 1)
-        self.right_rmarm.rm_movej_p(target_endpose[6:], 50, 0, 0, 1)
-
-    def write_left_joint_slow(self, left_joint: list):
-        assert len(left_joint) == self.left_offset, "len of left master joint is not equal the count of left joint"
-        self.left_rmarm.rm_movej(left_joint, 5, 0, 0, 1)
-    
-    def write_right_joint_slow(self, right_joint: list):
-        assert len(right_joint) == self.left_offset, "len of right master joint is not equal the count of right joint"
-        self.right_rmarm.rm_movej(right_joint, 5, 0, 0, 1)
-
-    def write_left_joint_canfd(self, left_joint: list):
-        assert len(left_joint) == self.left_offset, "len of left master joint is not equal the count of left joint"
-        self.left_rmarm.rm_movej_canfd(left_joint, False)
-        
-    def write_right_joint_canfd(self, right_joint: list):
-        assert len(right_joint) == self.left_offset, "len of right master joint is not equal the count of right joint"
-        self.right_rmarm.rm_movej_canfd(right_joint, False)
-
-    def write_endpose_canfd(self, target_endpose: list):
-        assert len(target_endpose) == 12, "the length of target pose is not equal 12"
-        self.left_rmarm.rm_movep_canfd(target_endpose[:6], False)
-        self.right_rmarm.rm_movep_canfd(target_endpose[6:], False)
-
-    def write_dual_gripper(self, left_gripper: int, right_gripper: int):
-        try:
-            self.left_rmarm.rm_set_gripper_position(left_gripper, False, 2)
-            self.right_rmarm.rm_set_gripper_position(right_gripper, False, 2)
-        finally:
-            self.gripper_busy = False
-
-    def _execute_gripper_thread(self, left_gripper: int, right_gripper: int):
-        if not getattr(self, 'gripper_busy'):
-            setattr(self, 'gripper_busy', True)
-            
-            threading.Thread(
-                target=self.write_dual_gripper,
-                args=(left_gripper, right_gripper),
-                daemon=True
-            ).start()
-
-    def read_current_arm_joint_state(self):
-        return self.left_rmarm.rm_get_current_arm_state()[1]['joint'] + self.right_rmarm.rm_get_current_arm_state()[1]['joint']
-    
-    def read_current_arm_endpose_state(self):
-        return self.left_rmarm.rm_get_current_arm_state()[1]['pose'] + self.right_rmarm.rm_get_current_arm_state()[1]['pose']
-    
-    ########################## lerobot function ##########################
-
-    def connect(self, enable: bool = True) -> bool:
-        """使能机械臂并检测使能状态"""
-        with self._timeout_context() as is_timeout_valid:
-            while is_timeout_valid():
-                try:
-                    if enable:
-                        left_ret = self.left_rmarm.rm_get_current_arm_state()
-                        right_ret = self.right_rmarm.rm_get_current_arm_state()
-                        if left_ret[0] == 0 and right_ret[0] == 0:
-                            print("机械臂使能成功")
-                            return True
-                    else:
-                        RoboticArm.rm_destory()
-                        print("机械臂断开连接")
-                        return True
-                except Exception as e:
-                    print(f"连接异常: {e}")
-                time.sleep(1)
-        print("连接超时")
-        return False
-
-    def set_calibration(self):
-        raise NotImplementedError
-    
-    def revert_calibration(self):
-        raise NotImplementedError
-
-    def apply_calibration(self):
-        """
-            移动到初始位置
-        """
-        self.write(target_joint=self.init_joint_position)
-
-    def write(self, target_joint: list):
-        """写入关节位置"""
-        self._validate_joint_count(target_joint, self.joint_count)
-        
-        left_joints = target_joint[:self.left_offset]
-        left_gripper = target_joint[self.left_offset]
-        right_joints = target_joint[self.left_offset+1:-1]
-        right_gripper = target_joint[-1]
-        
-        self.left_rmarm.rm_movej_canfd(left_joints, follow=False)
-        # self.left_rmarm.rm_movej_follow(left_joints)
-        # self.left_rmarm.rm_set_gripper_position(left_gripper, block=False, timeout=2)
-        self.right_rmarm.rm_movej_canfd(right_joints, follow=False)
-        # self.right_rmarm.rm_movej_follow(right_joints)
-        # self.right_rmarm.rm_set_gripper_position(right_gripper, block=False, timeout=2)
-        self._execute_gripper_thread(left_gripper, right_gripper)
-
-    
-    def read(self) -> Dict:
-        """读取机械臂状态 - 直接从缓存获取"""
-        with self._cache_lock:
-            return self._state_cache.copy()
-    
-    def safe_disconnect(self):
-        """安全断开连接"""
-        try:
-            self.write(target_joint=self.safe_disable_position)
-            time.sleep(2)  # 等待移动完成
-        except Exception as e:
-            print(f"移动到安全位置失败: {e}")
-        finally:
-            RoboticArm.rm_destory()
-
-    ########################## lerobot function ##########################

lerobot/common/robot_devices/robots/configs.py

@@ -1,822 +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 abc
-from dataclasses import dataclass, field
-from typing import Sequence
-
-import draccus
-
-from lerobot.common.robot_devices.cameras.configs import (
-    CameraConfig,
-    IntelRealSenseCameraConfig,
-    OpenCVCameraConfig,
-)
-from lerobot.common.robot_devices.motors.configs import (
-    DynamixelMotorsBusConfig,
-    FeetechMotorsBusConfig,
-    MotorsBusConfig,
-    RealmanMotorsBusConfig,
-    RealmanDualMotorsBusConfig
-)
-
-
-@dataclass
-class RobotConfig(draccus.ChoiceRegistry, abc.ABC):
-    @property
-    def type(self) -> str:
-        return self.get_choice_name(self.__class__)
-
-
-# TODO(rcadene, aliberts): remove ManipulatorRobotConfig abstraction
-@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."
-                    )
-
-
-@RobotConfig.register_subclass("aloha")
-@dataclass
-class AlohaRobotConfig(ManipulatorRobotConfig):
-    # Specific to Aloha, LeRobot comes with default calibration files. Assuming the motors have been
-    # properly assembled, no manual calibration step is expected. If you need to run manual calibration,
-    # simply update this path to ".cache/calibration/aloha"
-    calibration_dir: str = ".cache/calibration/aloha_default"
-
-    # /!\ FOR SAFETY, READ THIS /!\
-    # `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.
-    # For Aloha, for every goal position request, motor rotations are capped at 5 degrees by default.
-    # When you feel more confident with teleoperation or running the policy, you can extend
-    # this safety limit and even removing it by setting it to `null`.
-    # Also, everything is expected to work safely out-of-the-box, but we highly advise to
-    # first try to teleoperate the grippers only (by commenting out the rest of the motors in this yaml),
-    # then to gradually add more motors (by uncommenting), until you can teleoperate both arms fully
-    max_relative_target: int | None = 5
-
-    leader_arms: dict[str, MotorsBusConfig] = field(
-        default_factory=lambda: {
-            "left": DynamixelMotorsBusConfig(
-                # window_x
-                port="/dev/ttyDXL_leader_left",
-                motors={
-                    # name: (index, model)
-                    "waist": [1, "xm430-w350"],
-                    "shoulder": [2, "xm430-w350"],
-                    "shoulder_shadow": [3, "xm430-w350"],
-                    "elbow": [4, "xm430-w350"],
-                    "elbow_shadow": [5, "xm430-w350"],
-                    "forearm_roll": [6, "xm430-w350"],
-                    "wrist_angle": [7, "xm430-w350"],
-                    "wrist_rotate": [8, "xl430-w250"],
-                    "gripper": [9, "xc430-w150"],
-                },
-            ),
-            "right": DynamixelMotorsBusConfig(
-                # window_x
-                port="/dev/ttyDXL_leader_right",
-                motors={
-                    # name: (index, model)
-                    "waist": [1, "xm430-w350"],
-                    "shoulder": [2, "xm430-w350"],
-                    "shoulder_shadow": [3, "xm430-w350"],
-                    "elbow": [4, "xm430-w350"],
-                    "elbow_shadow": [5, "xm430-w350"],
-                    "forearm_roll": [6, "xm430-w350"],
-                    "wrist_angle": [7, "xm430-w350"],
-                    "wrist_rotate": [8, "xl430-w250"],
-                    "gripper": [9, "xc430-w150"],
-                },
-            ),
-        }
-    )
-
-    follower_arms: dict[str, MotorsBusConfig] = field(
-        default_factory=lambda: {
-            "left": DynamixelMotorsBusConfig(
-                port="/dev/ttyDXL_follower_left",
-                motors={
-                    # name: (index, model)
-                    "waist": [1, "xm540-w270"],
-                    "shoulder": [2, "xm540-w270"],
-                    "shoulder_shadow": [3, "xm540-w270"],
-                    "elbow": [4, "xm540-w270"],
-                    "elbow_shadow": [5, "xm540-w270"],
-                    "forearm_roll": [6, "xm540-w270"],
-                    "wrist_angle": [7, "xm540-w270"],
-                    "wrist_rotate": [8, "xm430-w350"],
-                    "gripper": [9, "xm430-w350"],
-                },
-            ),
-            "right": DynamixelMotorsBusConfig(
-                port="/dev/ttyDXL_follower_right",
-                motors={
-                    # name: (index, model)
-                    "waist": [1, "xm540-w270"],
-                    "shoulder": [2, "xm540-w270"],
-                    "shoulder_shadow": [3, "xm540-w270"],
-                    "elbow": [4, "xm540-w270"],
-                    "elbow_shadow": [5, "xm540-w270"],
-                    "forearm_roll": [6, "xm540-w270"],
-                    "wrist_angle": [7, "xm540-w270"],
-                    "wrist_rotate": [8, "xm430-w350"],
-                    "gripper": [9, "xm430-w350"],
-                },
-            ),
-        }
-    )
-
-    # Troubleshooting: If one of your IntelRealSense cameras freeze during
-    # data recording due to bandwidth limit, you might need to plug the camera
-    # on another USB hub or PCIe card.
-    cameras: dict[str, CameraConfig] = field(
-        default_factory=lambda: {
-            "cam_high": IntelRealSenseCameraConfig(
-                serial_number=128422271347,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-            "cam_low": IntelRealSenseCameraConfig(
-                serial_number=130322270656,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-            "cam_left_wrist": IntelRealSenseCameraConfig(
-                serial_number=218622272670,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-            "cam_right_wrist": IntelRealSenseCameraConfig(
-                serial_number=130322272300,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-        }
-    )
-
-    mock: bool = False
-
-
-@RobotConfig.register_subclass("koch")
-@dataclass
-class KochRobotConfig(ManipulatorRobotConfig):
-    calibration_dir: str = ".cache/calibration/koch"
-    # `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": DynamixelMotorsBusConfig(
-                port="/dev/tty.usbmodem585A0085511",
-                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: dict[str, MotorsBusConfig] = field(
-        default_factory=lambda: {
-            "main": DynamixelMotorsBusConfig(
-                port="/dev/tty.usbmodem585A0076891",
-                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"],
-                },
-            ),
-        }
-    )
-
-    cameras: dict[str, CameraConfig] = field(
-        default_factory=lambda: {
-            "laptop": OpenCVCameraConfig(
-                camera_index=0,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-            "phone": OpenCVCameraConfig(
-                camera_index=1,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-        }
-    )
-
-    # ~ Koch specific settings ~
-    # Sets 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.
-    gripper_open_degree: float = 35.156
-
-    mock: bool = False
-
-
-@RobotConfig.register_subclass("koch_bimanual")
-@dataclass
-class KochBimanualRobotConfig(ManipulatorRobotConfig):
-    calibration_dir: str = ".cache/calibration/koch_bimanual"
-    # `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: {
-            "left": DynamixelMotorsBusConfig(
-                port="/dev/tty.usbmodem585A0085511",
-                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"],
-                },
-            ),
-            "right": 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: dict[str, MotorsBusConfig] = field(
-        default_factory=lambda: {
-            "left": DynamixelMotorsBusConfig(
-                port="/dev/tty.usbmodem585A0076891",
-                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"],
-                },
-            ),
-            "right": 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"],
-                },
-            ),
-        }
-    )
-
-    cameras: dict[str, CameraConfig] = field(
-        default_factory=lambda: {
-            "laptop": OpenCVCameraConfig(
-                camera_index=0,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-            "phone": OpenCVCameraConfig(
-                camera_index=1,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-        }
-    )
-
-    # ~ Koch specific settings ~
-    # Sets 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.
-    gripper_open_degree: float = 35.156
-
-    mock: bool = False
-
-
-@RobotConfig.register_subclass("moss")
-@dataclass
-class MossRobotConfig(ManipulatorRobotConfig):
-    calibration_dir: str = ".cache/calibration/moss"
-    # `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",
-                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",
-                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"],
-                },
-            ),
-        }
-    )
-
-    cameras: dict[str, CameraConfig] = field(
-        default_factory=lambda: {
-            "laptop": OpenCVCameraConfig(
-                camera_index=0,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-            "phone": OpenCVCameraConfig(
-                camera_index=1,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-        }
-    )
-
-    mock: bool = False
-
-
-@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",
-                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",
-                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"],
-                },
-            ),
-        }
-    )
-
-    cameras: dict[str, CameraConfig] = field(
-        default_factory=lambda: {
-            "laptop": OpenCVCameraConfig(
-                camera_index=0,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-            "phone": OpenCVCameraConfig(
-                camera_index=1,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-        }
-    )
-
-    mock: bool = False
-
-
-@RobotConfig.register_subclass("so100")
-@dataclass
-class So100RobotConfig(ManipulatorRobotConfig):
-    calibration_dir: str = ".cache/calibration/so100"
-    # `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",
-                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",
-                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"],
-                },
-            ),
-        }
-    )
-
-    cameras: dict[str, CameraConfig] = field(
-        default_factory=lambda: {
-            "laptop": OpenCVCameraConfig(
-                camera_index=0,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-            "phone": OpenCVCameraConfig(
-                camera_index=1,
-                fps=30,
-                width=640,
-                height=480,
-            ),
-        }
-    )
-
-    mock: bool = False
-
-
-@RobotConfig.register_subclass("stretch")
-@dataclass
-class StretchRobotConfig(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
-
-    cameras: dict[str, CameraConfig] = field(
-        default_factory=lambda: {
-            "navigation": OpenCVCameraConfig(
-                camera_index="/dev/hello-nav-head-camera",
-                fps=10,
-                width=1280,
-                height=720,
-                rotation=-90,
-            ),
-            "head": IntelRealSenseCameraConfig(
-                name="Intel RealSense D435I",
-                fps=30,
-                width=640,
-                height=480,
-                rotation=90,
-            ),
-            "wrist": IntelRealSenseCameraConfig(
-                name="Intel RealSense D405",
-                fps=30,
-                width=640,
-                height=480,
-            ),
-        }
-    )
-
-    mock: bool = False
-
-
-@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
-
-
-@RobotConfig.register_subclass("realman")
-@dataclass
-class RealmanRobotConfig(RobotConfig):
-    inference_time: bool = False
-    max_gripper: int = 990
-    min_gripper: int = 10
-    servo_config_file: str = "/home/maic/LYT/lerobot/lerobot/common/robot_devices/teleop/servo_arm.yaml"
-    
-
-    left_follower_arm: dict[str, MotorsBusConfig] = field(
-        default_factory=lambda: {
-            "main": RealmanMotorsBusConfig(
-                ip = "192.168.3.18",
-                port = 8080,
-                motors={
-                    # name: (index, model)
-                    "joint_1": [1, "realman"],
-                    "joint_2": [2, "realman"],
-                    "joint_3": [3, "realman"],
-                    "joint_4": [4, "realman"],
-                    "joint_5": [5, "realman"],
-                    "joint_6": [6, "realman"],
-                    "gripper": [7, "realman"],
-                },
-                init_joint = {'joint': [-90, 90, 90, 90, 90, -90, 10]}
-            )
-        }
-    )
-
-    cameras: dict[str, CameraConfig] = field(
-        default_factory=lambda: {
-            # "one": OpenCVCameraConfig(
-            #     camera_index=4,
-            #     fps=30,
-            #     width=640,
-            #     height=480,
-            # ),
-            "left": IntelRealSenseCameraConfig(
-                serial_number="153122077516",
-                fps=30,
-                width=640,
-                height=480,
-                use_depth=False
-            ),
-            # "right": IntelRealSenseCameraConfig(
-            #     serial_number="405622075165",
-            #     fps=30,
-            #     width=640,
-            #     height=480,
-            #     use_depth=False
-            # ),
-            "front": IntelRealSenseCameraConfig(
-                serial_number="145422072751",
-                fps=30,
-                width=640,
-                height=480,
-                use_depth=False
-            ),
-            "high": IntelRealSenseCameraConfig(
-                serial_number="145422072193",
-                fps=30,
-                width=640,
-                height=480,
-                use_depth=False
-            ),
-        }
-    )
-
-
-
-@RobotConfig.register_subclass("realman_dual")
-@dataclass
-class RealmanDualRobotConfig(RobotConfig):
-    inference_time: bool = False
-    max_gripper: int = 990
-    min_gripper: int = 10
-    servo_config_file: str = "/home/maic/LYT/lerobot/lerobot/common/robot_devices/teleop/servo_dual.yaml"
-    left_end_control_guid: str = '0300b14bff1100003708000010010000'
-    right_end_control_guid: str = '030003f05e0400008e02000010010000'
-
-    follower_arm: dict[str, MotorsBusConfig] = field(
-        default_factory=lambda: {
-            "main": RealmanDualMotorsBusConfig(
-                axis= {'left_joint': 6, 'left_gripper': 1, 'right_joint': 6, 'right_gripper': 1},
-                left_ip = "192.168.3.18",
-                left_port = 8080,
-                right_ip = "192.168.3.19",
-                right_port = 8080,
-                init_joint = {'joint': [-170, 90, 0, 90, 120, 0, 10, 170, 90, 0, -90, 120, 0, 10]},
-                motors={
-                    # name: (index, model)
-                    "left_joint_1": [1, "realman"],
-                    "left_joint_2": [2, "realman"],
-                    "left_joint_3": [3, "realman"],
-                    "left_joint_4": [4, "realman"],
-                    "left_joint_5": [5, "realman"],
-                    "left_joint_6": [6, "realman"],
-                    "left_gripper": [7, "realman"],
-                    "right_joint_1": [8, "realman"],
-                    "right_joint_2": [9, "realman"],
-                    "right_joint_3": [10, "realman"],
-                    "right_joint_4": [11, "realman"],
-                    "right_joint_5": [12, "realman"],
-                    "right_joint_6": [13, "realman"],
-                    "right_gripper": [14, "realman"]
-                },
-            )
-        }
-    )
-
-    cameras: dict[str, CameraConfig] = field(
-        default_factory=lambda: {
-            "left": IntelRealSenseCameraConfig(
-                serial_number="153122077516",
-                fps=30,
-                width=640,
-                height=480,
-                use_depth=False
-            ),
-            "right": IntelRealSenseCameraConfig(
-                serial_number="405622075165",
-                fps=30,
-                width=640,
-                height=480,
-                use_depth=False
-            ),
-            "front": IntelRealSenseCameraConfig(
-                serial_number="145422072751",
-                fps=30,
-                width=640,
-                height=480,
-                use_depth=False
-            ),
-            "high": IntelRealSenseCameraConfig(
-                serial_number="145422072193",
-                fps=30,
-                width=640,
-                height=480,
-                use_depth=False
-            ),
-        }
-    )

lerobot/common/robot_devices/robots/dynamixel_calibration.py

@@ -1,144 +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.
-
-"""Logic to calibrate a robot arm built with dynamixel motors"""
-# TODO(rcadene, aliberts): move this logic into the robot code when refactoring
-
-import numpy as np
-
-from lerobot.common.robot_devices.motors.dynamixel import (
-    CalibrationMode,
-    TorqueMode,
-    convert_degrees_to_steps,
-)
-from lerobot.common.robot_devices.motors.utils import MotorsBus
-
-URL_TEMPLATE = (
-    "https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp"
-)
-
-# The following positions are provided in nominal degree range ]-180, +180[
-# For more info on these constants, see comments in the code where they get used.
-ZERO_POSITION_DEGREE = 0
-ROTATED_POSITION_DEGREE = 90
-
-
-def assert_drive_mode(drive_mode):
-    # `drive_mode` is in [0,1] with 0 means original rotation direction for the motor, and 1 means inverted.
-    if not np.all(np.isin(drive_mode, [0, 1])):
-        raise ValueError(f"`drive_mode` contains values other than 0 or 1: ({drive_mode})")
-
-
-def apply_drive_mode(position, drive_mode):
-    assert_drive_mode(drive_mode)
-    # Convert `drive_mode` from [0, 1] with 0 indicates original rotation direction and 1 inverted,
-    # to [-1, 1] with 1 indicates original rotation direction and -1 inverted.
-    signed_drive_mode = -(drive_mode * 2 - 1)
-    position *= signed_drive_mode
-    return position
-
-
-def compute_nearest_rounded_position(position, models):
-    delta_turn = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, models)
-    nearest_pos = np.round(position.astype(float) / delta_turn) * delta_turn
-    return nearest_pos.astype(position.dtype)
-
-
-def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """This function ensures that a neural network trained on data collected on a given robot
-    can work on another robot. For instance before calibration, setting a same goal position
-    for each motor of two different robots will get two very different positions. But after calibration,
-    the two robots will move to the same position.To this end, this function computes the homing offset
-    and the drive mode for each motor of a given robot.
-
-    Homing offset is used to shift the motor position to a ]-2048, +2048[ nominal range (when the motor uses 2048 steps
-    to complete a half a turn). This range is set around an arbitrary "zero position" corresponding to all motor positions
-    being 0. During the calibration process, you will need to manually move the robot to this "zero position".
-
-    Drive mode is used to invert the rotation direction of the motor. This is useful when some motors have been assembled
-    in the opposite orientation for some robots. During the calibration process, you will need to manually move the robot
-    to the "rotated position".
-
-    After calibration, the homing offsets and drive modes are stored in a cache.
-
-    Example of usage:
-    ```python
-    run_arm_calibration(arm, "koch", "left", "follower")
-    ```
-    """
-    if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
-        raise ValueError("To run calibration, the torque must be disabled on all motors.")
-
-    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
-
-    print("\nMove arm to zero position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero"))
-    input("Press Enter to continue...")
-
-    # We arbitrarily chose our zero target position to be a straight horizontal position with gripper upwards and closed.
-    # It is easy to identify and all motors are in a "quarter turn" position. Once calibration is done, this position will
-    # correspond to every motor angle being 0. If you set all 0 as Goal Position, the arm will move in this position.
-    zero_target_pos = convert_degrees_to_steps(ZERO_POSITION_DEGREE, arm.motor_models)
-
-    # Compute homing offset so that `present_position + homing_offset ~= target_position`.
-    zero_pos = arm.read("Present_Position")
-    zero_nearest_pos = compute_nearest_rounded_position(zero_pos, arm.motor_models)
-    homing_offset = zero_target_pos - zero_nearest_pos
-
-    # The rotated target position corresponds to a rotation of a quarter turn from the zero position.
-    # This allows to identify the rotation direction of each motor.
-    # For instance, if the motor rotates 90 degree, and its value is -90 after applying the homing offset, then we know its rotation direction
-    # is inverted. However, for the calibration being successful, we need everyone to follow the same target position.
-    # Sometimes, there is only one possible rotation direction. For instance, if the gripper is closed, there is only one direction which
-    # corresponds to opening the gripper. When the rotation direction is ambiguous, we arbitrarily rotate clockwise from the point of view
-    # of the previous motor in the kinetic chain.
-    print("\nMove arm to rotated target position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rotated"))
-    input("Press Enter to continue...")
-
-    rotated_target_pos = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, arm.motor_models)
-
-    # Find drive mode by rotating each motor by a quarter of a turn.
-    # Drive mode indicates if the motor rotation direction should be inverted (=1) or not (=0).
-    rotated_pos = arm.read("Present_Position")
-    drive_mode = (rotated_pos < zero_pos).astype(np.int32)
-
-    # Re-compute homing offset to take into account drive mode
-    rotated_drived_pos = apply_drive_mode(rotated_pos, drive_mode)
-    rotated_nearest_pos = compute_nearest_rounded_position(rotated_drived_pos, arm.motor_models)
-    homing_offset = rotated_target_pos - rotated_nearest_pos
-
-    print("\nMove arm to rest position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rest"))
-    input("Press Enter to continue...")
-    print()
-
-    # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180]
-    calib_mode = [CalibrationMode.DEGREE.name] * len(arm.motor_names)
-
-    # TODO(rcadene): make type of joints (DEGREE or LINEAR) configurable from yaml?
-    if robot_type in ["aloha"] and "gripper" in arm.motor_names:
-        # Joints with linear motions (like gripper of Aloha) are expressed in nominal range of [0, 100]
-        calib_idx = arm.motor_names.index("gripper")
-        calib_mode[calib_idx] = CalibrationMode.LINEAR.name
-
-    calib_data = {
-        "homing_offset": homing_offset.tolist(),
-        "drive_mode": drive_mode.tolist(),
-        "start_pos": zero_pos.tolist(),
-        "end_pos": rotated_pos.tolist(),
-        "calib_mode": calib_mode,
-        "motor_names": arm.motor_names,
-    }
-    return calib_data

lerobot/common/robot_devices/robots/feetech_calibration.py

@@ -1,506 +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.
-
-"""Logic to calibrate a robot arm built with feetech motors"""
-# TODO(rcadene, aliberts): move this logic into the robot code when refactoring
-
-import time
-
-import numpy as np
-
-from lerobot.common.robot_devices.motors.feetech import (
-    CalibrationMode,
-    TorqueMode,
-    convert_degrees_to_steps,
-)
-from lerobot.common.robot_devices.motors.utils import MotorsBus
-
-URL_TEMPLATE = (
-    "https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp"
-)
-
-# The following positions are provided in nominal degree range ]-180, +180[
-# For more info on these constants, see comments in the code where they get used.
-ZERO_POSITION_DEGREE = 0
-ROTATED_POSITION_DEGREE = 90
-
-
-def reset_middle_positions(arm: MotorsBus):
-    input("Please move the robot to the new middle position for calibration, then press Enter...")
-    # Write 128 to Torque_Enable for all motors.
-    arm.write("Torque_Enable", 128)
-
-
-def assert_drive_mode(drive_mode):
-    # `drive_mode` is in [0,1] with 0 means original rotation direction for the motor, and 1 means inverted.
-    if not np.all(np.isin(drive_mode, [0, 1])):
-        raise ValueError(f"`drive_mode` contains values other than 0 or 1: ({drive_mode})")
-
-
-def apply_drive_mode(position, drive_mode):
-    assert_drive_mode(drive_mode)
-    # Convert `drive_mode` from [0, 1] with 0 indicates original rotation direction and 1 inverted,
-    # to [-1, 1] with 1 indicates original rotation direction and -1 inverted.
-    signed_drive_mode = -(drive_mode * 2 - 1)
-    position *= signed_drive_mode
-    return position
-
-
-def move_until_block(arm, motor_name, positive_direction=True, while_move_hook=None):
-    count = 0
-    while True:
-        present_pos = arm.read("Present_Position", motor_name)
-        if positive_direction:
-            # Move +100 steps every time. Lower the steps to lower the speed at which the arm moves.
-            arm.write("Goal_Position", present_pos + 100, motor_name)
-        else:
-            arm.write("Goal_Position", present_pos - 100, motor_name)
-
-        if while_move_hook is not None:
-            while_move_hook()
-
-        present_pos = arm.read("Present_Position", motor_name).item()
-        present_speed = arm.read("Present_Speed", motor_name).item()
-        present_current = arm.read("Present_Current", motor_name).item()
-        # present_load = arm.read("Present_Load", motor_name).item()
-        # present_voltage = arm.read("Present_Voltage", motor_name).item()
-        # present_temperature = arm.read("Present_Temperature", motor_name).item()
-
-        # print(f"{present_pos=}")
-        # print(f"{present_speed=}")
-        # print(f"{present_current=}")
-        # print(f"{present_load=}")
-        # print(f"{present_voltage=}")
-        # print(f"{present_temperature=}")
-
-        if present_speed == 0 and present_current > 40:
-            count += 1
-            if count > 100 or present_current > 300:
-                return present_pos
-        else:
-            count = 0
-
-
-def move_to_calibrate(
-    arm,
-    motor_name,
-    invert_drive_mode=False,
-    positive_first=True,
-    in_between_move_hook=None,
-    while_move_hook=None,
-):
-    initial_pos = arm.read("Present_Position", motor_name)
-
-    if positive_first:
-        p_present_pos = move_until_block(
-            arm, motor_name, positive_direction=True, while_move_hook=while_move_hook
-        )
-    else:
-        n_present_pos = move_until_block(
-            arm, motor_name, positive_direction=False, while_move_hook=while_move_hook
-        )
-
-    if in_between_move_hook is not None:
-        in_between_move_hook()
-
-    if positive_first:
-        n_present_pos = move_until_block(
-            arm, motor_name, positive_direction=False, while_move_hook=while_move_hook
-        )
-    else:
-        p_present_pos = move_until_block(
-            arm, motor_name, positive_direction=True, while_move_hook=while_move_hook
-        )
-
-    zero_pos = (n_present_pos + p_present_pos) / 2
-
-    calib_data = {
-        "initial_pos": initial_pos,
-        "homing_offset": zero_pos if invert_drive_mode else -zero_pos,
-        "invert_drive_mode": invert_drive_mode,
-        "drive_mode": -1 if invert_drive_mode else 0,
-        "zero_pos": zero_pos,
-        "start_pos": n_present_pos if invert_drive_mode else p_present_pos,
-        "end_pos": p_present_pos if invert_drive_mode else n_present_pos,
-    }
-    return calib_data
-
-
-def apply_offset(calib, offset):
-    calib["zero_pos"] += offset
-    if calib["drive_mode"]:
-        calib["homing_offset"] += offset
-    else:
-        calib["homing_offset"] -= offset
-    return calib
-
-
-def run_arm_auto_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    if robot_type == "so100":
-        return run_arm_auto_calibration_so100(arm, robot_type, arm_name, arm_type)
-    elif robot_type == "moss":
-        return run_arm_auto_calibration_moss(arm, robot_type, arm_name, arm_type)
-    else:
-        raise ValueError(robot_type)
-
-
-def run_arm_auto_calibration_so100(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """All the offsets and magic numbers are hand tuned, and are unique to SO-100 follower arms"""
-    if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
-        raise ValueError("To run calibration, the torque must be disabled on all motors.")
-
-    if not (robot_type == "so100" and arm_type == "follower"):
-        raise NotImplementedError("Auto calibration only supports the follower of so100 arms for now.")
-
-    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
-
-    print("\nMove arm to initial position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="initial"))
-    input("Press Enter to continue...")
-
-    # Lower the acceleration of the motors (in [0,254])
-    initial_acceleration = arm.read("Acceleration")
-    arm.write("Lock", 0)
-    arm.write("Acceleration", 10)
-    time.sleep(1)
-
-    arm.write("Torque_Enable", TorqueMode.ENABLED.value)
-
-    print(f'{arm.read("Present_Position", "elbow_flex")=}')
-
-    calib = {}
-
-    init_wf_pos = arm.read("Present_Position", "wrist_flex")
-    init_sl_pos = arm.read("Present_Position", "shoulder_lift")
-    init_ef_pos = arm.read("Present_Position", "elbow_flex")
-    arm.write("Goal_Position", init_wf_pos - 800, "wrist_flex")
-    arm.write("Goal_Position", init_sl_pos + 150 + 1024, "shoulder_lift")
-    arm.write("Goal_Position", init_ef_pos - 2048, "elbow_flex")
-    time.sleep(2)
-
-    print("Calibrate shoulder_pan")
-    calib["shoulder_pan"] = move_to_calibrate(arm, "shoulder_pan")
-    arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
-    time.sleep(1)
-
-    print("Calibrate gripper")
-    calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True)
-    time.sleep(1)
-
-    print("Calibrate wrist_flex")
-    calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex")
-    calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=80)
-
-    def in_between_move_hook():
-        nonlocal arm, calib
-        time.sleep(2)
-        ef_pos = arm.read("Present_Position", "elbow_flex")
-        sl_pos = arm.read("Present_Position", "shoulder_lift")
-        arm.write("Goal_Position", ef_pos + 1024, "elbow_flex")
-        arm.write("Goal_Position", sl_pos - 1024, "shoulder_lift")
-        time.sleep(2)
-
-    print("Calibrate elbow_flex")
-    calib["elbow_flex"] = move_to_calibrate(
-        arm, "elbow_flex", positive_first=False, in_between_move_hook=in_between_move_hook
-    )
-    calib["elbow_flex"] = apply_offset(calib["elbow_flex"], offset=80 - 1024)
-
-    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1024 + 512, "elbow_flex")
-    time.sleep(1)
-
-    def in_between_move_hook():
-        nonlocal arm, calib
-        arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"], "elbow_flex")
-
-    print("Calibrate shoulder_lift")
-    calib["shoulder_lift"] = move_to_calibrate(
-        arm,
-        "shoulder_lift",
-        invert_drive_mode=True,
-        positive_first=False,
-        in_between_move_hook=in_between_move_hook,
-    )
-    # add an 30 steps as offset to align with body
-    calib["shoulder_lift"] = apply_offset(calib["shoulder_lift"], offset=1024 - 50)
-
-    def while_move_hook():
-        nonlocal arm, calib
-        positions = {
-            "shoulder_lift": round(calib["shoulder_lift"]["zero_pos"] - 1600),
-            "elbow_flex": round(calib["elbow_flex"]["zero_pos"] + 1700),
-            "wrist_flex": round(calib["wrist_flex"]["zero_pos"] + 800),
-            "gripper": round(calib["gripper"]["end_pos"]),
-        }
-        arm.write("Goal_Position", list(positions.values()), list(positions.keys()))
-
-    arm.write("Goal_Position", round(calib["shoulder_lift"]["zero_pos"] - 1600), "shoulder_lift")
-    time.sleep(2)
-    arm.write("Goal_Position", round(calib["elbow_flex"]["zero_pos"] + 1700), "elbow_flex")
-    time.sleep(2)
-    arm.write("Goal_Position", round(calib["wrist_flex"]["zero_pos"] + 800), "wrist_flex")
-    time.sleep(2)
-    arm.write("Goal_Position", round(calib["gripper"]["end_pos"]), "gripper")
-    time.sleep(2)
-
-    print("Calibrate wrist_roll")
-    calib["wrist_roll"] = move_to_calibrate(
-        arm, "wrist_roll", invert_drive_mode=True, positive_first=False, while_move_hook=while_move_hook
-    )
-
-    arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"], "wrist_roll")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["gripper"]["start_pos"], "gripper")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"], "wrist_flex")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 2048, "elbow_flex")
-    arm.write("Goal_Position", calib["shoulder_lift"]["zero_pos"] - 2048, "shoulder_lift")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
-    time.sleep(1)
-
-    calib_modes = []
-    for name in arm.motor_names:
-        if name == "gripper":
-            calib_modes.append(CalibrationMode.LINEAR.name)
-        else:
-            calib_modes.append(CalibrationMode.DEGREE.name)
-
-    calib_dict = {
-        "homing_offset": [calib[name]["homing_offset"] for name in arm.motor_names],
-        "drive_mode": [calib[name]["drive_mode"] for name in arm.motor_names],
-        "start_pos": [calib[name]["start_pos"] for name in arm.motor_names],
-        "end_pos": [calib[name]["end_pos"] for name in arm.motor_names],
-        "calib_mode": calib_modes,
-        "motor_names": arm.motor_names,
-    }
-
-    # Re-enable original accerlation
-    arm.write("Lock", 0)
-    arm.write("Acceleration", initial_acceleration)
-    time.sleep(1)
-
-    return calib_dict
-
-
-def run_arm_auto_calibration_moss(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """All the offsets and magic numbers are hand tuned, and are unique to SO-100 follower arms"""
-    if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
-        raise ValueError("To run calibration, the torque must be disabled on all motors.")
-
-    if not (robot_type == "moss" and arm_type == "follower"):
-        raise NotImplementedError("Auto calibration only supports the follower of moss arms for now.")
-
-    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
-
-    print("\nMove arm to initial position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="initial"))
-    input("Press Enter to continue...")
-
-    # Lower the acceleration of the motors (in [0,254])
-    initial_acceleration = arm.read("Acceleration")
-    arm.write("Lock", 0)
-    arm.write("Acceleration", 10)
-    time.sleep(1)
-
-    arm.write("Torque_Enable", TorqueMode.ENABLED.value)
-
-    sl_pos = arm.read("Present_Position", "shoulder_lift")
-    arm.write("Goal_Position", sl_pos - 1024 - 450, "shoulder_lift")
-    ef_pos = arm.read("Present_Position", "elbow_flex")
-    arm.write("Goal_Position", ef_pos + 1024 + 450, "elbow_flex")
-    time.sleep(2)
-
-    calib = {}
-
-    print("Calibrate shoulder_pan")
-    calib["shoulder_pan"] = move_to_calibrate(arm, "shoulder_pan")
-    arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
-    time.sleep(1)
-
-    print("Calibrate gripper")
-    calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True)
-    time.sleep(1)
-
-    print("Calibrate wrist_flex")
-    calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex", invert_drive_mode=True)
-    calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=-210 + 1024)
-
-    wr_pos = arm.read("Present_Position", "wrist_roll")
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
-    time.sleep(1)
-    arm.write("Goal_Position", wr_pos - 1024, "wrist_roll")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 2048, "wrist_flex")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["gripper"]["end_pos"], "gripper")
-    time.sleep(1)
-
-    print("Calibrate wrist_roll")
-    calib["wrist_roll"] = move_to_calibrate(arm, "wrist_roll", invert_drive_mode=True)
-    calib["wrist_roll"] = apply_offset(calib["wrist_roll"], offset=790)
-
-    arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"] - 1024, "wrist_roll")
-    arm.write("Goal_Position", calib["gripper"]["start_pos"], "gripper")
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"], "wrist_roll")
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 2048, "wrist_flex")
-
-    def in_between_move_elbow_flex_hook():
-        nonlocal arm, calib
-        arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"], "wrist_flex")
-
-    print("Calibrate elbow_flex")
-    calib["elbow_flex"] = move_to_calibrate(
-        arm,
-        "elbow_flex",
-        invert_drive_mode=True,
-        in_between_move_hook=in_between_move_elbow_flex_hook,
-    )
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
-
-    def in_between_move_shoulder_lift_hook():
-        nonlocal arm, calib
-        sl = arm.read("Present_Position", "shoulder_lift")
-        arm.write("Goal_Position", sl - 1500, "shoulder_lift")
-        time.sleep(1)
-        arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1536, "elbow_flex")
-        time.sleep(1)
-        arm.write("Goal_Position", calib["wrist_flex"]["start_pos"], "wrist_flex")
-        time.sleep(1)
-
-    print("Calibrate shoulder_lift")
-    calib["shoulder_lift"] = move_to_calibrate(
-        arm, "shoulder_lift", in_between_move_hook=in_between_move_shoulder_lift_hook
-    )
-    calib["shoulder_lift"] = apply_offset(calib["shoulder_lift"], offset=-1024)
-
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["shoulder_lift"]["zero_pos"] + 2048, "shoulder_lift")
-    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] - 1024 - 400, "elbow_flex")
-    time.sleep(2)
-
-    calib_modes = []
-    for name in arm.motor_names:
-        if name == "gripper":
-            calib_modes.append(CalibrationMode.LINEAR.name)
-        else:
-            calib_modes.append(CalibrationMode.DEGREE.name)
-
-    calib_dict = {
-        "homing_offset": [calib[name]["homing_offset"] for name in arm.motor_names],
-        "drive_mode": [calib[name]["drive_mode"] for name in arm.motor_names],
-        "start_pos": [calib[name]["start_pos"] for name in arm.motor_names],
-        "end_pos": [calib[name]["end_pos"] for name in arm.motor_names],
-        "calib_mode": calib_modes,
-        "motor_names": arm.motor_names,
-    }
-
-    # Re-enable original accerlation
-    arm.write("Lock", 0)
-    arm.write("Acceleration", initial_acceleration)
-    time.sleep(1)
-
-    return calib_dict
-
-
-def run_arm_manual_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """This function ensures that a neural network trained on data collected on a given robot
-    can work on another robot. For instance before calibration, setting a same goal position
-    for each motor of two different robots will get two very different positions. But after calibration,
-    the two robots will move to the same position.To this end, this function computes the homing offset
-    and the drive mode for each motor of a given robot.
-
-    Homing offset is used to shift the motor position to a ]-2048, +2048[ nominal range (when the motor uses 2048 steps
-    to complete a half a turn). This range is set around an arbitrary "zero position" corresponding to all motor positions
-    being 0. During the calibration process, you will need to manually move the robot to this "zero position".
-
-    Drive mode is used to invert the rotation direction of the motor. This is useful when some motors have been assembled
-    in the opposite orientation for some robots. During the calibration process, you will need to manually move the robot
-    to the "rotated position".
-
-    After calibration, the homing offsets and drive modes are stored in a cache.
-
-    Example of usage:
-    ```python
-    run_arm_calibration(arm, "so100", "left", "follower")
-    ```
-    """
-    if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
-        raise ValueError("To run calibration, the torque must be disabled on all motors.")
-
-    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
-
-    reset_middle_positions(arm)
-
-    print("\nMove arm to zero position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero"))
-    input("Press Enter to continue...")
-
-    # We arbitrarily chose our zero target position to be a straight horizontal position with gripper upwards and closed.
-    # It is easy to identify and all motors are in a "quarter turn" position. Once calibration is done, this position will
-    # correspond to every motor angle being 0. If you set all 0 as Goal Position, the arm will move in this position.
-    zero_target_pos = convert_degrees_to_steps(ZERO_POSITION_DEGREE, arm.motor_models)
-
-    # Compute homing offset so that `present_position + homing_offset ~= target_position`.
-    zero_pos = arm.read("Present_Position")
-    homing_offset = zero_target_pos - zero_pos
-
-    # The rotated target position corresponds to a rotation of a quarter turn from the zero position.
-    # This allows to identify the rotation direction of each motor.
-    # For instance, if the motor rotates 90 degree, and its value is -90 after applying the homing offset, then we know its rotation direction
-    # is inverted. However, for the calibration being successful, we need everyone to follow the same target position.
-    # Sometimes, there is only one possible rotation direction. For instance, if the gripper is closed, there is only one direction which
-    # corresponds to opening the gripper. When the rotation direction is ambiguous, we arbitrarily rotate clockwise from the point of view
-    # of the previous motor in the kinetic chain.
-    print("\nMove arm to rotated target position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rotated"))
-    input("Press Enter to continue...")
-
-    rotated_target_pos = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, arm.motor_models)
-
-    # Find drive mode by rotating each motor by a quarter of a turn.
-    # Drive mode indicates if the motor rotation direction should be inverted (=1) or not (=0).
-    rotated_pos = arm.read("Present_Position")
-    drive_mode = (rotated_pos < zero_pos).astype(np.int32)
-
-    # Re-compute homing offset to take into account drive mode
-    rotated_drived_pos = apply_drive_mode(rotated_pos, drive_mode)
-    homing_offset = rotated_target_pos - rotated_drived_pos
-
-    print("\nMove arm to rest position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rest"))
-    input("Press Enter to continue...")
-    print()
-
-    # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180]
-    calib_modes = []
-    for name in arm.motor_names:
-        if name == "gripper":
-            calib_modes.append(CalibrationMode.LINEAR.name)
-        else:
-            calib_modes.append(CalibrationMode.DEGREE.name)
-
-    calib_dict = {
-        "homing_offset": homing_offset.tolist(),
-        "drive_mode": drive_mode.tolist(),
-        "start_pos": zero_pos.tolist(),
-        "end_pos": rotated_pos.tolist(),
-        "calib_mode": calib_modes,
-        "motor_names": arm.motor_names,
-    }
-    return calib_dict

lerobot/common/robot_devices/robots/lekiwi_remote.py

@@ -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.robot_devices.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.robot_devices.robots.feetech_calibration import run_arm_manual_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_arm_manual_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.robot_devices.cameras.utils import make_cameras_from_configs
-    from lerobot.common.robot_devices.motors.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()

lerobot/common/robot_devices/robots/manipulator.py

@@ -1,627 +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 json
-import logging
-import time
-import warnings
-from pathlib import Path
-
-import numpy as np
-import torch
-
-from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs
-from lerobot.common.robot_devices.motors.utils import MotorsBus, make_motors_buses_from_configs
-from lerobot.common.robot_devices.robots.configs import ManipulatorRobotConfig
-from lerobot.common.robot_devices.robots.utils import get_arm_id
-from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
-
-
-def ensure_safe_goal_position(
-    goal_pos: torch.Tensor, present_pos: torch.Tensor, max_relative_target: float | list[float]
-):
-    # Cap relative action target magnitude for safety.
-    diff = goal_pos - present_pos
-    max_relative_target = torch.tensor(max_relative_target)
-    safe_diff = torch.minimum(diff, max_relative_target)
-    safe_diff = torch.maximum(safe_diff, -max_relative_target)
-    safe_goal_pos = present_pos + safe_diff
-
-    if not torch.allclose(goal_pos, safe_goal_pos):
-        logging.warning(
-            "Relative goal position magnitude had to be clamped to be safe.\n"
-            f"  requested relative goal position target: {diff}\n"
-            f"    clamped relative goal position target: {safe_diff}"
-        )
-
-    return safe_goal_pos
-
-
-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 RobotDeviceAlreadyConnectedError(
-                "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.robot_devices.motors.dynamixel import TorqueMode
-        elif self.robot_type in ["so100", "so101", "moss", "lekiwi"]:
-            from lerobot.common.robot_devices.motors.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)
-
-        self.activate_calibration()
-
-        # 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", "so101", "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 activate_calibration(self):
-        """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].
-        """
-
-        def load_or_run_calibration_(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:
-                # TODO(rcadene): display a warning in __init__ if calibration file not available
-                print(f"Missing calibration file '{arm_calib_path}'")
-
-                if self.robot_type in ["koch", "koch_bimanual", "aloha"]:
-                    from lerobot.common.robot_devices.robots.dynamixel_calibration import run_arm_calibration
-
-                    calibration = run_arm_calibration(arm, self.robot_type, name, arm_type)
-
-                elif self.robot_type in ["so100", "so101", "moss", "lekiwi"]:
-                    from lerobot.common.robot_devices.robots.feetech_calibration import (
-                        run_arm_manual_calibration,
-                    )
-
-                    calibration = run_arm_manual_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
-
-        for name, arm in self.follower_arms.items():
-            calibration = load_or_run_calibration_(name, arm, "follower")
-            arm.set_calibration(calibration)
-        for name, arm in self.leader_arms.items():
-            calibration = load_or_run_calibration_(name, arm, "leader")
-            arm.set_calibration(calibration)
-
-    def set_koch_robot_preset(self):
-        def set_operating_mode_(arm):
-            from lerobot.common.robot_devices.motors.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)
-            # 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.follower_arms[name].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.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 RobotDeviceNotConnectedError(
-                "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 RobotDeviceNotConnectedError(
-                "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 RobotDeviceNotConnectedError(
-                "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 RobotDeviceNotConnectedError(
-                "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()

lerobot/common/robot_devices/robots/mobile_manipulator.py

@@ -1,703 +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.robot_devices.cameras.utils import make_cameras_from_configs
-from lerobot.common.robot_devices.motors.feetech import TorqueMode
-from lerobot.common.robot_devices.motors.utils import MotorsBus, make_motors_buses_from_configs
-from lerobot.common.robot_devices.robots.configs import LeKiwiRobotConfig
-from lerobot.common.robot_devices.robots.feetech_calibration import run_arm_manual_calibration
-from lerobot.common.robot_devices.robots.utils import get_arm_id
-from lerobot.common.robot_devices.utils import RobotDeviceNotConnectedError
-
-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_arm_manual_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 RobotDeviceNotConnectedError("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 RobotDeviceNotConnectedError("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 RobotDeviceNotConnectedError("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 RobotDeviceNotConnectedError("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.")

lerobot/common/robot_devices/robots/realman.py

@@ -1,292 +0,0 @@
-"""
-    Teleoperation Realman with a PS5 controller and 
-"""
-
-import time
-import torch
-import numpy as np
-from dataclasses import dataclass, field, replace
-from collections import deque
-from lerobot.common.robot_devices.teleop.realman_single import HybridController
-from lerobot.common.robot_devices.motors.utils import get_motor_names, make_motors_buses_from_configs
-from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs
-from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
-from lerobot.common.robot_devices.robots.configs import RealmanRobotConfig
-
-
-class RealmanRobot:
-    def __init__(self, config: RealmanRobotConfig | None = None, **kwargs):
-        if config is None:
-            config = RealmanRobotConfig()
-        # Overwrite config arguments using kwargs
-        self.config = replace(config, **kwargs)
-        self.robot_type = self.config.type
-        self.inference_time = self.config.inference_time # if it is inference time
-        
-        # build cameras
-        self.cameras = make_cameras_from_configs(self.config.cameras)
-        
-        # build realman motors
-        self.piper_motors = make_motors_buses_from_configs(self.config.left_follower_arm)
-        self.arm = self.piper_motors['main']
-        
-        # build init teleop info
-        self.init_info = {
-            'init_joint': self.arm.init_joint_position,
-            'init_pose': self.arm.init_pose,
-            'max_gripper': config.max_gripper,
-            'min_gripper': config.min_gripper,
-            'servo_config_file': config.servo_config_file
-        }
-        
-        # build state-action cache
-        self.joint_queue = deque(maxlen=2)
-        self.last_endpose = self.arm.init_pose
-
-        # build gamepad teleop
-        if not self.inference_time:
-            self.teleop = HybridController(self.init_info)
-        else:
-            self.teleop = None
-        
-        self.logs = {}
-        self.is_connected = False
-
-    @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 = get_motor_names(self.piper_motors)
-        state_names = get_motor_names(self.piper_motors)
-        return {
-            "action": {
-                "dtype": "float32",
-                "shape": (len(action_names),),
-                "names": action_names,
-            },
-            "observation.state": {
-                "dtype": "float32",
-                "shape": (len(state_names),),
-                "names": state_names,
-            },
-        }
-    
-    @property
-    def has_camera(self):
-        return len(self.cameras) > 0
-
-    @property
-    def num_cameras(self):
-        return len(self.cameras)
-
-
-    def connect(self) -> None:
-        """Connect RealmanArm and cameras"""
-        if self.is_connected:
-            raise RobotDeviceAlreadyConnectedError(
-                "RealmanArm is already connected. Do not run `robot.connect()` twice."
-            )
-        
-        # connect RealmanArm
-        self.arm.connect(enable=True)
-        print("RealmanArm conneted")
-
-        # connect cameras
-        for name in self.cameras:
-            self.cameras[name].connect()
-            self.is_connected = self.is_connected and self.cameras[name].is_connected
-            print(f"camera {name} conneted")
-        
-        print("All connected")
-        self.is_connected = True
-        
-        self.run_calibration()
-
-
-    def disconnect(self) -> None:
-        """move to home position, disenable piper and cameras"""
-        # move piper to home position, disable
-        if not self.inference_time:
-            self.teleop.stop()
-
-        # disconnect piper
-        self.arm.safe_disconnect()
-        print("RealmanArm disable after 5 seconds")
-        time.sleep(5)
-        self.arm.connect(enable=False)
-
-        # disconnect cameras
-        if len(self.cameras) > 0:
-            for cam in self.cameras.values():
-                cam.disconnect()
-
-        self.is_connected = False
-
-
-    def run_calibration(self):
-        """move piper to the home position"""
-        if not self.is_connected:
-            raise ConnectionError()
-        
-        self.arm.apply_calibration()
-        if not self.inference_time:
-            self.teleop.reset()
-
-
-    def teleop_step(
-        self, record_data=False
-    ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
-        if not self.is_connected:
-            raise ConnectionError()
-
-        if self.teleop is None and self.inference_time:
-            self.teleop = HybridController(self.init_info)
-
-        # read target pose state as 
-        before_read_t = time.perf_counter()
-        state = self.arm.read() # read current joint position from robot
-        action = self.teleop.get_action() # target joint position and pose end pos from gamepad
-        self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-
-        if action['control_mode'] == 'joint':
-            # 关节控制模式（主模式）
-            current_pose = self.arm.read_current_arm_endpose_state()
-            self.teleop.update_endpose_state(current_pose)
-
-            target_joints = action['joint_angles'][:-1]
-            self.arm.write_gripper(action['gripper'])
-            print(action['gripper'])
-            if action['master_controller_status']['infrared'] == 1:
-                if action['master_controller_status']['button'] == 1:
-                    self.arm.write_joint_canfd(target_joints)
-                else:
-                    self.arm.write_joint_slow(target_joints)
-                
-            # do action
-            before_write_t = time.perf_counter()
-            self.joint_queue.append(list(self.arm.read().values()))
-            self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
-
-        else:
-            target_pose = list(action['end_pose'])
-            # do action
-            before_write_t = time.perf_counter()
-            if self.last_endpose != target_pose:
-                self.arm.write_endpose_canfd(target_pose)
-                self.last_endpose = target_pose
-            self.arm.write_gripper(action['gripper'])
-            
-            target_joints = self.arm.read_current_arm_joint_state()
-            self.joint_queue.append(list(self.arm.read().values()))
-            self.teleop.update_joint_state(target_joints)
-            self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
-        
-        if not record_data:
-            return
-        
-        state = torch.as_tensor(list(self.joint_queue[0]), dtype=torch.float32)
-        action = torch.as_tensor(list(self.joint_queue[-1]), dtype=torch.float32)
-
-        # 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 dictionnaries
-        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 send_action(self, action: torch.Tensor) -> torch.Tensor:
-        """Write the predicted actions from policy to the motors"""
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                "Piper is not connected. You need to run `robot.connect()`."
-            )
-
-        # send to motors, torch to list
-        target_joints = action.tolist()
-        len_joint = len(target_joints) - 1
-        target_joints = [target_joints[i]*180 for i in range(len_joint)] + [target_joints[-1]]
-        target_joints[-1] = int(target_joints[-1]*500 + 500)
-        self.arm.write(target_joints)
-
-        return action
-
-
-
-    def capture_observation(self) -> dict:
-        """capture current images and joint positions"""
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                "Piper is not connected. You need to run `robot.connect()`."
-            )
-        
-        # read current joint positions
-        before_read_t = time.perf_counter()
-        state = self.arm.read()  # 6 joints + 1 gripper
-        self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-
-        state = torch.as_tensor(list(state.values()), dtype=torch.float32)
-
-        # read 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 dictionnaries 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 teleop_safety_stop(self):
-        """ move to home position after record one episode """
-        self.run_calibration()
-
-    
-    def __del__(self):
-        if self.is_connected:
-            self.disconnect()
-            if not self.inference_time:
-                self.teleop.stop()
-
-
-if __name__ == '__main__':
-    robot = RealmanRobot()
-    robot.connect()
-    # robot.run_calibration()
-    while True:
-        robot.teleop_step(record_data=True)
-    
-    robot.capture_observation()
-    dummy_action = torch.Tensor([-0.40586111280653214, 0.5522833506266276, 0.4998166826036241, -0.3539944542778863, -0.524433347913954, 0.9064999898274739, 0.482])
-    robot.send_action(dummy_action)
-    robot.disconnect()
-    print('ok')

lerobot/common/robot_devices/robots/realman_dual.py

@@ -1,804 +0,0 @@
-"""
-    Teleoperation Realman with a PS5 controller and LLM interaction
-"""
-
-import time
-import torch
-import numpy as np
-import logging
-from typing import Optional, Tuple, Dict
-from dataclasses import dataclass, field, replace
-from collections import deque
-import signal
-import sys
-
-from lerobot.common.robot_devices.teleop.realman_aloha_dual import HybridController
-from lerobot.common.robot_devices.motors.utils import get_motor_names, make_motors_buses_from_configs
-from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs
-from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
-from lerobot.common.robot_devices.robots.configs import RealmanDualRobotConfig
-from lerobot.common.robot_devices.robots.utils import (
-    ask_llm, 
-    extract_json_from_response,
-    create_keyboard_listener,
-    start_keyboard_listener,
-    stop_keyboard_listener,
-    speak_async,
-    handle_llm_interaction_with_images
-)
-
-
-class RealmanDualRobot:
-    """RealmanDual机器人控制类，支持双臂操作和LLM交互"""
-    
-    def __init__(self, config: RealmanDualRobotConfig | None = None, **kwargs):
-        if config is None:
-            config = RealmanDualRobotConfig()
-        
-        # 配置初始化
-        self.config = replace(config, **kwargs)
-        self.robot_type = self.config.type
-        self.inference_time = self.config.inference_time
-        
-        # 硬件初始化
-        self.cameras = make_cameras_from_configs(self.config.cameras)
-        self.piper_motors = make_motors_buses_from_configs(self.config.follower_arm)
-        self.arm = self.piper_motors['main']
-        
-        # 控制系统初始化
-        self._initialize_teleop()
-        self._initialize_state()
-        self._initialize_keyboard_interface()
-        
-        # 状态标志
-        self._shutdown_flag = False
-        self._llm_triggered = False
-
-    def _initialize_teleop(self):
-        """初始化遥操作控制器"""
-        self.init_info = {
-            'init_joint': self.arm.init_joint_position,
-            'init_pose': self.arm.init_pose,
-            'max_gripper': self.config.max_gripper,
-            'min_gripper': self.config.min_gripper,
-            'servo_config_file': self.config.servo_config_file,
-            'end_control_info': {
-                'left': self.config.left_end_control_guid,
-                'right': self.config.right_end_control_guid
-            }
-        }
-        
-        if not self.inference_time:
-            self.teleop = HybridController(self.init_info)
-        else:
-            self.teleop = None
-
-    def _initialize_state(self):
-        """初始化状态管理"""
-        self.joint_queue = deque(maxlen=2)
-        self.last_endpose = self.arm.init_pose
-        self.logs = {}
-        self.is_connected = False
-
-    def _initialize_keyboard_interface(self):
-        """初始化键盘交互接口"""
-        self.w_pressed = False  # W键触发LLM
-        self.q_pressed = False  # Q键退出
-        self.keyboard_listener = None
-        self._start_keyboard_listener()
-
-    def _start_keyboard_listener(self):
-        """启动键盘监听器"""
-        def on_key_press(key_char):
-            if key_char == 'w':
-                self.w_pressed = True
-                print("检测到W键按下")
-            elif key_char == 'q':
-                self.q_pressed = True
-                print("检测到Q键按下")
-        
-        self.keyboard_listener = create_keyboard_listener(on_key_press)
-        success = start_keyboard_listener(self.keyboard_listener)
-        
-        if success:
-            print("键盘监听器启动成功 (W键:调用LLM, Q键:退出)")
-        else:
-            print("键盘监听器启动失败")
-
-    def _read_robot_state(self) -> dict:
-        """读取机器人状态"""
-        before_read_t = time.perf_counter()
-        from copy import deepcopy
-        state = deepcopy(self.arm.read())
-        self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-        return state
-
-    def _execute_action(self, action: dict, state: dict):
-        """执行机器人动作"""
-        before_write_t = time.perf_counter()
-        
-        if action['control_mode'] == 'joint':
-            pass
-        else:
-            if list(action['pose'].values()) != list(state['pose'].values()):
-                pose = list(action['pose'].values())
-                self.arm.write_endpose_canfd(pose)
-            elif list(action['joint'].values()) != list(state['joint'].values()):
-                target_joint = list(action['joint'].values())
-                self.arm.write(target_joint)
-        
-        self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
-
-    def _prepare_record_data(self) -> Tuple[Dict, Dict]:
-        """准备记录数据 - 保持原有逻辑"""
-        if len(self.joint_queue) < 2:
-            return {}, {}
-        
-        state = torch.as_tensor(list(self.joint_queue[0]), dtype=torch.float32)
-        action = torch.as_tensor(list(self.joint_queue[-1]), dtype=torch.float32)
-        
-        # 捕获图像
-        images = self._capture_images()
-        
-        # 构建输出字典
-        obs_dict = {
-            "observation.state": state,
-            **{f"observation.images.{name}": img for name, img in images.items()}
-        }
-        action_dict = {"action": action}
-        return obs_dict, action_dict
-
-    def _update_state_queue(self):
-        """更新状态队列"""
-        current_state = self.arm.read()['joint']
-        current_state_lst = []
-        for data in current_state:
-            if "joint" in data:
-                current_state_lst.append(current_state[data] / 180)
-            elif "gripper" in data:
-                current_state_lst.append((current_state[data]-500)/500)
-        self.joint_queue.append(current_state_lst)
-
-    def _capture_images(self) -> Dict[str, torch.Tensor]:
-        """捕获摄像头图像"""
-        images = {}
-        for name, camera in self.cameras.items():
-            before_camread_t = time.perf_counter()
-            image = camera.async_read()
-            images[name] = torch.from_numpy(image)
-            self.logs[f"read_camera_{name}_dt_s"] = camera.logs["delta_timestamp_s"]
-            self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t
-        return images
-
-    def _handle_llm_interaction(self, obs_dict: Dict) -> bool:
-        """处理LLM交互逻辑"""
-        print("[W键已按下] 正在准备数据并调用LLM...")
-        
-        # 提取图像数据
-        camera_images = {}
-        for key, value in obs_dict.items():
-            if key.startswith("observation.images."):
-                camera_name = key.replace("observation.images.", "")
-                camera_images[camera_name] = value.cpu().numpy()
-        
-        # 使用utils中的函数处理LLM交互
-        success, response = handle_llm_interaction_with_images(
-            "将超声仪左下角试管架上的蓝色试管移动到超声仪中", 
-            camera_images
-        )
-        
-        return success
-
-    def connect(self) -> None:
-        """连接机器人和摄像头"""
-        if self.is_connected:
-            raise RobotDeviceAlreadyConnectedError(
-                "RealmanArm is already connected. Do not run `robot.connect()` twice."
-            )
-        
-        # 连接机械臂
-        self.arm.connect(enable=True)
-        print("RealmanArm conneted")
-        
-        # 连接摄像头
-        for name in self.cameras:
-            self.cameras[name].connect()
-            self.is_connected = self.is_connected and self.cameras[name].is_connected
-            print(f"camera {name} conneted")
-        
-        print("All connected")
-        self.is_connected = True
-        self.run_calibration()
-
-    def disconnect(self) -> None:
-        """断开连接"""
-        if self._shutdown_flag:
-            return
-        self._shutdown_flag = True
-        
-        try:
-            # 停止键盘监听器
-            stop_keyboard_listener(self.keyboard_listener)
-            print("键盘监听器已停止")
-            
-            # 停止遥操作控制器
-            if hasattr(self, 'teleop') and self.teleop and not self.inference_time:
-                self.teleop.stop()
-                print("遥操作控制器已停止")
-            
-            # 断开机械臂连接
-            if hasattr(self, 'arm'):
-                try:
-                    self.arm.safe_disconnect()
-                    print("RealmanArm 安全断开连接")
-                    time.sleep(2)
-                    self.arm.connect(enable=False)
-                    print("RealmanArm 已禁用")
-                except Exception as e:
-                    print(f"断开机械臂连接时出错: {e}")
-            
-            # 断开摄像头连接
-            if len(self.cameras) > 0:
-                for name, cam in self.cameras.items():
-                    try:
-                        cam.disconnect()
-                        print(f"Camera {name} 已断开连接")
-                    except Exception as e:
-                        print(f"断开相机 {name} 时出错: {e}")
-            
-            self.is_connected = False
-            print("所有设备已断开连接")
-            
-        except Exception as e:
-            print(f"断开连接时发生错误: {e}")
-
-    def run_calibration(self):
-        """运行标定"""
-        if not self.is_connected:
-            raise ConnectionError()
-        self.arm.apply_calibration()
-        if not self.inference_time:
-            self.teleop.reset()
-
-    def teleop_step(self, record_data=False) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
-        """遥操作步骤 - 保持原有数据记录逻辑，添加LLM交互"""
-        if not self.is_connected:
-            raise ConnectionError()
-        
-        if self.teleop is None and self.inference_time:
-            self.teleop = HybridController(self.init_info)
-        
-        try:
-            # 检查退出条件
-            if self.q_pressed:
-                print("检测到Q键，任务终止...")
-                speak_async("任务已终止")
-                raise KeyboardInterrupt("用户请求退出")
-            
-            # 执行基础遥操作
-            state = self._read_robot_state()
-            action = self.teleop.get_action(state)
-            self._execute_action(action, state)
-            
-            # 更新状态队列
-            self._update_state_queue()
-            time.sleep(0.019)  # 50Hz
-            
-            # 处理数据记录
-            if record_data:
-                data = self._prepare_record_data()
-                
-                # 处理LLM交互请求（只有在有有效数据时才处理）
-                if data[0] and self.w_pressed:  # 如果有有效数据且W键被按下
-                    self.w_pressed = False  # 重置标志位
-                    self._llm_triggered = True
-                    success = self._handle_llm_interaction(data[0])
-                    if not success:
-                        print("LLM交互处理失败")
-                
-                # 如果没有有效数据，创建默认数据
-                if not data[0]:
-                    # 创建默认的观测数据
-                    if len(self.joint_queue) > 0:
-                        state_tensor = torch.as_tensor(list(self.joint_queue[-1]), dtype=torch.float32)
-                    else:
-                        state_tensor = torch.zeros(14, dtype=torch.float32)  # 根据你的机器人调整维度
-                    
-                    # 捕获当前图像
-                    images = self._capture_images()
-                    
-                    obs_dict = {
-                        "observation.state": state_tensor,
-                        **{f"observation.images.{name}": img for name, img in images.items()}
-                    }
-                    action_dict = {"action": state_tensor}  # 使用相同的状态作为动作
-                    data = (obs_dict, action_dict)
-                
-                return data
-            
-            return None
-            
-        except KeyboardInterrupt:
-            # 重新抛出键盘中断，让上层处理
-            raise
-        except Exception as e:
-            logging.error(f"遥操作步骤失败: {e}")
-            
-            # 即使出错，在record_data=True时也要返回有效数据
-            if record_data:
-                # 创建紧急默认数据
-                state_tensor = torch.zeros(14, dtype=torch.float32)  # 根据你的机器人调整维度
-                images = {}
-                try:
-                    images = self._capture_images()
-                except:
-                    # 如果连图像都无法捕获，创建空图像
-                    for camera_name in self.cameras.keys():
-                        images[camera_name] = torch.zeros((480, 640, 3), dtype=torch.uint8)
-                
-                obs_dict = {
-                    "observation.state": state_tensor,
-                    **{f"observation.images.{name}": img for name, img in images.items()}
-                }
-                action_dict = {"action": state_tensor}
-                return obs_dict, action_dict
-            
-            return None
-
-    def send_action(self, action: torch.Tensor) -> torch.Tensor:
-        """发送动作到机器人"""
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                "Piper is not connected. You need to run `robot.connect()`."
-            )
-        
-        target_joints = action.tolist()
-        len_joint = len(target_joints) - 1
-        target_joints = [target_joints[i]*180 for i in range(len_joint)] + [target_joints[-1]]
-        target_joints[-1] = int(target_joints[-1]*500 + 500)
-        self.arm.write(target_joints)
-        
-        return action
-
-    def capture_observation(self) -> dict:
-        """捕获当前观测"""
-        if not self.is_connected:
-            raise RobotDeviceNotConnectedError(
-                "Piper is not connected. You need to run `robot.connect()`."
-            )
-        
-        before_read_t = time.perf_counter()
-        state = self.arm.read()
-        self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-        
-        state = torch.as_tensor(list(state.values()), dtype=torch.float32)
-        
-        # 读取图像
-        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
-        
-        # 构建观测字典
-        obs_dict = {}
-        obs_dict["observation.state"] = state
-        for name in self.cameras:
-            obs_dict[f"observation.images.{name}"] = images[name]
-        return obs_dict
-
-    def teleop_safety_stop(self):
-        """遥操作安全停止"""
-        self.run_calibration()
-
-    @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 = get_motor_names(self.piper_motors)
-        state_names = get_motor_names(self.piper_motors)
-        return {
-            "action": {
-                "dtype": "float32",
-                "shape": (len(action_names),),
-                "names": action_names,
-            },
-            "observation.state": {
-                "dtype": "float32",
-                "shape": (len(state_names),),
-                "names": state_names,
-            },
-        }
-
-    @property
-    def has_camera(self):
-        return len(self.cameras) > 0
-
-    @property
-    def num_cameras(self):
-        return len(self.cameras)
-
-    def __del__(self):
-        """析构函数"""
-        try:
-            if not self._shutdown_flag:
-                self.disconnect()
-        except:
-            pass
-
-
-def signal_handler(signum, frame):
-    """信号处理器"""
-    print("\n收到中断信号，正在安全退出...")
-    sys.exit(0)
-
-
-if __name__ == '__main__':
-    signal.signal(signal.SIGINT, signal_handler)
-    robot = None
-    
-    try:
-        robot = RealmanDualRobot()
-        robot.connect()
-        
-        print("RealmanDual 机器人控制已启动")
-        print("操作说明：")
-        print("   - 使用手柄进行遥操作控制")
-        print("   - 按 W 键：调用LLM分析当前场景")
-        print("   - 按 Q 键：安全退出程序")
-        print("   - Ctrl+C：强制退出")
-        print("\n等待操作...")
-        
-        while True:
-            result = robot.teleop_step(record_data=True)
-            if result is None:
-                continue
-            # 这里可以处理记录的数据
-            
-    except KeyboardInterrupt:
-        print("\n收到键盘中断信号")
-    except Exception as e:
-        print(f"程序运行出错: {e}")
-    finally:
-        if robot:
-            try:
-                robot.disconnect()
-            except:
-                pass
-        print("程序已完全退出")
-# """
-#     Teleoperation Realman with a PS5 controller and 
-# """
-
-# import time
-# import torch
-# import numpy as np
-# import logging
-# from typing import Optional, Tuple, Dict
-# from dataclasses import dataclass, field, replace
-# from collections import deque
-# from lerobot.common.robot_devices.teleop.realman_aloha_dual import HybridController
-# from lerobot.common.robot_devices.motors.utils import get_motor_names, make_motors_buses_from_configs
-# from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs
-# from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
-# from lerobot.common.robot_devices.robots.configs import RealmanDualRobotConfig
-# from lerobot.common.robot_devices.robots.utils import ask_llm
-
-
-
-# class RealmanDualRobot:
-#     def __init__(self, config: RealmanDualRobotConfig | None = None, **kwargs):
-#         if config is None:
-#             config = RealmanDualRobotConfig()
-#         # Overwrite config arguments using kwargs
-#         self.config = replace(config, **kwargs)
-#         self.robot_type = self.config.type
-#         self.inference_time = self.config.inference_time # if it is inference time
-        
-#         # build cameras
-#         self.cameras = make_cameras_from_configs(self.config.cameras)
-#         # build realman motors
-#         self.piper_motors = make_motors_buses_from_configs(self.config.follower_arm)
-#         self.arm = self.piper_motors['main']
-        
-#         # 初始化遥操作
-#         self._initialize_teleop()
-#         # init state
-#         self._initialize_state()
-
-#     def _initialize_teleop(self):
-#         """初始化遥操作"""
-#         self.init_info = {
-#             'init_joint': self.arm.init_joint_position,
-#             'init_pose': self.arm.init_pose,
-#             'max_gripper': self.config.max_gripper,
-#             'min_gripper': self.config.min_gripper,
-#             'servo_config_file': self.config.servo_config_file,
-#             'end_control_info': {'left': self.config.left_end_control_guid , 'right': self.config.right_end_control_guid}
-#         }
-        
-#         if not self.inference_time:
-#             self.teleop = HybridController(self.init_info)
-#         else:
-#             self.teleop = None
-            
-#     def _initialize_state(self):
-#         """初始化状态"""
-#         self.joint_queue = deque(maxlen=2)
-#         self.last_endpose = self.arm.init_pose
-#         self.logs = {}
-#         self.is_connected = False
-
-#     def _read_robot_state(self) -> dict:
-#         """读取机器人状态"""
-#         before_read_t = time.perf_counter()
-#         from copy import deepcopy
-#         state = deepcopy(self.arm.read())
-#         self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-#         return state
-
-#     def _execute_action(self, action: dict, state: dict):
-#         """执行动作"""
-#         before_write_t = time.perf_counter()
-
-#         if action['control_mode'] == 'joint':
-#             # self.arm.write_action(action, state)
-#             pass
-#         else:
-#             if list(action['pose'].values()) != list(state['pose'].values()):
-#                 pose = list(action['pose'].values())
-#                 self.arm.write_endpose_canfd(pose)
-            
-#             elif list(action['joint'].values()) != list(state['joint'].values()):
-#                 target_joint = list(action['joint'].values())
-#                 self.arm.write(target_joint)
-        
-#         self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
-
-#     def _prepare_record_data(self) -> Tuple[Dict, Dict]:
-#         """准备记录数据"""
-#         if len(self.joint_queue) < 2:
-#             return {}, {}
-            
-#         state = torch.as_tensor(list(self.joint_queue[0]), dtype=torch.float32)
-#         action = torch.as_tensor(list(self.joint_queue[-1]), dtype=torch.float32)
-#         # 捕获图像
-#         images = self._capture_images()
-#         # 构建输出字典
-#         obs_dict = {
-#             "observation.state": state,
-#             **{f"observation.images.{name}": img for name, img in images.items()}
-#         }
-#         action_dict = {"action": action}
-#         return obs_dict, action_dict
-    
-#     def _update_state_queue(self):
-#         """更新状态队列"""
-#         current_state = self.arm.read()['joint']
-#         current_state_lst = []
-#         for data in current_state:
-#             if "joint" in data:
-#                 current_state_lst.append(current_state[data] / 180)
-#             elif "gripper" in data:
-#                 current_state_lst.append((current_state[data]-500)/500)
-#         self.joint_queue.append(current_state_lst)
-    
-#     def _capture_images(self) -> Dict[str, torch.Tensor]:
-#         """捕获图像"""
-#         images = {}
-#         for name, camera in self.cameras.items():
-#             before_camread_t = time.perf_counter()
-#             image = camera.async_read()
-#             images[name] = torch.from_numpy(image)
-            
-#             self.logs[f"read_camera_{name}_dt_s"] = camera.logs["delta_timestamp_s"]
-#             self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t
-#         return images
-
-#     @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 = get_motor_names(self.piper_motors)
-#         state_names = get_motor_names(self.piper_motors)
-#         return {
-#             "action": {
-#                 "dtype": "float32",
-#                 "shape": (len(action_names),),
-#                 "names": action_names,
-#             },
-#             "observation.state": {
-#                 "dtype": "float32",
-#                 "shape": (len(state_names),),
-#                 "names": state_names,
-#             },
-#         }
-    
-#     @property
-#     def has_camera(self):
-#         return len(self.cameras) > 0
-
-#     @property
-#     def num_cameras(self):
-#         return len(self.cameras)
-
-
-#     def connect(self) -> None:
-#         """Connect RealmanArm and cameras"""
-#         if self.is_connected:
-#             raise RobotDeviceAlreadyConnectedError(
-#                 "RealmanArm is already connected. Do not run `robot.connect()` twice."
-#             )
-        
-#         # connect RealmanArm
-#         self.arm.connect(enable=True)
-#         print("RealmanArm conneted")
-
-#         # connect cameras
-#         for name in self.cameras:
-#             self.cameras[name].connect()
-#             self.is_connected = self.is_connected and self.cameras[name].is_connected
-#             print(f"camera {name} conneted")
-        
-#         print("All connected")
-#         self.is_connected = True
-        
-#         self.run_calibration()
-
-
-#     def disconnect(self) -> None:
-#         """move to home position, disenable piper and cameras"""
-#         # move piper to home position, disable
-#         if not self.inference_time:
-#             self.teleop.stop()
-
-#         # disconnect piper
-#         self.arm.safe_disconnect()
-#         print("RealmanArm disable after 5 seconds")
-#         time.sleep(5)
-#         self.arm.connect(enable=False)
-
-#         # disconnect cameras
-#         if len(self.cameras) > 0:
-#             for cam in self.cameras.values():
-#                 cam.disconnect()
-
-#         self.is_connected = False
-
-
-#     def run_calibration(self):
-#         """move piper to the home position"""
-#         if not self.is_connected:
-#             raise ConnectionError()
-        
-#         self.arm.apply_calibration()
-#         if not self.inference_time:
-#             self.teleop.reset()
-
-#     def teleop_step(
-#         self, record_data=False
-#     ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
-#         if not self.is_connected:
-#             raise ConnectionError()
-
-#         if self.teleop is None and self.inference_time:
-#             self.teleop = HybridController(self.init_info)
-
-#         try:
-#             # 读取当前状态
-#             state = self._read_robot_state()
-#             # 获取动作
-#             action = self.teleop.get_action(state)
-#             self._execute_action(action, state)
-#             # 更新状态队列
-#             self._update_state_queue()
-#             time.sleep(0.019) # 50HZ
-
-#             if record_data:
-#                 data = self._prepare_record_data()
-#                 if data[0]:
-#                 #     # ask_llm("将超声仪左下角试管架上的试管移动到超声仪中", data[0])
-#                     pass
-#                 return data
-            
-#         except Exception as e:
-#             logging.error(f"遥操作步骤失败: {e}")
-#             return None
-
-
-#     def send_action(self, action: torch.Tensor) -> torch.Tensor:
-#         """Write the predicted actions from policy to the motors"""
-#         if not self.is_connected:
-#             raise RobotDeviceNotConnectedError(
-#                 "Piper is not connected. You need to run `robot.connect()`."
-#             )
-
-#         # send to motors, torch to list
-#         target_joints = action.tolist()
-#         len_joint = len(target_joints) - 1
-#         target_joints = [target_joints[i]*180 for i in range(len_joint)] + [target_joints[-1]]
-#         target_joints[-1] = int(target_joints[-1]*500 + 500)
-#         self.arm.write(target_joints)
-
-#         return action
-
-
-#     def capture_observation(self) -> dict:
-#         """capture current images and joint positions"""
-#         if not self.is_connected:
-#             raise RobotDeviceNotConnectedError(
-#                 "Piper is not connected. You need to run `robot.connect()`."
-#             )
-        
-#         # read current joint positions
-#         before_read_t = time.perf_counter()
-#         state = self.arm.read()  # 6 joints + 1 gripper
-#         self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-
-#         state = torch.as_tensor(list(state.values()), dtype=torch.float32)
-
-#         # read 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 dictionnaries 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 teleop_safety_stop(self):
-#         """ move to home position after record one episode """
-#         self.run_calibration()
-
-    
-#     def __del__(self):
-#         if self.is_connected:
-#             self.disconnect()
-#             if not self.inference_time:
-#                 self.teleop.stop()
-
-
-# if __name__ == '__main__':
-#     robot = RealmanDualRobot()
-#     robot.connect()
-#     # robot.run_calibration()
-#     while True:
-#         robot.teleop_step(record_data=True)
-    
-#     # robot.capture_observation()
-#     # dummy_action = torch.Tensor([-0.40586111280653214, 0.5522833506266276, 0.4998166826036241, -0.3539944542778863, -0.524433347913954, 0.9064999898274739, 0.482])
-#     # robot.send_action(dummy_action)
-#     # robot.disconnect()
-#     # print('ok')

lerobot/common/robot_devices/robots/stretch.py

@@ -1,208 +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 time
-from dataclasses import replace
-
-import torch
-from stretch_body.gamepad_teleop import GamePadTeleop
-from stretch_body.robot import Robot as StretchAPI
-from stretch_body.robot_params import RobotParams
-
-from lerobot.common.robot_devices.robots.configs import StretchRobotConfig
-
-
-class StretchRobot(StretchAPI):
-    """Wrapper of stretch_body.robot.Robot"""
-
-    def __init__(self, config: StretchRobotConfig | None = None, **kwargs):
-        super().__init__()
-        if config is None:
-            self.config = StretchRobotConfig(**kwargs)
-        else:
-            # Overwrite config arguments using kwargs
-            self.config = replace(config, **kwargs)
-
-        self.robot_type = self.config.type
-        self.cameras = self.config.cameras
-        self.is_connected = False
-        self.teleop = None
-        self.logs = {}
-
-        # TODO(aliberts): test this
-        RobotParams.set_logging_level("WARNING")
-        RobotParams.set_logging_formatter("brief_console_formatter")
-
-        self.state_keys = None
-        self.action_keys = None
-
-    def connect(self) -> None:
-        self.is_connected = self.startup()
-        if not self.is_connected:
-            print("Another process is already using Stretch. Try running 'stretch_free_robot_process.py'")
-            raise ConnectionError()
-
-        for name in self.cameras:
-            self.cameras[name].connect()
-            self.is_connected = self.is_connected and self.cameras[name].is_connected
-
-        if not self.is_connected:
-            print("Could not connect to the cameras, check that all cameras are plugged-in.")
-            raise ConnectionError()
-
-        self.run_calibration()
-
-    def run_calibration(self) -> None:
-        if not self.is_homed():
-            self.home()
-
-    def teleop_step(
-        self, record_data=False
-    ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
-        # TODO(aliberts): return ndarrays instead of torch.Tensors
-        if not self.is_connected:
-            raise ConnectionError()
-
-        if self.teleop is None:
-            self.teleop = GamePadTeleop(robot_instance=False)
-            self.teleop.startup(robot=self)
-
-        before_read_t = time.perf_counter()
-        state = self.get_state()
-        action = self.teleop.gamepad_controller.get_state()
-        self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-
-        before_write_t = time.perf_counter()
-        self.teleop.do_motion(robot=self)
-        self.push_command()
-        self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
-
-        if self.state_keys is None:
-            self.state_keys = list(state)
-
-        if not record_data:
-            return
-
-        state = torch.as_tensor(list(state.values()))
-        action = torch.as_tensor(list(action.values()))
-
-        # 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 get_state(self) -> dict:
-        status = self.get_status()
-        return {
-            "head_pan.pos": status["head"]["head_pan"]["pos"],
-            "head_tilt.pos": status["head"]["head_tilt"]["pos"],
-            "lift.pos": status["lift"]["pos"],
-            "arm.pos": status["arm"]["pos"],
-            "wrist_pitch.pos": status["end_of_arm"]["wrist_pitch"]["pos"],
-            "wrist_roll.pos": status["end_of_arm"]["wrist_roll"]["pos"],
-            "wrist_yaw.pos": status["end_of_arm"]["wrist_yaw"]["pos"],
-            "gripper.pos": status["end_of_arm"]["stretch_gripper"]["pos"],
-            "base_x.vel": status["base"]["x_vel"],
-            "base_y.vel": status["base"]["y_vel"],
-            "base_theta.vel": status["base"]["theta_vel"],
-        }
-
-    def capture_observation(self) -> dict:
-        # TODO(aliberts): return ndarrays instead of torch.Tensors
-        before_read_t = time.perf_counter()
-        state = self.get_state()
-        self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-
-        if self.state_keys is None:
-            self.state_keys = list(state)
-
-        state = torch.as_tensor(list(state.values()))
-
-        # 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 = {}
-        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:
-        # TODO(aliberts): return ndarrays instead of torch.Tensors
-        if not self.is_connected:
-            raise ConnectionError()
-
-        if self.teleop is None:
-            self.teleop = GamePadTeleop(robot_instance=False)
-            self.teleop.startup(robot=self)
-
-        if self.action_keys is None:
-            dummy_action = self.teleop.gamepad_controller.get_state()
-            self.action_keys = list(dummy_action.keys())
-
-        action_dict = dict(zip(self.action_keys, action.tolist(), strict=True))
-
-        before_write_t = time.perf_counter()
-        self.teleop.do_motion(state=action_dict, robot=self)
-        self.push_command()
-        self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
-
-        # TODO(aliberts): return action_sent when motion is limited
-        return action
-
-    def print_logs(self) -> None:
-        pass
-        # TODO(aliberts): move robot-specific logs logic here
-
-    def teleop_safety_stop(self) -> None:
-        if self.teleop is not None:
-            self.teleop._safety_stop(robot=self)
-
-    def disconnect(self) -> None:
-        self.stop()
-        if self.teleop is not None:
-            self.teleop.gamepad_controller.stop()
-            self.teleop.stop()
-
-        if len(self.cameras) > 0:
-            for cam in self.cameras.values():
-                cam.disconnect()
-
-        self.is_connected = False
-
-    def __del__(self):
-        self.disconnect()

lerobot/common/robot_devices/robots/utils.py

@@ -1,386 +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.
-from typing import Protocol, Dict
-# Robot configuration imports
-from lerobot.common.robot_devices.robots.configs import (
-    AlohaRobotConfig,
-    KochBimanualRobotConfig,
-    KochRobotConfig,
-    LeKiwiRobotConfig,
-    ManipulatorRobotConfig,
-    MossRobotConfig,
-    RobotConfig,
-    So100RobotConfig,
-    So101RobotConfig,
-    StretchRobotConfig,
-    RealmanRobotConfig,
-    RealmanDualRobotConfig
-)
-# Added library imports for LLM interaction
-import os
-import base64
-from io import BytesIO
-import json
-from openai import OpenAI
-from PIL import Image
-import numpy as np
-import torch
-def get_arm_id(name, arm_type):
-    """Returns the string identifier of a robot arm."""
-    return f"{name}_{arm_type}"
-class Robot(Protocol):
-    robot_type: str
-    features: dict
-    cameras: Dict 
-    def connect(self): ...
-    def run_calibration(self): ...
-    def teleop_step(self, record_data=False): ...
-    def capture_observation(self) -> Dict: ...
-    def send_action(self, action): ...
-    def disconnect(self): ...
-def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:
-    if robot_type == "aloha":
-        return AlohaRobotConfig(**kwargs)
-    elif robot_type == "koch":
-        return KochRobotConfig(**kwargs)
-    elif robot_type == "koch_bimanual":
-        return KochBimanualRobotConfig(**kwargs)
-    elif robot_type == "moss":
-        return MossRobotConfig(**kwargs)
-    elif robot_type == "so100":
-        return So100RobotConfig(**kwargs)
-    elif robot_type == "so101":
-        return So101RobotConfig(**kwargs)
-    elif robot_type == "stretch":
-        return StretchRobotConfig(**kwargs)
-    elif robot_type == "lekiwi":
-        return LeKiwiRobotConfig(**kwargs)
-    elif robot_type == 'realman':
-        return RealmanRobotConfig(**kwargs)
-    elif robot_type == 'realman_dual':
-        return RealmanDualRobotConfig(**kwargs)
-    else:
-        raise ValueError(f"Robot type '{robot_type}' is not available.")
-def make_robot_from_config(config: RobotConfig):
-    if isinstance(config, ManipulatorRobotConfig):
-        from lerobot.common.robot_devices.robots.manipulator import ManipulatorRobot
-        return ManipulatorRobot(config)
-    elif isinstance(config, LeKiwiRobotConfig):
-        from lerobot.common.robot_devices.robots.mobile_manipulator import MobileManipulator
-        return MobileManipulator(config)
-    elif isinstance(config, RealmanRobotConfig):
-        from lerobot.common.robot_devices.robots.realman import RealmanRobot
-        return RealmanRobot(config)
-    elif isinstance(config, RealmanDualRobotConfig):
-        from lerobot.common.robot_devices.robots.realman_dual import RealmanDualRobot
-        return RealmanDualRobot(config)
-    else:
-        from lerobot.common.robot_devices.robots.stretch import StretchRobot
-        return StretchRobot(config)
-def make_robot(robot_type: str, **kwargs) -> Robot:
-    config = make_robot_config(robot_type, **kwargs)
-    return make_robot_from_config(config)
-# 全局变量，用于管理会话历史和API客户端
-conversation_history = []
-conversation_client = None
-def reset_conversation_history():
-    """清空对话历史，开始一个新任务。"""
-    global conversation_history
-    conversation_history = []
-    print("对话历史已重置。")
-
-def extract_json_from_response(response: str) -> dict:
-    """
-    从LLM响应中提取JSON格式的指令 - 从realman_dual.py移动过来
-    
-    Args:
-        response: LLM的原始响应文本
-        
-    Returns:
-        dict: 解析后的JSON指令字典
-    """
-    try:
-        # 尝试直接解析整个响应
-        return json.loads(response)
-    except:
-        # 尝试从响应中查找JSON部分
-        import re
-        json_pattern = r'\{[^{}]*\}'
-        matches = re.findall(json_pattern, response)
-        
-        for match in matches:
-            try:
-                return json.loads(match)
-            except:
-                continue
-        
-        # 如果无法解析JSON，返回默认格式
-        return {
-            "action": "unknown",
-            "description": response,
-            "status": "parse_error"
-        }
-
-def ask_llm(query: str, state: dict):
-    """
-    向大型语言模型发送查询，并获取下一步操作指令。
-    优化版本：只保留最新的4张图片，避免Token超限
-    """
-    global conversation_client
-    global conversation_history
-    
-    # 将NumPy数组转换为Base64编码的图片
-    def numpy_to_base64(img_array):
-        if img_array.dtype != np.uint8:
-            if img_array.max() <= 1.0:
-                img_array = (img_array * 255).astype(np.uint8)
-            else:
-                img_array = img_array.astype(np.uint8)
-        image = Image.fromarray(img_array)
-        buffered = BytesIO()
-        image.save(buffered, format="PNG")
-        img_base64 = base64.b64encode(buffered.getvalue()).decode('utf-8')
-        return img_base64
-    
-    # 初始化API客户端
-    api_key = "sk-H5FY8bQn6ZwBCja56280C3A4C8824017A4CdB683Dc990e35"
-    base_url = "https://api.apiyi.com/v1"
-    
-    # if not api_key:
-    #     raise ValueError("API Key为空！请直接在代码中填写。")
-    
-    if conversation_client is None:
-        conversation_client = OpenAI(api_key=api_key, base_url=base_url)
-    
-    # 仅在对话历史为空时添加系统提示
-    if not conversation_history:
-        system_prompt = f"""
-你是一个聪明的双臂机器人助手：
-**机器人配置：**
-- 左臂：配备夹爪，可以抓取物体
-- 右臂：没有夹爪，主要用于辅助定位和观察
-- 四个摄像头：high camera(俯视角)、front camera(正视角)、left camera(左臂视角)、right camera(右臂视角)
-**任务目标：** {query}
-**工作流程：**
-1. 我向你展示当前4个摄像头的画面
-2. 你分析场景，给出下一步具体操作指令
-3. 我执行你的指令后，再次更新画面
-4. 重复w此过程直到完成任务
-**重要要求：**
-- 每次只给出ONE STEP最关键的操作指令
-- 指令要具体明确，便于执行（如"将左臂夹爪移动到试管正上方5cm处，准备下降抓取"）
-- 当视角不清晰时，要求调整摄像头位置
-- 左臂负责抓取，右臂负责辅助观察和定位
-- 给出的指令必须可执行，避免模糊描述
-**输出格式要求：**
-- 使用纯文本输出，不要使用任何Markdown格式符号
-- 不要使用星号、井号、下划线等格式化符号
-- 直接给出简洁分析和具体操作指令
-- 输出内容适合语音播报
-**输出格式：**
-简洁分析 + 具体操作指令
-        """
-        conversation_history.append({"role": "system", "content": system_prompt})
-    
-    # 如果对话历史中有图片消息，只保留最近的一轮对话（系统消息 + 最后一轮用户-助手对话）
-    if len(conversation_history) > 3:  # 系统消息 + 用户消息 + 助手回复
-        # 保留系统消息
-        system_msg = conversation_history[0]
-        # 只保留最后一轮对话（最后的用户消息和助手回复）
-        recent_messages = conversation_history[-2:]  # 最后2条消息
-        conversation_history = [system_msg] + recent_messages
-        print("清理对话历史，避免Token超限")
-    
-    # 构建本次用户输入的消息内容
-    user_content = [{"type": "text", "text": "这是当前的4个摄像头画面，请根据任务目标分析场景并给出下一步具体操作指令。"}]
-    
-    # 添加图片，按照指定顺序
-    camera_order = ['high', 'front', 'left', 'right']
-    for camera_name in camera_order:
-        if camera_name in state:
-            img_base64 = numpy_to_base64(state[camera_name])
-            user_content.append({
-                "type": "image_url",
-                "image_url": {
-                    "url": f"data:image/png;base64,{img_base64}",
-                    "detail": "high"  # 使用高清晰度分析
-                }
-            })
-    
-    # 将当前用户输入添加到对话历史中
-    conversation_history.append({"role": "user", "content": user_content})
-    
-    try:
-        completion = conversation_client.chat.completions.create(
-            model="claude-sonnet-4-20250514",
-            messages=conversation_history,
-            max_tokens=800, 
-            temperature=0.3  
-        )
-        
-        response_message = completion.choices[0].message
-        response_content = response_message.content
-        
-        # 将模型的响应添加到历史中
-        conversation_history.append(response_message)
-        
-        print(f"机器人响应: {response_content}")
-        return response_content
-        
-    except Exception as e:
-        error_msg = str(e)
-        print(f"调用LLM时出错: {error_msg}")
-        
-        # Token超限时的特殊处理
-        if "token_limit_exceeded" in error_msg or "context_length_exceeded" in error_msg:
-            print("检测到Token超限，清空对话历史重新开始...")
-            # 清空除系统消息外的所有历史
-            if conversation_history:
-                conversation_history = [conversation_history[0]]  # 只保留系统消息
-            # 移除刚刚添加的用户消息，准备重试
-            if conversation_history and len(conversation_history) > 1:
-                conversation_history.pop()
-            return "由于对话历史过长，已重置对话。请再次按Enter键继续。"
-        
-        # 其他错误时也要清理最后的用户输入
-        if conversation_history and conversation_history[-1]["role"] == "user":
-            conversation_history.pop()
-        return None
-    
-def extract_json_from_response(response: str) -> dict:
-    """
-    从LLM响应中提取结构化指令 - 优化版本
-    """
-    try:
-        # 尝试直接解析整个响应
-        return json.loads(response)
-    except:
-        # 如果无法解析JSON，创建结构化的响应
-        return {
-            "action": "move_arm",  # 默认动作类型
-            "description": response.strip(),
-            "arm": "left",  # 默认使用左臂
-            "target": "unknown",
-            "status": "ready"
-        }
-
-
-from pynput import keyboard
-import threading
-
-def create_keyboard_listener(on_key_press_callback):
-    """
-    创建键盘监听器
-    
-    Args:
-        on_key_press_callback: 按键回调函数，接收参数 (key_char)
-    
-    Returns:
-        keyboard.Listener: 键盘监听器对象
-    """
-    def on_press(key):
-        try:
-            if hasattr(key, 'char') and key.char:
-                on_key_press_callback(key.char.lower())
-        except AttributeError:
-            pass
-    
-    return keyboard.Listener(on_press=on_press)
-
-def start_keyboard_listener(listener):
-    """启动键盘监听器"""
-    try:
-        listener.start()
-        return True
-    except Exception as e:
-        print(f"键盘监听器启动失败: {e}")
-        return False
-
-def stop_keyboard_listener(listener):
-    """停止键盘监听器"""
-    try:
-        if listener and listener.is_alive():
-            listener.stop()
-            return True
-    except Exception:
-        pass
-    return False
-
-def speak_async(text: str):
-    """异步语音播报"""
-    from lerobot.common.utils.utils import say
-    
-    def speak_thread():
-        try:
-            print(f"开始语音播报: {text}")
-            say(text, blocking=True)
-            print("语音播报完成")
-        except Exception as e:
-            print(f"语音播报失败: {e}")
-    
-    thread = threading.Thread(target=speak_thread, daemon=True)
-    thread.start()
-
-def handle_llm_interaction_with_images(query: str, camera_images: dict):
-    """
-    处理LLM交互，接收相机图像字典
-    
-    Args:
-        query: 用户查询文本
-        camera_images: 相机图像字典 {camera_name: numpy_array}
-    
-    Returns:
-        tuple: (success: bool, response: str)
-    """
-    if len(camera_images) != 4:
-        print(f"警告: 期望4个相机，实际获取到{len(camera_images)}个")
-        return False, "相机数量不足"
-    
-    print("图像提取成功，正在向LLM发送请求...")
-    
-    try:
-        response = ask_llm(query, camera_images)
-        if response:
-            if "已重置对话" in response:
-                print("对话历史已重置，请再次按键继续")
-                return True, response
-            
-            instruction = extract_json_from_response(response)
-            print("\n下一步指令:")
-            print(f"操作类型: {instruction.get('action', 'unknown')}")
-            print(f"详细描述: {instruction.get('description', '无描述')}")
-            
-            # 准备语音播报内容
-            if "description" in instruction:
-                desc = instruction['description']
-                if len(desc) > 100:
-                    sentences = desc.split('。')
-                    key_instruction = sentences[-2] if len(sentences) > 1 else desc[:50]
-                else:
-                    key_instruction = desc
-                speech_text = f"下一步操作：{key_instruction}"
-                print(f"即将播报: {speech_text}")
-                speak_async(speech_text)
-            
-            return True, response
-        else:
-            print("LLM请求失败，可能是网络问题")
-            speak_async("LLM请求失败")
-            return False, "LLM请求失败"
-            
-    except Exception as e:
-        print(f"LLM交互出错: {e}")
-        print("建议：1.检查网络连接 2.检查API密钥 3.稍后重试")
-        speak_async("LLM交互失败")
-        return False, f"LLM交互出错: {e}"

lerobot/common/robot_devices/teleop/find_gamepad.py

@@ -1,18 +0,0 @@
-import pygame
-
-def find_controller_index():
-    # 获取所有 pygame 控制器的设备路径
-    pygame_joysticks = {}
-    for i in range(pygame.joystick.get_count()):
-        joystick = pygame.joystick.Joystick(i)
-        joystick.init()
-        pygame_joysticks[joystick.get_guid()] = {
-            'index': i,
-            'device_name': joystick.get_name()
-        }
-
-    return pygame_joysticks
-
-
-if __name__ == '__main__':
-    print(find_controller_index())

lerobot/common/robot_devices/teleop/gamepad.py

@@ -1,421 +0,0 @@
-import pygame
-import threading
-import time
-import logging
-from typing import Dict
-from dataclasses import dataclass
-from lerobot.common.robot_devices.teleop.find_gamepad import find_controller_index
-from lerobot.common.robot_devices.teleop.servo_server import ServoArmServer
-
-
-class RealmanAlohaMaster:
-    def __init__(self, config):
-        self.config = config
-        self._initialize_master_arm()
-    
-    def _initialize_master_arm(self):
-        """初始化主控臂"""
-        try:
-            self.master_dual_arm = ServoArmServer(self.config.config_file)
-        except Exception as e:
-            logging.error(f"初始化主控臂失败: {e}")
-            raise
-
-    def get_action(self) -> Dict:
-        """获取控制动作"""
-        try:
-            master_joint_actions = self.master_dual_arm.get_joint_data()
-            return self._format_action(master_joint_actions)
-        except Exception as e:
-            logging.error(f"获取动作失败: {e}")
-
-    def _format_action(self, master_joint_actions: dict) -> dict:
-        """格式化动作数据"""
-        master_controller_status = {
-            'left': master_joint_actions['left_controller_status'],
-            'right': master_joint_actions['right_controller_status']
-        }
-        
-        return {
-            'control_mode': 'joint',
-            'master_joint_actions': master_joint_actions['dual_joint_actions'],
-            'left_joint_actions': master_joint_actions['left_joint_actions'][:-1],
-            'right_joint_actions': master_joint_actions['right_joint_actions'][:-1],
-            'left_gripper_actions': master_joint_actions['left_joint_actions'][-1],  # 修复bug
-            'right_gripper_actions': master_joint_actions['right_joint_actions'][-1],
-            'master_controller_status': master_controller_status
-        }
-
-    def stop(self):
-        """停止控制器"""
-        try:
-            if hasattr(self, 'master_dual_arm') and self.master_dual_arm:
-                self.master_dual_arm.shutdown()
-            print("混合控制器已退出")
-        except Exception as e:
-            logging.error(f"停止控制器失败: {e}")
-
-
-
-class DummyEndposeMaster:
-    def __init__(self, config):
-        # 初始化pygame
-        pygame.init()
-        pygame.joystick.init()
-        # 获取所有 USB 游戏控制器的信息
-        self.joysticks = find_controller_index()
-        print(self.joysticks)
-        self.control_info = config.end_control_info
-        left_stick = self._init_stick('left')
-        right_stick = self._init_stick('right')
-        self.controllers = [left_stick, right_stick]
-
-    def _init_stick(self, arm_name:str):
-        stick_info = {}
-        stick_info['index'] = self.joysticks[self.control_info[arm_name]]['index']
-        stick_info['guid'] = self.control_info[arm_name]
-        stick_info['name'] = f'{arm_name}'
-        device_name = self.joysticks[self.control_info[arm_name]]['device_name']
-        stick = XboxStick(stick_info) if "Xbox" in device_name else FlightStick(stick_info)
-        stick.start_polling()
-        return stick
-    
-    def get_action(self, state) -> Dict:
-        from copy import deepcopy
-
-        new_state = deepcopy(state)
-        gamepad_action = {}
-        xyz = []
-        rxryrz = []
-        gripper = []
-        """获取控制动作"""
-        try:
-            for i, controller in enumerate(self.controllers):
-                # states = controller.get_raw_states()
-                gamepad_action.update(controller.get_control_signal(controller.name))
-                xyz += [f"{controller.name}_x", f"{controller.name}_y", f"{controller.name}_z"]
-                rxryrz += [f"{controller.name}_joint_4", f"{controller.name}_joint_5", f"{controller.name}_joint_6"]
-                gripper += [f"{controller.name}_gripper"]
-
-            for name in xyz:
-                new_state['pose'][name] += (gamepad_action[name] * gamepad_action['xyz_vel'] * gamepad_action[name.split('_')[0]+'_ratio'])
-            
-            for name in gripper:
-                new_state['joint'][name] += int(gamepad_action[name] * gamepad_action['gripper_vel'] * gamepad_action[name.split('_')[0]+'_ratio'])
-                new_state['joint'][name] = min(990, max(0, new_state['joint'][name]))
-
-            for name in rxryrz:
-                new_state['joint'][name] += (gamepad_action[name] * gamepad_action['rxyz_vel'] * gamepad_action[name.split('_')[0]+'_ratio'])
-
-            new_state['control_mode'] = 'endpose'
-            return new_state
-
-        except Exception as e:
-            logging.error(f"获取动作失败: {e}")
-
-    def stop(self):
-        """停止控制器"""
-        try:
-            # 停止轮询线程
-            for controller in self.controllers:
-                controller.stop_polling()
-        except Exception as e:
-            logging.error(f"停止控制器失败: {e}")
-
-
-
-class ControllerBase:
-    def __init__(self, joystick_info: dict):
-        # 初始化手柄对象
-        self.joystick = pygame.joystick.Joystick(joystick_info['index'])
-        self.joystick.init()
-        self.name = joystick_info['name']
-        self.guid = joystick_info['guid']
-
-        # 存储所有控制器状态的字典
-        self.states = {
-            'buttons': [False] * self.joystick.get_numbuttons(),  # 按钮状态
-            'axes': [0.0] * self.joystick.get_numaxes(),          # 摇杆和轴状态
-            'hats': [(0, 0)] * self.joystick.get_numhats()       # 舵状态
-        }
-
-        # deadzone
-        self.deadzone = 0.15
-        # validzone
-        self.validzone = 0.05
-        self.ratio = 1
-        self.gripper_vel = 100
-        self.rxyz_vel = 5
-        self.xyz_vel = 0.02
-        self.scale_up = 2
-        self.scale_down = 10
-
-        # 线程控制标志
-        self.running = False
-
-    def start_polling(self):
-        """启动线程以轮询控制器状态"""
-        if not self.running:
-            self.running = True
-            self.thread = threading.Thread(target=self._poll_controller)
-            self.thread.start()
-
-    def stop_polling(self):
-        """停止线程"""
-        if self.running:
-            self.running = False
-            self.thread.join()
-
-    def _poll_controller(self):
-        """后台线程函数，用于轮询控制器状态"""
-        while self.running:
-            # 处理pygame事件
-            pygame.event.pump()
-
-            # 获取按钮状态
-            for i in range(self.joystick.get_numbuttons()):
-                self.states['buttons'][i] = self.joystick.get_button(i)
-
-            # 获取摇杆和轴状态（通常范围是 -1.0 到 1.0）
-            for i in range(self.joystick.get_numaxes()):
-                self.states['axes'][i] = self.joystick.get_axis(i)
-
-            # 获取舵状态（通常返回一个元组 (x, y)，值范围为 -1, 0, 1）
-            for i in range(self.joystick.get_numhats()):
-                self.states['hats'][i] = self.joystick.get_hat(i)
-
-            # 控制轮询频率
-            time.sleep(0.01)
-
-    def get_raw_states(self):
-        """获取当前控制器状态"""
-        return self.states
-
-class FlightStick(ControllerBase):
-    def __init__(self, joystick_info):
-        super().__init__(joystick_info)
-
-    def get_x_control_signal(self):
-        x = 0
-        if self.states['axes'][0] > self.validzone:
-            x = 1
-        elif self.states['axes'][0] < -self.validzone:
-            x = -1
-        return x
-    
-    def get_y_control_signal(self):
-        y = 0
-        if self.states['axes'][1] > self.validzone:
-            y = -1
-        elif self.states['axes'][1] < -self.validzone:
-            y = 1
-        return y 
-    
-    def get_z_control_signal(self):
-        z = 0
-        if self.states['buttons'][0]:
-            z = 1
-        elif self.states['buttons'][1]:
-            z = -1
-        return z
-    
-    def get_gripper_control_signal(self):
-        gripper = 0
-        if self.states['buttons'][2] == 1:
-            gripper = 1
-        elif  self.states['buttons'][3] == 1:
-            gripper = -1
-        return gripper
-    
-    def get_ratio_control_signal(self):
-        ratio = self.ratio
-        if self.states['axes'][2] > 0.8:
-            ratio = self.ratio / self.scale_down 
-        elif self.states['axes'][2] < -0.8:
-            ratio = self.ratio * self.scale_up
-        return ratio
-    
-    def get_rx_control_signal(self):
-        rx = 0
-        if self.states['hats'][0][0] == -1:
-            rx = 1
-        elif self.states['hats'][0][0] == 1:
-            rx = -1
-        else:
-            rx = 0
-        return rx
-
-    def get_ry_control_signal(self):
-        ry = 0
-        if self.states['hats'][0][1] == 1:
-            ry = -1
-        elif self.states['hats'][0][1] == -1:
-            ry = 1
-        else:
-            ry = 0
-        return ry
-
-    def get_rz_control_signal(self):
-        rz = 0
-        if self.states['axes'][3] < -self.validzone:
-            rz = -1
-        elif self.states['axes'][3] > self.validzone:
-            rz = 1
-        else:
-            rz = 0
-        return rz
-
-    def get_control_signal(self, prefix: str = ""):
-        """获取所有控制信号"""
-        return {
-            f'{prefix}_x': self.get_x_control_signal(),
-            f'{prefix}_y': self.get_y_control_signal(),
-            f'{prefix}_z': self.get_z_control_signal(),
-            f'{prefix}_joint_4': self.get_rx_control_signal(),
-            f'{prefix}_joint_5': self.get_ry_control_signal(),
-            f'{prefix}_joint_6': self.get_rz_control_signal(),
-            f'{prefix}_gripper': self.get_gripper_control_signal(),
-            f'{prefix}_ratio': self.get_ratio_control_signal(),
-            'gripper_vel': self.gripper_vel,
-            'rxyz_vel': self.rxyz_vel,
-            'xyz_vel': self.xyz_vel
-        }
-
-
-
-class XboxStick(ControllerBase):
-    def __init__(self, joystick_info: dict):
-        super().__init__(joystick_info)
-    
-    def get_x_control_signal(self):
-        """获取 X 轴控制信号"""
-        x = 0
-        if self.states['hats'][0][0] == -1:
-            x = 1
-        elif self.states['hats'][0][0] == 1:
-            x = -1
-        return x
-    
-    def get_y_control_signal(self):
-        """获取 Y 轴控制信号"""
-        y = 0
-        if self.states['hats'][0][1] == 1:
-            y = -1
-        elif self.states['hats'][0][1] == -1:
-            y = 1
-        return y
-    
-    def get_z_control_signal(self):
-        """获取 Z 轴控制信号"""
-        z = 0
-        if self.states['axes'][4] > self.deadzone:  # A 按钮
-            z = -1
-        elif self.states['axes'][4] < -self.deadzone:  # B 按钮
-            z = 1
-        return z
-    
-    def get_ratio_control_signal(self):
-        """获取速度控制信号"""
-        ratio = self.ratio
-        if self.states['axes'][2] > 0.8:  # LT 按钮
-            ratio = self.ratio * self.scale_up
-        elif self.states['axes'][5] > 0.8:  # RT 按钮
-            ratio = self.ratio / self.scale_down
-        return ratio
-    
-    def get_gripper_control_signal(self):
-        gripper = 0
-        if self.states['buttons'][0] == 1:
-            gripper = 1
-        elif  self.states['buttons'][1] == 1:
-            gripper = -1
-        return gripper
-    
-    def get_rx_control_signal(self):
-        """获取 RX 轴控制信号"""
-        rx = 0
-        if self.states['axes'][0] > self.deadzone:  # 左舵
-            rx = -1
-        elif self.states['axes'][0] < -self.deadzone:  # 右舵
-            rx = 1
-        return rx
-    
-    def get_ry_control_signal(self):
-        """获取 RY 轴控制信号"""
-        ry = 0
-        if self.states['axes'][1] > self.deadzone:  # 上舵
-            ry = 1
-        elif self.states['axes'][1] < -self.deadzone:  # 下舵
-            ry = -1
-        return ry
-    
-    def get_rz_control_signal(self):
-        """获取 RZ 轴控制信号"""
-        rz = 0
-        if self.states['buttons'][4] == 1:  # 左摇杆
-            rz = 1
-        elif self.states['buttons'][5] == 1:  # 右摇杆
-            rz = -1
-        return rz
-    
-    def get_control_signal(self, prefix: str = ""):
-        """获取所有控制信号"""
-        return {
-            f'{prefix}_x': self.get_x_control_signal(),
-            f'{prefix}_y': self.get_y_control_signal(),
-            f'{prefix}_z': self.get_z_control_signal(),
-            f'{prefix}_joint_4': self.get_rx_control_signal(),
-            f'{prefix}_joint_5': self.get_ry_control_signal(),
-            f'{prefix}_joint_6': self.get_rz_control_signal(),
-            f'{prefix}_gripper': self.get_gripper_control_signal(),
-            f'{prefix}_ratio': self.get_ratio_control_signal(),
-            'gripper_vel': self.gripper_vel,
-            'rxyz_vel': self.rxyz_vel,
-            'xyz_vel': self.xyz_vel
-        }
-    
-
-@dataclass
-class ControllerConfig:
-    """控制器配置"""
-    init_joint: list
-    init_pose: list
-    max_gripper: int
-    min_gripper: int
-    config_file: str
-    end_control_info: dict 
-
-
-def parse_init_info(init_info: dict) -> ControllerConfig:
-    """解析初始化信息"""
-    return ControllerConfig(
-        init_joint=init_info['init_joint'],
-        init_pose=init_info.get('init_pose', [0]*12),
-        max_gripper=init_info['max_gripper'],
-        min_gripper=init_info['min_gripper'],
-        config_file=init_info['servo_config_file'],
-        end_control_info=init_info['end_control_info']
-    )
-
-
-
-if __name__ == "__main__":
-    config = {
-        'init_joint': {'joint': [-170, 90, 0, 90, 120, 0, 10, 170, 90, 0, -90, 120, 0, 10]},
-        'init_pose': {},
-        'max_gripper': {},
-        'min_gripper': {},
-        'servo_config_file': {},
-        'end_control_info': {'left': "0300b14bff1100003708000010010000" , 'right': '0300509d5e040000120b000009050000'}
-    }
-    config = parse_init_info(config)
-    endpose_arm = DummyEndposeMaster(config)
-    while True:
-        gamepad_action = {}
-        xyz = []
-        for i, controller in enumerate(endpose_arm.controllers):
-            # states = controller.get_raw_states()
-            gamepad_action.update(controller.get_control_signal(controller.name))
-            xyz += [f"{controller.name}_x", f"{controller.name}_y", f"{controller.name}_z"]
-        time.sleep(1)
-        print(gamepad_action)

lerobot/common/robot_devices/teleop/realman_aloha_dual.py

@@ -1,76 +0,0 @@
-import time
-import logging
-from typing import Dict
-from dataclasses import dataclass
-from lerobot.common.robot_devices.teleop.gamepad import RealmanAlohaMaster, DummyEndposeMaster
-
-
-@dataclass
-class ControllerConfig:
-    """控制器配置"""
-    init_joint: list
-    init_pose: list
-    max_gripper: int
-    min_gripper: int
-    config_file: str
-    end_control_info: dict
-
-
-class HybridController:
-    def __init__(self, init_info):
-        self.config = self._parse_init_info(init_info)
-        self.joint = self.config.init_joint.copy()
-        self.pose = self.config.init_pose.copy()
-
-        self.joint_arm = RealmanAlohaMaster(self.config)
-        self.endpose_arm = DummyEndposeMaster(self.config)
-
-    def _parse_init_info(self, init_info: dict) -> ControllerConfig:
-        """解析初始化信息"""
-        return ControllerConfig(
-            init_joint=init_info['init_joint'],
-            init_pose=init_info.get('init_pose', [0]*12),
-            max_gripper=init_info['max_gripper'],
-            min_gripper=init_info['min_gripper'],
-            config_file=init_info['servo_config_file'],
-            end_control_info=init_info['end_control_info']
-        )
-
-    def get_action(self, state) -> Dict:
-        """获取控制动作"""
-        try:
-            endpose_action = self.endpose_arm.get_action(state)
-            return endpose_action
-            # return self.joint_arm.get_action()
-
-        except Exception as e:
-            logging.error(f"获取动作失败: {e}")
-
-    def stop(self):
-        self.joint_arm.stop()
-    
-    def reset(self):
-        """重置控制器"""
-        self.joint = self.config.init_joint.copy()
-        self.pose = self.config.init_pose.copy()
-        self.joint_control_mode = True
-
-
-# 使用示例
-if __name__ == "__main__":
-    init_info = {
-        'init_joint': [-175, 90, 90, 45, 90, -90, 10, 175, 90, 90, -45, 90, 90, 10],
-        'init_pose': [[-0.0305, 0.125938, 0.13153, 3.141, 0.698, -1.57, -0.030486, -0.11487, 0.144707, 3.141, 0.698, 1.57]],
-        'max_gripper': 990,
-        'min_gripper': 10,
-        'servo_config_file': '/home/maic/LYT/lerobot/lerobot/common/robot_devices/teleop/servo_dual.yaml',
-        'end_control_info': {'left': '0300b14bff1100003708000010010000', 'right': '030003f05e0400008e02000010010000'}
-    }
-    arm_controller = HybridController(init_info)
-    time.sleep(1)
-    try:
-        while True:
-            print(arm_controller.get_action())
-            time.sleep(1)
-    except KeyboardInterrupt:
-        arm_controller.stop()

lerobot/common/robot_devices/teleop/realman_mix.yaml

@@ -1,4 +0,0 @@
-init_joint: [-90, 90, 90, -90, -90, 90]
-max_gripper: 990
-min_gripper: 10
-servo_config_file: "/home/maic/LYT/lerobot/lerobot/common/robot_devices/teleop/servo_arm.yaml"

lerobot/common/robot_devices/teleop/realman_single.py

@@ -1,466 +0,0 @@
-import pygame
-import threading
-import time
-import serial
-import binascii
-import logging
-import yaml
-from typing import Dict
-from Robotic_Arm.rm_robot_interface import *
-
-
-
-class ServoArm:
-    def __init__(self, config_file="config.yaml"):
-        """初始化机械臂的串口连接并发送初始数据。
-
-        Args:
-            config_file (str): 配置文件的路径。
-        """
-        self.config = self._load_config(config_file)
-        self.port = self.config["port"]
-        self.baudrate = self.config["baudrate"]
-        self.joint_hex_data = self.config["joint_hex_data"]
-        self.control_hex_data = self.config["control_hex_data"]
-        self.arm_axis = self.config.get("arm_axis", 7)
-
-        try:
-            self.serial_conn = serial.Serial(self.port, self.baudrate, timeout=0)
-            self.bytes_to_send = binascii.unhexlify(self.joint_hex_data.replace(" ", ""))
-            self.serial_conn.write(self.bytes_to_send)
-            time.sleep(1)
-            self.connected = True
-            logging.info(f"串口连接成功: {self.port}")
-        except Exception as e:
-            logging.error(f"串口连接失败: {e}")
-            self.connected = False
-
-    def _load_config(self, config_file):
-        """加载配置文件。
-
-        Args:
-            config_file (str): 配置文件的路径。
-
-        Returns:
-            dict: 配置文件内容。
-        """
-        try:
-            with open(config_file, "r") as file:
-                config = yaml.safe_load(file)
-            return config
-        except Exception as e:
-            logging.error(f"配置文件加载失败: {e}")
-            # 返回默认配置
-            return {
-                "port": "/dev/ttyUSB0",
-                "baudrate": 460800,
-                "joint_hex_data": "55 AA 02 00 00 67",
-                "control_hex_data": "55 AA 08 00 00 B9",
-                "arm_axis": 6
-            }
-
-    def _bytes_to_signed_int(self, byte_data):
-        """将字节数据转换为有符号整数。
-
-        Args:
-            byte_data (bytes): 字节数据。
-
-        Returns:
-            int: 有符号整数。
-        """
-        return int.from_bytes(byte_data, byteorder="little", signed=True)
-
-    def _parse_joint_data(self, hex_received):
-        """解析接收到的十六进制数据并提取关节数据。
-
-        Args:
-            hex_received (str): 接收到的十六进制字符串数据。
-
-        Returns:
-            dict: 解析后的关节数据。
-        """
-        logging.debug(f"hex_received: {hex_received}")
-        joints = {}
-        for i in range(self.arm_axis):
-            start = 14 + i * 10
-            end = start + 8
-            joint_hex = hex_received[start:end]
-            joint_byte_data = bytearray.fromhex(joint_hex)
-            joint_value = self._bytes_to_signed_int(joint_byte_data) / 10000.0
-            joints[f"joint_{i+1}"] = joint_value
-        grasp_start = 14 + self.arm_axis*10
-        grasp_hex = hex_received[grasp_start:grasp_start+8]
-        grasp_byte_data = bytearray.fromhex(grasp_hex)
-        # 夹爪进行归一化处理
-        grasp_value = self._bytes_to_signed_int(grasp_byte_data)/1000
-
-        joints["grasp"] = grasp_value
-        return joints
-    
-    def _parse_controller_data(self, hex_received):
-        status = {
-            'infrared': 0,
-            'button': 0
-        }
-        if len(hex_received) == 18:
-            status['infrared'] = self._bytes_to_signed_int(bytearray.fromhex(hex_received[12:14]))
-            status['button'] = self._bytes_to_signed_int(bytearray.fromhex(hex_received[14:16]))
-            # print(infrared)
-        return status
-
-    def get_joint_actions(self):
-        """从串口读取数据并解析关节动作。
-
-        Returns:
-            dict: 包含关节数据的字典。
-        """
-        if not self.connected:
-            return {}
-        
-        try:
-            self.serial_conn.write(self.bytes_to_send)
-            time.sleep(0.02)
-            bytes_received = self.serial_conn.read(self.serial_conn.inWaiting())
-            if len(bytes_received) == 0:
-                return {}
-            
-            hex_received = binascii.hexlify(bytes_received).decode("utf-8").upper()
-            actions = self._parse_joint_data(hex_received)
-            return actions
-        except Exception as e:
-            logging.error(f"读取串口数据错误: {e}")
-            return {}
-
-    def get_controller_status(self):
-        bytes_to_send = binascii.unhexlify(self.control_hex_data.replace(" ", ""))
-        self.serial_conn.write(bytes_to_send)
-        time.sleep(0.02)
-        bytes_received = self.serial_conn.read(self.serial_conn.inWaiting())
-        hex_received = binascii.hexlify(bytes_received).decode("utf-8").upper()
-        # print("control status:", hex_received)
-        status = self._parse_controller_data(hex_received)
-        return status
-
-    def close(self):
-        """关闭串口连接"""
-        if self.connected and hasattr(self, 'serial_conn'):
-            self.serial_conn.close()
-            self.connected = False
-            logging.info("串口连接已关闭")
-
-
-class HybridController:
-    def __init__(self, init_info):
-        # 初始化pygame和手柄
-        pygame.init()
-        pygame.joystick.init()
-        
-        # 检查是否有连接的手柄
-        if pygame.joystick.get_count() == 0:
-            raise Exception("未检测到手柄")
-        
-        # 初始化手柄
-        self.joystick = pygame.joystick.Joystick(0)
-        self.joystick.init()
-        # 摇杆死区
-        self.deadzone = 0.15
-        # 控制模式: True为关节控制（主模式），False为末端控制
-        self.joint_control_mode = True
-        # 精细控制模式
-        self.fine_control_mode = False
-        
-        # 初始化末端姿态和关节角度
-        self.init_joint = init_info['init_joint']
-        self.init_pose = init_info.get('init_pose', [0]*6)
-        self.max_gripper = init_info['max_gripper']
-        self.min_gripper = init_info['min_gripper']
-        servo_config_file = init_info['servo_config_file']
-        self.joint = self.init_joint.copy()
-        self.pose = self.init_pose.copy()
-        self.pose_speeds = [0.0] * 6
-        self.joint_speeds = [0.0] * 6
-        self.tozero = False
-        
-        # 主臂关节状态
-        self.master_joint_actions = {}
-        self.master_controller_status = {}
-        self.use_master_arm = False
-        
-        # 末端位姿限制
-        self.pose_limits = [
-            (-0.800, 0.800),     # X (m)
-            (-0.800, 0.800),     # Y (m)
-            (-0.800, 0.800),     # Z (m)
-            (-3.14, 3.14),       # RX (rad)
-            (-3.14, 3.14),       # RY (rad)
-            (-3.14, 3.14)        # RZ (rad)
-        ]
-        
-        # 关节角度限制 (度)
-        self.joint_limits = [
-            (-180, 180),  # joint 1
-            (-180, 180),  # joint 2
-            (-180, 180),  # joint 3
-            (-180, 180),  # joint 4
-            (-180, 180),  # joint 5
-            (-180, 180)   # joint 6
-        ]
-        
-        # 控制参数
-        self.linear_step = 0.002      # 线性移动步长(m)
-        self.angular_step = 0.01      # 角度步长(rad)
-        
-        # 夹爪状态和速度
-        self.gripper_speed = 10
-        self.gripper = self.min_gripper
-        
-        # 初始化串口通信（主臂关节状态获取）
-        self.servo_arm = None
-        if servo_config_file:
-            try:
-                self.servo_arm = ServoArm(servo_config_file)
-                self.use_master_arm = True
-                logging.info("串口主臂连接成功，启用主从控制模式")
-            except Exception as e:
-                logging.error(f"串口主臂连接失败: {e}")
-                self.use_master_arm = False
-        
-        # 启动更新线程
-        self.running = True
-        self.thread = threading.Thread(target=self.update_controller)
-        self.thread.start()
-        
-        print("混合控制器已启动")
-        print("主控制模式: 关节控制")
-        if self.use_master_arm:
-            print("主从控制: 启用")
-        print("Back按钮: 切换控制模式(关节/末端)")
-        print("L3按钮: 切换精细控制模式")
-        print("Start按钮: 重置到初始位置")
-    
-    def _apply_nonlinear_mapping(self, value):
-        """应用非线性映射以提高控制精度"""
-        sign = 1 if value >= 0 else -1
-        return sign * (abs(value) ** 2)
-    
-    def _normalize_angle(self, angle):
-        """将角度归一化到[-π, π]范围内"""
-        import math
-        while angle > math.pi:
-            angle -= 2 * math.pi
-        while angle < -math.pi:
-            angle += 2 * math.pi
-        return angle
-    
-    def update_controller(self):
-        while self.running:
-            try:
-                pygame.event.pump()
-            except Exception as e:
-                print(f"控制器错误: {e}")
-                self.stop()
-                continue
-            
-            # 检查控制模式切换 (Back按钮)
-            if self.joystick.get_button(6):  # Back按钮
-                self.joint_control_mode = not self.joint_control_mode
-                mode_str = "关节控制" if self.joint_control_mode else "末端位姿控制"
-                print(f"切换到{mode_str}模式")
-                time.sleep(0.3)  # 防止多次触发
-            
-            # 检查精细控制模式切换 (L3按钮)
-            if self.joystick.get_button(10):  # L3按钮
-                self.fine_control_mode = not self.fine_control_mode
-                print(f"切换到{'精细' if self.fine_control_mode else '普通'}控制模式")
-                time.sleep(0.3)  # 防止多次触发
-            
-            # 检查重置按钮 (Start按钮)
-            if self.joystick.get_button(7):  # Start按钮
-                print("重置机械臂到初始位置...")
-                # print("init_joint", self.init_joint.copy())
-                self.tozero = True
-                self.joint = self.init_joint.copy()
-                self.pose = self.init_pose.copy()
-                self.pose_speeds = [0.0] * 6
-                self.joint_speeds = [0.0] * 6
-                self.gripper_speed = 10
-                self.gripper = self.min_gripper
-                print("机械臂已重置到初始位置")
-                time.sleep(0.3)  # 防止多次触发
-
-            # 从串口获取主臂关节状态
-            if self.servo_arm and self.servo_arm.connected:
-                try:
-                    self.master_joint_actions = self.servo_arm.get_joint_actions()
-                    self.master_controller_status = self.servo_arm.get_controller_status()
-                    if self.master_joint_actions:
-                        logging.debug(f"主臂关节状态: {self.master_joint_actions}")
-                    
-                except Exception as e:
-                    logging.error(f"获取主臂状态错误: {e}")
-                    self.master_joint_actions = {}
-            # print(self.master_joint_actions)
-
-            # 根据控制模式更新相应的控制逻辑
-            if self.joint_control_mode:
-                # 关节控制模式下，优先使用主臂数据，Xbox作为辅助
-                self.update_joint_control()
-            else:
-                # 末端控制模式，使用Xbox控制
-                self.update_end_pose()
-            time.sleep(0.02)
-            # print('gripper:', self.gripper)
-    
-    def update_joint_control(self):
-        """更新关节角度控制 - 优先使用主臂数据"""
-        if self.use_master_arm and self.master_joint_actions:
-            # 主从控制模式：直接使用主臂的关节角度
-            try:
-                # 将主臂关节角度映射到从臂
-                for i in range(6):  # 假设只有6个关节需要控制
-                    joint_key = f"joint_{i+1}"
-                    if joint_key in self.master_joint_actions:
-                        # 直接使用主臂的关节角度（已经是度数）
-                        self.joint[i] = self.master_joint_actions[joint_key]
-                        
-                        # 应用关节限制
-                        min_val, max_val = self.joint_limits[i]
-                        self.joint[i] = max(min_val, min(max_val, self.joint[i]))
-                
-                # print(self.joint)
-                logging.debug(f"主臂关节映射到从臂: {self.joint[:6]}")
-                
-            except Exception as e:
-                logging.error(f"主臂数据映射错误: {e}")
-        
-        # 如果有主臂夹爪数据，使用主臂夹爪状态
-        if self.use_master_arm and "grasp" in self.master_joint_actions:
-            self.gripper = self.master_joint_actions["grasp"] * 1000
-            self.joint[-1] = self.gripper
-    
-
-    def update_end_pose(self):
-        """更新末端位姿控制"""
-        # 根据控制模式调整步长
-        current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0)
-        current_angular_step = self.angular_step * (0.1 if self.fine_control_mode else 1.0)
-        
-        # 方向键控制XY
-        hat = self.joystick.get_hat(0)
-        hat_up = hat[1] == 1     # Y+
-        hat_down = hat[1] == -1  # Y-
-        hat_left = hat[0] == -1  # X-
-        hat_right = hat[0] == 1  # X+
-        
-        # 右摇杆控制Z
-        right_y_raw = -self.joystick.get_axis(4)
-        # 左摇杆控制RZ
-        left_y_raw = -self.joystick.get_axis(1)
-        
-        # 应用死区
-        right_y = 0.0 if abs(right_y_raw) < self.deadzone else right_y_raw
-        left_y = 0.0 if abs(left_y_raw) < self.deadzone else left_y_raw
-
-        # 计算各轴速度
-        self.pose_speeds[1] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0)  # Y
-        self.pose_speeds[0] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0)  # X
-        
-        # 设置Z速度（右摇杆Y轴控制）
-        z_mapping = self._apply_nonlinear_mapping(right_y)
-        self.pose_speeds[2] = z_mapping * current_linear_step  # Z
-        
-        # L1/R1控制RX旋转
-        LB = self.joystick.get_button(4)  # RX-
-        RB = self.joystick.get_button(5)  # RX+
-        self.pose_speeds[3] = (-current_angular_step if LB else (current_angular_step if RB else 0.0))
-        
-        # △/□控制RY旋转
-        triangle = self.joystick.get_button(2)  # RY+
-        square = self.joystick.get_button(3)    # RY-
-        self.pose_speeds[4] = (current_angular_step if triangle else (-current_angular_step if square else 0.0))
-        
-        # 左摇杆Y轴控制RZ旋转
-        rz_mapping = self._apply_nonlinear_mapping(left_y)
-        self.pose_speeds[5] = rz_mapping * current_angular_step * 2  # RZ
-        
-        # 夹爪控制（圈/叉）
-        circle = self.joystick.get_button(1)  # 夹爪开
-        cross = self.joystick.get_button(0)   # 夹爪关
-        if circle:
-            self.gripper = min(self.max_gripper, self.gripper + self.gripper_speed)
-        elif cross:
-            self.gripper = max(self.min_gripper, self.gripper - self.gripper_speed)
-
-        # 更新末端位姿
-        for i in range(6):
-            self.pose[i] += self.pose_speeds[i]
-        
-        # 角度归一化处理
-        for i in range(3, 6):
-            self.pose[i] = self._normalize_angle(self.pose[i])
-
-    def update_joint_state(self, joint):
-        self.joint = joint
-        # self.tozero = False
-
-    def update_endpose_state(self, end_pose):
-        self.pose = end_pose
-        # self.tozero = False
-    
-    def update_tozero_state(self, tozero):
-        self.tozero = tozero
-
-            
-    def get_action(self) -> Dict:
-        """获取当前控制命令"""
-        return {
-            'control_mode': 'joint' if self.joint_control_mode else 'end_pose',
-            'use_master_arm': self.use_master_arm,
-            'master_joint_actions': self.master_joint_actions,
-            'master_controller_status': self.master_controller_status,
-            'end_pose': self.pose,
-            'joint_angles': self.joint,
-            'gripper': int(self.gripper),
-            'tozero': self.tozero
-        }
-    
-    def stop(self):
-        """停止控制器"""
-        self.running = False
-        if self.thread.is_alive():
-            self.thread.join()
-        if self.servo_arm:
-            self.servo_arm.close()
-        pygame.quit()
-        print("混合控制器已退出")
-    
-    def reset(self):
-        """重置到初始状态"""
-        self.joint = self.init_joint.copy()
-        self.pose = self.init_pose.copy()
-        self.pose_speeds = [0.0] * 6
-        self.joint_speeds = [0.0] * 6
-        self.gripper_speed = 10
-        self.gripper = self.min_gripper
-        print("已重置到初始状态")
-
-
-# 使用示例
-if __name__ == "__main__":
-    # 初始化睿尔曼机械臂
-    arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
-    # 创建机械臂连接
-    handle = arm.rm_create_robot_arm("192.168.3.18", 8080)
-    print(f"机械臂连接ID: {handle.id}")
-    init_pose = arm.rm_get_current_arm_state()[1]['pose']
-
-    with open('/home/maic/LYT/lerobot/lerobot/common/robot_devices/teleop/realman_mix.yaml', "r") as file:
-        config = yaml.safe_load(file)
-    config['init_pose'] = init_pose
-    arm_controller = HybridController(config)
-    try:
-        while True:
-            print(arm_controller.get_action())
-            time.sleep(0.1)
-    except KeyboardInterrupt:
-        arm_controller.stop()

lerobot/common/robot_devices/teleop/servo_arm.yaml

@@ -1,6 +0,0 @@
-port: /dev/ttyUSB0
-right_port: /dev/ttyUSB1
-baudrate: 460800
-joint_hex_data: "55 AA 02 00 00 67"
-control_hex_data: "55 AA 08 00 00 B9"
-arm_axis: 6

lerobot/common/robot_devices/teleop/servo_dual.yaml

@@ -1,6 +0,0 @@
-left_port: /dev/ttyUSB0
-right_port: /dev/ttyUSB1
-baudrate: 460800
-joint_hex_data: "55 AA 02 00 00 67"
-control_hex_data: "55 AA 08 00 00 B9"
-arm_axis: 6

lerobot/common/robot_devices/teleop/servo_server.py

@@ -1,321 +0,0 @@
-import threading
-import time
-import serial
-import binascii
-import logging
-import yaml
-from typing import Dict
-
-# logging.basicConfig(
-#     level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s"
-# )
-
-
-class ServoArmServer:
-    def __init__(self, config_file="servo_dual.yaml"):
-        """初始化服务器，创建左右机械臂实例"""
-        self.config_file = config_file
-        self.left_servo_arm = None
-        self.right_servo_arm = None
-        self.running = False
-        self.data_lock = threading.Lock()
-        self.latest_data = {
-            'left_joint_actions': {},
-            'right_joint_actions': {},
-            'left_controller_status': {},
-            'right_controller_status': {},
-            'timestamp': time.time()
-        }
-        
-        # 初始化机械臂
-        self._initialize_arms()
-        # 启动数据采集线程
-        self._start_data_collection()
-
-
-    def _initialize_arms(self):
-        """初始化左右机械臂"""
-        try:
-            self.left_servo_arm = ServoArm(self.config_file, "left_port")
-            logging.info("左master机械臂初始化成功")
-        except Exception as e:
-            logging.error(f"左master机械臂初始化失败: {e}")
-            
-        try:
-            self.right_servo_arm = ServoArm(self.config_file, "right_port")
-            logging.info("右master机械臂初始化成功")
-        except Exception as e:
-            logging.error(f"右master机械臂初始化失败: {e}")
-    
-    def _start_data_collection(self):
-        """启动数据采集线程"""
-        self.running = True
-        
-        # 创建左臂数据采集线程
-        self.left_data_thread = threading.Thread(target=self._left_arm_data_loop)
-        self.left_data_thread.daemon = True
-        self.left_data_thread.start()
-        
-        # 创建右臂数据采集线程
-        self.right_data_thread = threading.Thread(target=self._right_arm_data_loop)
-        self.right_data_thread.daemon = True
-        self.right_data_thread.start()
-        
-        logging.info("左右机械臂数据采集线程已启动")
-    
-    def _left_arm_data_loop(self):
-        """左机械臂数据采集循环"""
-        while self.running:
-            try:
-                left_actions = {}
-                left_controller_status = {}
-                
-                # 获取左机械臂数据
-                if self.left_servo_arm and self.left_servo_arm.connected:
-                    left_actions = self.left_servo_arm.get_joint_actions()
-                    left_controller_status = self.left_servo_arm.get_controller_status()
-                
-                if self._check_val_safety(left_actions) == False:
-                    time.sleep(0.02)
-                    continue
-                # 更新左机械臂数据
-                with self.data_lock:
-                    self.latest_data['left_joint_actions'] = [left_actions[k] for k in left_actions]
-                    self.latest_data['left_controller_status'] = left_controller_status
-                    # 更新dual_joint_actions
-                    if self.latest_data['right_joint_actions']:
-                        self.latest_data['dual_joint_actions'] = self.latest_data['left_joint_actions'] + self.latest_data['right_joint_actions']
-                    self.latest_data['timestamp'] = time.time()
-                
-                time.sleep(0.02)  # 50Hz采集频率
-                
-            except Exception as e:
-                logging.error(f"左机械臂数据采集错误: {e}")
-                time.sleep(0.1)
-
-    def _right_arm_data_loop(self):
-        """右机械臂数据采集循环"""
-        while self.running:
-            try:
-                right_actions = {}
-                right_controller_status = {}
-                
-                # 获取右机械臂数据
-                if self.right_servo_arm and self.right_servo_arm.connected:
-                    right_actions = self.right_servo_arm.get_joint_actions()
-                    right_controller_status = self.right_servo_arm.get_controller_status()
-                
-                if self._check_val_safety(right_actions) == False:
-                    time.sleep(0.02)
-                    continue
-                # 更新右机械臂数据
-                with self.data_lock:
-                    self.latest_data['right_joint_actions'] = [right_actions[k] for k in right_actions]
-                    self.latest_data['right_controller_status'] = right_controller_status
-                    # 更新dual_joint_actions
-                    if self.latest_data['left_joint_actions']:
-                        self.latest_data['dual_joint_actions'] = self.latest_data['left_joint_actions'] + self.latest_data['right_joint_actions']
-                    self.latest_data['timestamp'] = time.time()
-                
-                time.sleep(0.02)  # 50Hz采集频率
-                
-            except Exception as e:
-                logging.error(f"右机械臂数据采集错误: {e}")
-                time.sleep(0.1)
-    
-    def _check_val_safety(self, data: dict):
-        data = [data[k] for k in data]
-        ret = True
-        if len(data) != self.left_servo_arm.arm_axis + 1:
-            ret = False
-        for v in data:
-            if v > 180 or v < -180:
-                ret = False
-        return ret
-
-    # ZeroRPC 服务方法
-    def get_joint_data(self):
-        """获取最新的关节数据"""
-        with self.data_lock:
-            return self.latest_data.copy()
-    
-    def get_left_joint_actions(self):
-        """获取左机械臂关节数据和控制器状态"""
-        with self.data_lock:
-            return {
-                'data': self.latest_data['left_joint_actions'],
-                'controller_status': self.latest_data['left_controller_status'],
-                'timestamp': self.latest_data['timestamp']
-            }
-    
-    def get_right_joint_actions(self):
-        """获取右机械臂关节数据和控制器状态"""
-        with self.data_lock:
-            return {
-                'data': self.latest_data['right_joint_actions'],
-                'controller_status': self.latest_data['right_controller_status'],
-                'timestamp': self.latest_data['timestamp']
-            }
-    
-    def get_connection_status(self):
-        """获取连接状态"""
-        return {
-            'left_connected': self.left_servo_arm.connected if self.left_servo_arm else False,
-            'right_connected': self.right_servo_arm.connected if self.right_servo_arm else False,
-            'server_running': self.running
-        }
-    
-    def ping(self):
-        """测试连接"""
-        return "pong"
-    
-    def shutdown(self):
-        """关闭服务器"""
-        logging.info("正在关闭服务器...")
-        self.running = False
-        
-        if self.left_servo_arm:
-            self.left_servo_arm.close()
-        if self.right_servo_arm:
-            self.right_servo_arm.close()
-            
-        return "Server shutdown"
-
-
-class ServoArm:
-    def __init__(self, config_file="config.yaml", port_name="left_port"):
-        """初始化机械臂的串口连接并发送初始数据。
-
-        Args:
-            config_file (str): 配置文件的路径。
-        """
-        self.config = self._load_config(config_file)
-        self.port = self.config[port_name]
-        self.baudrate = self.config["baudrate"]
-        self.joint_hex_data = self.config["joint_hex_data"]
-        self.control_hex_data = self.config["control_hex_data"]
-        self.arm_axis = self.config.get("arm_axis", 7)
-
-        try:
-            self.serial_conn = serial.Serial(self.port, self.baudrate, timeout=0)
-            self.bytes_to_send = binascii.unhexlify(self.joint_hex_data.replace(" ", ""))
-            self.serial_conn.write(self.bytes_to_send)
-            time.sleep(1)
-            self.connected = True
-            logging.info(f"串口连接成功: {self.port}")
-        except Exception as e:
-            logging.error(f"串口连接失败: {e}")
-            self.connected = False
-
-    def _load_config(self, config_file):
-        """加载配置文件。
-
-        Args:
-            config_file (str): 配置文件的路径。
-
-        Returns:
-            dict: 配置文件内容。
-        """
-        try:
-            with open(config_file, "r") as file:
-                config = yaml.safe_load(file)
-            return config
-        except Exception as e:
-            logging.error(f"配置文件加载失败: {e}")
-            # 返回默认配置
-            return {
-                "port": "/dev/ttyUSB0",
-                "baudrate": 460800,
-                "joint_hex_data": "55 AA 02 00 00 67",
-                "control_hex_data": "55 AA 08 00 00 B9",
-                "arm_axis": 6
-            }
-
-    def _bytes_to_signed_int(self, byte_data):
-        """将字节数据转换为有符号整数。
-
-        Args:
-            byte_data (bytes): 字节数据。
-
-        Returns:
-            int: 有符号整数。
-        """
-        return int.from_bytes(byte_data, byteorder="little", signed=True)
-
-    def _parse_joint_data(self, hex_received):
-        """解析接收到的十六进制数据并提取关节数据。
-
-        Args:
-            hex_received (str): 接收到的十六进制字符串数据。
-
-        Returns:
-            dict: 解析后的关节数据。
-        """
-        logging.debug(f"hex_received: {hex_received}")
-        joints = {}
-        for i in range(self.arm_axis):
-            start = 14 + i * 10
-            end = start + 8
-            joint_hex = hex_received[start:end]
-            joint_byte_data = bytearray.fromhex(joint_hex)
-            joint_value = self._bytes_to_signed_int(joint_byte_data) / 10000.0
-            joints[f"joint_{i+1}"] = joint_value #/ 180
-        grasp_start = 14 + self.arm_axis*10
-        grasp_hex = hex_received[grasp_start:grasp_start+8]
-        grasp_byte_data = bytearray.fromhex(grasp_hex)
-        # 夹爪进行归一化处理
-        grasp_value = self._bytes_to_signed_int(grasp_byte_data)/1000
-
-        joints["grasp"] = grasp_value
-        return joints
-    
-    def _parse_controller_data(self, hex_received):
-        status = {
-            'infrared': 0,
-            'button': 0
-        }
-        if len(hex_received) == 18:
-            status['infrared'] = self._bytes_to_signed_int(bytearray.fromhex(hex_received[12:14]))
-            status['button'] = self._bytes_to_signed_int(bytearray.fromhex(hex_received[14:16]))
-            # print(infrared)
-        return status
-
-    def get_joint_actions(self):
-        """从串口读取数据并解析关节动作。
-
-        Returns:
-            dict: 包含关节数据的字典。
-        """
-        if not self.connected:
-            return {}
-        
-        try:
-            self.serial_conn.write(self.bytes_to_send)
-            time.sleep(0.025)
-            bytes_received = self.serial_conn.read(self.serial_conn.inWaiting())
-            if len(bytes_received) == 0:
-                return {}
-            
-            hex_received = binascii.hexlify(bytes_received).decode("utf-8").upper()
-            actions = self._parse_joint_data(hex_received)
-            return actions
-        except Exception as e:
-            logging.error(f"读取串口数据错误: {e}")
-            return {}
-
-    def get_controller_status(self):
-        bytes_to_send = binascii.unhexlify(self.control_hex_data.replace(" ", ""))
-        self.serial_conn.write(bytes_to_send)
-        time.sleep(0.025)
-        bytes_received = self.serial_conn.read(self.serial_conn.inWaiting())
-        hex_received = binascii.hexlify(bytes_received).decode("utf-8").upper()
-        # print("control status:", hex_received)
-        status = self._parse_controller_data(hex_received)
-        return status
-
-    def close(self):
-        """关闭串口连接"""
-        if self.connected and hasattr(self, 'serial_conn'):
-            self.serial_conn.close()
-            self.connected = False
-            logging.info("串口连接已关闭")

lerobot/common/robots/__init__.py

@@ -0,0 +1,3 @@
+from .config import RobotConfig
+from .robot import Robot
+from .utils import make_robot_from_config

lerobot/common/robots/config.py

@@ -0,0 +1,42 @@
+# 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 abc
+from dataclasses import dataclass
+from pathlib import Path
+
+import draccus
+
+
+@dataclass(kw_only=True)
+class RobotConfig(draccus.ChoiceRegistry, abc.ABC):
+    # Allows to distinguish between different robots of the same type
+    id: str | None = None
+    # Directory to store calibration file
+    calibration_dir: Path | None = None
+
+    def __post_init__(self):
+        if hasattr(self, "cameras"):
+            cameras = self.cameras
+            if cameras:
+                for cam_name, cam_config in cameras.items():
+                    for attr in ["width", "height", "fps"]:
+                        if getattr(cam_config, attr) is None:
+                            raise ValueError(
+                                f"Camera config for '{cam_name}' has None value for required attribute '{attr}'"
+                            )
+
+    @property
+    def type(self) -> str:
+        return self.get_choice_name(self.__class__)

lerobot/common/robots/koch_follower/README.md

@@ -0,0 +1,328 @@
+# Using the [Koch v1.1](https://github.com/jess-moss/koch-v1-1) with LeRobot
+
+## Table of Contents
+
+  - [A. Order and Assemble the parts](#a-order-and-assemble-the-parts)
+  - [B. Install LeRobot](#b-install-lerobot)
+  - [C. Configure the Motors](#c-configure-the-motors)
+  - [D. Calibrate](#d-calibrate)
+  - [E. Teleoperate](#e-teleoperate)
+  - [F. Record a dataset](#f-record-a-dataset)
+  - [G. Visualize a dataset](#g-visualize-a-dataset)
+  - [H. Replay an episode](#h-replay-an-episode)
+  - [I. Train a policy](#i-train-a-policy)
+  - [J. Evaluate your policy](#j-evaluate-your-policy)
+  - [K. More Information](#k-more-information)
+
+## A. Order and Assemble the parts
+
+Follow the sourcing and assembling instructions provided on the [Koch v1.1 Github page](https://github.com/jess-moss/koch-v1-1). This will guide you through setting up both the follower and leader arms, as shown in the image below.
+
+<div style="text-align:center;">
+  <img src="../media/tutorial/koch_v1_1_leader_follower.webp?raw=true" alt="Koch v1.1 leader and follower arms" title="Koch v1.1 leader and follower arms" width="50%">
+</div>
+
+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
+```
+
+Note that the command above is equivalent to running the following script:
+<details>
+<summary>Setup script</summary>
+
+  ```python
+  from lerobot.common.teleoperators.koch import KochLeader, KochLeaderConfig
+
+  config = KochLeaderConfig(
+      port="/dev/tty.usbmodem575E0031751",
+  )
+  leader = KochLeader(config)
+  leader.setup_motors()
+  ```
+</details>
+
+
+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.
+
+
+<details>
+<summary>Troubleshooting</summary>
+
+  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).
+</details>
+
+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                                                                                                                                             |
+| ------------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| <img src="../media/so100/follower_zero.webp?raw=true" alt="SO-100 follower arm zero position" title="SO-100 follower arm zero position" style="width:100%;"> | <img src="../media/so100/follower_rotated.webp?raw=true" alt="SO-100 follower arm rotated position" title="SO-100 follower arm rotated position" style="width:100%;"> | <img src="../media/so100/follower_rest.webp?raw=true" alt="SO-100 follower arm rest position" title="SO-100 follower arm rest position" style="width:100%;"> |
+
+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                                                                                                                                       |
+| ------------------------------------------------------------------------------------------------------------------------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| <img src="../media/so100/leader_zero.webp?raw=true" alt="SO-100 leader arm zero position" title="SO-100 leader arm zero position" style="width:100%;"> | <img src="../media/so100/leader_rotated.webp?raw=true" alt="SO-100 leader arm rotated position" title="SO-100 leader arm rotated position" style="width:100%;"> | <img src="../media/so100/leader_rest.webp?raw=true" alt="SO-100 leader arm rest position" title="SO-100 leader arm rest position" style="width:100%;"> |
+
+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).

lerobot/common/robots/koch_follower/__init__.py

@@ -0,0 +1,2 @@
+from .config_koch_follower import KochFollowerConfig
+from .koch_follower import KochFollower

lerobot/common/robots/koch_follower/config_koch_follower.py

@@ -0,0 +1,36 @@
+# 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 import CameraConfig
+
+from ..config import RobotConfig
+
+
+@RobotConfig.register_subclass("koch_follower")
+@dataclass
+class KochFollowerConfig(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)

lerobot/common/robots/koch_follower/koch_follower.py

@@ -0,0 +1,233 @@
+#!/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.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 (
+    DynamixelMotorsBus,
+    OperatingMode,
+)
+
+from ..robot import Robot
+from ..utils import ensure_safe_goal_position
+from .config_koch_follower import KochFollowerConfig
+
+logger = logging.getLogger(__name__)
+
+
+class KochFollower(Robot):
+    """
+    - [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
+    """
+
+    config_class = KochFollowerConfig
+    name = "koch_follower"
+
+    def __init__(self, config: KochFollowerConfig):
+        super().__init__(config)
+        self.config = config
+        self.bus = DynamixelMotorsBus(
+            port=self.config.port,
+            motors={
+                "shoulder_pan": Motor(1, "xl430-w250", MotorNormMode.RANGE_M100_100),
+                "shoulder_lift": Motor(2, "xl430-w250", MotorNormMode.RANGE_M100_100),
+                "elbow_flex": Motor(3, "xl330-m288", MotorNormMode.RANGE_M100_100),
+                "wrist_flex": Motor(4, "xl330-m288", MotorNormMode.RANGE_M100_100),
+                "wrist_roll": Motor(5, "xl330-m288", MotorNormMode.RANGE_M100_100),
+                "gripper": Motor(6, "xl330-m288", 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.bus.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.bus.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.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}")
+        self.bus.disable_torque()
+        for motor in self.bus.motors:
+            self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
+
+        input(f"Move {self} to the middle of its range of motion and press ENTER....")
+        homing_offsets = self.bus.set_half_turn_homings()
+
+        full_turn_motors = ["shoulder_pan", "wrist_roll"]
+        unknown_range_motors = [motor for motor in self.bus.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.bus.record_ranges_of_motion(unknown_range_motors)
+        for motor in full_turn_motors:
+            range_mins[motor] = 0
+            range_maxes[motor] = 4095
+
+        self.calibration = {}
+        for motor, m in self.bus.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.bus.write_calibration(self.calibration)
+        self._save_calibration()
+        logger.info(f"Calibration saved to {self.calibration_fpath}")
+
+    def configure(self) -> None:
+        with self.bus.torque_disabled():
+            self.bus.configure_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
+            for motor in self.bus.motors:
+                if motor != "gripper":
+                    self.bus.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
+            # 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.
+            self.bus.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value)
+
+            # 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.bus.write("Position_P_Gain", "elbow_flex", 1500)
+            self.bus.write("Position_I_Gain", "elbow_flex", 0)
+            self.bus.write("Position_D_Gain", "elbow_flex", 600)
+
+    def setup_motors(self) -> None:
+        for motor in reversed(self.bus.motors):
+            input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+            self.bus.setup_motor(motor)
+            print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
+
+    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.bus.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, float]) -> dict[str, float]:
+        """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 (dict[str, float]): The goal positions for the motors.
+
+        Returns:
+            dict[str, float]: 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.bus.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.bus.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.bus.disconnect(self.config.disable_torque_on_disconnect)
+        for cam in self.cameras.values():
+            cam.disconnect()
+
+        logger.info(f"{self} disconnected.")

lerobot/common/robots/lekiwi/README.md

@@ -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.

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

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

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.")

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

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()

lerobot/common/robots/robot.py

@@ -0,0 +1,105 @@
+# 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 abc
+from pathlib import Path
+from typing import Any
+
+import draccus
+
+from lerobot.common.constants import HF_LEROBOT_CALIBRATION, ROBOTS
+from lerobot.common.motors import MotorCalibration
+
+from .config import RobotConfig
+
+
+# TODO(aliberts): action/obs typing such as Generic[ObsType, ActType] similar to gym.Env ?
+# https://github.com/Farama-Foundation/Gymnasium/blob/3287c869f9a48d99454306b0d4b4ec537f0f35e3/gymnasium/core.py#L23
+class Robot(abc.ABC):
+    """The main LeRobot class for implementing robots."""
+
+    # Set these in ALL subclasses
+    config_class: RobotConfig
+    name: str
+
+    def __init__(self, config: RobotConfig):
+        self.robot_type = self.name
+        self.id = config.id
+        self.calibration_dir = (
+            config.calibration_dir if config.calibration_dir else HF_LEROBOT_CALIBRATION / ROBOTS / self.name
+        )
+        self.calibration_dir.mkdir(parents=True, exist_ok=True)
+        self.calibration_fpath = self.calibration_dir / f"{self.id}.json"
+        self.calibration: dict[str, MotorCalibration] = {}
+        if self.calibration_fpath.is_file():
+            self._load_calibration()
+
+    def __str__(self) -> str:
+        return f"{self.id} {self.__class__.__name__}"
+
+    # TODO(aliberts): create a proper Feature class for this that links with datasets
+    @abc.abstractproperty
+    def observation_features(self) -> dict:
+        pass
+
+    @abc.abstractproperty
+    def action_features(self) -> dict:
+        pass
+
+    @abc.abstractproperty
+    def is_connected(self) -> bool:
+        pass
+
+    @abc.abstractmethod
+    def connect(self, calibrate: bool = True) -> None:
+        """Connects to the robot."""
+        pass
+
+    @abc.abstractproperty
+    def is_calibrated(self) -> bool:
+        pass
+
+    @abc.abstractmethod
+    def calibrate(self) -> None:
+        """Calibrates the robot."""
+        pass
+
+    def _load_calibration(self, fpath: Path | None = None) -> None:
+        fpath = self.calibration_fpath if fpath is None else fpath
+        with open(fpath) as f, draccus.config_type("json"):
+            self.calibration = draccus.load(dict[str, MotorCalibration], f)
+
+    def _save_calibration(self, fpath: Path | None = None) -> None:
+        fpath = self.calibration_fpath if fpath is None else fpath
+        with open(fpath, "w") as f, draccus.config_type("json"):
+            draccus.dump(self.calibration, f, indent=4)
+
+    @abc.abstractmethod
+    def configure(self) -> None:
+        pass
+
+    @abc.abstractmethod
+    def get_observation(self) -> dict[str, Any]:
+        """Gets observation from the robot."""
+        pass
+
+    @abc.abstractmethod
+    def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
+        """Sends actions to the robot."""
+        pass
+
+    @abc.abstractmethod
+    def disconnect(self) -> None:
+        """Disconnects from the robot."""
+        pass

lerobot/common/robots/so100_follower/README.md

@@ -193,7 +193,7 @@ python lerobot/scripts/configure_motor.py \
   --brand feetech \
   --model sts3215 \
   --baudrate 1000000 \
-  --ID 1
+  --id 1
 ```
 
 > [!NOTE]
@@ -206,7 +206,7 @@ python lerobot/scripts/configure_motor.py \
   --brand feetech \
   --model sts3215 \
   --baudrate 1000000 \
-  --ID 2
+  --id 2
 ```
 
 Redo the process for all your motors until ID 6. Do the same for the 6 motors of the leader arm.

lerobot/common/robots/so100_follower/__init__.py

@@ -0,0 +1,2 @@
+from .config_so100_follower import SO100FollowerConfig
+from .so100_follower import SO100Follower

lerobot/common/robots/so100_follower/config_so100_follower.py

@@ -0,0 +1,36 @@
+# 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 import CameraConfig
+
+from ..config import RobotConfig
+
+
+@RobotConfig.register_subclass("so100_follower")
+@dataclass
+class SO100FollowerConfig(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)

lerobot/common/robots/so100_follower/so100_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 .config_so100_follower import SO100FollowerConfig
+
+logger = logging.getLogger(__name__)
+
+
+class SO100Follower(Robot):
+    """
+    [SO-100 Follower Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio
+    """
+
+    config_class = SO100FollowerConfig
+    name = "so100_follower"
+
+    def __init__(self, config: SO100FollowerConfig):
+        super().__init__(config)
+        self.config = config
+        self.bus = 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.bus.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.bus.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.bus.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.bus.is_calibrated
+
+    def calibrate(self) -> None:
+        logger.info(f"\nRunning calibration of {self}")
+        self.bus.disable_torque()
+        for motor in self.bus.motors:
+            self.bus.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.bus.set_half_turn_homings()
+
+        full_turn_motor = "wrist_roll"
+        unknown_range_motors = [motor for motor in self.bus.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.bus.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.bus.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.bus.write_calibration(self.calibration)
+        self._save_calibration()
+        print("Calibration saved to", self.calibration_fpath)
+
+    def configure(self) -> None:
+        with self.bus.torque_disabled():
+            self.bus.configure_motors()
+            for motor in self.bus.motors:
+                self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+                # Set P_Coefficient to lower value to avoid shakiness (Default is 32)
+                self.bus.write("P_Coefficient", motor, 16)
+                # Set I_Coefficient and D_Coefficient to default value 0 and 32
+                self.bus.write("I_Coefficient", motor, 0)
+                self.bus.write("D_Coefficient", motor, 32)
+
+    def setup_motors(self) -> None:
+        for motor in reversed(self.bus.motors):
+            input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+            self.bus.setup_motor(motor)
+            print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
+
+    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.bus.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.bus.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.bus.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.bus.disconnect(self.config.disable_torque_on_disconnect)
+        for cam in self.cameras.values():
+            cam.disconnect()
+
+        logger.info(f"{self} disconnected.")

lerobot/common/robots/so101_follower/README.md

@@ -61,7 +61,7 @@ conda install ffmpeg -c conda-forge
 
 Install 🤗 LeRobot:
 ```bash
-cd lerobot && pip install -e ".[feetech]"
+cd lerobot && pip install ".[feetech]"
 ```
 
 > [!NOTE]

lerobot/common/robots/so101_follower/__init__.py

@@ -0,0 +1,2 @@
+from .config_so101_follower import SO101FollowerConfig
+from .so101_follower import SO101Follower

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)