diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index 39153af1..ae8c907a 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/lerobot/common/robot_devices/motors/feetech.py @@ -137,16 +137,14 @@ def convert_degrees_to_ticks(degrees, model): return int(degrees * (resolutions / 360.0)) -def adjusted_to_homing_ticks( - raw_motor_ticks: int, encoder_offset: int, model: str, motorbus, motor_id: int -) -> int: +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]. """ resolutions = MODEL_RESOLUTION[model] # Add offset and wrap within resolution - ticks = (raw_motor_ticks + encoder_offset) % resolutions + ticks = (raw_motor_ticks) % 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: @@ -165,9 +163,7 @@ def adjusted_to_homing_ticks( return ticks -def adjusted_to_motor_ticks( - adjusted_pos: int, encoder_offset: int, model: str, motorbus, motor_id: int -) -> int: +def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: int) -> int: """ Inverse of adjusted_to_homing_ticks(). """ @@ -184,7 +180,7 @@ def adjusted_to_motor_ticks( resolutions = MODEL_RESOLUTION[model] # Remove offset and wrap within resolution - ticks = (adjusted_pos - encoder_offset) % resolutions + ticks = (adjusted_pos) % resolutions return ticks @@ -453,11 +449,10 @@ class FeetechMotorsBus: calib_mode = self.calibration["calib_mode"][calib_idx] if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: - homing_offset = self.calibration["homing_offset"][calib_idx] motor_idx, model = self.motors[name] # Convert raw motor ticks to homed ticks, then convert the homed ticks to degrees - values[i] = adjusted_to_homing_ticks(values[i], homing_offset, model, self, motor_idx) + values[i] = adjusted_to_homing_ticks(values[i], model, self, motor_idx) values[i] = convert_ticks_to_degrees(values[i], model) elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: @@ -490,12 +485,11 @@ class FeetechMotorsBus: calib_mode = self.calibration["calib_mode"][calib_idx] if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: - homing_offset = self.calibration["homing_offset"][calib_idx] motor_idx, model = self.motors[name] # Convert degrees to homed ticks, then convert the homed ticks to raw ticks values[i] = convert_degrees_to_ticks(values[i], model) - values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx) + values[i] = adjusted_to_motor_ticks(values[i], model, self, motor_idx) elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: start_pos = self.calibration["start_pos"][calib_idx] diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py index 7f44fde7..6e61143e 100644 --- a/lerobot/common/robot_devices/robots/feetech_calibration.py +++ b/lerobot/common/robot_devices/robots/feetech_calibration.py @@ -41,32 +41,38 @@ 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 + 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! + motor_bus.write("Lock", 0) + + # Confirm that the offset is zero by reading it back + confirmed_offset = motor_bus.read("Offset")[motor_id - 1] + print(f"Offset for motor {motor_id} reset to: {confirmed_offset}") + return confirmed_offset + + def calibrate_homing_motor(motor_id, motor_bus): - """ - 1) Reads servo ticks. - 2) Calculates the offset so that 'home_ticks' becomes 0. - 3) Returns the offset - """ + reset_offset(motor_id, motor_bus) home_ticks = motor_bus.read("Present_Position")[motor_id - 1] # Read index starts at 0 + print(f"Encoder offset (present position in homing position): {home_ticks}") - # Calculate how many ticks to shift so that 'home_ticks' becomes 0 - raw_offset = -home_ticks # negative of home_ticks - encoder_offset = raw_offset % 4096 # wrap to [0..4095] - - # Convert to a signed range [-2048..2047] - if encoder_offset > 2047: - encoder_offset -= 4096 - - print(f"Encoder offset: {encoder_offset}") - - return encoder_offset + return home_ticks 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) + reset_offset(motor_id, motor_bus) + input(f"Close the {motor_name}, then press Enter...") start_pos = motor_bus.read("Present_Position")[motor_id - 1] # Read index starts ar 0 print(f" [Motor {motor_id}] start position recorded: {start_pos}") diff --git a/lerobot/scripts/calibration_visualization.py b/lerobot/scripts/calibration_visualization.py index c646a21d..92bce3b4 100644 --- a/lerobot/scripts/calibration_visualization.py +++ b/lerobot/scripts/calibration_visualization.py @@ -12,33 +12,98 @@ from lerobot.common.robot_devices.motors.feetech import ( ) # Replace this import with your real config class import -# (e.g., So100RobotConfig or any other) +# (e.g., So100RobotConfig or any other). from lerobot.common.robot_devices.robots.configs import So100RobotConfig from lerobot.common.robot_devices.robots.utils import make_robot_from_config +def apply_feetech_offsets_from_calibration(motors_bus: FeetechMotorsBus, 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 + motors_bus.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 motors_bus.motors: + print(f"Warning: '{m_name}' not found in motors_bus.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("CLAMPED! -> shouldn't happen") + elif new_offset < -2047: + new_offset = -2047 + print("CLAMPED! -> shouldn't 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 + motors_bus.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}" + ) + + motors_bus.write("Lock", 0) + print("Offsets have been saved to EEPROM successfully.") + + def debug_feetech_positions(cfg, arm_arg: str): """ Reads each joint’s (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks. + Optionally writes offsets from the calibration file into servo EEPROM. :param cfg: A config object (e.g. So100RobotConfig). :param arm_arg: One of "main_leader" or "main_follower". + :param apply_offsets: If True, calls apply_feetech_offsets_from_calibration() to save offsets to EEPROM. """ # 1) Make the Robot from your config - # e.g. `So100RobotConfig` might already be the "robot object", - # or if it is purely a config structure, do: robot = make_robot_from_config(cfg). robot = make_robot_from_config(cfg) # 2) Parse which arm we want: 'main_leader' or 'main_follower' if arm_arg not in ("main_leader", "main_follower"): raise ValueError("Please specify --arm=main_leader or --arm=main_follower") + # Grab the relevant bus config from your robot’s arms bus_config = robot.leader_arms["main"] if arm_arg == "main_leader" else robot.follower_arms["main"] # 3) Create the Feetech bus from that config bus = FeetechMotorsBus(bus_config) - # 4) (Optional) Load calibration if it exists + # 4) Load calibration if it exists calib_file = Path(robot.calibration_dir) / f"{arm_arg}.json" if calib_file.exists(): with open(calib_file) as f: @@ -52,6 +117,11 @@ def debug_feetech_positions(cfg, arm_arg: str): bus.connect() print(f"Connected to Feetech bus on port: {bus.port}") + # 5b) Apply offset to servo EEPROM so the servo’s Present_Position is hardware-shifted + if calibration_dict is not None: + print("Applying offsets from calibration object to servo EEPROM. Be careful—this is permanent!") + apply_feetech_offsets_from_calibration(bus, calibration_dict) + # 6) Disable torque on all motors so you can move them freely by hand bus.write("Torque_Enable", 0, motor_names=bus.motor_names) print("Torque disabled on all joints.") @@ -59,12 +129,12 @@ def debug_feetech_positions(cfg, arm_arg: str): try: print("\nPress Ctrl+C to quit.\n") while True: - # (a) Read *raw* positions (no calibration) + # (a) Read *raw* positions (no calibration). This bypasses bus.apply_calibration raw_positions = bus.read_with_motor_ids( bus.motor_models, bus.motor_indices, data_name="Present_Position" ) - # (b) Read *already-homed* positions (calibration is applied automatically) + # (b) Read *already-homed* positions (calibration is applied automatically inside bus.read()) homed_positions = bus.read("Present_Position") # Print them side by side @@ -74,21 +144,15 @@ def debug_feetech_positions(cfg, arm_arg: str): raw_ticks = raw_positions[i] # 0..4095 homed_val = homed_positions[i] # degrees or % if linear - # If you want to see how offset is used inside bus.read(), do it manually: - offset = 0 - 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] - # Manually compute "adjusted ticks" from raw ticks - manual_adjusted = adjusted_to_homing_ticks(raw_ticks, offset, model, bus, motor_idx) + manual_adjusted = adjusted_to_homing_ticks(raw_ticks, model, bus, motor_idx) # Convert to degrees manual_degs = convert_ticks_to_degrees(manual_adjusted, model) - # Convert to ticks + # Convert that deg back to ticks manual_ticks = convert_degrees_to_ticks(manual_degs, model) - # Invert - inv_ticks = adjusted_to_motor_ticks(manual_ticks, offset, model, bus, motor_idx) + # Then invert them using offset & bus drive mode + inv_ticks = adjusted_to_motor_ticks(manual_ticks, model, bus, motor_idx) print( f"{name:15s} | " @@ -122,5 +186,5 @@ if __name__ == "__main__": # 1) Instantiate the config you want (So100RobotConfig, or whichever your project uses). cfg = So100RobotConfig() - # 2) Call the function with (cfg, args.arm) + # 2) Call the function with (cfg, args.arm, args.apply_offsets) debug_feetech_positions(cfg, arm_arg=args.arm)