From 1993d29296323598415172e3bd51e9d1b02b4ff9 Mon Sep 17 00:00:00 2001 From: Remi Cadene Date: Fri, 12 Jul 2024 11:24:29 +0200 Subject: [PATCH] Remove int32 convertion function, Set PID 1500, 0, 400 for elbow --- .../common/robot_devices/motors/dynamixel.py | 31 +++---------------- lerobot/common/robot_devices/robots/koch.py | 8 +++++ 2 files changed, 12 insertions(+), 27 deletions(-) diff --git a/lerobot/common/robot_devices/motors/dynamixel.py b/lerobot/common/robot_devices/motors/dynamixel.py index e7359a324..7930c57a4 100644 --- a/lerobot/common/robot_devices/motors/dynamixel.py +++ b/lerobot/common/robot_devices/motors/dynamixel.py @@ -102,27 +102,6 @@ MODEL_CONTROL_TABLE = { NUM_READ_RETRY = 10 -# TODO(rcadene): find better namming for these functions -def uint32_to_int32(values: np.ndarray): - """ - Convert an unsigned 32-bit integer array to a signed 32-bit integer array. - """ - for i in range(len(values)): - if values[i] is not None and values[i] > 2147483647: - values[i] = values[i] - 4294967296 - return values - - -def int32_to_uint32(values: np.ndarray): - """ - Convert a signed 32-bit integer array to an unsigned 32-bit integer array. - """ - for i in range(len(values)): - if values[i] is not None and values[i] < 0: - values[i] = values[i] + 4294967296 - return values - - def get_group_sync_key(data_name, motor_names): group_key = f"{data_name}_" + "_".join(motor_names) return group_key @@ -385,9 +364,9 @@ class DynamixelMotorsBus: values = np.array(values) - # TODO(rcadene): explain why + # Convert to signed int to use range [-2048, 2048] for our motor positions. if data_name in CONVERT_UINT32_TO_INT32_REQUIRED: - values = uint32_to_int32(values) + values = values.astype(np.int32) if data_name in CALIBRATION_REQUIRED: values = self.apply_calibration(values, motor_names) @@ -431,10 +410,6 @@ class DynamixelMotorsBus: if data_name in CALIBRATION_REQUIRED: values = self.revert_calibration(values, motor_names) - # TODO(rcadene): why dont we do it? - # if data_name in CONVERT_INT32_TO_UINT32_REQUIRED: - # values = int32_to_uint32(values) - values = values.tolist() assert_same_address(self.model_ctrl_table, models, data_name) @@ -448,6 +423,8 @@ class DynamixelMotorsBus: ) for idx, value in zip(motor_ids, values, strict=True): + # Note: No need to convert back into unsigned int, since this byte preprocessing + # already handles it for us. if bytes == 1: data = [ DXL_LOBYTE(DXL_LOWORD(value)), diff --git a/lerobot/common/robot_devices/robots/koch.py b/lerobot/common/robot_devices/robots/koch.py index af86aeff9..be8f2866f 100644 --- a/lerobot/common/robot_devices/robots/koch.py +++ b/lerobot/common/robot_devices/robots/koch.py @@ -206,6 +206,7 @@ class KochRobotConfig: class KochRobot: + # TODO(rcadene): Implement force feedback """Tau Robotics: https://tau-robotics.com Example of highest frequency teleoperation without camera: @@ -356,6 +357,13 @@ class KochRobot: for name in self.leader_arms: self.leader_arms[name].set_calibration(calibration[f"leader_{name}"]) + # 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") + # Enable torque on all motors of the follower arms for name in self.follower_arms: self.follower_arms[name].write("Torque_Enable", 1)