From 0136912fa4728c70a3ca0dbf8f98ab6a1b6933fc Mon Sep 17 00:00:00 2001 From: Pepijn Date: Wed, 9 Jul 2025 19:21:44 +0200 Subject: [PATCH] Print more --- src/lerobot/bi_teleoperate.py | 69 ++++++++++++------- .../so101_follower_torque/so101_follower_t.py | 21 +++--- 2 files changed, 55 insertions(+), 35 deletions(-) diff --git a/src/lerobot/bi_teleoperate.py b/src/lerobot/bi_teleoperate.py index 913d88dcc..5d937804c 100644 --- a/src/lerobot/bi_teleoperate.py +++ b/src/lerobot/bi_teleoperate.py @@ -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 diff --git a/src/lerobot/robots/so101_follower_torque/so101_follower_t.py b/src/lerobot/robots/so101_follower_torque/so101_follower_t.py index 1f5e3a46f..82a5179e3 100644 --- a/src/lerobot/robots/so101_follower_torque/so101_follower_t.py +++ b/src/lerobot/robots/so101_follower_torque/so101_follower_t.py @@ -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]: """