diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index 0d5480f7a..dc268e560 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/lerobot/common/robot_devices/motors/feetech.py @@ -1,9 +1,11 @@ import enum +import json import logging import math import time import traceback from copy import deepcopy +from pathlib import Path import numpy as np import tqdm @@ -292,6 +294,7 @@ class FeetechMotorsBus: self.port_handler = None self.packet_handler = None self.calibration = None + self.calibration_path = None self.is_connected = False self.group_readers = {} self.group_writers = {} @@ -392,8 +395,9 @@ class FeetechMotorsBus: def motor_indices(self) -> list[int]: return [idx for idx, _ in self.motors.values()] - def set_calibration(self, calibration: dict[str, list]): + def set_calibration(self, calibration: dict[str, list], calibration_file: Path = None): self.calibration = calibration + self.calibration_file = calibration_file def apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None): """This function apply the calibration, automatically detects out of range errors for motors values and attempt to correct. @@ -579,6 +583,16 @@ class FeetechMotorsBus: # A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096. self.calibration["homing_offset"][calib_idx] += resolution * factor + # Once autocorrect calibration is done, also store the new homing_offset in yaml + if self.calibration_file is not None: + calibration = self.calibration + print( + f"Adjusting and storing homing calibration on axis '{calib_idx}' after triggering autocorrect_calibration" + ) + self.calibration_file.parent.mkdir(parents=True, exist_ok=True) + with open(self.calibration_file, "w") as f: + json.dump(calibration, f) + def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): """Inverse of `apply_calibration`.""" if motor_names is None: diff --git a/lerobot/common/robot_devices/robots/manipulator.py b/lerobot/common/robot_devices/robots/manipulator.py index 618105064..59c293d6f 100644 --- a/lerobot/common/robot_devices/robots/manipulator.py +++ b/lerobot/common/robot_devices/robots/manipulator.py @@ -384,14 +384,14 @@ class ManipulatorRobot: with open(arm_calib_path, "w") as f: json.dump(calibration, f) - return calibration + return calibration, arm_calib_path for name, arm in self.follower_arms.items(): - calibration = load_or_run_calibration_(name, arm, "follower") - arm.set_calibration(calibration) + calibration, arm_calib_path = load_or_run_calibration_(name, arm, "follower") + arm.set_calibration(calibration, calibration_file=arm_calib_path) for name, arm in self.leader_arms.items(): - calibration = load_or_run_calibration_(name, arm, "leader") - arm.set_calibration(calibration) + calibration, arm_calib_path = load_or_run_calibration_(name, arm, "leader") + arm.set_calibration(calibration, calibration_file=arm_calib_path) def set_koch_robot_preset(self): def set_operating_mode_(arm):