Add back drive mode

lerobot/common/robot_devices/motors/feetech.py

@@ -141,6 +141,10 @@ 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]
@@ -163,7 +167,15 @@ def adjusted_to_homing_ticks(
     motorbus.previous_value[motor_id - 1] = shifted
     motorbus.multi_turn_index[motor_id - 1] = multi_turn_index
 
-    return shifted + (multi_turn_index * resolutions)
+    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.
+    if drive_mode:
+        ticks *= -1
+
+    return ticks
 
 
 def adjusted_to_motor_ticks(
@@ -173,6 +185,13 @@ def adjusted_to_motor_ticks(
     Inverse of adjusted_to_homing_ticks().
     Converts homed servo ticks (with multi-turn indexing) back to [0..4095].
     """
+    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
+
     resolutions = MODEL_RESOLUTION[model]
     # Get the current multi-turn index and remove that offset
     multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
@@ -456,7 +475,6 @@ class FeetechMotorsBus:
             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]
                 motor_idx, model = self.motors[name]
 
@@ -464,12 +482,6 @@ class FeetechMotorsBus:
                 values[i] = adjusted_to_homing_ticks(values[i], homing_offset, model, self, motor_idx)
                 values[i] = convert_ticks_to_degrees(values[i], model)
 
-                # 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.
-                if drive_mode:
-                    values[i] *= -1
-
             elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
                 start_pos = self.calibration["start_pos"][calib_idx]
                 end_pos = self.calibration["end_pos"][calib_idx]
@@ -500,7 +512,6 @@ class FeetechMotorsBus:
             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]
                 motor_idx, model = self.motors[name]
 
@@ -508,11 +519,6 @@ class FeetechMotorsBus:
                 values[i] = convert_degrees_to_ticks(values[i], model, self, motor_idx)
                 values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx)
 
-                # Remove drive mode, which is the rotation direction of the motor, to come back to
-                # actual motor rotation direction which can be arbitrary.
-                if drive_mode:
-                    values[i] *= -1
-
             elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
                 start_pos = self.calibration["start_pos"][calib_idx]
                 end_pos = self.calibration["end_pos"][calib_idx]

lerobot/scripts/calibration_visualization.py

@@ -84,6 +84,7 @@ def debug_feetech_positions(cfg, arm_arg: str):
                 manual_adjusted = adjusted_to_homing_ticks(raw_ticks, offset, model, bus, motor_idx)
                 # Convert to degrees
                 manual_degs = convert_ticks_to_degrees(manual_adjusted, model)
+
                 # Convert to ticks
                 manual_ticks = convert_degrees_to_ticks(manual_degs, model, bus, motor_idx)
                 # Invert
@@ -96,7 +97,7 @@ def debug_feetech_positions(cfg, arm_arg: str):
                     f"MANUAL_ADJ_TICKS={manual_adjusted:6d} | "
                     f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
                     f"INV_TICKS={manual_ticks:6d} | "
-                    f"INV_STEPS={inv_ticks:4d}"
+                    f"INV_TICKS={inv_ticks:4d}"
                 )
             print("----------------------------------------------------")
             time.sleep(0.25)  # slow down loop