tangger/lerobot
1
0
Fork 1
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

Store homing after autocorrect calib

Browse Source 
Store new homing position in json after autocorrect calibration
This commit is contained in:
Pepijn
2025-01-24 17:13:31 +01:00
parent 4def6d6ac2
commit e6305ff5fd
2 changed files with 20 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
lerobot/common/robot_devices/motors/feetech.py
View File

@@ -1,9 +1,11 @@
import enum import enum
import json
import logging import logging
import math import math
import time import time
import traceback import traceback
from copy import deepcopy from copy import deepcopy
from pathlib import Path
import numpy as np import numpy as np
import tqdm import tqdm
@@ -292,6 +294,7 @@ class FeetechMotorsBus:
self.port_handler = None self.port_handler = None
self.packet_handler = None self.packet_handler = None
self.calibration = None self.calibration = None
self.calibration_path = None
self.is_connected = False self.is_connected = False
self.group_readers = {} self.group_readers = {}
self.group_writers = {} self.group_writers = {}
@@ -392,8 +395,9 @@ class FeetechMotorsBus:
def motor_indices(self) -> list[int]: def motor_indices(self) -> list[int]:
return [idx for idx, _ in self.motors.values()] 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 = calibration
self.calibration_file = calibration_file
def apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None): 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. """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. # 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 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): def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
"""Inverse of `apply_calibration`.""" """Inverse of `apply_calibration`."""
if motor_names is None: if motor_names is None:

10
lerobot/common/robot_devices/robots/manipulator.py
View File

@@ -384,14 +384,14 @@ class ManipulatorRobot:
with open(arm_calib_path, "w") as f: with open(arm_calib_path, "w") as f:
json.dump(calibration, f) json.dump(calibration, f)
return calibration return calibration, arm_calib_path
for name, arm in self.follower_arms.items(): for name, arm in self.follower_arms.items():
calibration = load_or_run_calibration_(name, arm, "follower") calibration, arm_calib_path = load_or_run_calibration_(name, arm, "follower")
arm.set_calibration(calibration) arm.set_calibration(calibration, calibration_file=arm_calib_path)
for name, arm in self.leader_arms.items(): for name, arm in self.leader_arms.items():
calibration = load_or_run_calibration_(name, arm, "leader") calibration, arm_calib_path = load_or_run_calibration_(name, arm, "leader")
arm.set_calibration(calibration) arm.set_calibration(calibration, calibration_file=arm_calib_path)
def set_koch_robot_preset(self): def set_koch_robot_preset(self):
def set_operating_mode_(arm): def set_operating_mode_(arm):