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

Fix calibration and ofset for single turn works!

Browse Source 
test by running using calibration and  then run visualize calibration and each non linear joint has value of 2047 in middle at homed position, thus the 0<->4096 point is shifted for each motor to be on the other side of the homing position.
This commit is contained in:
Pepijn
2025-03-13 10:47:49 +01:00
parent 80c272e1f9
commit b31283ed6c
3 changed files with 109 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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]

38
lerobot/common/robot_devices/robots/feetech_calibration.py
View File

@@ -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}")

98
lerobot/scripts/calibration_visualization.py
View File

@@ -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)