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

WIP

This commit is contained in:
Remi Cadene 2024-07-29 23:44:02 +02:00
parent e1d7330613
commit b3e23e96f4
2 changed files with 23 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
lerobot/common/robot_devices/motors/dynamixel.py
View File

@ -109,6 +109,7 @@ MODEL_CONTROL_TABLE = {
"xl430-w250": X_SERIES_CONTROL_TABLE, "xl430-w250": X_SERIES_CONTROL_TABLE,
"xm430-w350": X_SERIES_CONTROL_TABLE, "xm430-w350": X_SERIES_CONTROL_TABLE,
"xm540-w270": X_SERIES_CONTROL_TABLE, "xm540-w270": X_SERIES_CONTROL_TABLE,
"xc430-w150": X_SERIES_CONTROL_TABLE,
} }
MODEL_RESOLUTION = { MODEL_RESOLUTION = {
@ -118,6 +119,7 @@ MODEL_RESOLUTION = {
"xl430-w250": 4096, "xl430-w250": 4096,
"xm430-w350": 4096, "xm430-w350": 4096,
"xm540-w270": 4096, "xm540-w270": 4096,
"xc430-w150": 4096,
} }
MODEL_BAUDRATE_TABLE = { MODEL_BAUDRATE_TABLE = {
@ -127,6 +129,7 @@ MODEL_BAUDRATE_TABLE = {
"xl430-w250": X_SERIES_BAUDRATE_TABLE, "xl430-w250": X_SERIES_BAUDRATE_TABLE,
"xm430-w350": X_SERIES_BAUDRATE_TABLE, "xm430-w350": X_SERIES_BAUDRATE_TABLE,
"xm540-w270": X_SERIES_BAUDRATE_TABLE, "xm540-w270": X_SERIES_BAUDRATE_TABLE,
"xc430-w150": X_SERIES_BAUDRATE_TABLE,
} }
NUM_READ_RETRY = 10 NUM_READ_RETRY = 10

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

@ -30,8 +30,16 @@ URL_90_DEGREE_POSITION = {
######################################################################## ########################################################################
# In range ]-2048, 2048[ # In range ]-2048, 2048[
TARGET_HORIZONTAL_POSITION = np.array([0, -1024, 1024, 0, -1024, 0]) # TARGET_HORIZONTAL_POSITION = np.array([0, -1024, 1024, 0, -1024, 0])
TARGET_90_DEGREE_POSITION = np.array([1024, 0, 0, 1024, 0, -1024]) # TARGET_90_DEGREE_POSITION = np.array([1024, 0, 0, 1024, 0, -1024])
TARGET_HORIZONTAL_POSITION = np.array([0, -1024, -1024, 1024, 1024, 0, 0, 0, 0])
TARGET_90_DEGREE_POSITION = np.array([1024, 0, 0, 0, 0, 1024, 1024, 1024, 1024])
# NULL_POSITION = np.array([0, 0, 0, 0, 0, 0])
NULL_POSITION = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])
# NULL_DRIVE = np.array([False, False, False, False, False, False])
NULL_DRIVE = np.array([False, False, False, False, False, False, False, False, False])
# In range ]-180, 180[ # In range ]-180, 180[
GRIPPER_OPEN = np.array([-35.156]) GRIPPER_OPEN = np.array([-35.156])
@ -102,9 +110,7 @@ def compute_homing_offset(
arm: DynamixelMotorsBus, drive_mode: list[bool], target_position: np.array arm: DynamixelMotorsBus, drive_mode: list[bool], target_position: np.array
) -> np.array: ) -> np.array:
# Get the present positions of the servos # Get the present positions of the servos
present_positions = apply_calibration( present_positions = apply_calibration(arm.read("Present_Position"), NULL_POSITION, drive_mode)
arm.read("Present_Position"), np.array([0, 0, 0, 0, 0, 0]), drive_mode
)
nearest_positions = compute_nearest_rounded_positions(present_positions) nearest_positions = compute_nearest_rounded_positions(present_positions)
correction = compute_corrections(nearest_positions, drive_mode, target_position) correction = compute_corrections(nearest_positions, drive_mode, target_position)
@ -113,9 +119,7 @@ def compute_homing_offset(
def compute_drive_mode(arm: DynamixelMotorsBus, offset: np.array): def compute_drive_mode(arm: DynamixelMotorsBus, offset: np.array):
# Get current positions # Get current positions
present_positions = apply_calibration( present_positions = apply_calibration(arm.read("Present_Position"), offset, NULL_DRIVE)
arm.read("Present_Position"), offset, np.array([False, False, False, False, False, False])
)
nearest_positions = compute_nearest_rounded_positions(present_positions) nearest_positions = compute_nearest_rounded_positions(present_positions)
@ -160,9 +164,7 @@ def run_arm_calibration(arm: MotorsBus, name: str, arm_type: str):
) )
input("Press Enter to continue...") input("Press Enter to continue...")
horizontal_homing_offset = compute_homing_offset( horizontal_homing_offset = compute_homing_offset(arm, NULL_DRIVE, TARGET_HORIZONTAL_POSITION)
arm, [False, False, False, False, False, False], TARGET_HORIZONTAL_POSITION
)
# TODO(rcadene): document what position 2 mean # TODO(rcadene): document what position 2 mean
print( print(
@ -380,10 +382,10 @@ class KochRobot:
# Set better PID values to close the gap between recored states and actions # 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 # TODO(rcadene): Implement an automatic procedure to set optimial PID values for each motor
for name in self.follower_arms: # for name in self.follower_arms:
self.follower_arms[name].write("Position_P_Gain", 1500, "elbow_flex") # 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_I_Gain", 0, "elbow_flex")
self.follower_arms[name].write("Position_D_Gain", 600, "elbow_flex") # self.follower_arms[name].write("Position_D_Gain", 600, "elbow_flex")
# Enable torque on all motors of the follower arms # Enable torque on all motors of the follower arms
for name in self.follower_arms: for name in self.follower_arms:
@ -391,9 +393,9 @@ class KochRobot:
# Enable torque on the gripper of the leader arms, and move it to 45 degrees, # 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. # so that we can use it as a trigger to close the gripper of the follower arms.
for name in self.leader_arms: # for name in self.leader_arms:
self.leader_arms[name].write("Torque_Enable", 1, "gripper") # self.leader_arms[name].write("Torque_Enable", 1, "gripper")
self.leader_arms[name].write("Goal_Position", GRIPPER_OPEN, "gripper") # self.leader_arms[name].write("Goal_Position", GRIPPER_OPEN, "gripper")
# Connect the cameras # Connect the cameras
for name in self.cameras: for name in self.cameras: