diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index d81fad802..ba794dde1 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/lerobot/common/robot_devices/motors/feetech.py @@ -144,14 +144,14 @@ def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_i """ resolutions = MODEL_RESOLUTION[model] - # 1) Shift raw ticks by half-resolution so 2048 -> 0, then wrap [0..res-1]. + # Shift raw ticks by half-resolution so 2048 -> 0, then wrap [0..res-1]. ticks = (raw_motor_ticks - (resolutions // 2)) % resolutions - # 2) If above halfway, fold it into negative territory => [-2048..+2047]. + # If above halfway, fold it into negative territory => [-2048..+2047]. if ticks > (resolutions // 2): ticks -= resolutions - # 3) Optionally flip sign if drive_mode is set. + # 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] @@ -167,7 +167,7 @@ def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: i 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. """ - # 1) Flip sign if drive_mode was set. + # 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] @@ -177,8 +177,8 @@ def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: i resolutions = MODEL_RESOLUTION[model] - # 2) Shift by +half-resolution and wrap into [0..res-1]. - # This undoes the earlier shift by -half-resolution. + # 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/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py index 6e61143ec..f84b5285f 100644 --- a/lerobot/common/robot_devices/robots/feetech_calibration.py +++ b/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 = { diff --git a/lerobot/common/robot_devices/robots/manipulator.py b/lerobot/common/robot_devices/robots/manipulator.py index 37fe71b4c..92fb0fafc 100644 --- a/lerobot/common/robot_devices/robots/manipulator.py +++ b/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}: " diff --git a/lerobot/scripts/calibration_visualization.py b/lerobot/scripts/calibration_visualization_so100.py similarity index 73% rename from lerobot/scripts/calibration_visualization.py rename to lerobot/scripts/calibration_visualization_so100.py index 92bce3b45..8abbb9ee3 100644 --- a/lerobot/scripts/calibration_visualization.py +++ b/lerobot/scripts/calibration_visualization_so100.py @@ -10,18 +10,14 @@ from lerobot.common.robot_devices.motors.feetech import ( convert_degrees_to_ticks, convert_ticks_to_degrees, ) - -# Replace this import with your real config class import -# (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): +def apply_feetech_offsets_from_calibration(motorsbus: 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). + 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 @@ -32,24 +28,24 @@ def apply_feetech_offsets_from_calibration(motors_bus: FeetechMotorsBus, calibra 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 - motors_bus.write("Lock", 1) + # 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 motors_bus.motors: - print(f"Warning: '{m_name}' not found in motors_bus.motors; skipping offset.") + 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") + 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 @@ -58,10 +54,14 @@ def apply_feetech_offsets_from_calibration(motors_bus: FeetechMotorsBus, calibra # Clamp to [-2047..+2047] if new_offset > 2047: new_offset = 2047 - print("CLAMPED! -> shouldn't happen") + 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("CLAMPED! -> shouldn't happen") + 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 @@ -70,40 +70,32 @@ def apply_feetech_offsets_from_calibration(motors_bus: FeetechMotorsBus, calibra # 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) + # Write offset 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}" ) - motors_bus.write("Lock", 0) + motorsbus.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 robot = make_robot_from_config(cfg) - # 2) Parse which arm we want: 'main_leader' or 'main_follower' + # 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) Load calibration if it exists + # 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: @@ -113,31 +105,29 @@ def debug_feetech_positions(cfg, arm_arg: str): else: print(f"No calibration file found at {calib_file}, skipping calibration set.") - # 5) Connect to the bus 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 + # 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 + # 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.") try: print("\nPress Ctrl+C to quit.\n") while True: - # (a) Read *raw* positions (no calibration). This bypasses bus.apply_calibration + # Read *raw* positions (no 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 inside bus.read()) + # Read *already-homed* positions homed_positions = bus.read("Present_Position") - # Print them side by side for i, name in enumerate(bus.motor_names): motor_idx, model = bus.motors[name] @@ -164,7 +154,7 @@ def debug_feetech_positions(cfg, arm_arg: str): f"INV_TICKS={inv_ticks:4d} " ) print("----------------------------------------------------") - time.sleep(0.25) # slow down loop + time.sleep(0.25) except KeyboardInterrupt: pass finally: @@ -182,9 +172,5 @@ if __name__ == "__main__": help="Which arm to debug: 'main_leader' or 'main_follower'.", ) args = parser.parse_args() - - # 1) Instantiate the config you want (So100RobotConfig, or whichever your project uses). cfg = So100RobotConfig() - - # 2) Call the function with (cfg, args.arm, args.apply_offsets) debug_feetech_positions(cfg, arm_arg=args.arm) diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py index f5d667cef..aba188cde 100644 --- a/lerobot/scripts/configure_motor.py +++ b/lerobot/scripts/configure_motor.py @@ -149,15 +149,10 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des): motor_bus.write("Min_Angle_Limit", 0) # default 0 motor_bus.write("Offset", 0) motor_bus.write("Mode", 0) - motor_bus.write("Goal_Position", 0) - motor_bus.write("Torque_Enable", 0) + motor_bus.write("Goal_Position", 2048) motor_bus.write("Lock", 1) print("Offset", motor_bus.read("Offset")) - # TODO(pepijn): Add when doing a motor configuration, you instantainesly home the position (while assembling) - # single_calibration = single_motor_calibration(motor_bus, motor_idx_des) - # Then store single_calibration in yaml via dict (not overwrite but add the single calibration each time for motor id) - except Exception as e: print(f"Error occurred during motor configuration: {e}")