From bc953e4b5adf7b492f0aefe1226e200f37f6d6c1 Mon Sep 17 00:00:00 2001 From: Pepijn Date: Sat, 13 Sep 2025 21:17:51 +0200 Subject: [PATCH] add degree to aloha --- examples/aloha_record.py | 85 +++++++++++++++++++ examples/aloha_teleop.py | 31 ++++++- .../teleoperators/widowx/config_widowx.py | 2 + src/lerobot/teleoperators/widowx/widowx.py | 17 ++-- 4 files changed, 123 insertions(+), 12 deletions(-) create mode 100644 examples/aloha_record.py diff --git a/examples/aloha_record.py b/examples/aloha_record.py new file mode 100644 index 000000000..b4d6a5d26 --- /dev/null +++ b/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) diff --git a/examples/aloha_teleop.py b/examples/aloha_teleop.py index d8f6d835a..04a735bcc 100644 --- a/examples/aloha_teleop.py +++ b/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) diff --git a/src/lerobot/teleoperators/widowx/config_widowx.py b/src/lerobot/teleoperators/widowx/config_widowx.py index b629fea46..0c4818181 100644 --- a/src/lerobot/teleoperators/widowx/config_widowx.py +++ b/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 diff --git a/src/lerobot/teleoperators/widowx/widowx.py b/src/lerobot/teleoperators/widowx/widowx.py index b012c9494..7148f9624 100644 --- a/src/lerobot/teleoperators/widowx/widowx.py +++ b/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), - "shoulder": Motor(2, "xm430-w350", MotorNormMode.RANGE_M100_100), - "shoulder_shadow": Motor(3, "xm430-w350", MotorNormMode.RANGE_M100_100), - "elbow": Motor(4, "xm430-w350", MotorNormMode.RANGE_M100_100), - "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), + "waist": Motor(1, "xm430-w350", norm_mode_body), + "shoulder": Motor(2, "xm430-w350", norm_mode_body), + "shoulder_shadow": Motor(3, "xm430-w350", norm_mode_body), + "elbow": Motor(4, "xm430-w350", norm_mode_body), + "elbow_shadow": Motor(5, "xm430-w350", norm_mode_body), + "forearm_roll": Motor(6, "xm430-w350", norm_mode_body), + "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