With multi turn (code is good, but feetech can't handle multi turn commands)

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 the ticks to degrees 
-
+    return ticks * (360.0/resolutions)
-    # Convert to range [-180, 180]
-    degrees = (degrees + 180) % 360 - 180
-    return degrees
 
 
-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)
+    return int(degrees * (resolutions/360.0))
-
-    # Add back multi-turn ticks
-    ticks += multi_turn_index * resolutions
-
-    return int(ticks)
 
 
 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]
+    return ticks
-    # 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
 
 
 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()
+

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(),

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"HOMED_FROM_READ={homed_val:7.2f} | "
-                    f"MANUAL_ADJ_TICKS={manual_adjusted:6d} | "
+                    f"HOMED_TICKS={manual_adjusted:6d} | "
                     f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
-                    f"INV_TICKS={manual_ticks:6d} | "
+                    f"MANUAL_ADJ_TICKS={manual_ticks:6d} | "
-                    f"INV_TICKS={inv_ticks:4d}"
+                    f"INV_TICKS={inv_ticks:4d} | "
+                    f"MULTI_TURN_INDEX={multi_turn_index}"
                 )
             print("----------------------------------------------------")
             time.sleep(0.25)  # slow down loop