diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index f2e847c35..55e88f42d 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/lerobot/common/robot_devices/motors/feetech.py @@ -92,6 +92,7 @@ SCS_SERIES_CONTROL_TABLE = { "Status": (65, 1), "Moving": (66, 1), "Present_Current": (69, 2), + "Maximum_Acceleration": (85, 2), } SCS_SERIES_BAUDRATE_TABLE = { @@ -124,8 +125,8 @@ MODEL_BAUDRATE_TABLE = { "sts3215": SCS_SERIES_BAUDRATE_TABLE, } -NUM_READ_RETRY = 10 -NUM_WRITE_RETRY = 10 +NUM_READ_RETRY = 2 +NUM_WRITE_RETRY = 2 def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray: @@ -392,7 +393,7 @@ class FeetechMotorsBus: def set_calibration(self, calibration: dict[str, list]): pass - #self.calibration = calibration + # self.calibration = calibration def apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None): """This function apply the calibration, automatically detects out of range errors for motors values and attempt to correct. @@ -621,7 +622,7 @@ class FeetechMotorsBus: if data_name not in self.track_positions: self.track_positions[data_name] = { "prev": [None] * len(self.motor_names), - # Assume False at initialization + # Assume False at initialization "below_zero": [False] * len(self.motor_names), "above_max": [False] * len(self.motor_names), } @@ -640,7 +641,6 @@ class FeetechMotorsBus: # Detect a full rotation occured if abs(track["prev"][idx] - values[i]) > 2048: - # Position went below 0 and got reset to 4095 if track["prev"][idx] < values[i]: # So we set negative value by adding a full rotation diff --git a/lerobot/common/robot_devices/robots/manipulator.py b/lerobot/common/robot_devices/robots/manipulator.py index 1e8627a44..41cb10d4c 100644 --- a/lerobot/common/robot_devices/robots/manipulator.py +++ b/lerobot/common/robot_devices/robots/manipulator.py @@ -406,7 +406,6 @@ class ManipulatorRobot: for name in self.leader_arms: self.leader_arms[name].read("Present_Position") - # Connect the cameras for name in self.cameras: self.cameras[name].connect() @@ -536,12 +535,14 @@ class ManipulatorRobot: def set_so_100_robot_preset(self): for name in self.follower_arms: self.follower_arms[name].write("Mode", 0) - #self.follower_arms[name].write("P_Coefficient", 255, "shoulder_pan") + # self.follower_arms[name].write("P_Coefficient", 255, "shoulder_pan") self.follower_arms[name].write("P_Coefficient", 32, "shoulder_pan") - #self.follower_arms[name].write("D_Coefficient", 230, "shoulder_pan") + # self.follower_arms[name].write("D_Coefficient", 230, "shoulder_pan") self.follower_arms[name].write("D_Coefficient", 32, "shoulder_pan") - self.follower_arms[name].write("Acceleration", 254) - self.follower_arms[name].write("Minimum_Startup_Force", 16) + #self.follower_arms[name].write("Acceleration", 254) + #self.follower_arms[name].write("Minimum_Startup_Force", 16) + self.follower_arms[name].write("Lock", 0) + self.follower_arms[name].write("Maximum_Acceleration", 250) # for name in self.leader_arms: # self.leader_arms[name].write("Max_Torque_Limit", 50, "gripper") @@ -549,7 +550,6 @@ class ManipulatorRobot: # self.leader_arms[name].write("Torque_Enable", 1, "gripper") # self.leader_arms[name].write("Goal_Position", 2048, "gripper") - def teleop_step( self, record_data=False ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]: diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py index 078841088..f3b1917ae 100644 --- a/lerobot/scripts/configure_motor.py +++ b/lerobot/scripts/configure_motor.py @@ -13,21 +13,22 @@ python lerobot/scripts/configure_motor.py \ """ import argparse -import importlib import time def configure_motor(port, brand, model, motor_idx_des, baudrate_des): if brand == "feetech": - from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as motor_bus_class - from lerobot.common.robot_devices.motors.feetech import SCS_SERIES_BAUDRATE_TABLE as baudrate_table - from lerobot.common.robot_devices.motors.feetech import NUM_WRITE_RETRY as num_write_retry from lerobot.common.robot_devices.motors.feetech import MODEL_BAUDRATE_TABLE as model_baud_rate_table + from lerobot.common.robot_devices.motors.feetech import NUM_WRITE_RETRY as num_write_retry + from lerobot.common.robot_devices.motors.feetech import SCS_SERIES_BAUDRATE_TABLE as baudrate_table + from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as motor_bus_class elif brand == "dynamixel": - from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus as motor_bus_class - from lerobot.common.robot_devices.motors.dynamixel import X_SERIES_BAUDRATE_TABLE as baudrate_table + from lerobot.common.robot_devices.motors.dynamixel import ( + MODEL_BAUDRATE_TABLE as model_baud_rate_table, + ) from lerobot.common.robot_devices.motors.dynamixel import NUM_WRITE_RETRY as num_write_retry - from lerobot.common.robot_devices.motors.dynamixel import MODEL_BAUDRATE_TABLE as model_baud_rate_table + from lerobot.common.robot_devices.motors.dynamixel import X_SERIES_BAUDRATE_TABLE as baudrate_table + from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus as motor_bus_class else: raise ValueError( f"Currently we do not support this motor brand: {brand}. We currently support feetech and dynamixel motors." @@ -45,9 +46,7 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des): motor_model = model # Use the motor model passed via argument # Initialize the MotorBus with the correct port and motor configurations - motor_bus = motor_bus_class( - port=port, motors={motor_name: (motor_index_arbitrary, motor_model)} - ) + motor_bus = motor_bus_class(port=port, motors={motor_name: (motor_index_arbitrary, motor_model)}) # Try to connect to the motor bus and handle any connection-specific errors try: @@ -65,7 +64,7 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des): for baudrate in all_baudrates: motor_bus.set_bus_baudrate(baudrate) - present_ids = motor_bus.find_motor_indices(list(range(1,10))) + present_ids = motor_bus.find_motor_indices(list(range(1, 10))) if len(present_ids) > 1: raise ValueError( "Error: More than one motor ID detected. This script is designed to only handle one motor at a time. Please disconnect all but one motor." @@ -107,7 +106,6 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des): if present_baudrate_idx != baudrate_idx: raise OSError("Failed to write baudrate.") - motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0) print(f"Setting its index to desired index {motor_idx_des}") @@ -116,7 +114,7 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des): present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx_des, "ID") if present_idx != motor_idx_des: raise OSError("Failed to write index.") - + if brand == "feetech": motor_bus.write("Goal_Position", 2047) time.sleep(4) @@ -125,7 +123,6 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des): breakpoint() motor_bus.read("Present_Position") - except Exception as e: print(f"Error occurred during motor configuration: {e}")