From fa8ed524fa75651fd5b7c60b5b1a3ed1c816a7e7 Mon Sep 17 00:00:00 2001 From: Pepijn Date: Wed, 29 Jan 2025 16:45:47 +0100 Subject: [PATCH] add first setup new calibration --- examples/10_use_so100.md | 11 - .../common/robot_devices/motors/feetech.py | 196 ++---------------- lerobot/scripts/read_pos.py | 174 ++++++++++++++++ 3 files changed, 192 insertions(+), 189 deletions(-) create mode 100644 lerobot/scripts/read_pos.py diff --git a/examples/10_use_so100.md b/examples/10_use_so100.md index 076b4662..1ce4cc41 100644 --- a/examples/10_use_so100.md +++ b/examples/10_use_so100.md @@ -369,15 +369,4 @@ If you have any question or need help, please reach out on Discord in the channe > [!CAUTION] > Advises about risks or negative outcomes of certain actions. - - -

- - -

Centered

diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index 0d5480f7..06589bd9 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/lerobot/common/robot_devices/motors/feetech.py @@ -1,6 +1,4 @@ import enum -import logging -import math import time import traceback from copy import deepcopy @@ -98,10 +96,6 @@ SCS_SERIES_BAUDRATE_TABLE = { 7: 19_200, } -CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"] -CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"] - - MODEL_CONTROL_TABLE = { "scs_series": SCS_SERIES_CONTROL_TABLE, "sts3215": SCS_SERIES_CONTROL_TABLE, @@ -133,6 +127,17 @@ def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str return steps +def convert_steps_to_degrees(steps: int | np.ndarray, models: str | list[str]) -> np.ndarray: + """This function converts the step range to the degrees range for indicating motors rotation. + It assumes a motor achieves a full rotation by going from -180 degree position to +180. + The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation. + """ + resolutions = [MODEL_RESOLUTION[model] for model in models] + steps = np.array(steps, dtype=float) + degrees = steps * (360.0 / resolutions) + return degrees + + def convert_to_bytes(value, bytes, mock=False): if mock: return value @@ -395,33 +400,7 @@ class FeetechMotorsBus: def set_calibration(self, calibration: dict[str, list]): 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. - - For more info, see docstring of `apply_calibration` and `autocorrect_calibration`. - """ - try: - values = self.apply_calibration(values, motor_names) - except JointOutOfRangeError as e: - print(e) - self.autocorrect_calibration(values, motor_names) - values = self.apply_calibration(values, motor_names) - return values - def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): - """Convert from unsigned int32 joint position range [0, 2**32[ to the universal float32 nominal degree range ]-180.0, 180.0[ with - a "zero position" at 0 degree. - - Note: We say "nominal degree range" since the motors can take values outside this range. For instance, 190 degrees, if the motor - rotate more than a half a turn from the zero position. However, most motors can't rotate more than 180 degrees and will stay in this range. - - Joints values are original in [0, 2**32[ (unsigned int32). Each motor are expected to complete a full rotation - when given a goal position that is + or - their resolution. For instance, feetech xl330-m077 have a resolution of 4096, and - at any position in their original range, let's say the position 56734, they complete a full rotation clockwise by moving to 60830, - or anticlockwise by moving to 52638. The position in the original range is arbitrary and might change a lot between each motor. - To harmonize between motors of the same model, different robots, or even models of different brands, we propose to work - in the centered nominal degree range ]-180, 180[. - """ if motor_names is None: motor_names = self.motor_names @@ -482,103 +461,6 @@ class FeetechMotorsBus: return values - def autocorrect_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): - """This function automatically detects issues with values of motors after calibration, and correct for these issues. - - Some motors might have values outside of expected maximum bounds after calibration. - For instance, for a joint in degree, its value can be outside [-270, 270] degrees, which is totally unexpected given - a nominal range of [-180, 180] degrees, which represents half a turn to the left or right starting from zero position. - - Known issues: - #1: Motor value randomly shifts of a full turn, caused by hardware/connection errors. - #2: Motor internal homing offset is shifted of a full turn, caused by using default calibration (e.g Aloha). - #3: motor internal homing offset is shifted of less or more than a full turn, caused by using default calibration - or by human error during manual calibration. - - Issues #1 and #2 can be solved by shifting the calibration homing offset by a full turn. - Issue #3 will be visually detected by user and potentially captured by the safety feature `max_relative_target`, - that will slow down the motor, raise an error asking to recalibrate. Manual recalibrating will solve the issue. - - Note: A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096. - """ - if motor_names is None: - motor_names = self.motor_names - - # Convert from unsigned int32 original range [0, 2**32] to signed float32 range - values = values.astype(np.float32) - - for i, name in enumerate(motor_names): - calib_idx = self.calibration["motor_names"].index(name) - calib_mode = self.calibration["calib_mode"][calib_idx] - - if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: - drive_mode = self.calibration["drive_mode"][calib_idx] - homing_offset = self.calibration["homing_offset"][calib_idx] - _, model = self.motors[name] - resolution = self.model_resolution[model] - - if drive_mode: - values[i] *= -1 - - # Convert from initial range to range [-180, 180] degrees - calib_val = (values[i] + homing_offset) / (resolution // 2) * HALF_TURN_DEGREE - in_range = (calib_val > LOWER_BOUND_DEGREE) and (calib_val < UPPER_BOUND_DEGREE) - - # Solve this inequality to find the factor to shift the range into [-180, 180] degrees - # values[i] = (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE - # - HALF_TURN_DEGREE <= (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE <= HALF_TURN_DEGREE - # (- HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset) / resolution <= factor <= (HALF_TURN_DEGREE / 180 * (resolution // 2) - values[i] - homing_offset) / resolution - low_factor = ( - -HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset - ) / resolution - upp_factor = ( - HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset - ) / resolution - - elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: - start_pos = self.calibration["start_pos"][calib_idx] - end_pos = self.calibration["end_pos"][calib_idx] - - # Convert from initial range to range [0, 100] in % - calib_val = (values[i] - start_pos) / (end_pos - start_pos) * 100 - in_range = (calib_val > LOWER_BOUND_LINEAR) and (calib_val < UPPER_BOUND_LINEAR) - - # Solve this inequality to find the factor to shift the range into [0, 100] % - # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos + resolution * factor - start_pos - resolution * factor) * 100 - # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 - # 0 <= (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 <= 100 - # (start_pos - values[i]) / resolution <= factor <= (end_pos - values[i]) / resolution - low_factor = (start_pos - values[i]) / resolution - upp_factor = (end_pos - values[i]) / resolution - - if not in_range: - # Get first integer between the two bounds - if low_factor < upp_factor: - factor = math.ceil(low_factor) - - if factor > upp_factor: - raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]") - else: - factor = math.ceil(upp_factor) - - if factor > low_factor: - raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]") - - if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: - out_of_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees" - in_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees" - elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: - out_of_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %" - in_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %" - - logging.warning( - f"Auto-correct calibration of motor '{name}' by shifting value by {abs(factor)} full turns, " - f"from '{out_of_range_str}' to '{in_range_str}'." - ) - - # A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096. - self.calibration["homing_offset"][calib_idx] += resolution * factor - def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): """Inverse of `apply_calibration`.""" if motor_names is None: @@ -618,43 +500,6 @@ class FeetechMotorsBus: values = np.round(values).astype(np.int32) return values - def avoid_rotation_reset(self, values, motor_names, data_name): - if data_name not in self.track_positions: - self.track_positions[data_name] = { - "prev": [None] * len(self.motor_names), - # Assume False at initialization - "below_zero": [False] * len(self.motor_names), - "above_max": [False] * len(self.motor_names), - } - - track = self.track_positions[data_name] - - if motor_names is None: - motor_names = self.motor_names - - for i, name in enumerate(motor_names): - idx = self.motor_names.index(name) - - if track["prev"][idx] is None: - track["prev"][idx] = values[i] - continue - - # 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 - values[i] -= 4096 - - # Position went above 4095 and got reset to 0 - elif track["prev"][idx] > values[i]: - # So we add a full rotation - values[i] += 4096 - - track["prev"][idx] = values[i] - - return values - def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY): if self.mock: import tests.mock_scservo_sdk as scs @@ -724,7 +569,11 @@ class FeetechMotorsBus: group_key = get_group_sync_key(data_name, motor_names) if data_name not in self.group_readers: - # create new group reader + # Very Important to flush the buffer! + self.port_handler.ser.reset_output_buffer() + self.port_handler.ser.reset_input_buffer() + + # Create new group reader self.group_readers[group_key] = scs.GroupSyncRead( self.port_handler, self.packet_handler, addr, bytes ) @@ -749,15 +598,7 @@ class FeetechMotorsBus: values = np.array(values) - # Convert to signed int to use range [-2048, 2048] for our motor positions. - if data_name in CONVERT_UINT32_TO_INT32_REQUIRED: - values = values.astype(np.int32) - - if data_name in CALIBRATION_REQUIRED: - values = self.avoid_rotation_reset(values, motor_names, data_name) - - if data_name in CALIBRATION_REQUIRED and self.calibration is not None: - values = self.apply_calibration_autocorrect(values, motor_names) + # TODO: Apply calibration and homign offset, output is homeing = 0 and direction. # log the number of seconds it took to read the data from the motors delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names) @@ -829,8 +670,7 @@ class FeetechMotorsBus: motor_ids.append(motor_idx) models.append(model) - if data_name in CALIBRATION_REQUIRED and self.calibration is not None: - values = self.revert_calibration(values, motor_names) + # TODO: add invesre of apply homing to go back to motor ticks values = values.tolist() diff --git a/lerobot/scripts/read_pos.py b/lerobot/scripts/read_pos.py new file mode 100644 index 00000000..d310418a --- /dev/null +++ b/lerobot/scripts/read_pos.py @@ -0,0 +1,174 @@ +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()