Skip to content

Commit

Permalink
Add Aloha support
Browse files Browse the repository at this point in the history
  • Loading branch information
@Cadene
Cadene committed Jul 31, 2024
1 parent 8fa0171 commit e447b7c
Show file tree
Hide file tree
Showing 2 changed files with 64 additions and 56 deletions.
62 changes: 36 additions & 26 deletions lerobot/common/robot_devices/robots/koch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,13 +29,17 @@
# 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):
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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
Expand All @@ -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")
Expand Down Expand Up @@ -334,34 +343,35 @@ def connect(self):

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.
for name in self.leader_arms:
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()
Expand Down
58 changes: 28 additions & 30 deletions lerobot/configs/robot/aloha.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down

0 comments on commit e447b7c

Please sign in to comment.