hangX/lerobot
1
0
Fork 0
forked from tangger/lerobot
Code Pull Requests 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]
# 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
# 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):
ticks -= resolutions
# 3) Optionally flip sign if drive_mode is set.
# 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]
@@ -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]
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
if motorbus.calibration is not None:
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]
# 2) Shift by +half-resolution and wrap into [0..res-1].
# This undoes the earlier shift by -half-resolution.
# 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

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.
# See the License for the specific language governing permissions and
# 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
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):
# 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)
# Set offset to 0
motor_name = motor_bus.motor_names[motor_id - 1]
motor_bus.write("Offset", 0, motor_names=[motor_name])
# Close the write lock => Lock=0 => changes to EEPROM do persist!
# Close the write lock, changes to EEPROM do persist
motor_bus.write("Lock", 0)
# 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):
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)
@@ -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!")
# 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]
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):
"""
Reads 'calibration_dict' containing 'homing_offset' and 'motor_names',
then writes each motor's offset to the servo's internal Offset (0x1F) in EPROM,
except for any motor whose name contains "gripper" (skipped).
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
@@ -69,16 +68,16 @@ def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
1) Subtract 2047 from the old offset (so 0 -> 2047).
2) Clamp to [-2047..+2047].
3) Encode sign bit and magnitude into a 12-bit number.
4) Skip "gripper" motors, as they do not require this shift.
"""
homing_offsets = calibration_dict["homing_offset"]
motor_names = calibration_dict["motor_names"]
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)
# 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):
# If bus doesn’t have a motor named m_name, skip
if m_name not in motorsbus.motors:
@@ -86,7 +85,7 @@ def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
continue
if m_name == "gripper":
print("Skipping 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
@@ -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)
servo_offset = (direction_bit << 11) | magnitude
# Write to servo
# Write offset to servo
motorsbus.write("Offset", servo_offset, motor_names=m_name)
print(
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_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.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',
then writes each motor's offset to the servo's internal Offset (0x1F) in EPROM,
except for any motor whose name contains "gripper" (skipped).
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
@@ -32,24 +28,24 @@ def apply_feetech_offsets_from_calibration(motors_bus: FeetechMotorsBus, calibra
1) Subtract 2047 from the old offset (so 0 -> 2047).
2) Clamp to [-2047..+2047].
3) Encode sign bit and magnitude into a 12-bit number.
4) Skip "gripper" motors, as they do not require this shift.
"""
homing_offsets = calibration_dict["homing_offset"]
motor_names = calibration_dict["motor_names"]
start_pos = calibration_dict["start_pos"]
# 1) Open the write lock => Lock=1 => changes to EEPROM do NOT persist yet
motors_bus.write("Lock", 1)
# Open the write lock, changes to EEPROM do NOT persist yet
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):
# If bus doesn’t have a motor named m_name, skip
if m_name not in motors_bus.motors:
print(f"Warning: '{m_name}' not found in motors_bus.motors; skipping offset.")
if m_name not in motorsbus.motors:
print(f"Warning: '{m_name}' not found in motorsbus.motors; skipping offset.")
continue
if m_name == "gripper":
print("Skipping 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
@@ -58,10 +54,14 @@ def apply_feetech_offsets_from_calibration(motors_bus: FeetechMotorsBus, calibra
# Clamp to [-2047..+2047]
if 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:
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
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)
servo_offset = (direction_bit << 11) | magnitude
# Write to servo
motors_bus.write("Offset", servo_offset, motor_names=m_name)
# 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}"
)
motors_bus.write("Lock", 0)
motorsbus.write("Lock", 0)
print("Offsets have been saved to EEPROM successfully.")
def debug_feetech_positions(cfg, arm_arg: str):
"""
Reads each joint’s (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks.
Optionally writes offsets from the calibration file into servo EEPROM.
:param cfg: A config object (e.g. So100RobotConfig).
:param arm_arg: One of "main_leader" or "main_follower".
:param apply_offsets: If True, calls apply_feetech_offsets_from_calibration() to save offsets to EEPROM.
"""
# 1) Make the Robot from your config
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"):
raise ValueError("Please specify --arm=main_leader or --arm=main_follower")
# Grab the relevant bus config from your robot’s arms
bus_config = robot.leader_arms["main"] if arm_arg == "main_leader" else robot.follower_arms["main"]
# 3) Create the Feetech bus from that config
bus = FeetechMotorsBus(bus_config)
# 4) Load calibration if it exists
# Load calibration if it exists
calib_file = Path(robot.calibration_dir) / f"{arm_arg}.json"
if calib_file.exists():
with open(calib_file) as f:
@@ -113,31 +105,29 @@ def debug_feetech_positions(cfg, arm_arg: str):
else:
print(f"No calibration file found at {calib_file}, skipping calibration set.")
# 5) Connect to the bus
bus.connect()
print(f"Connected to Feetech bus on port: {bus.port}")
# 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:
print("Applying offsets from calibration object to servo EEPROM. Be careful—this is permanent!")
apply_feetech_offsets_from_calibration(bus, calibration_dict)
# 6) Disable torque on all motors so you can move them freely by hand
# Disable torque on all motors so you can move them freely by hand
bus.write("Torque_Enable", 0, motor_names=bus.motor_names)
print("Torque disabled on all joints.")
try:
print("\nPress Ctrl+C to quit.\n")
while True:
# (a) Read *raw* positions (no calibration). This bypasses bus.apply_calibration
# Read *raw* positions (no calibration).
raw_positions = bus.read_with_motor_ids(
bus.motor_models, bus.motor_indices, data_name="Present_Position"
)
# (b) Read *already-homed* positions (calibration is applied automatically inside bus.read())
# Read *already-homed* positions
homed_positions = bus.read("Present_Position")
# Print them side by side
for i, name in enumerate(bus.motor_names):
motor_idx, model = bus.motors[name]
@@ -164,7 +154,7 @@ def debug_feetech_positions(cfg, arm_arg: str):
f"INV_TICKS={inv_ticks:4d} "
)
print("----------------------------------------------------")
time.sleep(0.25) # slow down loop
time.sleep(0.25)
except KeyboardInterrupt:
pass
finally:
@@ -182,9 +172,5 @@ if __name__ == "__main__":
help="Which arm to debug: 'main_leader' or 'main_follower'.",
)
args = parser.parse_args()
# 1) Instantiate the config you want (So100RobotConfig, or whichever your project uses).
cfg = So100RobotConfig()
# 2) Call the function with (cfg, args.arm, args.apply_offsets)
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("Offset", 0)
motor_bus.write("Mode", 0)
motor_bus.write("Goal_Position", 0)
motor_bus.write("Torque_Enable", 0)
motor_bus.write("Goal_Position", 2048)
motor_bus.write("Lock", 1)
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:
print(f"Error occurred during motor configuration: {e}")