diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index d1d518ff..af3dabff 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/lerobot/common/robot_devices/motors/feetech.py @@ -1,3 +1,4 @@ + import enum import time import traceback @@ -113,26 +114,15 @@ NUM_WRITE_RETRY = 20 def convert_ticks_to_degrees(ticks, model): resolutions = MODEL_RESOLUTION[model] - degrees = (ticks / resolutions) * 360.0 # Convert to 0-360 range - - # Convert to range [-180, 180] - degrees = (degrees + 180) % 360 - 180 - return degrees + # Convert the ticks to degrees + return ticks * (360.0/resolutions) -def convert_degrees_to_ticks(degrees, model, motorbus, motor_id: int): - multi_turn_index = motorbus.multi_turn_index[motor_id - 1] + +def convert_degrees_to_ticks(degrees, model): resolutions = MODEL_RESOLUTION[model] - # Remove full rotations from degrees - base_degrees = degrees - (multi_turn_index * 360.0) - # Convert degrees to motor ticks - ticks = base_degrees / 180.0 * (resolutions / 2) - - # Add back multi-turn ticks - ticks += multi_turn_index * resolutions - - return int(ticks) + return int(degrees * (resolutions/360.0)) def adjusted_to_homing_ticks( @@ -141,37 +131,39 @@ def adjusted_to_homing_ticks( """ Shifts raw [0..4095] ticks by an encoder offset, modulo a single turn [0..4095]. """ - drive_mode = 0 - if motorbus.calibration is not None: - drive_mode = motorbus.calibration["drive_mode"][motor_id - 1] - # 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 + + # # 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: + if delta > (resolutions // 2): multi_turn_index -= 1 - elif delta < -resolutions // 2: + elif delta < (-resolutions // 2): multi_turn_index += 1 - - # Update stored values 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) # 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. + drive_mode = 0 + if motorbus.calibration is not None: + drive_mode = motorbus.calibration["drive_mode"][motor_id - 1] + if drive_mode: ticks *= -1 @@ -185,27 +177,27 @@ def adjusted_to_motor_ticks( 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. drive_mode = 0 if motorbus.calibration is not None: drive_mode = motorbus.calibration["drive_mode"][motor_id - 1] - # If inverted, flip the adjusted value back. + if drive_mode: - adjusted_pos *= -1 + ticks *= -1 - resolutions = MODEL_RESOLUTION[model] - # Get the current multi-turn index and remove that offset - multi_turn_index = motorbus.multi_turn_index[motor_id - 1] - adjusted_pos -= multi_turn_index * 4096 - - # Convert back into [−2048..2047] before final modulo - if adjusted_pos > 2047: - adjusted_pos -= 4096 - elif adjusted_pos < -2048: - adjusted_pos += 4096 - - # Map back to raw ticks [0..4095] - raw_ticks = (adjusted_pos - encoder_offset) % resolutions - return raw_ticks + return ticks def convert_to_bytes(value, bytes, mock=False): @@ -516,7 +508,7 @@ class FeetechMotorsBus: motor_idx, model = self.motors[name] # Convert degrees to homed ticks, then convert the homed ticks to raw ticks - values[i] = convert_degrees_to_ticks(values[i], model, self, motor_idx) + values[i] = convert_degrees_to_ticks(values[i], model) values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx) elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: @@ -757,3 +749,4 @@ class FeetechMotorsBus: def __del__(self): if getattr(self, "is_connected", False): self.disconnect() + diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py index cbdada0b..7a47dac2 100644 --- a/lerobot/common/robot_devices/robots/feetech_calibration.py +++ b/lerobot/common/robot_devices/robots/feetech_calibration.py @@ -147,7 +147,7 @@ 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, 1, 0, 0, 1, 0] + drive_modes = [0, 0, 0, 0, 0, 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 cb80716f..a1680c96 100644 --- a/lerobot/scripts/calibration_visualization.py +++ b/lerobot/scripts/calibration_visualization.py @@ -79,6 +79,7 @@ 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) @@ -86,18 +87,19 @@ def debug_feetech_positions(cfg, arm_arg: str): manual_degs = convert_ticks_to_degrees(manual_adjusted, model) # Convert to ticks - manual_ticks = convert_degrees_to_ticks(manual_degs, model, bus, motor_idx) + manual_ticks = convert_degrees_to_ticks(manual_degs, model) # Invert inv_ticks = adjusted_to_motor_ticks(manual_ticks, offset, model, bus, motor_idx) print( f"{name:15s} | " f"RAW={raw_ticks:4d} | " - f"HOMED={homed_val:7.2f} | " - f"MANUAL_ADJ_TICKS={manual_adjusted:6d} | " + f"HOMED_FROM_READ={homed_val:7.2f} | " + f"HOMED_TICKS={manual_adjusted:6d} | " f"MANUAL_ADJ_DEG={manual_degs:7.2f} | " - f"INV_TICKS={manual_ticks:6d} | " - f"INV_TICKS={inv_ticks:4d}" + f"MANUAL_ADJ_TICKS={manual_ticks:6d} | " + f"INV_TICKS={inv_ticks:4d} | " + f"MULTI_TURN_INDEX={multi_turn_index}" ) print("----------------------------------------------------") time.sleep(0.25) # slow down loop