Update feetech and manipulator

lerobot/common/robot_devices/motors/feetech.py

@@ -139,20 +139,19 @@ def convert_degrees_to_ticks(degrees, model):
 
 def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_id: int) -> int:
     """
-    Shifts raw [0..4095] ticks by an encoder offset, modulo a single turn [0..4095].
+    Takes a raw reading [0..(res-1)] (e.g. 0..4095) and shifts it so that '2048'
+    becomes 0 in the homed coordinate system ([-2048..+2047] for 4096 resolution).
     """
     resolutions = MODEL_RESOLUTION[model]
 
-    # Add offset and wrap within resolution
+    # 1) Shift raw ticks by half-resolution so 2048 -> 0, then wrap [0..res-1].
-    ticks = (raw_motor_ticks) % resolutions
+    ticks = (raw_motor_ticks - (resolutions // 2)) % resolutions
 
-    # # Re-center into a symmetric range (e.g., [-2048, 2047] if resolutions==4096) Thus the middle homing position will be virtual 0.
+    # 2) If above halfway, fold it into negative territory => [-2048..+2047].
-    if ticks > resolutions // 2:
+    if ticks > (resolutions // 2):
         ticks -= resolutions
 
-    # Update direction of rotation of the motor to match between leader and follower.
+    # 3) Optionally flip sign if drive_mode is set.
-    # 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]
@@ -165,11 +164,10 @@ def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_i
 
 def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: int) -> int:
     """
-    Inverse of adjusted_to_homing_ticks().
+    Inverse of adjusted_to_homing_ticks(). Takes a 'homed' position in [-2048..+2047]
+    and recovers the raw [0..(res-1)] ticks with 2048 as midpoint.
     """
-    # Update direction of rotation of the motor to match between leader and follower.
+    # 1) Flip sign if drive_mode was set.
-    # 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]
@@ -179,8 +177,9 @@ def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: i
 
     resolutions = MODEL_RESOLUTION[model]
 
-    # Remove offset and wrap within resolution
+    # 2) Shift by +half-resolution and wrap into [0..res-1].
-    ticks = (adjusted_pos) % resolutions
+    #    This undoes the earlier shift by -half-resolution.
+    ticks = (adjusted_pos + (resolutions // 2)) % resolutions
 
     return ticks
 

lerobot/common/robot_devices/robots/manipulator.py

@@ -54,6 +54,74 @@ def ensure_safe_goal_position(
     return safe_goal_pos
 
 
+def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
+    """
+    Reads 'calibration_dict' containing 'homing_offset' and 'motor_names',
+    then writes each motor's offset to the servo's internal Offset (0x1F) in EPROM,
+    except for any motor whose name contains "gripper" (skipped).
+
+    This version is modified so each homed position (originally 0) will now read
+    2047, i.e. 180° away from 0 in the 4096-count circle. Offsets are permanently
+    stored in EEPROM, so the servo's Present_Position is hardware-shifted even
+    after power cycling.
+
+    Steps:
+      1) Subtract 2047 from the old offset (so 0 -> 2047).
+      2) Clamp to [-2047..+2047].
+      3) Encode sign bit and magnitude into a 12-bit number.
+      4) Skip "gripper" motors, as they do not require this shift.
+    """
+
+    homing_offsets = calibration_dict["homing_offset"]
+    motor_names = calibration_dict["motor_names"]
+
+    # 1) Open the write lock => Lock=1 => changes to EEPROM do NOT persist yet
+    motorsbus.write("Lock", 1)
+
+    # 2) For each motor, set the 'Offset' parameter
+    for m_name, old_offset in zip(motor_names, homing_offsets, strict=False):
+        # If bus doesn’t have a motor named m_name, skip
+        if m_name not in motorsbus.motors:
+            print(f"Warning: '{m_name}' not found in motorsbus.motors; skipping offset.")
+            continue
+
+        if m_name == "gripper":
+            print("Skipping gripper")
+            continue
+
+        # Shift the offset so the homed position reads 2047
+        new_offset = old_offset - 2047
+
+        # Clamp to [-2047..+2047]
+        if new_offset > 2047:
+            new_offset = 2047
+            print(
+                f"Warning: '{new_offset}' is getting clamped because its larger then 2047; This should not happen!"
+            )
+        elif new_offset < -2047:
+            new_offset = -2047
+            print(
+                f"Warning: '{new_offset}' is getting clamped because its smaller then -2047; This should not happen!"
+            )
+
+        # Determine the direction (sign) bit and magnitude
+        direction_bit = 1 if new_offset < 0 else 0
+        magnitude = abs(new_offset)
+
+        # Combine sign bit (bit 11) with the magnitude (bits 0..10)
+        servo_offset = (direction_bit << 11) | magnitude
+
+        # Write to servo
+        motorsbus.write("Offset", servo_offset, motor_names=m_name)
+        print(
+            f"Set offset for {m_name}: "
+            f"old_offset={old_offset}, new_offset={new_offset}, servo_encoded={magnitude} + direction={direction_bit}"
+        )
+
+    motorsbus.write("Lock", 0)
+    print("Offsets have been saved to EEPROM successfully.")
+
+
 class ManipulatorRobot:
     # TODO(rcadene): Implement force feedback
     """This class allows to control any manipulator robot of various number of motors.
@@ -327,12 +395,24 @@ class ManipulatorRobot:
 
             return calibration
 
-        for name, arm in self.follower_arms.items():
+            # For each follower arm
-            calibration = load_or_run_calibration_(name, arm, "follower")
+
-            arm.set_calibration(calibration)
+        for name, arm_bus in self.follower_arms.items():
-        for name, arm in self.leader_arms.items():
+            calibration = load_or_run_calibration_(name, arm_bus, "follower")
-            calibration = load_or_run_calibration_(name, arm, "leader")
+            arm_bus.set_calibration(calibration)
-            arm.set_calibration(calibration)
+
+            # If this is a Feetech robot, also set the servo offset into EEPROM
+            if self.robot_type in ["so100", "lekiwi"]:
+                apply_feetech_offsets_from_calibration(arm_bus, calibration)
+
+        # For each leader arm
+        for name, arm_bus in self.leader_arms.items():
+            calibration = load_or_run_calibration_(name, arm_bus, "leader")
+            arm_bus.set_calibration(calibration)
+
+            # Optionally also set offset for leader if you want the servo offsets as well
+            if self.robot_type in ["so100", "lekiwi"]:
+                apply_feetech_offsets_from_calibration(arm_bus, calibration)
 
     def set_koch_robot_preset(self):
         def set_operating_mode_(arm):