Zero homing, same values as previous calib

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(

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

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