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

Added calibration visualization

Browse Source
This commit is contained in:
Pepijn
2025-02-18 15:04:25 +01:00
parent 27c6b84b1f
commit 63e6f7901e
5 changed files with 163 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -16,13 +16,6 @@ TIMEOUT_MS = 1000
MAX_ID_RANGE = 252
# The following bounds define the lower and upper joints range (after calibration).
# For joints in degree (i.e. revolute joints), their nominal range is [-180, 180] degrees
# which corresponds to a half rotation on the left and half rotation on the right.
# Some joints might require higher range, so we allow up to [-270, 270] degrees until
# an error is raised.
LOWER_BOUND_DEGREE = -180
UPPER_BOUND_DEGREE = 180
# For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper),
# their nominal range is [0, 100] %. For instance, for Aloha gripper, 0% is fully
# closed, and 100% is fully open. To account for slight calibration issue, we allow up to
@@ -32,7 +25,6 @@ UPPER_BOUND_LINEAR = 110
HALF_TURN_DEGREE = 180
# See this link for STS3215 Memory Table:
# https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true
# data_name: (address, size_byte)
@@ -119,25 +111,39 @@ NUM_READ_RETRY = 20
NUM_WRITE_RETRY = 20
def convert_degrees_to_steps(degrees: float | np.ndarray, models: list[str]) -> np.ndarray:
"""This function converts the degree range to the step range for indicating motors rotation.
It assumes a motor achieves a full rotation by going from -180 degree position to +180.
The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
def convert_degrees_to_steps(
degrees: float | np.ndarray, models: list[str], multi_turn_index: int
) -> np.ndarray:
"""
Converts degrees to motor steps with multi-turn tracking.
- Each full rotation (360°) corresponds to an additional 4096 steps.
"""
resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
steps = degrees / 180 * np.array(resolutions) / 2
steps = steps.astype(int)
return steps
# Remove full rotations from degrees
base_degrees = degrees - (multi_turn_index * 360.0)
# Convert degrees to motor steps
steps = base_degrees / 180.0 * (resolutions / 2)
# Add back multi-turn steps
steps += multi_turn_index * resolutions
return steps.astype(int)
def convert_steps_to_degrees(steps: int | np.ndarray, models: list[str]) -> np.ndarray:
"""This function converts the step range to the degrees range for indicating motors rotation.
It assumes a motor achieves a full rotation by going from -180 degree position to +180.
The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
"""
Converts motor steps to degrees while accounting for multi-turn motion.
- Each full rotation (4096 steps) adds ±360°.
"""
resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
steps = np.array(steps, dtype=float)
# Convert steps to degrees
degrees = steps * (360.0 / resolutions)
return degrees
@@ -145,38 +151,36 @@ def adjusted_to_homing_ticks(
raw_motor_ticks: int, encoder_offset: int, model: str, motorbus, motor_id: int
) -> int:
"""
Converts raw motor ticks [0..4095] to homed servo ticks in [-2048..2047].
Tracks multi-turn rotations by adjusting a multi-turn index.
Converts raw motor ticks [0..4095] to homed servo ticks with multi-turn indexing.
- Uses a rolling index to track full rotations (4096 steps each)
"""
resolutions = MODEL_RESOLUTION[model]
# Retrieve "previous shifted" and the multi-turn index
# Retrieve previous values for tracking
prev_value = motorbus.previous_value[motor_id - 1]
multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
# Compute the new shifted value in the range [-2048..2047]
# Compute new shifted position
shifted = (raw_motor_ticks + encoder_offset) % resolutions
if shifted > 2047:
shifted -= 4096
if shifted > resolutions // 2:
shifted -= resolutions # Normalize to [-2048, 2047]
# If we have a valid previous value, detect big jumps
if prev_value is not None:
delta = shifted - prev_value
# If we jump forward by more than half a turn, we must have wrapped negatively
if delta > 2048:
# If jump forward > 180° (2048 steps), assume full rotation
if delta > resolutions // 2:
multi_turn_index -= 1
# If we jump backward by more than half a turn, we must have wrapped positively
elif delta < -2048:
elif delta < -resolutions // 2:
multi_turn_index += 1
# Store the new shifted value and updated multi-turn index
# Update stored values
motorbus.previous_value[motor_id - 1] = shifted
motorbus.multi_turn_index[motor_id - 1] = multi_turn_index
# Return the final multi-turn adjusted position
return shifted + multi_turn_index * 4096
# Return final adjusted ticks with multi-turn indexing
return shifted + (multi_turn_index * resolutions)
def adjusted_to_motor_ticks(
@@ -485,22 +489,10 @@ class FeetechMotorsBus:
if drive_mode:
values[i] *= -1
if (values[i] < LOWER_BOUND_DEGREE) or (values[i] > UPPER_BOUND_DEGREE):
raise JointOutOfRangeError(
f"Wrong motor position range detected for {name}. "
f"Expected to be in nominal range of [-{HALF_TURN_DEGREE}, {HALF_TURN_DEGREE}] degrees (a full rotation), "
f"with a maximum range of [{LOWER_BOUND_DEGREE}, {UPPER_BOUND_DEGREE}] degrees to account for joints that can rotate a bit more, "
f"but present value is {values[i]} degree. "
"This might be due to a cable connection issue creating an artificial 360 degrees jump in motor values. "
"You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
)
elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
start_pos = self.calibration["start_pos"][calib_idx]
end_pos = self.calibration["end_pos"][calib_idx]
# TODO(pepijn): Check if a homing (something similar to homing) should be applied to linear joint (think so)
# Rescale the present position to a nominal range [0, 100] %,
# useful for joints with linear motions like Aloha gripper
values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100
@@ -532,7 +524,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_steps(values[i], [model])
values[i] = convert_degrees_to_steps(values[i], [model], self.multi_turn_index[motor_idx - 1])
values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx)
# Remove drive mode, which is the rotation direction of the motor, to come back to

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

