Add SO101

lerobot/common/teleoperators/so101_leader/__init__.py

@@ -0,0 +1,2 @@
+from .config_so101_leader import SO101LeaderConfig
+from .so101_leader import SO101Leader

lerobot/common/teleoperators/so101_leader/config_so101_leader.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from dataclasses import dataclass
+
+from ..config import TeleoperatorConfig
+
+
+@TeleoperatorConfig.register_subclass("so101_leader")
+@dataclass
+class SO101LeaderConfig(TeleoperatorConfig):
+    # Port to connect to the arm
+    port: str

lerobot/common/teleoperators/so101_leader/so101_leader.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import logging
+import time
+
+from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
+from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode
+from lerobot.common.motors.feetech import (
+    FeetechMotorsBus,
+    OperatingMode,
+)
+
+from ..teleoperator import Teleoperator
+from .config_so101_leader import SO101LeaderConfig
+
+logger = logging.getLogger(__name__)
+
+
+class SO101Leader(Teleoperator):
+    """
+    SO-101 Leader Arm designed by TheRobotStudio and Hugging Face.
+    """
+
+    config_class = SO101LeaderConfig
+    name = "so101_leader"
+
+    def __init__(self, config: SO101LeaderConfig):
+        super().__init__(config)
+        self.config = config
+        self.arm = FeetechMotorsBus(
+            port=self.config.port,
+            motors={
+                "shoulder_pan": Motor(1, "sts3215", MotorNormMode.RANGE_M100_100),
+                "shoulder_lift": Motor(2, "sts3215", MotorNormMode.RANGE_M100_100),
+                "elbow_flex": Motor(3, "sts3215", MotorNormMode.RANGE_M100_100),
+                "wrist_flex": Motor(4, "sts3215", MotorNormMode.RANGE_M100_100),
+                "wrist_roll": Motor(5, "sts3215", MotorNormMode.RANGE_M100_100),
+                "gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100),
+            },
+            calibration=self.calibration,
+        )
+
+    @property
+    def action_features(self) -> dict[str, type]:
+        return {f"{motor}.pos": float for motor in self.arm.motors}
+
+    @property
+    def feedback_features(self) -> dict[str, type]:
+        return {}
+
+    @property
+    def is_connected(self) -> bool:
+        return self.arm.is_connected
+
+    def connect(self, calibrate: bool = True) -> None:
+        if self.is_connected:
+            raise DeviceAlreadyConnectedError(f"{self} already connected")
+
+        self.arm.connect()
+        if not self.is_calibrated and calibrate:
+            self.calibrate()
+
+        self.configure()
+        logger.info(f"{self} connected.")
+
+    @property
+    def is_calibrated(self) -> bool:
+        return self.arm.is_calibrated
+
+    def calibrate(self) -> None:
+        logger.info(f"\nRunning calibration of {self}")
+        self.arm.disable_torque()
+        for motor in self.arm.motors:
+            self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+
+        input(f"Move {self} to the middle of its range of motion and press ENTER....")
+        homing_offsets = self.arm.set_half_turn_homings()
+
+        print(
+            "Move all joints sequentially through their entire ranges "
+            "of motion.\nRecording positions. Press ENTER to stop..."
+        )
+        range_mins, range_maxes = self.arm.record_ranges_of_motion()
+
+        self.calibration = {}
+        for motor, m in self.arm.motors.items():
+            self.calibration[motor] = MotorCalibration(
+                id=m.id,
+                drive_mode=0,
+                homing_offset=homing_offsets[motor],
+                range_min=range_mins[motor],
+                range_max=range_maxes[motor],
+            )
+
+        self.arm.write_calibration(self.calibration)
+        self._save_calibration()
+        logger.info(f"Calibration saved to {self.calibration_fpath}")
+
+    def configure(self) -> None:
+        self.arm.disable_torque()
+        self.arm.configure_motors()
+        for motor in self.arm.motors:
+            self.arm.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+
+    def setup_motors(self) -> None:
+        for motor in reversed(self.arm.motors):
+            input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+            self.arm.setup_motor(motor)
+            print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
+
+    def get_action(self) -> dict[str, float]:
+        start = time.perf_counter()
+        action = self.arm.sync_read("Present_Position")
+        action = {f"{motor}.pos": val for motor, val in action.items()}
+        dt_ms = (time.perf_counter() - start) * 1e3
+        logger.debug(f"{self} read action: {dt_ms:.1f}ms")
+        return action
+
+    def send_feedback(self, feedback: dict[str, float]) -> None:
+        # TODO(rcadene, aliberts): Implement force feedback
+        raise NotImplementedError
+
+    def disconnect(self) -> None:
+        if not self.is_connected:
+            DeviceNotConnectedError(f"{self} is not connected.")
+
+        self.arm.disconnect()
+        logger.info(f"{self} disconnected.")

lerobot/common/teleoperators/utils.py

@@ -15,6 +15,10 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> Teleoperator:
         from .so100_leader import SO100Leader
 
         return SO100Leader(config)
+    elif config.type == "so101_leader":
+        from .so101_leader import SO101Leader
+
+        return SO101Leader(config)
     elif config.type == "stretch3":
         from .stretch3_gamepad import Stretch3GamePad