match koch follower and robot as much as possible

examples/aloha_calibrate.py

@@ -0,0 +1,47 @@
+# ------------------------------------------------------------
+
+# config_follower_right = ViperXConfig(
+#     port="/dev/tty.usbserial-FT891KBG",
+#     id="viperx_right",
+# )
+
+# follower_right = ViperX(config_follower_right)
+# follower_right.connect(calibrate=False)
+# follower_right.calibrate()
+# follower_right.disconnect()
+
+# ------------------------------------------------------------
+
+# config_leader_right = WidowXConfig(
+#     port="/dev/tty.usbserial-FT89FM77",
+#     id="widowx_right",
+# )
+
+# leader_right = WidowX(config_leader_right)
+# leader_right.connect(calibrate=False)
+# leader_right.calibrate()
+# leader_right.disconnect()
+
+# ------------------------------------------------------------
+
+# config_follower_left = ViperXConfig(
+#     port="/dev/tty.usbserial-FT89FM09",
+#     id="viperx_left",
+# )
+
+# follower_left = ViperX(config_follower_left)
+# follower_left.connect(calibrate=False)
+# follower_left.calibrate()
+# follower_left.disconnect()
+
+# ------------------------------------------------------------
+
+# config_leader_left = WidowXConfig(
+#     port="/dev/tty.usbserial-FT891KPN",
+#     id="widowx_left",
+# )
+
+# leader_left = WidowX(config_leader_left)
+# leader_left.connect(calibrate=False)
+# leader_left.calibrate()
+# leader_left.disconnect()

examples/aloha_teleop.py

@@ -0,0 +1,70 @@
+import time
+
+from lerobot.robots.viperx import ViperX, ViperXConfig
+from lerobot.teleoperators.widowx import WidowX, WidowXConfig
+
+config_follower_right = ViperXConfig(
+    port="/dev/tty.usbserial-FT891KBG",
+    id="viperx_right",
+    max_relative_target=10.0,  # increased from default 5.0 to 10.0
+)
+
+config_leader_right = WidowXConfig(
+    port="/dev/tty.usbserial-FT89FM77",
+    id="widowx_right",
+    gripper_motor="xc430-w150",
+)
+
+config_follower_left = ViperXConfig(
+    port="/dev/tty.usbserial-FT89FM09",
+    id="viperx_left",
+    max_relative_target=10.0,  # increased from default 5.0 to 10.0
+)
+
+config_leader_left = WidowXConfig(
+    port="/dev/tty.usbserial-FT891KPN",
+    id="widowx_left",
+    gripper_motor="xl430-w250",
+)
+
+follower_right = ViperX(config_follower_right)
+follower_right.connect()
+
+leader_right = WidowX(config_leader_right)
+leader_right.connect()
+
+follower_left = ViperX(config_follower_left)
+follower_left.connect()
+
+leader_left = WidowX(config_leader_left)
+leader_left.connect()
+
+while True:
+    act_right = leader_right.get_action()
+    obs_right = follower_right.get_observation()
+
+    act_left = leader_left.get_action()
+    obs_left = follower_left.get_observation()
+
+    print("=" * 60)
+    print("ACTION (Leader Right):")
+    for key, value in act_right.items():
+        print(f"  {key:20}: {value:8.3f}")
+
+    print("\nOBSERVATION (Follower Right):")
+    for key, value in obs_right.items():
+        print(f"  {key:20}: {value:8.3f}")
+    print("=" * 60)
+    print("ACTION (Leader Left):")
+    for key, value in act_left.items():
+        print(f"  {key:20}: {value:8.3f}")
+
+    print("\nOBSERVATION (Follower Left):")
+    for key, value in obs_left.items():
+        print(f"  {key:20}: {value:8.3f}")
+    print("=" * 60)
+
+    follower_right.send_action(act_right)
+    follower_left.send_action(act_left)
+
+    time.sleep(0.02)

src/lerobot/robots/viperx/config_viperx.py

@@ -43,3 +43,6 @@ class ViperXConfig(RobotConfig):
     # 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.
+
+    # Set to `True` for backward compatibility with previous policies/dataset
+    use_degrees: bool = False

src/lerobot/robots/viperx/viperx.py

@@ -18,7 +18,6 @@ from functools import cached_property
 from typing import Any
 
 from lerobot.cameras.utils import make_cameras_from_configs
-from lerobot.constants import OBS_STATE
 from lerobot.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
 from lerobot.motors import Motor, MotorCalibration, MotorNormMode
 from lerobot.motors.dynamixel import (
@@ -45,22 +44,23 @@ class ViperX(Robot):
         self,
         config: ViperXConfig,
     ):
-        raise NotImplementedError
         super().__init__(config)
         self.config = config
