update comments, clean up

lerobot/common/robot_devices/robots/feetech_calibration.py

@@ -11,10 +11,6 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
-
-"""Logic to calibrate a robot arm built with feetech motors"""
-# TODO(rcadene, aliberts): move this logic into the robot code when refactoring
-
 import numpy as np
 
 from lerobot.common.robot_devices.motors.feetech import (
@@ -42,14 +38,14 @@ def get_calibration_modes(arm: MotorsBus):
 
 
 def reset_offset(motor_id, motor_bus):
-    # Open the write lock => Lock=1 => changes to EEPROM do NOT persist yet
+    # Open the write lock, changes to EEPROM do NOT persist yet
     motor_bus.write("Lock", 1)
 
     # Set offset to 0
     motor_name = motor_bus.motor_names[motor_id - 1]
     motor_bus.write("Offset", 0, motor_names=[motor_name])
 
-    # Close the write lock => Lock=0 => changes to EEPROM do persist!
+    # Close the write lock, changes to EEPROM do persist
     motor_bus.write("Lock", 0)
 
     # Confirm that the offset is zero by reading it back
@@ -69,7 +65,7 @@ def calibrate_homing_motor(motor_id, motor_bus):
 
 def calibrate_linear_motor(motor_id, motor_bus):
     motor_names = motor_bus.motor_names
-    motor_name = motor_names[motor_id - 1]  # TODO(pepijn): replace motor_id with motor index when (id-1)
+    motor_name = motor_names[motor_id - 1]
 
     reset_offset(motor_id, motor_bus)
 
@@ -166,7 +162,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. 1 = clockwise = positive range (0..180), 0 = clockwise = negative range (0..-180)
+    # Force drive_mode values (can be static)
     drive_modes = [0, 1, 0, 0, 1, 0]
 
     calib_dict = {

lerobot/common/robot_devices/robots/manipulator.py

@@ -57,8 +57,7 @@ def ensure_safe_goal_position(
 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).
+    then writes each motor's offset to the servo's internal Offset (0x1F) in EPROM.
 
     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
@@ -69,16 +68,16 @@ def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
       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"]
+    start_pos = calibration_dict["start_pos"]
 
-    # 1) Open the write lock => Lock=1 => changes to EEPROM do NOT persist yet
+    # Open the write lock, changes to EEPROM do NOT persist yet
     motorsbus.write("Lock", 1)
 
-    # 2) For each motor, set the 'Offset' parameter
+    # 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:
@@ -86,7 +85,7 @@ def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
             continue
 
         if m_name == "gripper":
-            print("Skipping gripper")
+            old_offset = start_pos  # If gripper set the offset to the start position of the gripper
             continue
 
         # Shift the offset so the homed position reads 2047
@@ -111,7 +110,7 @@ def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
         # Combine sign bit (bit 11) with the magnitude (bits 0..10)
         servo_offset = (direction_bit << 11) | magnitude
 
-        # Write to servo
+        # Write offset to servo
         motorsbus.write("Offset", servo_offset, motor_names=m_name)
         print(
             f"Set offset for {m_name}: "