New Feetech calibration (#859)

Co-authored-by: Pepijn <pepijn@huggingface.co>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

lerobot/common/teleoperators/keyboard/teleop_keyboard.py

@@ -55,7 +55,6 @@ class KeyboardTeleop(Teleoperator):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.pressed_keys = {}
         self.listener = None

lerobot/common/teleoperators/koch/teleop_koch.py

@@ -46,7 +46,6 @@ class KochTeleop(Teleoperator):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.arm = DynamixelMotorsBus(
             port=self.config.port,
@@ -106,19 +105,17 @@ class KochTeleop(Teleoperator):
         Rotations are expressed in degrees in nominal range of [-180, 180],
         and linear motions (like gripper of Aloha) in nominal range of [0, 100].
         """
-        arm_calib_path = self.calibration_dir / f"{self.id}.json"
-
-        if arm_calib_path.exists():
-            with open(arm_calib_path) as f:
+        if self.calibration_fpath.exists():
+            with open(self.calibration_fpath) as f:
                 calibration = json.load(f)
         else:
             # TODO(rcadene): display a warning in __init__ if calibration file not available
-            logging.info(f"Missing calibration file '{arm_calib_path}'")
+            logging.info(f"Missing calibration file '{self.calibration_fpath}'")
             calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "leader")
 
-            logging.info(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:
+            logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
+            self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.calibration_fpath, "w") as f:
                 json.dump(calibration, f)
 
         self.arm.set_calibration(calibration)

lerobot/common/teleoperators/so100/configuration_so100.py

@@ -24,13 +24,3 @@ from ..config import TeleoperatorConfig
 class SO100TeleopConfig(TeleoperatorConfig):
     # Port to connect to the teloperator
     port: str
-
-    mock: bool = False
-
-    # motors
-    shoulder_pan: tuple = (1, "sts3215")
-    shoulder_lift: tuple = (2, "sts3215")
-    elbow_flex: tuple = (3, "sts3215")
-    wrist_flex: tuple = (4, "sts3215")
-    wrist_roll: tuple = (5, "sts3215")
-    gripper: tuple = (6, "sts3215")

lerobot/common/teleoperators/so100/teleop_so100.py

@@ -24,7 +24,8 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte
 from lerobot.common.motors.feetech import (
     FeetechMotorsBus,
     TorqueMode,
-    run_arm_manual_calibration,
+    apply_feetech_offsets_from_calibration,
+    run_full_arm_calibration,
 )
 
 from ..teleoperator import Teleoperator
@@ -33,7 +34,7 @@ from .configuration_so100 import SO100TeleopConfig
 
 class SO100Teleop(Teleoperator):
     """
-    [SO-100 Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio
+    [SO-100 Leader Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio
     """
 
     config_class = SO100TeleopConfig
@@ -43,17 +44,16 @@ class SO100Teleop(Teleoperator):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.arm = FeetechMotorsBus(
             port=self.config.port,
             motors={
-                "shoulder_pan": config.shoulder_pan,
-                "shoulder_lift": config.shoulder_lift,
-                "elbow_flex": config.elbow_flex,
-                "wrist_flex": config.wrist_flex,
-                "wrist_roll": config.wrist_roll,
-                "gripper": config.gripper,
+                "shoulder_pan": (1, "sts3215"),
+                "shoulder_lift": (2, "sts3215"),
+                "elbow_flex": (3, "sts3215"),
+                "wrist_flex": (4, "sts3215"),
+                "wrist_roll": (5, "sts3215"),
+                "gripper": (6, "sts3215"),
             },
         )
 
@@ -86,11 +86,6 @@ class SO100Teleop(Teleoperator):
         self.arm.write("Torque_Enable", TorqueMode.DISABLED.value)
         self.calibrate()
 
-        # Enable torque on the gripper and move it to 45 degrees so that we can use it as a trigger.
-        logging.info("Activating torque.")
-        self.arm.write("Torque_Enable", TorqueMode.ENABLED.value, "gripper")
-        self.arm.write("Goal_Position", self.config.gripper_open_degree, "gripper")
-
         # Check arm can be read
         self.arm.read("Present_Position")
 
@@ -101,22 +96,21 @@ class SO100Teleop(Teleoperator):
         Rotations are expressed in degrees in nominal range of [-180, 180],
         and linear motions (like gripper of Aloha) in nominal range of [0, 100].
         """
-        arm_calib_path = self.calibration_dir / f"{self.id}.json"
-
-        if arm_calib_path.exists():
-            with open(arm_calib_path) as f:
+        if self.calibration_fpath.exists():
+            with open(self.calibration_fpath) as f:
                 calibration = json.load(f)
         else:
             # TODO(rcadene): display a warning in __init__ if calibration file not available
-            logging.info(f"Missing calibration file '{arm_calib_path}'")
-            calibration = run_arm_manual_calibration(self.arm, self.robot_type, self.name, "leader")
+            logging.info(f"Missing calibration file '{self.calibration_fpath}'")
+            calibration = run_full_arm_calibration(self.arm, self.robot_type, self.name, "leader")
 
-            logging.info(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:
+            logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
+            self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.calibration_fpath, "w") as f:
                 json.dump(calibration, f)
 
         self.arm.set_calibration(calibration)
+        apply_feetech_offsets_from_calibration(self.arm, calibration)
 
     def get_action(self) -> np.ndarray:
         """The returned action does not have a batch dimension."""

lerobot/common/teleoperators/teleoperator.py

@@ -14,12 +14,14 @@ class Teleoperator(abc.ABC):
     name: str
 
     def __init__(self, config: TeleoperatorConfig):
+        self.id = config.id
         self.calibration_dir = (
             config.calibration_dir
             if config.calibration_dir
             else HF_LEROBOT_CALIBRATION / TELEOPERATORS / self.name
         )
         self.calibration_dir.mkdir(parents=True, exist_ok=True)
+        self.calibration_fpath = self.calibration_dir / f"{self.id}.json"
 
     @abc.abstractproperty
     def action_feature(self) -> dict:

lerobot/common/teleoperators/widowx/teleop_widowx.py

@@ -43,7 +43,6 @@ class WidowXTeleop(Teleoperator):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.arm = DynamixelMotorsBus(
             port=self.config.port,
@@ -120,19 +119,17 @@ class WidowXTeleop(Teleoperator):
         Rotations are expressed in degrees in nominal range of [-180, 180],
         and linear motions (like gripper of Aloha) in nominal range of [0, 100].
         """
-        arm_calib_path = self.calibration_dir / f"{self.id}.json"
-
-        if arm_calib_path.exists():
-            with open(arm_calib_path) as f:
+        if self.calibration_fpath.exists():
+            with open(self.calibration_fpath) as f:
                 calibration = json.load(f)
         else:
             # TODO(rcadene): display a warning in __init__ if calibration file not available
-            logging.info(f"Missing calibration file '{arm_calib_path}'")
+            logging.info(f"Missing calibration file '{self.calibration_fpath}'")
             calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "leader")
 
-            logging.info(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:
+            logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
+            self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.calibration_fpath, "w") as f:
                 json.dump(calibration, f)
 
         self.arm.set_calibration(calibration)