+        norm_mode_body = MotorNormMode.DEGREES if config.use_degrees else MotorNormMode.RANGE_M100_100
         self.bus = DynamixelMotorsBus(
             port=self.config.port,
             motors={
-                "waist": Motor(1, "xm540-w270", MotorNormMode.RANGE_M100_100),
+                "waist": Motor(1, "xm540-w270", norm_mode_body),
-                "shoulder": Motor(2, "xm540-w270", MotorNormMode.RANGE_M100_100),
+                "shoulder": Motor(2, "xm540-w270", norm_mode_body),
-                "shoulder_shadow": Motor(3, "xm540-w270", MotorNormMode.RANGE_M100_100),
+                "shoulder_shadow": Motor(3, "xm540-w270", norm_mode_body),
-                "elbow": Motor(4, "xm540-w270", MotorNormMode.RANGE_M100_100),
+                "elbow": Motor(4, "xm540-w270", norm_mode_body),
-                "elbow_shadow": Motor(5, "xm540-w270", MotorNormMode.RANGE_M100_100),
+                "elbow_shadow": Motor(5, "xm540-w270", norm_mode_body),
-                "forearm_roll": Motor(6, "xm540-w270", MotorNormMode.RANGE_M100_100),
+                "forearm_roll": Motor(6, "xm540-w270", norm_mode_body),
-                "wrist_angle": Motor(7, "xm540-w270", MotorNormMode.RANGE_M100_100),
+                "wrist_angle": Motor(7, "xm540-w270", norm_mode_body),
-                "wrist_rotate": Motor(8, "xm430-w350", MotorNormMode.RANGE_M100_100),
+                "wrist_rotate": Motor(8, "xm430-w350", norm_mode_body),
                 "gripper": Motor(9, "xm430-w350", MotorNormMode.RANGE_0_100),
             },
+            calibration=self.calibration,
         )
         self.cameras = make_cameras_from_configs(config.cameras)
 
@@ -96,6 +96,9 @@ class ViperX(Robot):
 
         self.bus.connect()
         if not self.is_calibrated and calibrate:
+            logger.info(
+                "Mismatch between calibration values in the motor and the calibration file or no calibration file found"
+            )
             self.calibrate()
 
         for cam in self.cameras.values():
@@ -109,16 +112,24 @@ class ViperX(Robot):
         return self.bus.is_calibrated
 
     def calibrate(self) -> None:
-        raise NotImplementedError  # TODO(aliberts): adapt code below (copied from koch
-        logger.info(f"\nRunning calibration of {self}")
         self.bus.disable_torque()
+        if self.calibration:
+            # Calibration file exists, ask user whether to use it or run new calibration
+            user_input = input(
+                f"Press ENTER to use provided calibration file associated with the id {self.id}, or type 'c' and press ENTER to run calibration: "
+            )
+            if user_input.strip().lower() != "c":
+                logger.info(f"Writing calibration file associated with the id {self.id} to the motors")
+                self.bus.write_calibration(self.calibration)
+                return
+        logger.info(f"\nRunning calibration of {self}")
         for motor in self.bus.motors:
             self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
 
-        input("Move robot to the middle of its range of motion and press ENTER....")
+        input(f"Move {self} to the middle of its range of motion and press ENTER....")
         homing_offsets = self.bus.set_half_turn_homings()
 
-        full_turn_motors = ["shoulder_pan", "wrist_roll"]
+        full_turn_motors = ["shoulder", "forearm_roll", "wrist_rotate"]
         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 "
@@ -153,33 +164,23 @@ class ViperX(Robot):
             self.bus.write("Secondary_ID", "shoulder_shadow", 2)
             self.bus.write("Secondary_ID", "elbow_shadow", 4)
 
-            # Set a velocity limit of 131 as advised by Trossen Robotics
-            # TODO(aliberts): remove as it's actually useless in position control
-            self.bus.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
             for motor in self.bus.motors:
-                if motor != "gripper":
+                self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
-                    self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
 
+            # TODO(pepijn): Re enable this
             # 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).
-            self.bus.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value)
+            # self.bus.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value)
 
     def get_observation(self) -> dict[str, Any]:
         """The returned observations do not have a batch dimension."""
         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 = 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")

src/lerobot/teleoperators/widowx/config_widowx.py

@@ -23,3 +23,5 @@ from ..config import TeleoperatorConfig
 @dataclass
 class WidowXConfig(TeleoperatorConfig):
     port: str  # Port to connect to the arm
+
+    gripper_motor: str = "xl430-w250"  # This could be xc430-w150 or xl430-w250

src/lerobot/teleoperators/widowx/widowx.py

