diff --git a/lerobot/common/robot_devices/robots/koch.py b/lerobot/common/robot_devices/robots/koch.py index a1e070c1..cedbcf0b 100644 --- a/lerobot/common/robot_devices/robots/koch.py +++ b/lerobot/common/robot_devices/robots/koch.py @@ -29,13 +29,17 @@ KOCH_FIRST_POSITION = np.array([0, 0, 0, 0, 0, 0], dtype=np.int32) # by 90 degree. When the direction is ambiguous, always rotate on the right. Gripper is open, directed towards you. # TODO(rcadene): Take motor resolution into account instead of assuming 4096 KOCH_SECOND_POSITION = np.array([1024, 1024, 1024, 1024, 1024, 1024], dtype=np.int32) + # In nominal range ]-180, 180[ KOCH_GRIPPER_OPEN = 35.156 KOCH_REST_POSITION = np.array([0, 135, 90, 0, 0, KOCH_GRIPPER_OPEN]) -ALOHA_FIRST_POSITION = np.array([0, 0, 0, 0, 0, 0], dtype=np.int32) -ALOHA_SECOND_POSITION = np.array([0, 0, 0, 0, 0, 0], dtype=np.int32) -ALOHA_REST_POSITION = np.array([0, 0, 0, 0, 0, 0], dtype=np.int32) +# In nominal range ]-2048, 2048[ +ALOHA_FIRST_POSITION = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0], dtype=np.int32) +ALOHA_SECOND_POSITION = np.array([1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 512], dtype=np.int32) +# In nominal range ]-180, 180[ +ALOHA_GRIPPER_OPEN = 30 +ALOHA_REST_POSITION = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0], dtype=np.int32) def assert_robot_type(robot_type): @@ -79,11 +83,11 @@ def apply_drive_mode(position, drive_mode): return position -def compute_nearest_rounded_position(position): +def compute_nearest_rounded_position(position, second_position): # TODO(rcadene): Take motor resolution into account instead of assuming 4096 # Assumes 4096 steps for a full revolution for the motors # Hence 90 degree is 1024 steps - return np.round(position / 1024).astype(position.dtype) * 1024 + return np.round(position / second_position).astype(position.dtype) * second_position def reset_arm(arm: MotorsBus): @@ -118,10 +122,16 @@ def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="first")) input("Press Enter to continue...") + # The first position zeros all motors, i.e. after calibration, if write goal position to be all 0, + # the robot will move to this first position. + first_position = get_first_position(robot_type) + # The second position rotates all motors with 90 degrees angle clock-wise from the perspective of the first motor or the preceeding motor in the chain. + # Note: if 90 degree rotation cannot be achieved (e.g. gripper of Aloha), then it will rotate to 45 degrees. + second_position = get_second_position(robot_type) + # Compute homing offset so that `present_position + homing_offset ~= target_position` position = arm.read("Present_Position") - position = compute_nearest_rounded_position(position) - first_position = get_first_position(robot_type) + position = compute_nearest_rounded_position(position, second_position) homing_offset = first_position - position # TODO(rcadene): document what position 2 mean @@ -135,14 +145,13 @@ def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type # to indicate an inverted rotation direction. position = arm.read("Present_Position") position += homing_offset - position = compute_nearest_rounded_position(position) - second_position = get_second_position(robot_type) + position = compute_nearest_rounded_position(position, second_position) drive_mode = (position != second_position).astype(np.int32) # Re-compute homing offset to take into account drive mode position = arm.read("Present_Position") position = apply_drive_mode(position, drive_mode) - position = compute_nearest_rounded_position(position) + position = compute_nearest_rounded_position(position, second_position) homing_offset = second_position - position print("\nMove arm to rest position") @@ -334,27 +343,21 @@ class KochRobot: for name in self.leader_arms: values = self.leader_arms[name].read("Present_Position") - if (values < -180).any() or (values >= 180).any(): - raise ValueError( - f"At least one of the motor of the {name} leader arm has a joint value outside of its centered degree range of ]-180, 180[." - 'This "jump of range" can be caused by a hardware issue, or you might have unexpectedly completed a full rotation of the motor ' - "during manipulation or transportation of your robot. " - f"The values and motors: {values} {self.leader_arms[name].motor_names}.\n" - "Rotate the arm to fit the range ]-180, 180[ and relaunch the script, or recalibrate all motors by setting a different " - "calibration path during the instatiation of your robot (e.g. `--robot-overrides calibration_path=.cache/calibration/koch_v2.pkl`)" - ) - - # Set better PID values to close the gap between recored states and actions - # TODO(rcadene): Implement an automatic procedure to set optimial PID values for each motor - # for name in self.follower_arms: - # self.follower_arms[name].write("Position_P_Gain", 1500, "elbow_flex") - # self.follower_arms[name].write("Position_I_Gain", 0, "elbow_flex") - # self.follower_arms[name].write("Position_D_Gain", 600, "elbow_flex") + # if (values < -180).any() or (values >= 180).any(): + # raise ValueError( + # f"At least one of the motor of the {name} leader arm has a joint value outside of its centered degree range of ]-180, 180[." + # 'This "jump of range" can be caused by a hardware issue, or you might have unexpectedly completed a full rotation of the motor ' + # "during manipulation or transportation of your robot. " + # f"The values and motors: {values} {self.leader_arms[name].motor_names}.\n" + # "Rotate the arm to fit the range ]-180, 180[ and relaunch the script, or recalibrate all motors by setting a different " + # "calibration path during the instatiation of your robot (e.g. `--robot-overrides calibration_path=.cache/calibration/koch_v2.pkl`)" + # ) # Enable torque on all motors of the follower arms for name in self.follower_arms: self.follower_arms[name].write("Torque_Enable", 1) + # Custom setup for each robot type if self.robot_type == "koch": # Enable torque on the gripper of the leader arms, and move it to 45 degrees, # so that we can use it as a trigger to close the gripper of the follower arms. @@ -362,6 +365,13 @@ class KochRobot: self.leader_arms[name].write("Torque_Enable", 1, "gripper") self.leader_arms[name].write("Goal_Position", KOCH_GRIPPER_OPEN, "gripper") + # Set better PID values to close the gap between recorded states and actions + # TODO(rcadene): Implement an automatic procedure to set optimial PID values for each motor + for name in self.follower_arms: + self.follower_arms[name].write("Position_P_Gain", 1500, "elbow_flex") + self.follower_arms[name].write("Position_I_Gain", 0, "elbow_flex") + self.follower_arms[name].write("Position_D_Gain", 600, "elbow_flex") + # Connect the cameras for name in self.cameras: self.cameras[name].connect() diff --git a/lerobot/configs/robot/aloha.yaml b/lerobot/configs/robot/aloha.yaml index 79171b48..88d8b143 100644 --- a/lerobot/configs/robot/aloha.yaml +++ b/lerobot/configs/robot/aloha.yaml @@ -4,64 +4,62 @@ calibration_path: .cache/calibration/aloha.pkl leader_arms: left: _target_: lerobot.common.robot_devices.motors.dynamixel.DynamixelMotorsBus - #port: /dev/ttyDXL_master_left - port: /dev/tty.usbserial-FT89FM77 + port: /dev/ttyDXL_master_left + #port: /dev/tty.usbserial-FT89FM77 motors: # window_x # name: (index, model) - waist_a: [1, xm430-w350] - waist_b: [2, xm430-w350] - shoulder_a: [3, xm430-w350] - shoulder_b: [4, xm430-w350] - elbow: [5, xm430-w350] + waist: [1, xm430-w350] + shoulder_a: [2, xm430-w350] + shoulder_b: [3, xm430-w350] + elbow_a: [4, xm430-w350] + elbow_b: [5, xm430-w350] forearm_roll: [6, xm430-w350] wrist_angle: [7, xm430-w350] wrist_rotate: [8, xl430-w250] - # gripper: [9, xl430-w250] gripper: [9, xc430-w150] right: _target_: lerobot.common.robot_devices.motors.dynamixel.DynamixelMotorsBus - #port: /dev/ttyDXL_master_right - port: /dev/tty.usbserial-FT891KPN + port: /dev/ttyDXL_master_right + #port: /dev/tty.usbserial-FT891KPN motors: # window_x # name: (index, model) - waist_a: [1, xm430-w350] - waist_b: [2, xm430-w350] - shoulder_a: [3, xm430-w350] - shoulder_b: [4, xm430-w350] - elbow: [5, xm430-w350] + waist: [1, xm430-w350] + shoulder_a: [2, xm430-w350] + shoulder_b: [3, xm430-w350] + elbow_a: [4, xm430-w350] + elbow_b: [5, xm430-w350] forearm_roll: [6, xm430-w350] wrist_angle: [7, xm430-w350] wrist_rotate: [8, xl430-w250] - # gripper: [9, xl430-w250] gripper: [9, xc430-w150] follower_arms: left: _target_: lerobot.common.robot_devices.motors.dynamixel.DynamixelMotorsBus - #port: /dev/ttyDXL_puppet_left - port: /dev/tty.usbserial-FT891KBG + port: /dev/ttyDXL_puppet_left + #port: /dev/tty.usbserial-FT891KBG motors: # name: [index, model] - waist_a: [1, xm540-w270] - waist_b: [2, xm540-w270] - shoulder_a: [3, xm540-w270] - shoulder_b: [4, xm540-w270] - elbow: [5, xm540-w270] + waist: [1, xm540-w270] + shoulder_a: [2, xm540-w270] + shoulder_b: [3, xm540-w270] + elbow_a: [4, xm540-w270] + elbow_b: [5, xm540-w270] forearm_roll: [6, xm540-w270] wrist_angle: [7, xm540-w270] wrist_rotate: [8, xm430-w350] gripper: [9, xm430-w350] right: _target_: lerobot.common.robot_devices.motors.dynamixel.DynamixelMotorsBus - #port: /dev/ttyDXL_puppet_right - port: /dev/tty.usbserial-FT89FM09 + port: /dev/ttyDXL_puppet_right + #port: /dev/tty.usbserial-FT89FM09 motors: # name: [index, model] - waist_a: [1, xm540-w270] - waist_b: [2, xm540-w270] - shoulder_a: [3, xm540-w270] - shoulder_b: [4, xm540-w270] - elbow: [5, xm540-w270] + waist: [1, xm540-w270] + shoulder_a: [2, xm540-w270] + shoulder_b: [3, xm540-w270] + elbow_a: [4, xm540-w270] + elbow_b: [5, xm540-w270] forearm_roll: [6, xm540-w270] wrist_angle: [7, xm540-w270] wrist_rotate: [8, xm430-w350]