@@ -121,7 +121,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
print("\nMove arm to homing position")
print(
"See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")
) # TODO(pepijn): replace with new instruction homing pos (all motors in center)
) # TODO(pepijn): replace with new instruction homing pos (all motors in center) in tutorial
input("Press Enter to continue...")
start_positions = np.zeros(len(arm.motor_indices))
@@ -158,7 +158,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
def run_full_auto_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
"""TODO: Add this method later as extra
"""TODO(pepijn): Add this method later as extra
Example of usage:
```python
run_full_auto_arm_calibration(arm, "so100", "left", "follower")

121
lerobot/scripts/calibration_visualization.py Normal file
View File

@@ -0,0 +1,121 @@
import argparse
import json
import time
from pathlib import Path
from lerobot.common.robot_devices.motors.feetech import (
FeetechMotorsBus,
adjusted_to_homing_ticks,
adjusted_to_motor_ticks,
convert_steps_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 debug_feetech_positions(cfg, arm_arg: str):
"""
Reads each joint’s (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks.
:param cfg: A config object (e.g. So100RobotConfig).
:param arm_arg: One of "main_leader" or "main_follower".
"""
# 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")
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
calib_file = Path(robot.calibration_dir) / f"{arm_arg}.json"
if calib_file.exists():
with open(calib_file) as f:
calibration_dict = json.load(f)
bus.set_calibration(calibration_dict)
print(f"Loaded calibration from {calib_file}")
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}")
# 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.")
try:
print("\nPress Ctrl+C to quit.\n")
while True:
# (a) 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)
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]
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)
# Convert to degrees
manual_degs = convert_steps_to_degrees(manual_adjusted, [model])[0]
# Invert
inv_ticks = adjusted_to_motor_ticks(manual_adjusted, offset, model, bus, motor_idx)
print(
f"{name:15s} | "
f"RAW={raw_ticks:4d} | "
f"HOMED={homed_val:7.2f} | "
f"MANUAL_ADJ={manual_adjusted:6d} | "
f"DEG={manual_degs:7.2f} | "
f"INV={inv_ticks:4d}"
)
print("----------------------------------------------------")
time.sleep(0.25) # slow down loop
except KeyboardInterrupt:
pass
finally:
print("\nExiting. Disconnecting bus...")
bus.disconnect()
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Debug Feetech positions.")
parser.add_argument(
"--arm",
type=str,
choices=["main_leader", "main_follower"],
default="main_leader",
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)
debug_feetech_positions(cfg, arm_arg=args.arm)

5
lerobot/scripts/configure_motor.py
View File

@@ -141,10 +141,9 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
motor_bus.write("Lock", 1)
print("Offset", motor_bus.read("Offset"))
# TODO(pepijn):
# 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)
# TODO(pepijn): store single_calibration
# 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}")

112
lerobot/scripts/test_homing.py
View File

@@ -1,112 +0,0 @@
from lerobot.common.robot_devices.motors.feetech import (
adjusted_to_homing_ticks,
adjusted_to_motor_ticks,
convert_steps_to_degrees,
)
# TODO(pepijn): remove this file!
MODEL_RESOLUTION = {
"scs_series": 4096,
"sts3215": 4096,
}
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
"""
home_ticks = motor_bus.read("Present_Position")[0] # Read index starts at 0
# 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
def configure_and_calibrate():
from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as MotorsBusClass
motor_idx_des = 2 # index of motor
# Setup motor names, indices, and models
motor_name = "motor"
motor_index_arbitrary = motor_idx_des # Use the motor ID passed via argument
motor_model = "sts3215" # Use the motor model passed via argument
# Initialize the MotorBus with the correct port and motor configurations
motor_bus = MotorsBusClass(
port="/dev/tty.usbmodem58760431631", motors={motor_name: (motor_index_arbitrary, motor_model)}
)
# Try to connect to the motor bus and handle any connection-specific errors
try:
motor_bus.connect()
print(f"Connected on port {motor_bus.port}")
except OSError as e:
print(f"Error occurred when connecting to the motor bus: {e}")
return
present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx_des, "ID", num_retry=2)
if present_idx != motor_idx_des:
raise OSError("Failed to write index.")
# Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of
# the motors. Note: this configuration is not in the official STS3215 Memory Table
motor_bus.write("Lock", 0)
motor_bus.write("Maximum_Acceleration", 254)
motor_bus.write("Max_Angle_Limit", 4095) # default 4095
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("Lock", 1)
# Calibration
print("Offset", motor_bus.read("Offset"))
print("Max_Angle_Limit", motor_bus.read("Max_Angle_Limit"))
print("Min_Angle_Limit", motor_bus.read("Min_Angle_Limit"))
print("Goal_Position", motor_bus.read("Goal_Position"))
print("Present Position", motor_bus.read("Present_Position"))
input("Move the motor to middle (home) position, then press Enter...")
encoder_offset = calibrate_homing_motor(motor_idx_des, motor_bus)
try:
while True:
name = motor_bus.motor_names[0]
_, model = motor_bus.motors[name]
raw_motor_ticks = motor_bus.read("Present_Position")[0]
adjusted_ticks = adjusted_to_homing_ticks(raw_motor_ticks, encoder_offset, model, motor_bus, 1)
inverted_ticks = adjusted_to_motor_ticks(adjusted_ticks, encoder_offset, model, motor_bus, 1)
adjusted_degrees = convert_steps_to_degrees([adjusted_ticks], [model])
print(
f"Raw Motor ticks: {raw_motor_ticks} | Adjusted ticks: {adjusted_ticks} | Adjusted degrees: {adjusted_degrees} | Invert adjusted ticks: {inverted_ticks}"
)
# time.sleep(0.3)
except KeyboardInterrupt:
print("Stopped reading positions.")
except Exception as e:
print(f"Error occurred during motor configuration: {e}")
finally:
motor_bus.disconnect()
print("Disconnected from motor bus.")
if __name__ == "__main__":
configure_and_calibrate()