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,
"xm430-w350": X_SERIES_CONTROL_TABLE,
"xm540-w270": X_SERIES_CONTROL_TABLE,
"xc430-w150": X_SERIES_CONTROL_TABLE,
}
MODEL_RESOLUTION = {
@ -118,6 +119,7 @@ MODEL_RESOLUTION = {
"xl430-w250": 4096,
"xm430-w350": 4096,
"xm540-w270": 4096,
"xc430-w150": 4096,
}
MODEL_BAUDRATE_TABLE = {
@ -127,6 +129,7 @@ MODEL_BAUDRATE_TABLE = {
"xl430-w250": X_SERIES_BAUDRATE_TABLE,
"xm430-w350": X_SERIES_BAUDRATE_TABLE,
"xm540-w270": X_SERIES_BAUDRATE_TABLE,
"xc430-w150": X_SERIES_BAUDRATE_TABLE,
}
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[
TARGET_HORIZONTAL_POSITION = np.array([0, -1024, 1024, 0, -1024, 0])
TARGET_90_DEGREE_POSITION = np.array([1024, 0, 0, 1024, 0, -1024])
# TARGET_HORIZONTAL_POSITION = np.array([0, -1024, 1024, 0, -1024, 0])
# 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[
GRIPPER_OPEN = np.array([-35.156])
@ -102,9 +110,7 @@ def compute_homing_offset(
arm: DynamixelMotorsBus, drive_mode: list[bool], target_position: np.array
) -> np.array:
# Get the present positions of the servos
present_positions = apply_calibration(
arm.read("Present_Position"), np.array([0, 0, 0, 0, 0, 0]), drive_mode
)
present_positions = apply_calibration(arm.read("Present_Position"), NULL_POSITION, drive_mode)
nearest_positions = compute_nearest_rounded_positions(present_positions)
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):
# Get current positions
present_positions = apply_calibration(
arm.read("Present_Position"), offset, np.array([False, False, False, False, False, False])
)
present_positions = apply_calibration(arm.read("Present_Position"), offset, NULL_DRIVE)
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...")
horizontal_homing_offset = compute_homing_offset(
arm, [False, False, False, False, False, False], TARGET_HORIZONTAL_POSITION
)
horizontal_homing_offset = compute_homing_offset(arm, NULL_DRIVE, TARGET_HORIZONTAL_POSITION)
# TODO(rcadene): document what position 2 mean
print(
@ -380,10 +382,10 @@ class KochRobot:
# 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")
# 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")
# Enable torque on all motors of the 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,
# 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", GRIPPER_OPEN, "gripper")
# for name in self.leader_arms:
# self.leader_arms[name].write("Torque_Enable", 1, "gripper")
# self.leader_arms[name].write("Goal_Position", GRIPPER_OPEN, "gripper")
# Connect the cameras
for name in self.cameras: