diff --git a/examples/aloha_calibrate.py b/examples/aloha_calibrate.py new file mode 100644 index 000000000..94aa01a90 --- /dev/null +++ b/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() diff --git a/examples/aloha_teleop.py b/examples/aloha_teleop.py new file mode 100644 index 000000000..d8f6d835a --- /dev/null +++ b/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) diff --git a/src/lerobot/robots/viperx/config_viperx.py b/src/lerobot/robots/viperx/config_viperx.py index ed3876a9c..b6ea54e9a 100644 --- a/src/lerobot/robots/viperx/config_viperx.py +++ b/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 diff --git a/src/lerobot/robots/viperx/viperx.py b/src/lerobot/robots/viperx/viperx.py index 881640cd5..9583ae6fa 100644 --- a/src/lerobot/robots/viperx/viperx.py +++ b/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), - "shoulder": Motor(2, "xm540-w270", MotorNormMode.RANGE_M100_100), - "shoulder_shadow": Motor(3, "xm540-w270", MotorNormMode.RANGE_M100_100), - "elbow": Motor(4, "xm540-w270", MotorNormMode.RANGE_M100_100), - "elbow_shadow": Motor(5, "xm540-w270", MotorNormMode.RANGE_M100_100), - "forearm_roll": Motor(6, "xm540-w270", MotorNormMode.RANGE_M100_100), - "wrist_angle": Motor(7, "xm540-w270", MotorNormMode.RANGE_M100_100), - "wrist_rotate": Motor(8, "xm430-w350", MotorNormMode.RANGE_M100_100), + "waist": Motor(1, "xm540-w270", norm_mode_body), + "shoulder": Motor(2, "xm540-w270", norm_mode_body), + "shoulder_shadow": Motor(3, "xm540-w270", norm_mode_body), + "elbow": Motor(4, "xm540-w270", norm_mode_body), + "elbow_shadow": Motor(5, "xm540-w270", norm_mode_body), + "forearm_roll": Motor(6, "xm540-w270", norm_mode_body), + "wrist_angle": Motor(7, "xm540-w270", norm_mode_body), + "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") diff --git a/src/lerobot/teleoperators/widowx/config_widowx.py b/src/lerobot/teleoperators/widowx/config_widowx.py index 42fae12db..b629fea46 100644 --- a/src/lerobot/teleoperators/widowx/config_widowx.py +++ b/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 diff --git a/src/lerobot/teleoperators/widowx/widowx.py b/src/lerobot/teleoperators/widowx/widowx.py index 6becd767f..b012c9494 100644 --- a/src/lerobot/teleoperators/widowx/widowx.py +++ b/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), - "gripper": Motor(9, "xc430-w150", MotorNormMode.RANGE_0_100), + "wrist_rotate": Motor( + 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) - drive_modes = {motor: 1 if motor == "elbow_flex" else 0 for motor in self.bus.motors} + # self.bus.write("Drive_Mode", "el", DriveMode.INVERTED.value) + # 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.")