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

update comments, clean up

Browse Source
This commit is contained in:
Pepijn
2025-03-13 13:19:16 +01:00
parent ffc39a6a88
commit aaa6021475
5 changed files with 42 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -144,14 +144,14 @@ def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_i
""" """
resolutions = MODEL_RESOLUTION[model] resolutions = MODEL_RESOLUTION[model]
# 1) Shift raw ticks by half-resolution so 2048 -> 0, then wrap [0..res-1]. # Shift raw ticks by half-resolution so 2048 -> 0, then wrap [0..res-1].
ticks = (raw_motor_ticks - (resolutions // 2)) % resolutions ticks = (raw_motor_ticks - (resolutions // 2)) % resolutions
# 2) If above halfway, fold it into negative territory => [-2048..+2047]. # If above halfway, fold it into negative territory => [-2048..+2047].
if ticks > (resolutions // 2): if ticks > (resolutions // 2):
ticks -= resolutions ticks -= resolutions
# 3) Optionally flip sign if drive_mode is set. # Flip sign if drive_mode is set.
drive_mode = 0 drive_mode = 0
if motorbus.calibration is not None: if motorbus.calibration is not None:
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1] drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
@@ -167,7 +167,7 @@ def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: i
Inverse of adjusted_to_homing_ticks(). Takes a 'homed' position in [-2048..+2047] 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. and recovers the raw [0..(res-1)] ticks with 2048 as midpoint.
""" """
# 1) Flip sign if drive_mode was set. # Flip sign if drive_mode was set.
drive_mode = 0 drive_mode = 0
if motorbus.calibration is not None: if motorbus.calibration is not None:
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1] drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
@@ -177,8 +177,8 @@ def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: i
resolutions = MODEL_RESOLUTION[model] resolutions = MODEL_RESOLUTION[model]
# 2) Shift by +half-resolution and wrap into [0..res-1]. # Shift by +half-resolution and wrap into [0..res-1].
# This undoes the earlier shift by -half-resolution. # This undoes the earlier shift by -half-resolution.
ticks = (adjusted_pos + (resolutions // 2)) % resolutions ticks = (adjusted_pos + (resolutions // 2)) % resolutions
return ticks return ticks

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

@@ -11,10 +11,6 @@
# 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 numpy as np import numpy as np
from lerobot.common.robot_devices.motors.feetech import ( from lerobot.common.robot_devices.motors.feetech import (
@@ -42,14 +38,14 @@ def get_calibration_modes(arm: MotorsBus):
def reset_offset(motor_id, motor_bus): def reset_offset(motor_id, motor_bus):
# Open the write lock => Lock=1 => changes to EEPROM do NOT persist yet # Open the write lock, changes to EEPROM do NOT persist yet
motor_bus.write("Lock", 1) motor_bus.write("Lock", 1)
# Set offset to 0 # Set offset to 0
motor_name = motor_bus.motor_names[motor_id - 1] motor_name = motor_bus.motor_names[motor_id - 1]
motor_bus.write("Offset", 0, motor_names=[motor_name]) motor_bus.write("Offset", 0, motor_names=[motor_name])
# Close the write lock => Lock=0 => changes to EEPROM do persist! # Close the write lock, changes to EEPROM do persist
motor_bus.write("Lock", 0) motor_bus.write("Lock", 0)
# Confirm that the offset is zero by reading it back # Confirm that the offset is zero by reading it back
@@ -69,7 +65,7 @@ def calibrate_homing_motor(motor_id, motor_bus):
def calibrate_linear_motor(motor_id, motor_bus): def calibrate_linear_motor(motor_id, motor_bus):
motor_names = motor_bus.motor_names motor_names = motor_bus.motor_names
motor_name = motor_names[motor_id - 1] # TODO(pepijn): replace motor_id with motor index when (id-1) motor_name = motor_names[motor_id - 1]
reset_offset(motor_id, motor_bus) reset_offset(motor_id, motor_bus)
@@ -166,7 +162,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!") print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
# Force drive_mode values: motors 2 and 5 -> drive_mode 1; all others -> 0. 1 = clockwise = positive range (0..180), 0 = clockwise = negative range (0..-180) # Force drive_mode values (can be static)
drive_modes = [0, 1, 0, 0, 1, 0] drive_modes = [0, 1, 0, 0, 1, 0]
calib_dict = { calib_dict = {

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

@@ -57,8 +57,7 @@ def ensure_safe_goal_position(
def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict): def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
""" """
Reads 'calibration_dict' containing 'homing_offset' and 'motor_names', Reads 'calibration_dict' containing 'homing_offset' and 'motor_names',
then writes each motor's offset to the servo's internal Offset (0x1F) in EPROM, then writes each motor's offset to the servo's internal Offset (0x1F) in EPROM.
except for any motor whose name contains "gripper" (skipped).
This version is modified so each homed position (originally 0) will now read 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 2047, i.e. 180° away from 0 in the 4096-count circle. Offsets are permanently
@@ -69,16 +68,16 @@ def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
1) Subtract 2047 from the old offset (so 0 -> 2047). 1) Subtract 2047 from the old offset (so 0 -> 2047).
2) Clamp to [-2047..+2047]. 2) Clamp to [-2047..+2047].
3) Encode sign bit and magnitude into a 12-bit number. 3) Encode sign bit and magnitude into a 12-bit number.
4) Skip "gripper" motors, as they do not require this shift.
""" """
homing_offsets = calibration_dict["homing_offset"] homing_offsets = calibration_dict["homing_offset"]
motor_names = calibration_dict["motor_names"] motor_names = calibration_dict["motor_names"]
start_pos = calibration_dict["start_pos"]
# 1) Open the write lock => Lock=1 => changes to EEPROM do NOT persist yet # Open the write lock, changes to EEPROM do NOT persist yet
motorsbus.write("Lock", 1) motorsbus.write("Lock", 1)
# 2) For each motor, set the 'Offset' parameter # For each motor, set the 'Offset' parameter
for m_name, old_offset in zip(motor_names, homing_offsets, strict=False): 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 bus doesn’t have a motor named m_name, skip
if m_name not in motorsbus.motors: if m_name not in motorsbus.motors:
@@ -86,7 +85,7 @@ def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
continue continue
if m_name == "gripper": if m_name == "gripper":
print("Skipping gripper") old_offset = start_pos # If gripper set the offset to the start position of the gripper
continue continue
# Shift the offset so the homed position reads 2047 # Shift the offset so the homed position reads 2047
@@ -111,7 +110,7 @@ def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
# Combine sign bit (bit 11) with the magnitude (bits 0..10) # Combine sign bit (bit 11) with the magnitude (bits 0..10)
servo_offset = (direction_bit << 11) | magnitude servo_offset = (direction_bit << 11) | magnitude
# Write to servo # Write offset to servo
motorsbus.write("Offset", servo_offset, motor_names=m_name) motorsbus.write("Offset", servo_offset, motor_names=m_name)
print( print(
f"Set offset for {m_name}: " f"Set offset for {m_name}: "

64
lerobot/scripts/calibration_visualization.py → lerobot/scripts/calibration_visualization_so100.py
View File

@@ -10,18 +10,14 @@ from lerobot.common.robot_devices.motors.feetech import (
convert_degrees_to_ticks, convert_degrees_to_ticks,
convert_ticks_to_degrees, convert_ticks_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.configs import So100RobotConfig
from lerobot.common.robot_devices.robots.utils import make_robot_from_config from lerobot.common.robot_devices.robots.utils import make_robot_from_config
def apply_feetech_offsets_from_calibration(motors_bus: FeetechMotorsBus, calibration_dict: dict): def apply_feetech_offsets_from_calibration(motorsbus: FeetechMotorsBus, calibration_dict: dict):
""" """
Reads 'calibration_dict' containing 'homing_offset' and 'motor_names', Reads 'calibration_dict' containing 'homing_offset' and 'motor_names',
then writes each motor's offset to the servo's internal Offset (0x1F) in EPROM, then writes each motor's offset to the servo's internal Offset (0x1F) in EPROM.
except for any motor whose name contains "gripper" (skipped).
This version is modified so each homed position (originally 0) will now read 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 2047, i.e. 180° away from 0 in the 4096-count circle. Offsets are permanently
@@ -32,24 +28,24 @@ def apply_feetech_offsets_from_calibration(motors_bus: FeetechMotorsBus, calibra
1) Subtract 2047 from the old offset (so 0 -> 2047). 1) Subtract 2047 from the old offset (so 0 -> 2047).
2) Clamp to [-2047..+2047]. 2) Clamp to [-2047..+2047].
3) Encode sign bit and magnitude into a 12-bit number. 3) Encode sign bit and magnitude into a 12-bit number.
4) Skip "gripper" motors, as they do not require this shift.
""" """
homing_offsets = calibration_dict["homing_offset"] homing_offsets = calibration_dict["homing_offset"]
motor_names = calibration_dict["motor_names"] motor_names = calibration_dict["motor_names"]
start_pos = calibration_dict["start_pos"]
# 1) Open the write lock => Lock=1 => changes to EEPROM do NOT persist yet # Open the write lock, changes to EEPROM do NOT persist yet
motors_bus.write("Lock", 1) motorsbus.write("Lock", 1)
# 2) For each motor, set the 'Offset' parameter # For each motor, set the 'Offset' parameter
for m_name, old_offset in zip(motor_names, homing_offsets, strict=False): 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 bus doesn’t have a motor named m_name, skip
if m_name not in motors_bus.motors: if m_name not in motorsbus.motors:
print(f"Warning: '{m_name}' not found in motors_bus.motors; skipping offset.") print(f"Warning: '{m_name}' not found in motorsbus.motors; skipping offset.")
continue continue
if m_name == "gripper": if m_name == "gripper":
print("Skipping gripper") old_offset = start_pos # If gripper set the offset to the start position of the gripper
continue continue
# Shift the offset so the homed position reads 2047 # Shift the offset so the homed position reads 2047
@@ -58,10 +54,14 @@ def apply_feetech_offsets_from_calibration(motors_bus: FeetechMotorsBus, calibra
# Clamp to [-2047..+2047] # Clamp to [-2047..+2047]
if new_offset > 2047: if new_offset > 2047:
new_offset = 2047 new_offset = 2047
print("CLAMPED! -> shouldn't happen") print(
f"Warning: '{new_offset}' is getting clamped because its larger then 2047; This should not happen!"
)
elif new_offset < -2047: elif new_offset < -2047:
new_offset = -2047 new_offset = -2047
print("CLAMPED! -> shouldn't happen") print(
f"Warning: '{new_offset}' is getting clamped because its smaller then -2047; This should not happen!"
)
# Determine the direction (sign) bit and magnitude # Determine the direction (sign) bit and magnitude
direction_bit = 1 if new_offset < 0 else 0 direction_bit = 1 if new_offset < 0 else 0
@@ -70,40 +70,32 @@ def apply_feetech_offsets_from_calibration(motors_bus: FeetechMotorsBus, calibra
# Combine sign bit (bit 11) with the magnitude (bits 0..10) # Combine sign bit (bit 11) with the magnitude (bits 0..10)
servo_offset = (direction_bit << 11) | magnitude servo_offset = (direction_bit << 11) | magnitude
# Write to servo # Write offset to servo
motors_bus.write("Offset", servo_offset, motor_names=m_name) motorsbus.write("Offset", servo_offset, motor_names=m_name)
print( print(
f"Set offset for {m_name}: " f"Set offset for {m_name}: "
f"old_offset={old_offset}, new_offset={new_offset}, servo_encoded={magnitude} + direction={direction_bit}" f"old_offset={old_offset}, new_offset={new_offset}, servo_encoded={magnitude} + direction={direction_bit}"
) )
motors_bus.write("Lock", 0) motorsbus.write("Lock", 0)
print("Offsets have been saved to EEPROM successfully.") print("Offsets have been saved to EEPROM successfully.")
def debug_feetech_positions(cfg, arm_arg: str): 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. Reads each joint’s (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks.
Optionally writes offsets from the calibration file into servo EEPROM.
:param cfg: A config object (e.g. So100RobotConfig).
:param arm_arg: One of "main_leader" or "main_follower". :param arm_arg: One of "main_leader" or "main_follower".
:param apply_offsets: If True, calls apply_feetech_offsets_from_calibration() to save offsets to EEPROM.
""" """
# 1) Make the Robot from your config
robot = make_robot_from_config(cfg) robot = make_robot_from_config(cfg)
# 2) Parse which arm we want: 'main_leader' or 'main_follower' # Parse which arm we want: 'main_leader' or 'main_follower'
if arm_arg not in ("main_leader", "main_follower"): if arm_arg not in ("main_leader", "main_follower"):
raise ValueError("Please specify --arm=main_leader or --arm=main_follower") raise ValueError("Please specify --arm=main_leader or --arm=main_follower")
# Grab the relevant bus config from your robot’s arms
bus_config = robot.leader_arms["main"] if arm_arg == "main_leader" else robot.follower_arms["main"] 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) bus = FeetechMotorsBus(bus_config)
# 4) Load calibration if it exists # Load calibration if it exists
calib_file = Path(robot.calibration_dir) / f"{arm_arg}.json" calib_file = Path(robot.calibration_dir) / f"{arm_arg}.json"
if calib_file.exists(): if calib_file.exists():
with open(calib_file) as f: with open(calib_file) as f:
@@ -113,31 +105,29 @@ def debug_feetech_positions(cfg, arm_arg: str):
else: else:
print(f"No calibration file found at {calib_file}, skipping calibration set.") print(f"No calibration file found at {calib_file}, skipping calibration set.")
# 5) Connect to the bus
bus.connect() bus.connect()
print(f"Connected to Feetech bus on port: {bus.port}") print(f"Connected to Feetech bus on port: {bus.port}")
# 5b) Apply offset to servo EEPROM so the servo’s Present_Position is hardware-shifted # Apply offset to servo EEPROM so the servo’s Present_Position is hardware-shifted
if calibration_dict is not None: if calibration_dict is not None:
print("Applying offsets from calibration object to servo EEPROM. Be careful—this is permanent!") print("Applying offsets from calibration object to servo EEPROM. Be careful—this is permanent!")
apply_feetech_offsets_from_calibration(bus, calibration_dict) apply_feetech_offsets_from_calibration(bus, calibration_dict)
# 6) Disable torque on all motors so you can move them freely by hand # Disable torque on all motors so you can move them freely by hand
bus.write("Torque_Enable", 0, motor_names=bus.motor_names) bus.write("Torque_Enable", 0, motor_names=bus.motor_names)
print("Torque disabled on all joints.") print("Torque disabled on all joints.")
try: try:
print("\nPress Ctrl+C to quit.\n") print("\nPress Ctrl+C to quit.\n")
while True: while True:
# (a) Read *raw* positions (no calibration). This bypasses bus.apply_calibration # Read *raw* positions (no calibration).
raw_positions = bus.read_with_motor_ids( raw_positions = bus.read_with_motor_ids(
bus.motor_models, bus.motor_indices, data_name="Present_Position" bus.motor_models, bus.motor_indices, data_name="Present_Position"
) )
# (b) Read *already-homed* positions (calibration is applied automatically inside bus.read()) # Read *already-homed* positions
homed_positions = bus.read("Present_Position") homed_positions = bus.read("Present_Position")
# Print them side by side
for i, name in enumerate(bus.motor_names): for i, name in enumerate(bus.motor_names):
motor_idx, model = bus.motors[name] motor_idx, model = bus.motors[name]
@@ -164,7 +154,7 @@ def debug_feetech_positions(cfg, arm_arg: str):
f"INV_TICKS={inv_ticks:4d} " f"INV_TICKS={inv_ticks:4d} "
) )
print("----------------------------------------------------") print("----------------------------------------------------")
time.sleep(0.25) # slow down loop time.sleep(0.25)
except KeyboardInterrupt: except KeyboardInterrupt:
pass pass
finally: finally:
@@ -182,9 +172,5 @@ if __name__ == "__main__":
help="Which arm to debug: 'main_leader' or 'main_follower'.", help="Which arm to debug: 'main_leader' or 'main_follower'.",
) )
args = parser.parse_args() args = parser.parse_args()
# 1) Instantiate the config you want (So100RobotConfig, or whichever your project uses).
cfg = So100RobotConfig() cfg = So100RobotConfig()
# 2) Call the function with (cfg, args.arm, args.apply_offsets)
debug_feetech_positions(cfg, arm_arg=args.arm) debug_feetech_positions(cfg, arm_arg=args.arm)

7
lerobot/scripts/configure_motor.py
View File

@@ -149,15 +149,10 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
motor_bus.write("Min_Angle_Limit", 0) # default 0 motor_bus.write("Min_Angle_Limit", 0) # default 0
motor_bus.write("Offset", 0) motor_bus.write("Offset", 0)
motor_bus.write("Mode", 0) motor_bus.write("Mode", 0)
motor_bus.write("Goal_Position", 0) motor_bus.write("Goal_Position", 2048)
motor_bus.write("Torque_Enable", 0)
motor_bus.write("Lock", 1) motor_bus.write("Lock", 1)
print("Offset", motor_bus.read("Offset")) print("Offset", motor_bus.read("Offset"))
# 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)
# Then store single_calibration in yaml via dict (not overwrite but add the single calibration each time for motor id)
except Exception as e: except Exception as e:
print(f"Error occurred during motor configuration: {e}") print(f"Error occurred during motor configuration: {e}")