add degree to aloha

examples/aloha_record.py

@@ -0,0 +1,85 @@
+import time
+
+from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig
+from lerobot.robots.viperx import ViperX, ViperXConfig
+from lerobot.teleoperators.widowx import WidowX, WidowXConfig
+
+# TODO: pepijn create a Aloha robot with has the two robots integrated with the 3 camera's (teleoprators can still be separate for now)
+# TODO: pepijn add record loop and dataset etc...
+
+camera_config = {
+    "front": OpenCVCameraConfig(index_or_path=0, width=640, height=480, fps=30),
+    "wrist_right": OpenCVCameraConfig(index_or_path=0, width=640, height=480, fps=30),
+    "wrist_left": OpenCVCameraConfig(index_or_path=0, width=640, height=480, fps=30),
+}
+
+config_follower_right = ViperXConfig(
+    port="/dev/tty.usbserial-FT891KBG",
+    id="viperx_right",
+    max_relative_target=20.0,  # increased from default 5.0
+    use_degrees=True,
+    cameras=camera_config,
+)
+
+config_leader_right = WidowXConfig(
+    port="/dev/tty.usbserial-FT89FM77",
+    id="widowx_right",
+    gripper_motor="xc430-w150",
+    use_degrees=True,
+)
+
+config_follower_left = ViperXConfig(
+    port="/dev/tty.usbserial-FT89FM09",
+    id="viperx_left",
+    max_relative_target=20.0,  # increased from default 5.0
+    use_degrees=True,
+)
+
+config_leader_left = WidowXConfig(
+    port="/dev/tty.usbserial-FT891KPN",
+    id="widowx_left",
+    gripper_motor="xl430-w250",
+    use_degrees=True,
+)
+
+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)

examples/aloha_teleop.py

@@ -1,32 +1,47 @@
 import time
 
+from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig
 from lerobot.robots.viperx import ViperX, ViperXConfig
 from lerobot.teleoperators.widowx import WidowX, WidowXConfig
+from lerobot.utils.visualization_utils import _init_rerun, log_rerun_data
+
+camera_config = {
+    "front": OpenCVCameraConfig(index_or_path=0, width=640, height=480, fps=30),
+    "wrist_right": OpenCVCameraConfig(index_or_path=1, width=640, height=480, fps=30),
+    "wrist_left": OpenCVCameraConfig(index_or_path=2, width=640, height=480, fps=30),
+}
 
 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
+    use_degrees=True,
+    cameras=camera_config,
 )
 
 config_leader_right = WidowXConfig(
     port="/dev/tty.usbserial-FT89FM77",
     id="widowx_right",
     gripper_motor="xc430-w150",
+    use_degrees=True,
 )
 
 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
+    use_degrees=True,
 )
 
 config_leader_left = WidowXConfig(
     port="/dev/tty.usbserial-FT891KPN",
     id="widowx_left",
     gripper_motor="xl430-w250",
+    use_degrees=True,
 )
 
+_init_rerun(session_name="teleop")
+
 follower_right = ViperX(config_follower_right)
 follower_right.connect()
 
@@ -39,6 +54,7 @@ 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()
@@ -49,21 +65,28 @@ while True:
     print("=" * 60)
     print("ACTION (Leader Right):")
     for key, value in act_right.items():
-        print(f"  {key:20}: {value:8.3f}")
+        if key.endswith(".pos"):
+            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}")
+        if key.endswith(".pos"):
+            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}")
+        if key.endswith(".pos"):
+            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}")
+        if key.endswith(".pos"):
+            print(f"  {key:20}: {value:8.3f}")
     print("=" * 60)
 
+    log_rerun_data({**obs_right, **obs_left}, {**act_right, **act_left})
+
     follower_right.send_action(act_right)
     follower_left.send_action(act_left)
 

src/lerobot/teleoperators/widowx/config_widowx.py

@@ -25,3 +25,5 @@ class WidowXConfig(TeleoperatorConfig):
     port: str  # Port to connect to the arm
 
     gripper_motor: str = "xl430-w250"  # This could be xc430-w150 or xl430-w250
+
+    use_degrees: bool = False

src/lerobot/teleoperators/widowx/widowx.py

@@ -41,18 +41,19 @@ class WidowX(Teleoperator):
     def __init__(self, config: WidowXConfig):
         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, "xm430-w350", MotorNormMode.RANGE_M100_100),
+                "waist": Motor(1, "xm430-w350", norm_mode_body),
-                "shoulder": Motor(2, "xm430-w350", MotorNormMode.RANGE_M100_100),
+                "shoulder": Motor(2, "xm430-w350", norm_mode_body),
-                "shoulder_shadow": Motor(3, "xm430-w350", MotorNormMode.RANGE_M100_100),
+                "shoulder_shadow": Motor(3, "xm430-w350", norm_mode_body),
-                "elbow": Motor(4, "xm430-w350", MotorNormMode.RANGE_M100_100),
+                "elbow": Motor(4, "xm430-w350", norm_mode_body),
-                "elbow_shadow": Motor(5, "xm430-w350", MotorNormMode.RANGE_M100_100),
+                "elbow_shadow": Motor(5, "xm430-w350", norm_mode_body),
-                "forearm_roll": Motor(6, "xm430-w350", MotorNormMode.RANGE_M100_100),
+                "forearm_roll": Motor(6, "xm430-w350", norm_mode_body),
-                "wrist_angle": Motor(7, "xm430-w350", MotorNormMode.RANGE_M100_100),
+                "wrist_angle": Motor(7, "xm430-w350", norm_mode_body),
                 "wrist_rotate": Motor(
-                    8, "xm430-w350", MotorNormMode.RANGE_M100_100
+                    8, "xm430-w350", norm_mode_body
                 ),  # This could be xl430-w250 or xm430-w350
                 "gripper": Motor(
                     9, self.config.gripper_motor, MotorNormMode.RANGE_0_100