diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index ae8c907a8..d81fad802 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/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 - ticks = (raw_motor_ticks) % resolutions + # 1) Shift raw ticks by half-resolution so 2048 -> 0, then wrap [0..res-1]. + 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. - if ticks > resolutions // 2: + # 2) If above halfway, fold it into negative territory => [-2048..+2047]. + 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 - # opposite direction in term of rotation than the motor of the follower on the same joint. + # 3) Optionally flip sign if drive_mode is set. 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. - # 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. + # 1) Flip sign if drive_mode was set. 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 - ticks = (adjusted_pos) % resolutions + # 2) Shift by +half-resolution and wrap into [0..res-1]. + # This undoes the earlier shift by -half-resolution. + ticks = (adjusted_pos + (resolutions // 2)) % resolutions return ticks diff --git a/lerobot/common/robot_devices/robots/manipulator.py b/lerobot/common/robot_devices/robots/manipulator.py index 815a3f1de..37fe71b4c 100644 --- a/lerobot/common/robot_devices/robots/manipulator.py +++ b/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(): - calibration = load_or_run_calibration_(name, arm, "follower") - arm.set_calibration(calibration) - for name, arm in self.leader_arms.items(): - calibration = load_or_run_calibration_(name, arm, "leader") - arm.set_calibration(calibration) + # For each follower arm + + for name, arm_bus in self.follower_arms.items(): + calibration = load_or_run_calibration_(name, arm_bus, "follower") + arm_bus.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):