hangX/lerobot
1
0
Fork 0
forked from tangger/lerobot
Code Pull Requests Activity

With multi turn (code is good, but feetech can't handle multi turn commands)

Browse Source
This commit is contained in:
Pepijn
2025-02-19 14:51:01 +01:00
parent 18ae1b802d
commit 645bfd78d8
3 changed files with 44 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,3 +1,4 @@
import enum
import time
import traceback
@@ -113,26 +114,15 @@ NUM_WRITE_RETRY = 20
def convert_ticks_to_degrees(ticks, model):
resolutions = MODEL_RESOLUTION[model]
degrees = (ticks / resolutions) * 360.0 # Convert to 0-360 range
# Convert to range [-180, 180]
degrees = (degrees + 180) % 360 - 180
return degrees
# Convert the ticks to degrees
return ticks * (360.0/resolutions)
def convert_degrees_to_ticks(degrees, model, motorbus, motor_id: int):
multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
def convert_degrees_to_ticks(degrees, model):
resolutions = MODEL_RESOLUTION[model]
# Remove full rotations from degrees
base_degrees = degrees - (multi_turn_index * 360.0)
# Convert degrees to motor ticks
ticks = base_degrees / 180.0 * (resolutions / 2)
# Add back multi-turn ticks
ticks += multi_turn_index * resolutions
return int(ticks)
return int(degrees * (resolutions/360.0))
def adjusted_to_homing_ticks(
@@ -141,37 +131,39 @@ def adjusted_to_homing_ticks(
"""
Shifts raw [0..4095] ticks by an encoder offset, modulo a single turn [0..4095].
"""
drive_mode = 0
if motorbus.calibration is not None:
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
# Retrieve previous values for tracking
prev_value = motorbus.previous_value[motor_id - 1]
multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
resolutions = MODEL_RESOLUTION[model]
# Add offset and wrap within resolution
shifted = (raw_motor_ticks + encoder_offset) % resolutions
# # Re-center into a symmetric range (e.g., [-2048, 2047] if resolutions==4096) Thus the middle homing position will be virtual 0.
if shifted > resolutions // 2:
shifted -= resolutions
# Update multi turn values if needed
if prev_value is not None:
delta = shifted - prev_value
# If jump forward > 180° (2048 steps), assume full rotation
if delta > resolutions // 2:
if delta > (resolutions // 2):
multi_turn_index -= 1
elif delta < -resolutions // 2:
elif delta < (-resolutions // 2):
multi_turn_index += 1
# Update stored values
motorbus.previous_value[motor_id - 1] = shifted
motorbus.multi_turn_index[motor_id - 1] = multi_turn_index
# Apply the multi turn to output so we can track beyong -180..180 degrees or -2048..2048 ticks
ticks = shifted + (multi_turn_index * resolutions)
# Update direction of rotation of the motor to match between leader and follower.
# In fact, the motor of the leader for a given joint can be assembled in an
# opposite direction in term of rotation than the motor of the follower on the same joint.
drive_mode = 0
if motorbus.calibration is not None:
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
if drive_mode:
ticks *= -1
@@ -185,27 +177,27 @@ def adjusted_to_motor_ticks(
Inverse of adjusted_to_homing_ticks().
Converts homed servo ticks (with multi-turn indexing) back to [0..4095].
"""
multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
resolutions = MODEL_RESOLUTION[model]
# Remove offset and wrap within resolution
shifted = (adjusted_pos - encoder_offset) % resolutions
# Apply the multi turn to output ticks because goal position can have input of -32000...32000
ticks = shifted + (multi_turn_index * resolutions)
# Update direction of rotation of the motor to match between leader and follower.
# In fact, the motor of the leader for a given joint can be assembled in an
# opposite direction in term of rotation than the motor of the follower on the same joint.
drive_mode = 0
if motorbus.calibration is not None:
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
# If inverted, flip the adjusted value back.
if drive_mode:
adjusted_pos *= -1
ticks *= -1
resolutions = MODEL_RESOLUTION[model]
# Get the current multi-turn index and remove that offset
multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
adjusted_pos -= multi_turn_index * 4096
# Convert back into [−2048..2047] before final modulo
if adjusted_pos > 2047:
adjusted_pos -= 4096
elif adjusted_pos < -2048:
adjusted_pos += 4096
# Map back to raw ticks [0..4095]
raw_ticks = (adjusted_pos - encoder_offset) % resolutions
return raw_ticks
return ticks
def convert_to_bytes(value, bytes, mock=False):
@@ -516,7 +508,7 @@ class FeetechMotorsBus:
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, self, motor_idx)
values[i] = convert_degrees_to_ticks(values[i], model)
values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx)
elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
@@ -757,3 +749,4 @@ class FeetechMotorsBus:
def __del__(self):
if getattr(self, "is_connected", False):
self.disconnect()

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

@@ -147,7 +147,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.
drive_modes = [0, 1, 0, 0, 1, 0]
drive_modes = [0, 0, 0, 0, 0, 0]
calib_dict = {
"homing_offset": encoder_offsets.astype(int).tolist(),

12
lerobot/scripts/calibration_visualization.py
View File

@@ -79,6 +79,7 @@ def debug_feetech_positions(cfg, arm_arg: str):
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]
multi_turn_index = bus.multi_turn_index[offset_idx]
# Manually compute "adjusted ticks" from raw ticks
manual_adjusted = adjusted_to_homing_ticks(raw_ticks, offset, model, bus, motor_idx)
@@ -86,18 +87,19 @@ def debug_feetech_positions(cfg, arm_arg: str):
manual_degs = convert_ticks_to_degrees(manual_adjusted, model)
# Convert to ticks
manual_ticks = convert_degrees_to_ticks(manual_degs, model, bus, motor_idx)
manual_ticks = convert_degrees_to_ticks(manual_degs, model)
# Invert
inv_ticks = adjusted_to_motor_ticks(manual_ticks, offset, model, bus, motor_idx)
print(
f"{name:15s} | "
f"RAW={raw_ticks:4d} | "
f"HOMED={homed_val:7.2f} | "
f"MANUAL_ADJ_TICKS={manual_adjusted:6d} | "
f"HOMED_FROM_READ={homed_val:7.2f} | "
f"HOMED_TICKS={manual_adjusted:6d} | "
f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
f"INV_TICKS={manual_ticks:6d} | "
f"INV_TICKS={inv_ticks:4d}"
f"MANUAL_ADJ_TICKS={manual_ticks:6d} | "
f"INV_TICKS={inv_ticks:4d} | "
f"MULTI_TURN_INDEX={multi_turn_index}"
)
print("----------------------------------------------------")
time.sleep(0.25) # slow down loop