diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index af3dabff..f766a199 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/lerobot/common/robot_devices/motors/feetech.py @@ -118,7 +118,6 @@ def convert_ticks_to_degrees(ticks, model): return ticks * (360.0/resolutions) - def convert_degrees_to_ticks(degrees, model): resolutions = MODEL_RESOLUTION[model] # Convert degrees to motor ticks @@ -131,31 +130,14 @@ def adjusted_to_homing_ticks( """ Shifts raw [0..4095] ticks by an encoder offset, modulo a single turn [0..4095]. """ - # Retrieve previous values for tracking - prev_value = motorbus.previous_value[motor_id - 1] - multi_turn_index = motorbus.multi_turn_index[motor_id - 1] resolutions = MODEL_RESOLUTION[model] # Add offset and wrap within resolution - shifted = (raw_motor_ticks + encoder_offset) % resolutions + ticks = (raw_motor_ticks + encoder_offset) % resolutions # # Re-center into a symmetric range (e.g., [-2048, 2047] if resolutions==4096) Thus the middle homing position will be virtual 0. - if shifted > resolutions // 2: - shifted -= resolutions - - # Update multi turn values if needed - if prev_value is not None: - delta = shifted - prev_value - # If jump forward > 180° (2048 steps), assume full rotation - if delta > (resolutions // 2): - multi_turn_index -= 1 - elif delta < (-resolutions // 2): - multi_turn_index += 1 - motorbus.previous_value[motor_id - 1] = shifted - motorbus.multi_turn_index[motor_id - 1] = multi_turn_index - - # Apply the multi turn to output so we can track beyong -180..180 degrees or -2048..2048 ticks - ticks = shifted + (multi_turn_index * resolutions) + if ticks > resolutions // 2: + ticks -= resolutions # Update direction of rotation of the motor to match between leader and follower. # In fact, the motor of the leader for a given joint can be assembled in an @@ -175,18 +157,7 @@ def adjusted_to_motor_ticks( ) -> int: """ Inverse of adjusted_to_homing_ticks(). - Converts homed servo ticks (with multi-turn indexing) back to [0..4095]. """ - multi_turn_index = motorbus.multi_turn_index[motor_id - 1] - - resolutions = MODEL_RESOLUTION[model] - - # Remove offset and wrap within resolution - shifted = (adjusted_pos - encoder_offset) % resolutions - - # Apply the multi turn to output ticks because goal position can have input of -32000...32000 - ticks = shifted + (multi_turn_index * resolutions) - # Update direction of rotation of the motor to match between leader and follower. # In fact, the motor of the leader for a given joint can be assembled in an # opposite direction in term of rotation than the motor of the follower on the same joint. @@ -195,7 +166,12 @@ def adjusted_to_motor_ticks( drive_mode = motorbus.calibration["drive_mode"][motor_id - 1] if drive_mode: - ticks *= -1 + adjusted_pos *= -1 + + resolutions = MODEL_RESOLUTION[model] + + # Remove offset and wrap within resolution + ticks = (adjusted_pos - encoder_offset) % resolutions return ticks @@ -356,9 +332,6 @@ class FeetechMotorsBus: self.group_writers = {} self.logs = {} - self.multi_turn_index = self.multi_turn_index = [0] * len(self.motors) - self.previous_value = self.previous_value = [0] * len(self.motors) - def connect(self): if self.is_connected: raise RobotDeviceAlreadyConnectedError( diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py index 7a47dac2..caf80aac 100644 --- a/lerobot/common/robot_devices/robots/feetech_calibration.py +++ b/lerobot/common/robot_devices/robots/feetech_calibration.py @@ -118,10 +118,10 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...") - print("\nMove arm to homing position (middle)") + print("\nMove arm to homing position (zero)") print( "See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero") - ) # TODO(pepijn): replace with new instruction homing pos (all motors in center) in tutorial + ) # TODO(pepijn): replace with new instruction homing pos (all motors in zero) in tutorial input("Press Enter to continue...") start_positions = np.zeros(len(arm.motor_indices)) @@ -146,8 +146,9 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!") - # Force drive_mode values: motors 2 and 5 -> drive_mode 1; all others -> 0. - drive_modes = [0, 0, 0, 0, 0, 0] + # Force drive_mode values: motors 2 and 5 -> drive_mode 1; all others -> 0. 1 = clockwise = positive range (0..180), 0 = clockwise = negative range (0..-180) + # + drive_modes = [0, 1, 0, 0, 1, 0] calib_dict = { "homing_offset": encoder_offsets.astype(int).tolist(), diff --git a/lerobot/scripts/calibration_visualization.py b/lerobot/scripts/calibration_visualization.py index a1680c96..c646a21d 100644 --- a/lerobot/scripts/calibration_visualization.py +++ b/lerobot/scripts/calibration_visualization.py @@ -79,7 +79,6 @@ def debug_feetech_positions(cfg, arm_arg: str): if bus.calibration and name in bus.calibration["motor_names"]: offset_idx = bus.calibration["motor_names"].index(name) offset = bus.calibration["homing_offset"][offset_idx] - multi_turn_index = bus.multi_turn_index[offset_idx] # Manually compute "adjusted ticks" from raw ticks manual_adjusted = adjusted_to_homing_ticks(raw_ticks, offset, model, bus, motor_idx) @@ -98,8 +97,7 @@ def debug_feetech_positions(cfg, arm_arg: str): f"HOMED_TICKS={manual_adjusted:6d} | " f"MANUAL_ADJ_DEG={manual_degs:7.2f} | " f"MANUAL_ADJ_TICKS={manual_ticks:6d} | " - f"INV_TICKS={inv_ticks:4d} | " - f"MULTI_TURN_INDEX={multi_turn_index}" + f"INV_TICKS={inv_ticks:4d} " ) print("----------------------------------------------------") time.sleep(0.25) # slow down loop