wangzixiang
/
lerobot
mirror of https://github.com/huggingface/lerobot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add Aloha support

This commit is contained in:
Remi Cadene 2024-07-31 19:25:44 +02:00
parent 8fa017108f
commit e447b7c269
2 changed files with 64 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
lerobot/common/robot_devices/robots/koch.py
View File

@ -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()

58
lerobot/configs/robot/aloha.yaml
View File

@ -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]