admin/lerobot
1
0
Fork 0
mirror of https://github.com/huggingface/lerobot.git synced 2026-06-19 01:07:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

Print more

Browse Source
This commit is contained in:
Pepijn
2025-07-09 19:21:44 +02:00
parent 67d6bfee78
commit 0136912fa4
2 changed files with 55 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lerobot/bi_teleoperate.py
+45 -24
View File
@@ -28,12 +28,12 @@ def to_cmd(tau_vec, joints):
follower_cfg = SO101FollowerTConfig(
port="/dev/tty.usbmodem58760431551",
id="follower_arm_t",
id="follower_arm_tt",
)
leader_cfg = SO101FollowerTConfig(
port="/dev/tty.usbmodem58760428721",
id="leader_arm_t",
id="leader_arm_tt",
)
follower = SO101FollowerT(follower_cfg)
@@ -77,22 +77,15 @@ while True:
# Collect data for all motors
pos_f = {j: obs_f[f"{j}.pos"] for j in follower.bus.motors}
vel_f = {j: obs_f[f"{j}.vel"] for j in follower.bus.motors}
acc_f = {j: obs_f[f"{j}.acc"] for j in follower.bus.motors}
tau_meas_f = {j: obs_f[f"{j}.tau_meas"] for j in follower.bus.motors}
pos_l = {j: obs_l[f"{j}.pos"] for j in leader.bus.motors}
vel_l = {j: obs_l[f"{j}.vel"] for j in leader.bus.motors}
acc_l = {j: obs_l[f"{j}.acc"] for j in leader.bus.motors}
tau_meas_l = {j: obs_l[f"{j}.tau_meas"] for j in leader.bus.motors}
# Calculate velocities from position differences
if pos_f_prev is not None and pos_l_prev is not None:
vel_f = {j: (pos_f[j] - pos_f_prev[j]) / dt for j in follower.bus.motors}
vel_l = {j: (pos_l[j] - pos_l_prev[j]) / dt for j in leader.bus.motors}
else:
# First iteration: initialize with zeros
vel_f = dict.fromkeys(follower.bus.motors, 0.0)
vel_l = dict.fromkeys(leader.bus.motors, 0.0)
# Update previous positions for next iteration
pos_f_prev = pos_f.copy()
pos_l_prev = pos_l.copy()
@@ -245,24 +238,50 @@ while True:
if loop_count % (FRQ // PRINT_HZ) == 0:
hz = 1.0 / dt
# Detailed torque analysis mode
# Detailed torque analysis mode - LEADER
lines = [f"Loop {hz:6.1f} Hz Δt {dt * 1e3:5.2f} ms"]
lines.append("=" * 100)
lines.append("=" * 106)
lines.append("LEADER ARM TORQUE ANALYSIS:")
lines.append(
f"{'Joint':<13}{'L':>8}{'F':>8}{'Grav':>6}{'Inert':>6}{'Pos':>6}{'Vel':>6}{'Force':>6}{'Frict':>6}{'React':>6}{'Meas':>6}{'Cmd':>6}"
f"{'Joint':<13}{'Pos':>8}{'Grav':>6}{'Inert':>6}{'Track':>6}{'Vel':>6}{'Force':>6}{'Frict':>6}{'React':>6}{'Meas':>6}{'Cmd':>6}"
)
lines.append(
f"{'':13}{'(deg)':>8}{'(deg)':>8}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}"
f"{'':13}{'(deg)':>8}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}"
)
lines.append("-" * 100)
lines.append("-" * 86)
for i, j in enumerate(follower.bus.motors):
debug_f = debug_info_f[j]
for i, j in enumerate(leader.bus.motors):
debug_l = debug_info_l[j]
lines.append(
f"{j:<13s}"
f"{math.degrees(pos_l[j]):+8.1f}"
f"{debug_l['τ_gravity']:+6.2f}"
f"{debug_l['τ_inertia']:+6.2f}"
f"{debug_l['τ_pos']:+6.2f}"
f"{debug_l['τ_vel']:+6.2f}"
f"{debug_l['τ_force']:+6.2f}"
f"{debug_l['τ_friction']:+6.2f}"
f"{debug_l['τ_reaction']:+6.2f}"
f"{debug_l['τ_measured']:+6.2f}"
f"{tau_cmd_l[i]:+6.2f}"
)
lines.append("")
lines.append("FOLLOWER ARM TORQUE ANALYSIS:")
lines.append(
f"{'Joint':<13}{'Pos':>8}{'Grav':>6}{'Inert':>6}{'Track':>6}{'Vel':>6}{'Force':>6}{'Frict':>6}{'React':>6}{'Meas':>6}{'Cmd':>6}"
)
lines.append(
f"{'':13}{'(deg)':>8}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}"
)
lines.append("-" * 86)
for i, j in enumerate(follower.bus.motors):
debug_f = debug_info_f[j]
lines.append(
f"{j:<13s}"
f"{math.degrees(pos_f[j]):+8.1f}"
f"{debug_f['τ_gravity']:+6.2f}"
f"{debug_f['τ_inertia']:+6.2f}"
@@ -275,19 +294,21 @@ while True:
f"{tau_cmd_f[i]:+6.2f}"
)
lines.append("-" * 100)
lines.append("")
lines.append("=" * 86)
lines.append("TORQUE COMPONENT EXPLANATIONS:")
lines.append("• Grav (gravity) = Feed-forward gravity compensation")
lines.append("• Inert (inertia) = Feed-forward inertia compensation")
lines.append("• Pos (position) = Position tracking control (Kp error)")
lines.append("• Pos (joint pos) = Joint position in degrees")
lines.append("• Grav (gravity) = Feed-forward gravity compensation")
lines.append("• Inert (inertia) = Feed-forward inertia compensation")
lines.append("• Track (tracking) = Position tracking control (Kp error)")
lines.append("• Vel (velocity) = Velocity damping control (Kd error)")
lines.append("• Force (bilateral) = Force reflection between robots (telepresence)")
lines.append("• Frict (friction) = Feed-forward friction compensation (transparency)")
lines.append("• React (reaction) = External forces (human interaction, contact)")
lines.append("• Meas (measured) = Raw torque from motor current sensor")
lines.append("• Cmd (command) = Final torque sent to motor")
lines.append("-" * 100)
lines.append("Cmd = Pos + Vel + Force + Grav + Inert + Frict")
lines.append("-" * 86)
lines.append("Cmd = Track + Vel + Force + Grav + Inert + Frict")
lines.append(
"React = Meas - Grav - Inert - Frict (external forces)"
if use_friction_comp
src/lerobot/robots/so101_follower_torque/so101_follower_t.py
+10 -11
View File
@@ -15,7 +15,6 @@
# limitations under the License.
import logging
import math
import time
from functools import cached_property
from typing import Any
@@ -47,16 +46,16 @@ class SO101FollowerT(Robot):
_CURRENT_STEP_A: float = 6.5e-3 # 6.5 mA per register LSB #http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SMS-STS-emanual-229f4476422d4059abfb1cb0
_KT_NM_PER_AMP: float = 0.814 # Torque constant Kt [N·m/A] #https://www.feetechrc.com/811177.html
_MAX_CURRENT_A: float = 4.0 # Safe driver limit for this model
_MAX_CURRENT_A: float = 2.0 # Safe driver limit for this model
# Control gains for bilateral teleoperation
_KP_GAINS = { # Position gains [Nm/rad] - higher for proximal joints
"shoulder_pan": 7.0,
"shoulder_lift": 15.0,
"elbow_flex": 12.0,
"wrist_flex": 6.0,
"wrist_roll": 4.0,
"gripper": 2.0,
_KP_GAINS = { # Position gains [Nm/rad] - reduced for bilateral stability
"shoulder_pan": 5.0, # Reduced from 7.0
"shoulder_lift": 10.0, # Reduced from 15.0
"elbow_flex": 8.0, # Reduced from 12.0 (main problem joint)
"wrist_flex": 4.0, # Reduced from 6.0
"wrist_roll": 3.0, # Reduced from 4.0
"gripper": 1.5, # Reduced from 2.0
}
_KD_GAINS = { # Velocity gains [Nm⋅s/rad] - matched to position gains
@@ -190,8 +189,8 @@ class SO101FollowerT(Robot):
return counts
def _deg_to_rad(self, deg: dict[str, float | int]) -> dict[str, float]:
"""GDegrees to radians."""
return {m: math.radians(float(v)) for m, v in deg.items()}
"""Degrees to radians."""
return {m: np.deg2rad(float(v)) for m, v in deg.items()}
def _gravity_from_q(self, q_rad: dict[str, float]) -> dict[str, float]:
"""