diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index 219e5ab8..441d1ee0 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/lerobot/common/robot_devices/motors/feetech.py @@ -96,6 +96,8 @@ SCS_SERIES_BAUDRATE_TABLE = { 7: 19_200, } +CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"] + MODEL_CONTROL_TABLE = { "scs_series": SCS_SERIES_CONTROL_TABLE, "sts3215": SCS_SERIES_CONTROL_TABLE, @@ -471,6 +473,8 @@ class FeetechMotorsBus: start_pos = self.calibration["start_pos"][calib_idx] end_pos = self.calibration["end_pos"][calib_idx] + # TODO(pepijn): Check if a homing (something similar to homing) should be applied to linear joint (think so) + # Rescale the present position to a nominal range [0, 100] %, # useful for joints with linear motions like Aloha gripper values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100 @@ -619,7 +623,7 @@ class FeetechMotorsBus: values = np.array(values) - if self.calibration is not None: + if data_name in CALIBRATION_REQUIRED and self.calibration is not None: values = self.apply_calibration(values, motor_names) # log the number of seconds it took to read the data from the motors @@ -692,7 +696,7 @@ class FeetechMotorsBus: motor_ids.append(motor_idx) models.append(model) - if self.calibration is not None: + if data_name in CALIBRATION_REQUIRED and self.calibration is not None: values = self.revert_calibration(values, motor_names) values = values.tolist() diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py index 5816126e..73e718e8 100644 --- a/lerobot/common/robot_devices/robots/feetech_calibration.py +++ b/lerobot/common/robot_devices/robots/feetech_calibration.py @@ -57,7 +57,7 @@ def calibrate_linear_motor(motor_id, motor_bus): start_pos = motor_bus.read("Present_Position")[motor_id - 1] # Read index starts ar 0 print(f" [Motor {motor_id}] start position recorded: {start_pos}") - input("Open the {motor_name} fully, then press Enter...") + input(f"Open the {motor_name} fully, then press Enter...") end_pos = motor_bus.read("Present_Position")[motor_id - 1] # Read index starts ar 0 print(f" [Motor {motor_id}] end position recorded: {end_pos}") diff --git a/lerobot/scripts/read_pos.py b/lerobot/scripts/read_pos.py deleted file mode 100644 index d310418a..00000000 --- a/lerobot/scripts/read_pos.py +++ /dev/null @@ -1,174 +0,0 @@ -import time - - -def calibrate_motor(motor_id, motor_bus): - """ - 1) Prompts user to move the motor to the "home" position. - 2) Reads servo ticks. - 3) Calculates the offset so that 'home_ticks' becomes 0. - 4) Returns the offset - """ - print(f"\n--- Calibrating Motor {motor_id} ---") - input("Move the motor to middle (home) position, then press Enter...") - home_ticks = motor_bus.read("Present_Position")[0] - print(f" [Motor {motor_id}] middle position recorded: {home_ticks}") - - # Calculate how many ticks to shift so that 'home_ticks' becomes 0 - raw_offset = -home_ticks # negative of home_ticks - encoder_offset = raw_offset % 4096 # wrap to [0..4095] - - # Convert to a signed range [-2048..2047] - if encoder_offset > 2047: - encoder_offset -= 4096 - - print(f"Encoder offset: {encoder_offset}") - - return encoder_offset - - -def adjusted__to_homing_ticks(raw_motor_ticks, encoder_offset): - shifted = (raw_motor_ticks + encoder_offset) % 4096 - if shifted > 2047: - shifted -= 4096 - return shifted - - -def adjusted_to_motor_ticks(adjusted_pos, encoder_offset): - """ - Inverse of read_adjusted_position(). - - adjusted_pos : int in [-2048 .. +2047] (the homed, shifted value) - encoder_offset : int (the offset computed so that `home` becomes zero) - - Returns the raw servo ticks in [0..4095]. - """ - if adjusted_pos < 0: - adjusted_pos += 4096 - - raw_ticks = (adjusted_pos - encoder_offset) % 4096 - return raw_ticks - - -def configure_and_calibrate(): - from lerobot.common.robot_devices.motors.feetech import ( - SCS_SERIES_BAUDRATE_TABLE as SERIES_BAUDRATE_TABLE, - ) - from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as MotorsBusClass - - motor_idx_des = 1 # index you want to assign - - # Setup motor names, indices, and models - motor_name = "motor" - motor_index_arbitrary = motor_idx_des # Use the motor ID passed via argument - motor_model = "sts3215" # Use the motor model passed via argument - - # Initialize the MotorBus with the correct port and motor configurations - motor_bus = MotorsBusClass( - port="/dev/tty.usbmodem585A0078271", motors={motor_name: (motor_index_arbitrary, motor_model)} - ) - - # Try to connect to the motor bus and handle any connection-specific errors - try: - motor_bus.connect() - print(f"Connected on port {motor_bus.port}") - except OSError as e: - print(f"Error occurred when connecting to the motor bus: {e}") - return - - try: - print("Scanning all baudrates and motor indices") - all_baudrates = set(SERIES_BAUDRATE_TABLE.values()) - motor_index = -1 # Set the motor index to an out-of-range value. - - for baudrate in all_baudrates: - motor_bus.set_bus_baudrate(baudrate) - 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." - ) - - if len(present_ids) == 1: - if motor_index != -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." - ) - motor_index = present_ids[0] - - if motor_index == -1: - raise ValueError("No motors detected. Please ensure you have one motor connected.") - - print(f"Motor index found at: {motor_index}") - - motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0) - - baudrate_des = 1000000 - - if baudrate != baudrate_des: - print(f"Setting its baudrate to {baudrate_des}") - baudrate_idx = list(SERIES_BAUDRATE_TABLE.values()).index(baudrate_des) - - # The write can fail, so we allow retries - motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Baud_Rate", baudrate_idx) - time.sleep(0.5) - motor_bus.set_bus_baudrate(baudrate_des) - present_baudrate_idx = motor_bus.read_with_motor_ids( - motor_bus.motor_models, motor_index, "Baud_Rate", num_retry=2 - ) - - if present_baudrate_idx != baudrate_idx: - raise OSError("Failed to write baudrate.") - - print(f"Setting its index to desired index {motor_idx_des}") - motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0) - motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "ID", motor_idx_des) - - present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx_des, "ID", num_retry=2) - if present_idx != motor_idx_des: - raise OSError("Failed to write index.") - - # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of - # the motors. Note: this configuration is not in the official STS3215 Memory Table - motor_bus.write("Lock", 0) - motor_bus.write("Maximum_Acceleration", 254) - - motor_bus.write("Max_Angle_Limit", 4095) # default 4095 - motor_bus.write("Min_Angle_Limit", 0) # default 0 - motor_bus.write("Offset", 0) - motor_bus.write("Mode", 0) - motor_bus.write("Goal_Position", 0) - motor_bus.write("Torque_Enable", 0) - - motor_bus.write("Lock", 1) - - # Calibration - print("Offset", motor_bus.read("Offset")) - print("Max_Angle_Limit", motor_bus.read("Max_Angle_Limit")) - print("Min_Angle_Limit", motor_bus.read("Min_Angle_Limit")) - print("Goal_Position", motor_bus.read("Goal_Position")) - print("Present Position", motor_bus.read("Present_Position")) - - encoder_offset = calibrate_motor(motor_idx_des, motor_bus) - - try: - while True: - raw_motor_ticks = motor_bus.read("Present_Position")[0] - adjusted_ticks = adjusted__to_homing_ticks(raw_motor_ticks, encoder_offset) - inverted_adjusted_ticks = adjusted_to_motor_ticks(adjusted_ticks, encoder_offset) - print( - f"Raw Motor ticks: {raw_motor_ticks} | Adjusted ticks: {adjusted_ticks} | Invert adjusted ticks: {inverted_adjusted_ticks}" - ) - time.sleep(0.3) - except KeyboardInterrupt: - print("Stopped reading positions.") - - except Exception as e: - print(f"Error occurred during motor configuration: {e}") - - finally: - motor_bus.disconnect() - print("Disconnected from motor bus.") - - -if __name__ == "__main__": - configure_and_calibrate()