wangzixiang
/
lerobot
mirror of https://github.com/huggingface/lerobot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

New Feetech calibration (#859) 

Co-authored-by: Pepijn <pepijn@huggingface.co>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
This commit is contained in:
Simon Alibert 2025-03-14 11:31:23 +01:00 committed by GitHub
parent ce63cfdb25
commit 1f7ddc1d76
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
25 changed files with 529 additions and 795 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
lerobot/common/motors/feetech/__init__.py
View File

@ -1,4 +1,9 @@
from .feetech import FeetechMotorsBus, TorqueMode from .feetech import FeetechMotorsBus, TorqueMode
from .feetech_calibration import run_arm_manual_calibration from .feetech_calibration import apply_feetech_offsets_from_calibration, run_full_arm_calibration
__all__ = ["FeetechMotorsBus", "TorqueMode", "run_arm_manual_calibration"] __all__ = [
"FeetechMotorsBus",
"TorqueMode",
"apply_feetech_offsets_from_calibration",
"run_full_arm_calibration",
]

300
lerobot/common/motors/feetech/feetech.py
View File

@ -13,8 +13,6 @@
# limitations under the License. # limitations under the License.
import enum import enum
import logging
import math
import time import time
import traceback import traceback
from copy import deepcopy from copy import deepcopy
@ -31,13 +29,6 @@ TIMEOUT_MS = 1000
MAX_ID_RANGE = 252 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 = -270
UPPER_BOUND_DEGREE = 270
# For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper), # 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 # 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 # closed, and 100% is fully open. To account for slight calibration issue, we allow up to
@ -47,7 +38,6 @@ UPPER_BOUND_LINEAR = 110
HALF_TURN_DEGREE = 180 HALF_TURN_DEGREE = 180
# See this link for STS3215 Memory Table: # See this link for STS3215 Memory Table:
# https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true # https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true
# data_name: (address, size_byte) # data_name: (address, size_byte)
@ -113,8 +103,6 @@ SCS_SERIES_BAUDRATE_TABLE = {
} }
CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"] CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"]
CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"]
MODEL_CONTROL_TABLE = { MODEL_CONTROL_TABLE = {
"scs_series": SCS_SERIES_CONTROL_TABLE, "scs_series": SCS_SERIES_CONTROL_TABLE,
@ -136,15 +124,63 @@ NUM_READ_RETRY = 20
NUM_WRITE_RETRY = 20 NUM_WRITE_RETRY = 20
def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray: def convert_ticks_to_degrees(ticks, model):
"""This function converts the degree range to the step range for indicating motors rotation. resolutions = MODEL_RESOLUTION[model]
It assumes a motor achieves a full rotation by going from -180 degree position to +180. # Convert the ticks to degrees
The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation. return ticks * (360.0 / resolutions)
def convert_degrees_to_ticks(degrees, model):
resolutions = MODEL_RESOLUTION[model]
# Convert degrees to motor ticks
return int(degrees * (resolutions / 360.0))
def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_id: int) -> int:
""" """
resolutions = [MODEL_RESOLUTION[model] for model in models] Takes a raw reading [0..(res-1)] (e.g. 0..4095) and shifts it so that '2048'
steps = degrees / 180 * np.array(resolutions) / 2 becomes 0 in the homed coordinate system ([-2048..+2047] for 4096 resolution).
steps = steps.astype(int) """
return steps resolutions = MODEL_RESOLUTION[model]
# Shift raw ticks by half-resolution so 2048 -> 0, then wrap [0..res-1].
ticks = (raw_motor_ticks - (resolutions // 2)) % resolutions
# If above halfway, fold it into negative territory => [-2048..+2047].
if ticks > (resolutions // 2):
ticks -= resolutions
# Flip sign if drive_mode is set.
drive_mode = 0
if motorbus.calibration is not None:
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
if drive_mode:
ticks *= -1
return ticks
def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: int) -> int:
"""
Inverse of adjusted_to_homing_ticks(). Takes a 'homed' position in [-2048..+2047]
and recovers the raw [0..(res-1)] ticks with 2048 as midpoint.
"""
# Flip sign if drive_mode was set.
drive_mode = 0
if motorbus.calibration is not None:
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
if drive_mode:
adjusted_pos *= -1
resolutions = MODEL_RESOLUTION[model]
# Shift by +half-resolution and wrap into [0..res-1].
# This undoes the earlier shift by -half-resolution.
ticks = (adjusted_pos + (resolutions // 2)) % resolutions
return ticks
def convert_to_bytes(value, bytes, mock=False): def convert_to_bytes(value, bytes, mock=False):
@ -304,8 +340,6 @@ class FeetechMotorsBus:
self.group_writers = {} self.group_writers = {}
self.logs = {} self.logs = {}
self.track_positions = {}
def connect(self): def connect(self):
if self.is_connected: if self.is_connected:
raise DeviceAlreadyConnectedError( raise DeviceAlreadyConnectedError(
@ -402,33 +436,7 @@ class FeetechMotorsBus:
def set_calibration(self, calibration: dict[str, list]): def set_calibration(self, calibration: dict[str, list]):
self.calibration = calibration self.calibration = calibration
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.
For more info, see docstring of `apply_calibration` and `autocorrect_calibration`.
"""
try:
values = self.apply_calibration(values, motor_names)
except JointOutOfRangeError as e:
print(e)
self.autocorrect_calibration(values, motor_names)
values = self.apply_calibration(values, motor_names)
return values
def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
"""Convert from unsigned int32 joint position range [0, 2**32[ to the universal float32 nominal degree range ]-180.0, 180.0[ with
a "zero position" at 0 degree.
Note: We say "nominal degree range" since the motors can take values outside this range. For instance, 190 degrees, if the motor
rotate more than a half a turn from the zero position. However, most motors can't rotate more than 180 degrees and will stay in this range.
Joints values are original in [0, 2**32[ (unsigned int32). Each motor are expected to complete a full rotation
when given a goal position that is + or - their resolution. For instance, feetech xl330-m077 have a resolution of 4096, and
at any position in their original range, let's say the position 56734, they complete a full rotation clockwise by moving to 60830,
or anticlockwise by moving to 52638. The position in the original range is arbitrary and might change a lot between each motor.
To harmonize between motors of the same model, different robots, or even models of different brands, we propose to work
in the centered nominal degree range ]-180, 180[.
"""
if motor_names is None: if motor_names is None:
motor_names = self.motor_names motor_names = self.motor_names
@ -440,34 +448,11 @@ class FeetechMotorsBus:
calib_mode = self.calibration["calib_mode"][calib_idx] calib_mode = self.calibration["calib_mode"][calib_idx]
if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
drive_mode = self.calibration["drive_mode"][calib_idx] motor_idx, model = self.motors[name]
homing_offset = self.calibration["homing_offset"][calib_idx]
_, model = self.motors[name]
resolution = self.model_resolution[model]
# Update direction of rotation of the motor to match between leader and follower. # Convert raw motor ticks to homed ticks, then convert the homed ticks to degrees
# In fact, the motor of the leader for a given joint can be assembled in an values[i] = adjusted_to_homing_ticks(values[i], model, self, motor_idx)
# opposite direction in term of rotation than the motor of the follower on the same joint. values[i] = convert_ticks_to_degrees(values[i], model)
if drive_mode:
values[i] *= -1
# Convert from range [-2**31, 2**31[ to
# nominal range ]-resolution, resolution[ (e.g. ]-2048, 2048[)
values[i] += homing_offset
# Convert from range ]-resolution, resolution[ to
# universal float32 centered degree range ]-180, 180[
values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE
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: elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
start_pos = self.calibration["start_pos"][calib_idx] start_pos = self.calibration["start_pos"][calib_idx]
@ -489,103 +474,6 @@ class FeetechMotorsBus:
return values return values
def autocorrect_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
"""This function automatically detects issues with values of motors after calibration, and correct for these issues.
Some motors might have values outside of expected maximum bounds after calibration.
For instance, for a joint in degree, its value can be outside [-270, 270] degrees, which is totally unexpected given
a nominal range of [-180, 180] degrees, which represents half a turn to the left or right starting from zero position.
Known issues:
#1: Motor value randomly shifts of a full turn, caused by hardware/connection errors.
#2: Motor internal homing offset is shifted of a full turn, caused by using default calibration (e.g Aloha).
#3: motor internal homing offset is shifted of less or more than a full turn, caused by using default calibration
or by human error during manual calibration.
Issues #1 and #2 can be solved by shifting the calibration homing offset by a full turn.
Issue #3 will be visually detected by user and potentially captured by the safety feature `max_relative_target`,
that will slow down the motor, raise an error asking to recalibrate. Manual recalibrating will solve the issue.
Note: A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096.
"""
if motor_names is None:
motor_names = self.motor_names
# Convert from unsigned int32 original range [0, 2**32] to signed float32 range
values = values.astype(np.float32)
for i, name in enumerate(motor_names):
calib_idx = self.calibration["motor_names"].index(name)
calib_mode = self.calibration["calib_mode"][calib_idx]
if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
drive_mode = self.calibration["drive_mode"][calib_idx]
homing_offset = self.calibration["homing_offset"][calib_idx]
_, model = self.motors[name]
resolution = self.model_resolution[model]
if drive_mode:
values[i] *= -1
# Convert from initial range to range [-180, 180] degrees
calib_val = (values[i] + homing_offset) / (resolution // 2) * HALF_TURN_DEGREE
in_range = (calib_val > LOWER_BOUND_DEGREE) and (calib_val < UPPER_BOUND_DEGREE)
# Solve this inequality to find the factor to shift the range into [-180, 180] degrees
# values[i] = (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE
# - HALF_TURN_DEGREE <= (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE <= HALF_TURN_DEGREE
# (- HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset) / resolution <= factor <= (HALF_TURN_DEGREE / 180 * (resolution // 2) - values[i] - homing_offset) / resolution
low_factor = (
-HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset
) / resolution
upp_factor = (
HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset
) / resolution
elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
start_pos = self.calibration["start_pos"][calib_idx]
end_pos = self.calibration["end_pos"][calib_idx]
# Convert from initial range to range [0, 100] in %
calib_val = (values[i] - start_pos) / (end_pos - start_pos) * 100
in_range = (calib_val > LOWER_BOUND_LINEAR) and (calib_val < UPPER_BOUND_LINEAR)
# Solve this inequality to find the factor to shift the range into [0, 100] %
# values[i] = (values[i] - start_pos + resolution * factor) / (end_pos + resolution * factor - start_pos - resolution * factor) * 100
# values[i] = (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100
# 0 <= (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 <= 100
# (start_pos - values[i]) / resolution <= factor <= (end_pos - values[i]) / resolution
low_factor = (start_pos - values[i]) / resolution
upp_factor = (end_pos - values[i]) / resolution
if not in_range:
# Get first integer between the two bounds
if low_factor < upp_factor:
factor = math.ceil(low_factor)
if factor > upp_factor:
raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]")
else:
factor = math.ceil(upp_factor)
if factor > low_factor:
raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]")
if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
out_of_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees"
in_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees"
elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
out_of_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %"
in_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %"
logging.warning(
f"Auto-correct calibration of motor '{name}' by shifting value by {abs(factor)} full turns, "
f"from '{out_of_range_str}' to '{in_range_str}'."
)
# 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
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:
@ -596,23 +484,11 @@ class FeetechMotorsBus:
calib_mode = self.calibration["calib_mode"][calib_idx] calib_mode = self.calibration["calib_mode"][calib_idx]
if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
drive_mode = self.calibration["drive_mode"][calib_idx] motor_idx, model = self.motors[name]
homing_offset = self.calibration["homing_offset"][calib_idx]
_, model = self.motors[name]
resolution = self.model_resolution[model]
# Convert from nominal 0-centered degree range [-180, 180] to # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
# 0-centered resolution range (e.g. [-2048, 2048] for resolution=4096) values[i] = convert_degrees_to_ticks(values[i], model)
values[i] = values[i] / HALF_TURN_DEGREE * (resolution // 2) values[i] = adjusted_to_motor_ticks(values[i], model, self, motor_idx)
# Subtract the homing offsets to come back to actual motor range of values
# which can be arbitrary.
values[i] -= homing_offset
# Remove drive mode, which is the rotation direction of the motor, to come back to
# actual motor rotation direction which can be arbitrary.
if drive_mode:
values[i] *= -1
elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
start_pos = self.calibration["start_pos"][calib_idx] start_pos = self.calibration["start_pos"][calib_idx]
@ -625,43 +501,6 @@ class FeetechMotorsBus:
values = np.round(values).astype(np.int32) values = np.round(values).astype(np.int32)
return values return values
def avoid_rotation_reset(self, values, motor_names, data_name):
if data_name not in self.track_positions:
self.track_positions[data_name] = {
"prev": [None] * len(self.motor_names),
# Assume False at initialization
"below_zero": [False] * len(self.motor_names),
"above_max": [False] * len(self.motor_names),
}
track = self.track_positions[data_name]
if motor_names is None:
motor_names = self.motor_names
for i, name in enumerate(motor_names):
idx = self.motor_names.index(name)
if track["prev"][idx] is None:
track["prev"][idx] = values[i]
continue
# Detect a full rotation occurred
if abs(track["prev"][idx] - values[i]) > 2048:
# Position went below 0 and got reset to 4095
if track["prev"][idx] < values[i]:
# So we set negative value by adding a full rotation
values[i] -= 4096
# Position went above 4095 and got reset to 0
elif track["prev"][idx] > values[i]:
# So we add a full rotation
values[i] += 4096
track["prev"][idx] = values[i]
return values
def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY): def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY):
if self.mock: if self.mock:
import tests.motors.mock_scservo_sdk as scs import tests.motors.mock_scservo_sdk as scs
@ -735,7 +574,7 @@ class FeetechMotorsBus:
self.port_handler.ser.reset_output_buffer() self.port_handler.ser.reset_output_buffer()
self.port_handler.ser.reset_input_buffer() self.port_handler.ser.reset_input_buffer()
# create new group reader # Create new group reader
self.group_readers[group_key] = scs.GroupSyncRead( self.group_readers[group_key] = scs.GroupSyncRead(
self.port_handler, self.packet_handler, addr, bytes self.port_handler, self.packet_handler, addr, bytes
) )
@ -760,15 +599,8 @@ class FeetechMotorsBus:
values = np.array(values) values = np.array(values)
# Convert to signed int to use range [-2048, 2048] for our motor positions.
if data_name in CONVERT_UINT32_TO_INT32_REQUIRED:
values = values.astype(np.int32)
if data_name in CALIBRATION_REQUIRED:
values = self.avoid_rotation_reset(values, motor_names, data_name)
if data_name in CALIBRATION_REQUIRED and self.calibration is not None: if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
values = self.apply_calibration_autocorrect(values, motor_names) values = self.apply_calibration(values, motor_names)
# log the number of seconds it took to read the data from the motors # log the number of seconds it took to read the data from the motors
delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names) delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names)

592
lerobot/common/motors/feetech/feetech_calibration.py
View File

@ -11,488 +11,244 @@
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and # See the License for the specific language governing permissions and
# limitations under the License. # limitations under the License.
"""Logic to calibrate a robot arm built with feetech motors"""
# TODO(rcadene, aliberts): move this logic into the robot code when refactoring
import time
import numpy as np import numpy as np
from ..motors_bus import MotorsBus from ..motors_bus import MotorsBus
from .feetech import ( from .feetech import (
CalibrationMode, CalibrationMode,
FeetechMotorsBus,
TorqueMode, TorqueMode,
convert_degrees_to_steps,
) )
URL_TEMPLATE = ( URL_TEMPLATE = (
"https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp" "https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp"
) )
# The following positions are provided in nominal degree range ]-180, +180[
# For more info on these constants, see comments in the code where they get used.
ZERO_POSITION_DEGREE = 0
ROTATED_POSITION_DEGREE = 90
def disable_torque(arm: MotorsBus):
def assert_drive_mode(drive_mode):
# `drive_mode` is in [0,1] with 0 means original rotation direction for the motor, and 1 means inverted.
if not np.all(np.isin(drive_mode, [0, 1])):
raise ValueError(f"`drive_mode` contains values other than 0 or 1: ({drive_mode})")
def apply_drive_mode(position, drive_mode):
assert_drive_mode(drive_mode)
# Convert `drive_mode` from [0, 1] with 0 indicates original rotation direction and 1 inverted,
# to [-1, 1] with 1 indicates original rotation direction and -1 inverted.
signed_drive_mode = -(drive_mode * 2 - 1)
position *= signed_drive_mode
return position
def move_until_block(arm, motor_name, positive_direction=True, while_move_hook=None):
count = 0
while True:
present_pos = arm.read("Present_Position", motor_name)
if positive_direction:
# Move +100 steps every time. Lower the steps to lower the speed at which the arm moves.
arm.write("Goal_Position", present_pos + 100, motor_name)
else:
arm.write("Goal_Position", present_pos - 100, motor_name)
if while_move_hook is not None:
while_move_hook()
present_pos = arm.read("Present_Position", motor_name).item()
present_speed = arm.read("Present_Speed", motor_name).item()
present_current = arm.read("Present_Current", motor_name).item()
# present_load = arm.read("Present_Load", motor_name).item()
# present_voltage = arm.read("Present_Voltage", motor_name).item()
# present_temperature = arm.read("Present_Temperature", motor_name).item()
# print(f"{present_pos=}")
# print(f"{present_speed=}")
# print(f"{present_current=}")
# print(f"{present_load=}")
# print(f"{present_voltage=}")
# print(f"{present_temperature=}")
if present_speed == 0 and present_current > 40:
count += 1
if count > 100 or present_current > 300:
return present_pos
else:
count = 0
def move_to_calibrate(
arm,
motor_name,
invert_drive_mode=False,
positive_first=True,
in_between_move_hook=None,
while_move_hook=None,
):
initial_pos = arm.read("Present_Position", motor_name)
if positive_first:
p_present_pos = move_until_block(
arm, motor_name, positive_direction=True, while_move_hook=while_move_hook
)
else:
n_present_pos = move_until_block(
arm, motor_name, positive_direction=False, while_move_hook=while_move_hook
)
if in_between_move_hook is not None:
in_between_move_hook()
if positive_first:
n_present_pos = move_until_block(
arm, motor_name, positive_direction=False, while_move_hook=while_move_hook
)
else:
p_present_pos = move_until_block(
arm, motor_name, positive_direction=True, while_move_hook=while_move_hook
)
zero_pos = (n_present_pos + p_present_pos) / 2
calib_data = {
"initial_pos": initial_pos,
"homing_offset": zero_pos if invert_drive_mode else -zero_pos,
"invert_drive_mode": invert_drive_mode,
"drive_mode": -1 if invert_drive_mode else 0,
"zero_pos": zero_pos,
"start_pos": n_present_pos if invert_drive_mode else p_present_pos,
"end_pos": p_present_pos if invert_drive_mode else n_present_pos,
}
return calib_data
def apply_offset(calib, offset):
calib["zero_pos"] += offset
if calib["drive_mode"]:
calib["homing_offset"] += offset
else:
calib["homing_offset"] -= offset
return calib
def run_arm_auto_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
if robot_type == "so100":
return run_arm_auto_calibration_so100(arm, robot_type, arm_name, arm_type)
elif robot_type == "moss":
return run_arm_auto_calibration_moss(arm, robot_type, arm_name, arm_type)
else:
raise ValueError(robot_type)
def run_arm_auto_calibration_so100(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
"""All the offsets and magic numbers are hand tuned, and are unique to SO-100 follower arms"""
if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any(): if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
raise ValueError("To run calibration, the torque must be disabled on all motors.") raise ValueError("To run calibration, the torque must be disabled on all motors.")
if not (robot_type == "so100" and arm_type == "follower"):
raise NotImplementedError("Auto calibration only supports the follower of so100 arms for now.")
print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...") def get_calibration_modes(arm: MotorsBus):
"""Returns calibration modes for each motor (DEGREE for rotational, LINEAR for gripper)."""
return [
CalibrationMode.LINEAR.name if name == "gripper" else CalibrationMode.DEGREE.name
for name in arm.motor_names
]
print("\nMove arm to initial position")
print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="initial"))
input("Press Enter to continue...")
# Lower the acceleration of the motors (in [0,254]) def reset_offset(motor_id, motor_bus):
initial_acceleration = arm.read("Acceleration") # Open the write lock, changes to EEPROM do NOT persist yet
arm.write("Lock", 0) motor_bus.write("Lock", 1)
arm.write("Acceleration", 10)
time.sleep(1)
arm.write("Torque_Enable", TorqueMode.ENABLED.value) # Set offset to 0
motor_name = motor_bus.motor_names[motor_id - 1]
motor_bus.write("Offset", 0, motor_names=[motor_name])
print(f'{arm.read("Present_Position", "elbow_flex")=}') # Close the write lock, changes to EEPROM do persist
motor_bus.write("Lock", 0)
calib = {} # 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
init_wf_pos = arm.read("Present_Position", "wrist_flex")
init_sl_pos = arm.read("Present_Position", "shoulder_lift")
init_ef_pos = arm.read("Present_Position", "elbow_flex")
arm.write("Goal_Position", init_wf_pos - 800, "wrist_flex")
arm.write("Goal_Position", init_sl_pos + 150 + 1024, "shoulder_lift")
arm.write("Goal_Position", init_ef_pos - 2048, "elbow_flex")
time.sleep(2)
print("Calibrate shoulder_pan") def calibrate_homing_motor(motor_id, motor_bus):
calib["shoulder_pan"] = move_to_calibrate(arm, "shoulder_pan") reset_offset(motor_id, motor_bus)
arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
time.sleep(1)
print("Calibrate gripper") home_ticks = motor_bus.read("Present_Position")[motor_id - 1] # Read index starts at 0
calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True) print(f"Encoder offset (present position in homing position): {home_ticks}")
time.sleep(1)
print("Calibrate wrist_flex") return home_ticks
calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex")
calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=80)
def in_between_move_hook():
nonlocal arm, calib
time.sleep(2)
ef_pos = arm.read("Present_Position", "elbow_flex")
sl_pos = arm.read("Present_Position", "shoulder_lift")
arm.write("Goal_Position", ef_pos + 1024, "elbow_flex")
arm.write("Goal_Position", sl_pos - 1024, "shoulder_lift")
time.sleep(2)
print("Calibrate elbow_flex") def calibrate_linear_motor(motor_id, motor_bus):
calib["elbow_flex"] = move_to_calibrate( motor_names = motor_bus.motor_names
arm, "elbow_flex", positive_first=False, in_between_move_hook=in_between_move_hook motor_name = motor_names[motor_id - 1]
)
calib["elbow_flex"] = apply_offset(calib["elbow_flex"], offset=80 - 1024)
arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1024 + 512, "elbow_flex") reset_offset(motor_id, motor_bus)
time.sleep(1)
def in_between_move_hook(): input(f"Close the {motor_name}, then press Enter...")
nonlocal arm, calib start_pos = motor_bus.read("Present_Position")[motor_id - 1] # Read index starts ar 0
arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"], "elbow_flex") print(f" [Motor {motor_id}] start position recorded: {start_pos}")
print("Calibrate shoulder_lift") input(f"Open the {motor_name} fully, then press Enter...")
calib["shoulder_lift"] = move_to_calibrate( end_pos = motor_bus.read("Present_Position")[motor_id - 1] # Read index starts ar 0
arm, print(f" [Motor {motor_id}] end position recorded: {end_pos}")
"shoulder_lift",
invert_drive_mode=True,
positive_first=False,
in_between_move_hook=in_between_move_hook,
)
# add an 30 steps as offset to align with body
calib["shoulder_lift"] = apply_offset(calib["shoulder_lift"], offset=1024 - 50)
def while_move_hook(): return start_pos, end_pos
nonlocal arm, calib
positions = {
"shoulder_lift": round(calib["shoulder_lift"]["zero_pos"] - 1600),
"elbow_flex": round(calib["elbow_flex"]["zero_pos"] + 1700),
"wrist_flex": round(calib["wrist_flex"]["zero_pos"] + 800),
"gripper": round(calib["gripper"]["end_pos"]),
}
arm.write("Goal_Position", list(positions.values()), list(positions.keys()))
arm.write("Goal_Position", round(calib["shoulder_lift"]["zero_pos"] - 1600), "shoulder_lift")
time.sleep(2)
arm.write("Goal_Position", round(calib["elbow_flex"]["zero_pos"] + 1700), "elbow_flex")
time.sleep(2)
arm.write("Goal_Position", round(calib["wrist_flex"]["zero_pos"] + 800), "wrist_flex")
time.sleep(2)
arm.write("Goal_Position", round(calib["gripper"]["end_pos"]), "gripper")
time.sleep(2)
print("Calibrate wrist_roll") def single_motor_calibration(arm: MotorsBus, motor_id: int):
calib["wrist_roll"] = move_to_calibrate( """Calibrates a single motor and returns its calibration data for updating the calibration file."""
arm, "wrist_roll", invert_drive_mode=True, positive_first=False, while_move_hook=while_move_hook
)
arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"], "wrist_roll") disable_torque(arm)
time.sleep(1) print(f"\n--- Calibrating Motor {motor_id} ---")
arm.write("Goal_Position", calib["gripper"]["start_pos"], "gripper")
time.sleep(1)
arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"], "wrist_flex")
time.sleep(1)
arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 2048, "elbow_flex")
arm.write("Goal_Position", calib["shoulder_lift"]["zero_pos"] - 2048, "shoulder_lift")
time.sleep(1)
arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
time.sleep(1)
calib_modes = [] start_pos = 0
for name in arm.motor_names: end_pos = 0
if name == "gripper": encoder_offset = 0
calib_modes.append(CalibrationMode.LINEAR.name)
if motor_id == 6:
start_pos, end_pos = calibrate_linear_motor(motor_id, arm)
else: else:
calib_modes.append(CalibrationMode.DEGREE.name) input("Move the motor to (zero) position, then press Enter...")
encoder_offset = calibrate_homing_motor(motor_id, arm)
print(f"Calibration for motor ID:{motor_id} done.")
# Create a calibration dictionary for the single motor
calib_dict = { calib_dict = {
"homing_offset": [calib[name]["homing_offset"] for name in arm.motor_names], "homing_offset": int(encoder_offset),
"drive_mode": [calib[name]["drive_mode"] for name in arm.motor_names], "drive_mode": 0,
"start_pos": [calib[name]["start_pos"] for name in arm.motor_names], "start_pos": int(start_pos),
"end_pos": [calib[name]["end_pos"] for name in arm.motor_names], "end_pos": int(end_pos),
"calib_mode": calib_modes, "calib_mode": get_calibration_modes(arm)[motor_id - 1],
"motor_names": arm.motor_names, "motor_name": arm.motor_names[motor_id - 1],
} }
# Re-enable original accerlation
arm.write("Lock", 0)
arm.write("Acceleration", initial_acceleration)
time.sleep(1)
return calib_dict return calib_dict
def run_arm_auto_calibration_moss(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str): def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
"""All the offsets and magic numbers are hand tuned, and are unique to SO-100 follower arms""" """
if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any(): Runs a full calibration process for all motors in a robotic arm.
raise ValueError("To run calibration, the torque must be disabled on all motors.")
if not (robot_type == "moss" and arm_type == "follower"): This function calibrates each motor in the arm, determining encoder offsets and
raise NotImplementedError("Auto calibration only supports the follower of moss arms for now.") start/end positions for linear and rotational motors. The calibration data is then
stored in a dictionary for later use.
print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...") **Calibration Process:**
- The user is prompted to move the arm to its homing position before starting.
- Motors with rotational motion are calibrated using a homing method.
- Linear actuators (e.g., grippers) are calibrated separately.
- Encoder offsets, start positions, and end positions are recorded.
print("\nMove arm to initial position") **Example Usage:**
print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="initial"))
input("Press Enter to continue...")
# Lower the acceleration of the motors (in [0,254])
initial_acceleration = arm.read("Acceleration")
arm.write("Lock", 0)
arm.write("Acceleration", 10)
time.sleep(1)
arm.write("Torque_Enable", TorqueMode.ENABLED.value)
sl_pos = arm.read("Present_Position", "shoulder_lift")
arm.write("Goal_Position", sl_pos - 1024 - 450, "shoulder_lift")
ef_pos = arm.read("Present_Position", "elbow_flex")
arm.write("Goal_Position", ef_pos + 1024 + 450, "elbow_flex")
time.sleep(2)
calib = {}
print("Calibrate shoulder_pan")
calib["shoulder_pan"] = move_to_calibrate(arm, "shoulder_pan")
arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
time.sleep(1)
print("Calibrate gripper")
calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True)
time.sleep(1)
print("Calibrate wrist_flex")
calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex", invert_drive_mode=True)
calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=-210 + 1024)
wr_pos = arm.read("Present_Position", "wrist_roll")
arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
time.sleep(1)
arm.write("Goal_Position", wr_pos - 1024, "wrist_roll")
time.sleep(1)
arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 2048, "wrist_flex")
time.sleep(1)
arm.write("Goal_Position", calib["gripper"]["end_pos"], "gripper")
time.sleep(1)
print("Calibrate wrist_roll")
calib["wrist_roll"] = move_to_calibrate(arm, "wrist_roll", invert_drive_mode=True)
calib["wrist_roll"] = apply_offset(calib["wrist_roll"], offset=790)
arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"] - 1024, "wrist_roll")
arm.write("Goal_Position", calib["gripper"]["start_pos"], "gripper")
arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
time.sleep(1)
arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"], "wrist_roll")
arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 2048, "wrist_flex")
def in_between_move_elbow_flex_hook():
nonlocal arm, calib
arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"], "wrist_flex")
print("Calibrate elbow_flex")
calib["elbow_flex"] = move_to_calibrate(
arm,
"elbow_flex",
invert_drive_mode=True,
in_between_move_hook=in_between_move_elbow_flex_hook,
)
arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
def in_between_move_shoulder_lift_hook():
nonlocal arm, calib
sl = arm.read("Present_Position", "shoulder_lift")
arm.write("Goal_Position", sl - 1500, "shoulder_lift")
time.sleep(1)
arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1536, "elbow_flex")
time.sleep(1)
arm.write("Goal_Position", calib["wrist_flex"]["start_pos"], "wrist_flex")
time.sleep(1)
print("Calibrate shoulder_lift")
calib["shoulder_lift"] = move_to_calibrate(
arm, "shoulder_lift", in_between_move_hook=in_between_move_shoulder_lift_hook
)
calib["shoulder_lift"] = apply_offset(calib["shoulder_lift"], offset=-1024)
arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
time.sleep(1)
arm.write("Goal_Position", calib["shoulder_lift"]["zero_pos"] + 2048, "shoulder_lift")
arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] - 1024 - 400, "elbow_flex")
time.sleep(2)
calib_modes = []
for name in arm.motor_names:
if name == "gripper":
calib_modes.append(CalibrationMode.LINEAR.name)
else:
calib_modes.append(CalibrationMode.DEGREE.name)
calib_dict = {
"homing_offset": [calib[name]["homing_offset"] for name in arm.motor_names],
"drive_mode": [calib[name]["drive_mode"] for name in arm.motor_names],
"start_pos": [calib[name]["start_pos"] for name in arm.motor_names],
"end_pos": [calib[name]["end_pos"] for name in arm.motor_names],
"calib_mode": calib_modes,
"motor_names": arm.motor_names,
}
# Re-enable original accerlation
arm.write("Lock", 0)
arm.write("Acceleration", initial_acceleration)
time.sleep(1)
return calib_dict
def run_arm_manual_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
"""This function ensures that a neural network trained on data collected on a given robot
can work on another robot. For instance before calibration, setting a same goal position
for each motor of two different robots will get two very different positions. But after calibration,
the two robots will move to the same position.To this end, this function computes the homing offset
and the drive mode for each motor of a given robot.
Homing offset is used to shift the motor position to a ]-2048, +2048[ nominal range (when the motor uses 2048 steps
to complete a half a turn). This range is set around an arbitrary "zero position" corresponding to all motor positions
being 0. During the calibration process, you will need to manually move the robot to this "zero position".
Drive mode is used to invert the rotation direction of the motor. This is useful when some motors have been assembled
in the opposite orientation for some robots. During the calibration process, you will need to manually move the robot
to the "rotated position".
After calibration, the homing offsets and drive modes are stored in a cache.
Example of usage:
```python ```python
run_arm_calibration(arm, "so100", "left", "follower") run_full_arm_calibration(arm, "so100", "left", "follower")
``` ```
""" """
if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any(): disable_torque(arm)
raise ValueError("To run calibration, the torque must be disabled on all motors.")
print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...") print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
print("\nMove arm to zero position") print("\nMove arm to homing position (middle)")
print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")) print(
"See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")
) # TODO(pepijn): replace with new instruction homing pos (all motors in middle) in tutorial
input("Press Enter to continue...") input("Press Enter to continue...")
# We arbitrarily chose our zero target position to be a straight horizontal position with gripper upwards and closed. start_positions = np.zeros(len(arm.motor_indices))
# It is easy to identify and all motors are in a "quarter turn" position. Once calibration is done, this position will end_positions = np.zeros(len(arm.motor_indices))
# correspond to every motor angle being 0. If you set all 0 as Goal Position, the arm will move in this position. encoder_offsets = np.zeros(len(arm.motor_indices))
zero_target_pos = convert_degrees_to_steps(ZERO_POSITION_DEGREE, arm.motor_models)
# Compute homing offset so that `present_position + homing_offset ~= target_position`. modes = get_calibration_modes(arm)
zero_pos = arm.read("Present_Position")
homing_offset = zero_target_pos - zero_pos
# The rotated target position corresponds to a rotation of a quarter turn from the zero position. for i, motor_id in enumerate(arm.motor_indices):
# This allows to identify the rotation direction of each motor. if modes[i] == CalibrationMode.DEGREE.name:
# For instance, if the motor rotates 90 degree, and its value is -90 after applying the homing offset, then we know its rotation direction encoder_offsets[i] = calibrate_homing_motor(motor_id, arm)
# is inverted. However, for the calibration being successful, we need everyone to follow the same target position. start_positions[i] = 0
# Sometimes, there is only one possible rotation direction. For instance, if the gripper is closed, there is only one direction which end_positions[i] = 0
# corresponds to opening the gripper. When the rotation direction is ambiguous, we arbitrarily rotate clockwise from the point of view
# of the previous motor in the kinetic chain.
print("\nMove arm to rotated target position")
print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rotated"))
input("Press Enter to continue...")
rotated_target_pos = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, arm.motor_models) for i, motor_id in enumerate(arm.motor_indices):
if modes[i] == CalibrationMode.LINEAR.name:
# Find drive mode by rotating each motor by a quarter of a turn. start_positions[i], end_positions[i] = calibrate_linear_motor(motor_id, arm)
# Drive mode indicates if the motor rotation direction should be inverted (=1) or not (=0). encoder_offsets[i] = 0
rotated_pos = arm.read("Present_Position")
drive_mode = (rotated_pos < zero_pos).astype(np.int32)
# Re-compute homing offset to take into account drive mode
rotated_drived_pos = apply_drive_mode(rotated_pos, drive_mode)
homing_offset = rotated_target_pos - rotated_drived_pos
print("\nMove arm to rest position") print("\nMove arm to rest position")
print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rest"))
input("Press Enter to continue...") input("Press Enter to continue...")
print()
# Joints with rotational motions are expressed in degrees in nominal range of [-180, 180] print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
calib_modes = []
for name in arm.motor_names: # Force drive_mode values (can be static)
if name == "gripper": drive_modes = [0, 1, 0, 0, 1, 0]
calib_modes.append(CalibrationMode.LINEAR.name)
else:
calib_modes.append(CalibrationMode.DEGREE.name)
calib_dict = { calib_dict = {
"homing_offset": homing_offset.tolist(), "homing_offset": encoder_offsets.astype(int).tolist(),
"drive_mode": drive_mode.tolist(), "drive_mode": drive_modes,
"start_pos": zero_pos.tolist(), "start_pos": start_positions.astype(int).tolist(),
"end_pos": rotated_pos.tolist(), "end_pos": end_positions.astype(int).tolist(),
"calib_mode": calib_modes, "calib_mode": get_calibration_modes(arm),
"motor_names": arm.motor_names, "motor_names": arm.motor_names,
} }
return calib_dict return calib_dict
def run_full_auto_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
"""TODO(pepijn): Add this method later as extra
Example of usage:
```python
run_full_auto_arm_calibration(arm, "so100", "left", "follower")
```
"""
print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
def apply_feetech_offsets_from_calibration(motorsbus: 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.
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.
"""
homing_offsets = calibration_dict["homing_offset"]
motor_names = calibration_dict["motor_names"]
start_pos = calibration_dict["start_pos"]
# Open the write lock, changes to EEPROM do NOT persist yet
motorsbus.write("Lock", 1)
# 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 motorsbus.motors:
print(f"Warning: '{m_name}' not found in motorsbus.motors; skipping offset.")
continue
if m_name == "gripper":
old_offset = start_pos # If gripper set the offset to the start position of the 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(
f"Warning: '{new_offset}' is getting clamped because its larger then 2047; This should not happen!"
)
elif new_offset < -2047:
new_offset = -2047
print(
f"Warning: '{new_offset}' is getting clamped because its smaller then -2047; This should not 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 offset to servo
motorsbus.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}"
)
motorsbus.write("Lock", 0)
print("Offsets have been saved to EEPROM successfully.")

15
lerobot/common/robots/koch/robot_koch.py
View File

@ -49,7 +49,6 @@ class KochRobot(Robot):
super().__init__(config) super().__init__(config)
self.config = config self.config = config
self.robot_type = config.type self.robot_type = config.type
self.id = config.id
self.arm = DynamixelMotorsBus( self.arm = DynamixelMotorsBus(
port=self.config.port, port=self.config.port,
@ -129,19 +128,17 @@ class KochRobot(Robot):
Rotations are expressed in degrees in nominal range of [-180, 180], Rotations are expressed in degrees in nominal range of [-180, 180],
and linear motions (like gripper of Aloha) in nominal range of [0, 100]. and linear motions (like gripper of Aloha) in nominal range of [0, 100].
""" """
arm_calib_path = self.calibration_dir / f"{self.config.id}.json" if self.calibration_fpath.exists():
with open(self.calibration_fpath) as f:
if arm_calib_path.exists():
with open(arm_calib_path) as f:
calibration = json.load(f) calibration = json.load(f)
else: else:
# TODO(rcadene): display a warning in __init__ if calibration file not available # TODO(rcadene): display a warning in __init__ if calibration file not available
logging.info(f"Missing calibration file '{arm_calib_path}'") logging.info(f"Missing calibration file '{self.calibration_fpath}'")
calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "follower") calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "follower")
logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'") logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
arm_calib_path.parent.mkdir(parents=True, exist_ok=True) self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
with open(arm_calib_path, "w") as f: with open(self.calibration_fpath, "w") as f:
json.dump(calibration, f) json.dump(calibration, f)
self.arm.set_calibration(calibration) self.arm.set_calibration(calibration)

4
lerobot/common/robots/lekiwi/lekiwi_remote.py
View File

@ -61,7 +61,7 @@ def calibrate_follower_arm(motors_bus, calib_dir_str):
calib_dir.mkdir(parents=True, exist_ok=True) calib_dir.mkdir(parents=True, exist_ok=True)
calib_file = calib_dir / "main_follower.json" calib_file = calib_dir / "main_follower.json"
try: try:
from lerobot.common.motors.feetech.feetech_calibration import run_arm_manual_calibration from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration
except ImportError: except ImportError:
print("[WARNING] Calibration function not available. Skipping calibration.") print("[WARNING] Calibration function not available. Skipping calibration.")
return return
@ -72,7 +72,7 @@ def calibrate_follower_arm(motors_bus, calib_dir_str):
print(f"[INFO] Loaded calibration from {calib_file}") print(f"[INFO] Loaded calibration from {calib_file}")
else: else:
print("[INFO] Calibration file not found. Running manual calibration...") print("[INFO] Calibration file not found. Running manual calibration...")
calibration = run_arm_manual_calibration(motors_bus, "lekiwi", "follower_arm", "follower") calibration = run_full_arm_calibration(motors_bus, "lekiwi", "follower_arm", "follower")
print(f"[INFO] Calibration complete. Saving to {calib_file}") print(f"[INFO] Calibration complete. Saving to {calib_file}")
with open(calib_file, "w") as f: with open(calib_file, "w") as f:
json.dump(calibration, f) json.dump(calibration, f)

4
lerobot/common/robots/lekiwi/robot_lekiwi.py
View File

@ -12,7 +12,7 @@ import zmq
from lerobot.common.cameras.utils import make_cameras_from_configs from lerobot.common.cameras.utils import make_cameras_from_configs
from lerobot.common.errors import DeviceNotConnectedError from lerobot.common.errors import DeviceNotConnectedError
from lerobot.common.motors.feetech.feetech import TorqueMode from lerobot.common.motors.feetech.feetech import TorqueMode
from lerobot.common.motors.feetech.feetech_calibration import run_arm_manual_calibration from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration
from lerobot.common.motors.motors_bus import MotorsBus from lerobot.common.motors.motors_bus import MotorsBus
from lerobot.common.motors.utils import make_motors_buses_from_configs from lerobot.common.motors.utils import make_motors_buses_from_configs
from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig
@ -253,7 +253,7 @@ class MobileManipulator:
calibration = json.load(f) calibration = json.load(f)
else: else:
print(f"Missing calibration file '{arm_calib_path}'") print(f"Missing calibration file '{arm_calib_path}'")
calibration = run_arm_manual_calibration(arm, self.robot_type, name, arm_type) calibration = run_full_arm_calibration(arm, self.robot_type, name, arm_type)
print(f"Calibration is done! Saving calibration file '{arm_calib_path}'") print(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
arm_calib_path.parent.mkdir(parents=True, exist_ok=True) arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
with open(arm_calib_path, "w") as f: with open(arm_calib_path, "w") as f:

95
lerobot/common/robots/manipulator.py
View File

@ -81,6 +81,73 @@ class ManipulatorRobotConfig(RobotConfig):
) )
def apply_feetech_offsets_from_calibration(motorsbus, 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.
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.
"""
homing_offsets = calibration_dict["homing_offset"]
motor_names = calibration_dict["motor_names"]
start_pos = calibration_dict["start_pos"]
# Open the write lock, changes to EEPROM do NOT persist yet
motorsbus.write("Lock", 1)
# 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 motorsbus.motors:
print(f"Warning: '{m_name}' not found in motorsbus.motors; skipping offset.")
continue
if m_name == "gripper":
old_offset = start_pos # If gripper set the offset to the start position of the 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(
f"Warning: '{new_offset}' is getting clamped because its larger then 2047; This should not happen!"
)
elif new_offset < -2047:
new_offset = -2047
print(
f"Warning: '{new_offset}' is getting clamped because its smaller then -2047; This should not 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 offset to servo
motorsbus.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}"
)
motorsbus.write("Lock", 0)
print("Offsets have been saved to EEPROM successfully.")
class ManipulatorRobot: class ManipulatorRobot:
# TODO(rcadene): Implement force feedback # TODO(rcadene): Implement force feedback
"""This class allows to control any manipulator robot of various number of motors. """This class allows to control any manipulator robot of various number of motors.
@ -342,10 +409,10 @@ class ManipulatorRobot:
elif self.robot_type in ["so100", "moss", "lekiwi"]: elif self.robot_type in ["so100", "moss", "lekiwi"]:
from lerobot.common.motors.feetech.feetech_calibration import ( from lerobot.common.motors.feetech.feetech_calibration import (
run_arm_manual_calibration, run_full_arm_calibration,
) )
calibration = run_arm_manual_calibration(arm, self.robot_type, name, arm_type) calibration = run_full_arm_calibration(arm, self.robot_type, name, arm_type)
print(f"Calibration is done! Saving calibration file '{arm_calib_path}'") print(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
arm_calib_path.parent.mkdir(parents=True, exist_ok=True) arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
@ -354,12 +421,24 @@ class ManipulatorRobot:
return calibration return calibration
for name, arm in self.follower_arms.items(): # For each follower arm
calibration = load_or_run_calibration_(name, arm, "follower")
arm.set_calibration(calibration) for name, arm_bus in self.follower_arms.items():
for name, arm in self.leader_arms.items(): calibration = load_or_run_calibration_(name, arm_bus, "follower")
calibration = load_or_run_calibration_(name, arm, "leader") arm_bus.set_calibration(calibration)
arm.set_calibration(calibration)
# If this is a Feetech robot, also set the servo offset into EEPROM
if self.robot_type in ["so100", "lekiwi"]:
apply_feetech_offsets_from_calibration(arm_bus, calibration)
# For each leader arm
for name, arm_bus in self.leader_arms.items():
calibration = load_or_run_calibration_(name, arm_bus, "leader")
arm_bus.set_calibration(calibration)
# Optionally also set offset for leader if you want the servo offsets as well
if self.robot_type in ["so100", "lekiwi"]:
apply_feetech_offsets_from_calibration(arm_bus, calibration)
def set_koch_robot_preset(self): def set_koch_robot_preset(self):
def set_operating_mode_(arm): def set_operating_mode_(arm):

4
lerobot/common/robots/mobile_manipulator.py
View File

@ -26,7 +26,7 @@ import zmq
from lerobot.common.cameras.utils import make_cameras_from_configs from lerobot.common.cameras.utils import make_cameras_from_configs
from lerobot.common.errors import DeviceNotConnectedError from lerobot.common.errors import DeviceNotConnectedError
from lerobot.common.motors.feetech.feetech import TorqueMode from lerobot.common.motors.feetech.feetech import TorqueMode
from lerobot.common.motors.feetech.feetech_calibration import run_arm_manual_calibration from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration
from lerobot.common.motors.motors_bus import MotorsBus from lerobot.common.motors.motors_bus import MotorsBus
from lerobot.common.motors.utils import make_motors_buses_from_configs from lerobot.common.motors.utils import make_motors_buses_from_configs
from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig
@ -267,7 +267,7 @@ class MobileManipulator:
calibration = json.load(f) calibration = json.load(f)
else: else:
print(f"Missing calibration file '{arm_calib_path}'") print(f"Missing calibration file '{arm_calib_path}'")
calibration = run_arm_manual_calibration(arm, self.robot_type, name, arm_type) calibration = run_full_arm_calibration(arm, self.robot_type, name, arm_type)
print(f"Calibration is done! Saving calibration file '{arm_calib_path}'") print(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
arm_calib_path.parent.mkdir(parents=True, exist_ok=True) arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
with open(arm_calib_path, "w") as f: with open(arm_calib_path, "w") as f:

21
lerobot/common/robots/moss/robot_moss.py
View File

@ -26,7 +26,8 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte
from lerobot.common.motors.feetech import ( from lerobot.common.motors.feetech import (
FeetechMotorsBus, FeetechMotorsBus,
TorqueMode, TorqueMode,
run_arm_manual_calibration, apply_feetech_offsets_from_calibration,
run_full_arm_calibration,
) )
from ..robot import Robot from ..robot import Robot
@ -46,7 +47,6 @@ class MossRobot(Robot):
super().__init__(config) super().__init__(config)
self.config = config self.config = config
self.robot_type = config.type self.robot_type = config.type
self.id = config.id
self.arm = FeetechMotorsBus( self.arm = FeetechMotorsBus(
port=self.config.port, port=self.config.port,
@ -133,22 +133,21 @@ class MossRobot(Robot):
Rotations are expressed in degrees in nominal range of [-180, 180], Rotations are expressed in degrees in nominal range of [-180, 180],
and linear motions (like gripper of Aloha) in nominal range of [0, 100]. and linear motions (like gripper of Aloha) in nominal range of [0, 100].
""" """
arm_calib_path = self.calibration_dir / f"{self.config.id}.json" if self.calibration_fpath.exists():
with open(self.calibration_fpath) as f:
if arm_calib_path.exists():
with open(arm_calib_path) as f:
calibration = json.load(f) calibration = json.load(f)
else: else:
# TODO(rcadene): display a warning in __init__ if calibration file not available # TODO(rcadene): display a warning in __init__ if calibration file not available
logging.info(f"Missing calibration file '{arm_calib_path}'") logging.info(f"Missing calibration file '{self.calibration_fpath}'")
calibration = run_arm_manual_calibration(self.arm, self.robot_type, self.name, "follower") calibration = run_full_arm_calibration(self.arm, self.robot_type, self.name, "follower")
logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'") logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
arm_calib_path.parent.mkdir(parents=True, exist_ok=True) self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
with open(arm_calib_path, "w") as f: with open(self.calibration_fpath, "w") as f:
json.dump(calibration, f) json.dump(calibration, f)
self.arm.set_calibration(calibration) self.arm.set_calibration(calibration)
apply_feetech_offsets_from_calibration(self.arm, calibration)
def get_observation(self) -> dict[str, np.ndarray]: def get_observation(self) -> dict[str, np.ndarray]:
"""The returned observations do not have a batch dimension.""" """The returned observations do not have a batch dimension."""

2
lerobot/common/robots/robot.py
View File

@ -17,10 +17,12 @@ class Robot(abc.ABC):
def __init__(self, config: RobotConfig): def __init__(self, config: RobotConfig):
self.robot_type = self.name self.robot_type = self.name
self.id = config.id
self.calibration_dir = ( self.calibration_dir = (
config.calibration_dir if config.calibration_dir else HF_LEROBOT_CALIBRATION / ROBOTS / self.name config.calibration_dir if config.calibration_dir else HF_LEROBOT_CALIBRATION / ROBOTS / self.name
) )
self.calibration_dir.mkdir(parents=True, exist_ok=True) self.calibration_dir.mkdir(parents=True, exist_ok=True)
self.calibration_fpath = self.calibration_dir / f"{self.id}.json"
# TODO(aliberts): create a proper Feature class for this that links with datasets # TODO(aliberts): create a proper Feature class for this that links with datasets
@abc.abstractproperty @abc.abstractproperty

6
lerobot/common/robots/so100/__init__.py
View File

@ -1,4 +1,4 @@
from .configuration_so100 import So100RobotConfig from .configuration_so100 import SO100RobotConfig
from .robot_so100 import So100Robot from .robot_so100 import SO100Robot
__all__ = ["So100RobotConfig", "So100Robot"] __all__ = ["SO100RobotConfig", "SO100Robot"]

12
lerobot/common/robots/so100/configuration_so100.py
View File

@ -7,7 +7,7 @@ from ..config import RobotConfig
@RobotConfig.register_subclass("so100") @RobotConfig.register_subclass("so100")
@dataclass @dataclass
class So100RobotConfig(RobotConfig): class SO100RobotConfig(RobotConfig):
# Port to connect to the robot # Port to connect to the robot
port: str port: str
@ -16,15 +16,5 @@ class So100RobotConfig(RobotConfig):
# the number of motors in your follower arms. # the number of motors in your follower arms.
max_relative_target: int | None = None max_relative_target: int | None = None
mock: bool = False
# motors
shoulder_pan: tuple = (1, "sts3215")
shoulder_lift: tuple = (2, "sts3215")
elbow_flex: tuple = (3, "sts3215")
wrist_flex: tuple = (4, "sts3215")
wrist_roll: tuple = (5, "sts3215")
gripper: tuple = (6, "sts3215")
# cameras # cameras
cameras: dict[str, CameraConfig] = field(default_factory=dict) cameras: dict[str, CameraConfig] = field(default_factory=dict)

45
lerobot/common/robots/so100/robot_so100.py
View File

@ -26,37 +26,37 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte
from lerobot.common.motors.feetech import ( from lerobot.common.motors.feetech import (
FeetechMotorsBus, FeetechMotorsBus,
TorqueMode, TorqueMode,
run_arm_manual_calibration, apply_feetech_offsets_from_calibration,
run_full_arm_calibration,
) )
from ..robot import Robot from ..robot import Robot
from ..utils import ensure_safe_goal_position from ..utils import ensure_safe_goal_position
from .configuration_so100 import So100RobotConfig from .configuration_so100 import SO100RobotConfig
class So100Robot(Robot): class SO100Robot(Robot):
""" """
[SO-100 Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio [SO-100 Follower Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio
""" """
config_class = So100RobotConfig config_class = SO100RobotConfig
name = "koch" name = "so100"
def __init__(self, config: So100RobotConfig): def __init__(self, config: SO100RobotConfig):
super().__init__(config) super().__init__(config)
self.config = config self.config = config
self.robot_type = config.type self.robot_type = config.type
self.id = config.id
self.arm = FeetechMotorsBus( self.arm = FeetechMotorsBus(
port=self.config.port, port=self.config.port,
motors={ motors={
"shoulder_pan": config.shoulder_pan, "shoulder_pan": (1, "sts3215"),
"shoulder_lift": config.shoulder_lift, "shoulder_lift": (2, "sts3215"),
"elbow_flex": config.elbow_flex, "elbow_flex": (3, "sts3215"),
"wrist_flex": config.wrist_flex, "wrist_flex": (4, "sts3215"),
"wrist_roll": config.wrist_roll, "wrist_roll": (5, "sts3215"),
"gripper": config.gripper, "gripper": (6, "sts3215"),
}, },
) )
self.cameras = make_cameras_from_configs(config.cameras) self.cameras = make_cameras_from_configs(config.cameras)
@ -133,22 +133,21 @@ class So100Robot(Robot):
Rotations are expressed in degrees in nominal range of [-180, 180], Rotations are expressed in degrees in nominal range of [-180, 180],
and linear motions (like gripper of Aloha) in nominal range of [0, 100]. and linear motions (like gripper of Aloha) in nominal range of [0, 100].
""" """
arm_calib_path = self.calibration_dir / f"{self.config.id}.json" if self.calibration_fpath.exists():
with open(self.calibration_fpath) as f:
if arm_calib_path.exists():
with open(arm_calib_path) as f:
calibration = json.load(f) calibration = json.load(f)
else: else:
# TODO(rcadene): display a warning in __init__ if calibration file not available # TODO(rcadene): display a warning in __init__ if calibration file not available
logging.info(f"Missing calibration file '{arm_calib_path}'") logging.info(f"Missing calibration file '{self.calibration_fpath}'")
calibration = run_arm_manual_calibration(self.arm, self.robot_type, self.name, "follower") calibration = run_full_arm_calibration(self.arm, self.robot_type, self.name, "follower")
logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'") logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
arm_calib_path.parent.mkdir(parents=True, exist_ok=True) self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
with open(arm_calib_path, "w") as f: with open(self.calibration_fpath, "w") as f:
json.dump(calibration, f) json.dump(calibration, f)
self.arm.set_calibration(calibration) self.arm.set_calibration(calibration)
apply_feetech_offsets_from_calibration(self.arm, calibration)
def get_observation(self) -> dict[str, np.ndarray]: def get_observation(self) -> dict[str, np.ndarray]:
"""The returned observations do not have a batch dimension.""" """The returned observations do not have a batch dimension."""

4
lerobot/common/robots/utils.py
View File

@ -42,9 +42,9 @@ def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:
return MossRobotConfig(**kwargs) return MossRobotConfig(**kwargs)
elif robot_type == "so100": elif robot_type == "so100":
from .so100.configuration_so100 import So100RobotConfig from .so100.configuration_so100 import SO100RobotConfig
return So100RobotConfig(**kwargs) return SO100RobotConfig(**kwargs)
elif robot_type == "stretch": elif robot_type == "stretch":
from .stretch3.configuration_stretch3 import Stretch3RobotConfig from .stretch3.configuration_stretch3 import Stretch3RobotConfig

15
lerobot/common/robots/viperx/robot_viperx.py
View File

@ -39,7 +39,6 @@ class ViperXRobot(Robot):
super().__init__(config) super().__init__(config)
self.config = config self.config = config
self.robot_type = config.type self.robot_type = config.type
self.id = config.id
self.arm = DynamixelMotorsBus( self.arm = DynamixelMotorsBus(
port=self.config.port, port=self.config.port,
@ -150,19 +149,17 @@ class ViperXRobot(Robot):
Rotations are expressed in degrees in nominal range of [-180, 180], Rotations are expressed in degrees in nominal range of [-180, 180],
and linear motions (like gripper of Aloha) in nominal range of [0, 100]. and linear motions (like gripper of Aloha) in nominal range of [0, 100].
""" """
arm_calib_path = self.calibration_dir / f"{self.config.id}.json" if self.calibration_fpath.exists():
with open(self.calibration_fpath) as f:
if arm_calib_path.exists():
with open(arm_calib_path) as f:
calibration = json.load(f) calibration = json.load(f)
else: else:
# TODO(rcadene): display a warning in __init__ if calibration file not available # TODO(rcadene): display a warning in __init__ if calibration file not available
logging.info(f"Missing calibration file '{arm_calib_path}'") logging.info(f"Missing calibration file '{self.calibration_fpath}'")
calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "follower") calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "follower")
logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'") logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
arm_calib_path.parent.mkdir(parents=True, exist_ok=True) self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
with open(arm_calib_path, "w") as f: with open(self.calibration_fpath, "w") as f:
json.dump(calibration, f) json.dump(calibration, f)
self.arm.set_calibration(calibration) self.arm.set_calibration(calibration)

1
lerobot/common/teleoperators/keyboard/teleop_keyboard.py
View File

@ -55,7 +55,6 @@ class KeyboardTeleop(Teleoperator):
super().__init__(config) super().__init__(config)
self.config = config self.config = config
self.robot_type = config.type self.robot_type = config.type
self.id = config.id
self.pressed_keys = {} self.pressed_keys = {}
self.listener = None self.listener = None

15
lerobot/common/teleoperators/koch/teleop_koch.py
View File

@ -46,7 +46,6 @@ class KochTeleop(Teleoperator):
super().__init__(config) super().__init__(config)
self.config = config self.config = config
self.robot_type = config.type self.robot_type = config.type
self.id = config.id
self.arm = DynamixelMotorsBus( self.arm = DynamixelMotorsBus(
port=self.config.port, port=self.config.port,
@ -106,19 +105,17 @@ class KochTeleop(Teleoperator):
Rotations are expressed in degrees in nominal range of [-180, 180], Rotations are expressed in degrees in nominal range of [-180, 180],
and linear motions (like gripper of Aloha) in nominal range of [0, 100]. and linear motions (like gripper of Aloha) in nominal range of [0, 100].
""" """
arm_calib_path = self.calibration_dir / f"{self.id}.json" if self.calibration_fpath.exists():
with open(self.calibration_fpath) as f:
if arm_calib_path.exists():
with open(arm_calib_path) as f:
calibration = json.load(f) calibration = json.load(f)
else: else:
# TODO(rcadene): display a warning in __init__ if calibration file not available # TODO(rcadene): display a warning in __init__ if calibration file not available
logging.info(f"Missing calibration file '{arm_calib_path}'") logging.info(f"Missing calibration file '{self.calibration_fpath}'")
calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "leader") calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "leader")
logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'") logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
arm_calib_path.parent.mkdir(parents=True, exist_ok=True) self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
with open(arm_calib_path, "w") as f: with open(self.calibration_fpath, "w") as f:
json.dump(calibration, f) json.dump(calibration, f)
self.arm.set_calibration(calibration) self.arm.set_calibration(calibration)

10
lerobot/common/teleoperators/so100/configuration_so100.py
View File

@ -24,13 +24,3 @@ from ..config import TeleoperatorConfig
class SO100TeleopConfig(TeleoperatorConfig): class SO100TeleopConfig(TeleoperatorConfig):
# Port to connect to the teloperator # Port to connect to the teloperator
port: str port: str
mock: bool = False
# motors
shoulder_pan: tuple = (1, "sts3215")
shoulder_lift: tuple = (2, "sts3215")
elbow_flex: tuple = (3, "sts3215")
wrist_flex: tuple = (4, "sts3215")
wrist_roll: tuple = (5, "sts3215")
gripper: tuple = (6, "sts3215")

40
lerobot/common/teleoperators/so100/teleop_so100.py
View File

@ -24,7 +24,8 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte
from lerobot.common.motors.feetech import ( from lerobot.common.motors.feetech import (
FeetechMotorsBus, FeetechMotorsBus,
TorqueMode, TorqueMode,
run_arm_manual_calibration, apply_feetech_offsets_from_calibration,
run_full_arm_calibration,
) )
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
@ -33,7 +34,7 @@ from .configuration_so100 import SO100TeleopConfig
class SO100Teleop(Teleoperator): class SO100Teleop(Teleoperator):
""" """
[SO-100 Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio [SO-100 Leader Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio
""" """
config_class = SO100TeleopConfig config_class = SO100TeleopConfig
@ -43,17 +44,16 @@ class SO100Teleop(Teleoperator):
super().__init__(config) super().__init__(config)
self.config = config self.config = config
self.robot_type = config.type self.robot_type = config.type
self.id = config.id
self.arm = FeetechMotorsBus( self.arm = FeetechMotorsBus(
port=self.config.port, port=self.config.port,
motors={ motors={
"shoulder_pan": config.shoulder_pan, "shoulder_pan": (1, "sts3215"),
"shoulder_lift": config.shoulder_lift, "shoulder_lift": (2, "sts3215"),
"elbow_flex": config.elbow_flex, "elbow_flex": (3, "sts3215"),
"wrist_flex": config.wrist_flex, "wrist_flex": (4, "sts3215"),
"wrist_roll": config.wrist_roll, "wrist_roll": (5, "sts3215"),
"gripper": config.gripper, "gripper": (6, "sts3215"),
}, },
) )
@ -86,11 +86,6 @@ class SO100Teleop(Teleoperator):
self.arm.write("Torque_Enable", TorqueMode.DISABLED.value) self.arm.write("Torque_Enable", TorqueMode.DISABLED.value)
self.calibrate() self.calibrate()
# Enable torque on the gripper and move it to 45 degrees so that we can use it as a trigger.
logging.info("Activating torque.")
self.arm.write("Torque_Enable", TorqueMode.ENABLED.value, "gripper")
self.arm.write("Goal_Position", self.config.gripper_open_degree, "gripper")
# Check arm can be read # Check arm can be read
self.arm.read("Present_Position") self.arm.read("Present_Position")
@ -101,22 +96,21 @@ class SO100Teleop(Teleoperator):
Rotations are expressed in degrees in nominal range of [-180, 180], Rotations are expressed in degrees in nominal range of [-180, 180],
and linear motions (like gripper of Aloha) in nominal range of [0, 100]. and linear motions (like gripper of Aloha) in nominal range of [0, 100].
""" """
arm_calib_path = self.calibration_dir / f"{self.id}.json" if self.calibration_fpath.exists():
with open(self.calibration_fpath) as f:
if arm_calib_path.exists():
with open(arm_calib_path) as f:
calibration = json.load(f) calibration = json.load(f)
else: else:
# TODO(rcadene): display a warning in __init__ if calibration file not available # TODO(rcadene): display a warning in __init__ if calibration file not available
logging.info(f"Missing calibration file '{arm_calib_path}'") logging.info(f"Missing calibration file '{self.calibration_fpath}'")
calibration = run_arm_manual_calibration(self.arm, self.robot_type, self.name, "leader") calibration = run_full_arm_calibration(self.arm, self.robot_type, self.name, "leader")
logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'") logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
arm_calib_path.parent.mkdir(parents=True, exist_ok=True) self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
with open(arm_calib_path, "w") as f: with open(self.calibration_fpath, "w") as f:
json.dump(calibration, f) json.dump(calibration, f)
self.arm.set_calibration(calibration) self.arm.set_calibration(calibration)
apply_feetech_offsets_from_calibration(self.arm, calibration)
def get_action(self) -> np.ndarray: def get_action(self) -> np.ndarray:
"""The returned action does not have a batch dimension.""" """The returned action does not have a batch dimension."""

2
lerobot/common/teleoperators/teleoperator.py
View File

@ -14,12 +14,14 @@ class Teleoperator(abc.ABC):
name: str name: str
def __init__(self, config: TeleoperatorConfig): def __init__(self, config: TeleoperatorConfig):
self.id = config.id
self.calibration_dir = ( self.calibration_dir = (
config.calibration_dir config.calibration_dir
if config.calibration_dir if config.calibration_dir
else HF_LEROBOT_CALIBRATION / TELEOPERATORS / self.name else HF_LEROBOT_CALIBRATION / TELEOPERATORS / self.name
) )
self.calibration_dir.mkdir(parents=True, exist_ok=True) self.calibration_dir.mkdir(parents=True, exist_ok=True)
self.calibration_fpath = self.calibration_dir / f"{self.id}.json"
@abc.abstractproperty @abc.abstractproperty
def action_feature(self) -> dict: def action_feature(self) -> dict:

15
lerobot/common/teleoperators/widowx/teleop_widowx.py
View File

@ -43,7 +43,6 @@ class WidowXTeleop(Teleoperator):
super().__init__(config) super().__init__(config)
self.config = config self.config = config
self.robot_type = config.type self.robot_type = config.type
self.id = config.id
self.arm = DynamixelMotorsBus( self.arm = DynamixelMotorsBus(
port=self.config.port, port=self.config.port,
@ -120,19 +119,17 @@ class WidowXTeleop(Teleoperator):
Rotations are expressed in degrees in nominal range of [-180, 180], Rotations are expressed in degrees in nominal range of [-180, 180],
and linear motions (like gripper of Aloha) in nominal range of [0, 100]. and linear motions (like gripper of Aloha) in nominal range of [0, 100].
""" """
arm_calib_path = self.calibration_dir / f"{self.id}.json" if self.calibration_fpath.exists():
with open(self.calibration_fpath) as f:
if arm_calib_path.exists():
with open(arm_calib_path) as f:
calibration = json.load(f) calibration = json.load(f)
else: else:
# TODO(rcadene): display a warning in __init__ if calibration file not available # TODO(rcadene): display a warning in __init__ if calibration file not available
logging.info(f"Missing calibration file '{arm_calib_path}'") logging.info(f"Missing calibration file '{self.calibration_fpath}'")
calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "leader") calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "leader")
logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'") logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
arm_calib_path.parent.mkdir(parents=True, exist_ok=True) self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
with open(arm_calib_path, "w") as f: with open(self.calibration_fpath, "w") as f:
json.dump(calibration, f) json.dump(calibration, f)
self.arm.set_calibration(calibration) self.arm.set_calibration(calibration)

100
lerobot/scripts/calibration_visualization_so100.py Normal file
View File

@ -0,0 +1,100 @@
"""
usage:
```python
python lerobot/scripts/calibration_visualization_so100.py \
--teleop.type=so100 \
--teleop.port=/dev/tty.usbmodem58760430541
python lerobot/scripts/calibration_visualization_so100.py \
--robot.type=so100 \
--robot.port=/dev/tty.usbmodem585A0084711
```
"""
import time
from dataclasses import dataclass
import draccus
from lerobot.common.motors.feetech.feetech import (
adjusted_to_homing_ticks,
adjusted_to_motor_ticks,
convert_degrees_to_ticks,
convert_ticks_to_degrees,
)
from lerobot.common.robots import RobotConfig
from lerobot.common.robots.so100 import SO100Robot, SO100RobotConfig # noqa: F401
from lerobot.common.teleoperators import TeleoperatorConfig
from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig # noqa: F401
@dataclass
class DebugFeetechConfig:
teleop: TeleoperatorConfig | None = None
robot: RobotConfig | None = None
def __post_init__(self):
if bool(self.teleop) == bool(self.robot):
raise ValueError("Select a single device.")
@draccus.wrap()
def debug_feetech_positions(cfg: DebugFeetechConfig):
"""
Reads each joint's (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks.
"""
device = SO100Teleop(cfg.teleop) if cfg.teleop else SO100Robot(cfg.robot)
device.connect()
# Disable torque on all motors so you can move them freely by hand
device.arm.write("Torque_Enable", 0, motor_names=device.arm.motor_names)
print("Torque disabled on all joints.")
try:
print("\nPress Ctrl+C to quit.\n")
while True:
# Read *raw* positions (no calibration).
raw_positions = device.arm.read_with_motor_ids(
device.arm.motor_models, device.arm.motor_indices, data_name="Present_Position"
)
# Read *already-homed* positions
homed_positions = device.arm.read("Present_Position")
for i, name in enumerate(device.arm.motor_names):
motor_idx, model = device.arm.motors[name]
raw_ticks = raw_positions[i] # 0..4095
homed_val = homed_positions[i] # degrees or % if linear
# Manually compute "adjusted ticks" from raw ticks
manual_adjusted = adjusted_to_homing_ticks(raw_ticks, model, device.arm, motor_idx)
# Convert to degrees
manual_degs = convert_ticks_to_degrees(manual_adjusted, model)
# Convert that deg back to ticks
manual_ticks = convert_degrees_to_ticks(manual_degs, model)
# Then invert them using offset & bus drive mode
inv_ticks = adjusted_to_motor_ticks(manual_ticks, model, device.arm, motor_idx)
print(
f"{name:15s} | "
f"RAW={raw_ticks:4d} | "
f"HOMED_FROM_READ={homed_val:7.2f} | "
f"HOMED_TICKS={manual_adjusted:6d} | "
f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
f"MANUAL_ADJ_TICKS={manual_ticks:6d} | "
f"INV_TICKS={inv_ticks:4d} "
)
print("----------------------------------------------------")
time.sleep(0.25)
except KeyboardInterrupt:
pass
finally:
print("\nExiting. Disconnecting bus...")
device.disconnect()
if __name__ == "__main__":
debug_feetech_positions()

11
lerobot/scripts/configure_motor.py
View File

@ -145,13 +145,12 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
# the motors. Note: this configuration is not in the official STS3215 Memory Table # the motors. Note: this configuration is not in the official STS3215 Memory Table
motor_bus.write("Lock", 0) motor_bus.write("Lock", 0)
motor_bus.write("Maximum_Acceleration", 254) motor_bus.write("Maximum_Acceleration", 254)
motor_bus.write("Max_Angle_Limit", 4095) # default 4095
motor_bus.write("Goal_Position", 2048) motor_bus.write("Min_Angle_Limit", 0) # default 0
time.sleep(4)
print("Present Position", motor_bus.read("Present_Position"))
motor_bus.write("Offset", 0) motor_bus.write("Offset", 0)
time.sleep(4) motor_bus.write("Mode", 0)
motor_bus.write("Goal_Position", 2048)
motor_bus.write("Lock", 1)
print("Offset", motor_bus.read("Offset")) print("Offset", motor_bus.read("Offset"))
except Exception as e: except Exception as e:

BIN
media/so100/follower_zero.webp
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 134 KiB

After

Width:  |  Height:  |  Size: 509 KiB

BIN
media/so100/leader_zero.webp
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 86 KiB

After

Width:  |  Height:  |  Size: 528 KiB