@@ -20,7 +20,6 @@ import time
 from lerobot.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
 from lerobot.motors import Motor, MotorCalibration, MotorNormMode
 from lerobot.motors.dynamixel import (
-    DriveMode,
     DynamixelMotorsBus,
     OperatingMode,
 )
@@ -40,7 +39,6 @@ class WidowX(Teleoperator):
     name = "widowx"
 
     def __init__(self, config: WidowXConfig):
-        raise NotImplementedError
         super().__init__(config)
         self.config = config
         self.bus = DynamixelMotorsBus(
@@ -53,9 +51,14 @@ class WidowX(Teleoperator):
                 "elbow_shadow": Motor(5, "xm430-w350", MotorNormMode.RANGE_M100_100),
                 "forearm_roll": Motor(6, "xm430-w350", MotorNormMode.RANGE_M100_100),
                 "wrist_angle": Motor(7, "xm430-w350", MotorNormMode.RANGE_M100_100),
-                "wrist_rotate": Motor(8, "xl430-w250", MotorNormMode.RANGE_M100_100),
+                "wrist_rotate": Motor(
-                "gripper": Motor(9, "xc430-w150", MotorNormMode.RANGE_0_100),
+                    8, "xm430-w350", MotorNormMode.RANGE_M100_100
+                ),  # This could be xl430-w250 or xm430-w350
+                "gripper": Motor(
+                    9, self.config.gripper_motor, MotorNormMode.RANGE_0_100
+                ),  # This could be xc430-w150 or xl430-w250
             },
+            calibration=self.calibration,
         )
 
     @property
@@ -76,6 +79,9 @@ class WidowX(Teleoperator):
 
         self.bus.connect()
         if not self.is_calibrated and calibrate:
+            logger.info(
+                "Mismatch between calibration values in the motor and the calibration file or no calibration file found"
+            )
             self.calibrate()
 
         self.configure()
@@ -86,19 +92,27 @@ class WidowX(Teleoperator):
         return self.bus.is_calibrated
 
     def calibrate(self) -> None:
-        raise NotImplementedError  # TODO(aliberts): adapt code below (copied from koch)
-        logger.info(f"\nRunning calibration of {self}")
         self.bus.disable_torque()
+        if self.calibration:
+            # Calibration file exists, ask user whether to use it or run new calibration
+            user_input = input(
+                f"Press ENTER to use provided calibration file associated with the id {self.id}, or type 'c' and press ENTER to run calibration: "
+            )
+            if user_input.strip().lower() != "c":
+                logger.info(f"Writing calibration file associated with the id {self.id} to the motors")
+                self.bus.write_calibration(self.calibration)
+                return
+        logger.info(f"\nRunning calibration of {self}")
         for motor in self.bus.motors:
             self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
 
-        self.bus.write("Drive_Mode", "elbow_flex", DriveMode.INVERTED.value)
+        # self.bus.write("Drive_Mode", "el", DriveMode.INVERTED.value)
-        drive_modes = {motor: 1 if motor == "elbow_flex" else 0 for motor in self.bus.motors}
+        # drive_modes = {motor: 1 if motor == ["elbow_shadow", "shoulder_shadow"] else 0 for motor in self.bus.motors}
 
-        input("Move robot to the middle of its range of motion and press ENTER....")
+        input(f"Move {self} to the middle of its range of motion and press ENTER....")
         homing_offsets = self.bus.set_half_turn_homings()
 
-        full_turn_motors = ["shoulder_pan", "wrist_roll"]
+        full_turn_motors = ["shoulder", "forearm_roll", "wrist_rotate"]
         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 "
@@ -113,7 +127,7 @@ class WidowX(Teleoperator):
         for motor, m in self.bus.motors.items():
             self.calibration[motor] = MotorCalibration(
                 id=m.id,
-                drive_mode=drive_modes[motor],
+                drive_mode=0,
                 homing_offset=homing_offsets[motor],
                 range_min=range_mins[motor],
                 range_max=range_maxes[motor],
@@ -133,6 +147,22 @@ class WidowX(Teleoperator):
         self.bus.write("Secondary_ID", "shoulder_shadow", 2)
         self.bus.write("Secondary_ID", "elbow_shadow", 4)
 
+        for motor in self.bus.motors:
+            self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)
+
+        # TODO(pepijn): Re enable this
+        # 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 gripper's goal pos in current position mode so that we can use it as a trigger.
+        # self.bus.enable_torque("gripper")
+
+        if self.is_calibrated:
+            self.bus.write("Goal_Position", "gripper", self.config.gripper_open_pos)
+
     def get_action(self) -> dict[str, float]:
         if not self.is_connected:
             raise DeviceNotConnectedError(f"{self} is not connected.")