Tune everything a bit

src/lerobot/bi_teleoperate.py

@@ -1,7 +1,6 @@
 import math
 import sys
 import time
-from concurrent.futures import ThreadPoolExecutor
 
 import numpy as np
 
@@ -10,7 +9,7 @@ from lerobot.robots.so101_follower_torque.so101_follower_t import SO101FollowerT
 from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.visualization_utils import _init_rerun, log_rerun_data
 
-FRQ = 300
+FRQ = 100
 PRINT_HZ = 10
 RERUN_HZ = 100
 ESC_CLR_EOL = "\x1b[K"
@@ -42,12 +41,10 @@ leader = SO101FollowerT(leader_cfg)
 follower.connect()
 leader.connect()
 
-exec_f = ThreadPoolExecutor(max_workers=1)
-exec_l = ThreadPoolExecutor(max_workers=1)
-
 # Initialize Rerun for visualization
 _init_rerun("bilateral_teleoperation")
 
+
 print("Starting 4-channel bilateral teleoperation")
 first_print = True
 loop_count = 0
@@ -60,9 +57,7 @@ pos_l_prev = None
 while True:
     tic = time.perf_counter()
 
-    fut_l = exec_l.submit(leader.get_observation)
-    fut_f = exec_f.submit(follower.get_observation)
-    obs_l, obs_f = fut_l.result(), fut_f.result()
+    obs_l, obs_f = leader.get_observation(), follower.get_observation()
 
     dt = tic - tic_prev
     tic_prev = tic
@@ -111,30 +106,47 @@ while True:
     inertia_f = follower._inertia_from_q_dq(pos_f, vel_f, acc_f)
     inertia_l = leader._inertia_from_q_dq(pos_l, vel_l, acc_l)
 
-    Kp = 8.0  # Position gain
-    Kd = 0.5  # Velocity gain
-    Kf = 0.05  # Force reflection gain
+    # Joint-specific control gains for better tracking
+    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,
+    }
+
+    Kd_gains = {  # Velocity gains [Nm⋅s/rad] - matched to position gains
+        "shoulder_pan": 0.4,
+        "shoulder_lift": 0.7,
+        "elbow_flex": 0.6,
+        "wrist_flex": 0.4,
+        "wrist_roll": 0.3,
+        "gripper": 0.2,
+    }
+
+    Kf = 0.05  # Force reflection gain (global)
 
-    # Friction compensation parameters (feedforward)
     use_friction_comp = True  # Enable/disable friction compensation
+    use_gravity_comp = True  # Set to True to enable gravity compensation
 
     # Friction model: τ_friction = b_viscous * ω + fc_coulomb * sign(ω)
     friction_viscous = {  # Viscous friction coefficient [Nm⋅s/rad] per joint
         "shoulder_pan": 0.02,
-        "shoulder_lift": 0.05,
-        "elbow_flex": 0.03,
-        "wrist_flex": 0.01,
-        "wrist_roll": 0.01,
-        "gripper": 0.005,
+        "shoulder_lift": 0.08,
+        "elbow_flex": 0.05,
+        "wrist_flex": 0.02,
+        "wrist_roll": 0.015,
+        "gripper": 0.01,
     }
 
-    friction_coulomb = {  # Coulomb friction [Nm] per joint
-        "shoulder_pan": 0.08,
-        "shoulder_lift": 0.15,
-        "elbow_flex": 0.10,
-        "wrist_flex": 0.05,
-        "wrist_roll": 0.03,
-        "gripper": 0.02,
+    friction_coulomb = {  # Coulomb friction [Nm] per joint - increased static friction
+        "shoulder_pan": 0.05,
+        "shoulder_lift": 0.30,
+        "elbow_flex": 0.20,
+        "wrist_flex": 0.10,
+        "wrist_roll": 0.06,
+        "gripper": 0.04,
     }
 
     for j in follower.bus.motors:
@@ -160,25 +172,35 @@ while True:
             tau_friction_f = 0.0
             tau_friction_l = 0.0
 
-        # Follower control with friction compensation
-        tau_pos_f = Kp * (theta_cmd_f - pos_f[j])
-        tau_vel_f = Kd * (omega_cmd_f - vel_f[j])
+        # Follower control with joint-specific gains and friction compensation
+        tau_pos_f = Kp_gains[j] * (theta_cmd_f - pos_f[j])
+        tau_vel_f = Kd_gains[j] * (omega_cmd_f - vel_f[j])
         tau_force_f = Kf * (tau_cmd_f_ref - tau_reaction_f[j])
-        tau_ref_f = tau_pos_f + tau_vel_f + tau_force_f + grav_f[j] + inertia_f[j] + tau_friction_f
 
-        # Leader control with friction compensation
-        tau_pos_l = Kp * (theta_cmd_l - pos_l[j])
-        tau_vel_l = Kd * (omega_cmd_l - vel_l[j])
+        # Apply gravity compensation conditionally
+        tau_gravity_f = grav_f[j] if use_gravity_comp else 0.0
+        tau_inertia_f = inertia_f[j] if use_gravity_comp else 0.0
+
+        tau_ref_f = tau_pos_f + tau_vel_f + tau_force_f + tau_gravity_f + tau_inertia_f + tau_friction_f
+
+        # Leader control with joint-specific gains and friction compensation
+        tau_pos_l = Kp_gains[j] * (theta_cmd_l - pos_l[j])
+        tau_vel_l = Kd_gains[j] * (omega_cmd_l - vel_l[j])
         tau_force_l = Kf * (tau_cmd_l_ref - tau_reaction_l[j])
-        tau_ref_l = tau_pos_l + tau_vel_l + tau_force_l + grav_l[j] + inertia_l[j] + tau_friction_l
+
+        # Apply gravity compensation conditionally
+        tau_gravity_l = grav_l[j] if use_gravity_comp else 0.0
+        tau_inertia_l = inertia_l[j] if use_gravity_comp else 0.0
+
+        tau_ref_l = tau_pos_l + tau_vel_l + tau_force_l + tau_gravity_l + tau_inertia_l + tau_friction_l
 
         tau_cmd_f.append(tau_ref_f)
         tau_cmd_l.append(tau_ref_l)
 
         # Store interaction torques (cmd minus deterministic components) for model
-        tau_interaction_l[j] = tau_ref_l - grav_l[j] - inertia_l[j] - tau_friction_l
+        tau_interaction_l[j] = tau_ref_l - tau_gravity_l - tau_inertia_l - tau_friction_l
         tau_interaction_f[j] = -(
-            tau_meas_f[j] - grav_f[j] - inertia_f[j] - tau_friction_f
+            tau_meas_f[j] - tau_gravity_f - tau_inertia_f - tau_friction_f
         )  # Invert to match leader
 
         # Store debug info
@@ -187,8 +209,8 @@ while True:
             "τ_pos": tau_pos_f,
             "τ_vel": tau_vel_f,
             "τ_force": tau_force_f,
-            "τ_gravity": grav_f[j],
-            "τ_inertia": inertia_f[j],
+            "τ_gravity": tau_gravity_f,  # Use conditional gravity value
+            "τ_inertia": tau_inertia_f,  # Use conditional inertia value
             "τ_friction": tau_friction_f,  # Add friction to debug info
             "τ_reaction": tau_reaction_f[j],
             "τ_ref": tau_ref_f,
@@ -201,8 +223,8 @@ while True:
             "τ_pos": tau_pos_l,
             "τ_vel": tau_vel_l,
             "τ_force": tau_force_l,
-            "τ_gravity": grav_l[j],
-            "τ_inertia": inertia_l[j],
+            "τ_gravity": tau_gravity_l,  # Use conditional gravity value
+            "τ_inertia": tau_inertia_l,  # Use conditional inertia value
             "τ_friction": tau_friction_l,  # Add friction to debug info
             "τ_reaction": tau_reaction_l[j],
             "τ_ref": tau_ref_l,
@@ -283,52 +305,23 @@ while True:
         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("Model-Based Analysis:")
-        lines.append(
-            f"{'Joint':<13} {'Gravity':<8} {'Inertia':<8} {'Friction':<8} {'Reaction':<8} {'Errors':<12} {'Balance':<8}"
-        )
-        lines.append("             (feed-fwd) (feed-fwd) (feed-fwd) (external) (pos/vel)   (τ_ref)")
-        for j in follower.bus.motors:
-            debug_f = debug_info_f[j]
-            g_dir = "↑" if debug_f["τ_gravity"] > 0 else "↓" if debug_f["τ_gravity"] < 0 else "≈0"
-            i_dir = "+" if debug_f["τ_inertia"] > 0.05 else "-" if debug_f["τ_inertia"] < -0.05 else "≈0"
-            f_dir = "+" if debug_f["τ_friction"] > 0.02 else "-" if debug_f["τ_friction"] < -0.02 else "≈0"
-            r_dir = "+" if debug_f["τ_reaction"] > 0.05 else "-" if debug_f["τ_reaction"] < -0.05 else "≈0"
-
-            pos_err_sign = "+" if debug_f["θ_err"] > 0 else "-" if debug_f["θ_err"] < 0 else "0"
-            vel_err_sign = "+" if debug_f["ω_err"] > 0 else "-" if debug_f["ω_err"] < 0 else "0"
-
-            # Check balance: τ_ref should equal sum of components
-            expected_sum = (
-                debug_f["τ_pos"]
-                + debug_f["τ_vel"]
-                + debug_f["τ_force"]
-                + debug_f["τ_gravity"]
-                + debug_f["τ_inertia"]
-                + debug_f["τ_friction"]
-            )
-            balance_check = "✓" if abs(expected_sum - debug_f["τ_ref"]) < 0.01 else "✗"
-
-            lines.append(
-                f"{j:<13s}"
-                f"{g_dir:<8s}"
-                f"{i_dir:<8s}"
-                f"{f_dir:<8s}"
-                f"{r_dir:<8s}"
-                f"θ:{pos_err_sign}ω:{vel_err_sign}    "
-                f"{balance_check}"
-            )
-
         lines.append("-" * 100)
         lines.append("Cmd = Pos + Vel + Force + Grav + Inert + Frict")
         lines.append("React = Meas - Grav - Inert  (external forces)")
         lines.append("Force = Kf × (reflect_other_robot - React)  (telepresence)")
         lines.append("Frict = b_visc×ω + f_coulomb×sign(ω)  (transparency)")
-        lines.append(f"Control Gains: Kp={Kp} (position) | Kd={Kd} (velocity) | Kf={Kf} (force reflection)")
+        lines.append(
+            f"Control Gains: Kp=joint-specific (position) | Kd=joint-specific (velocity) | Kf={Kf} (force reflection)"
+        )
+        lines.append(
+            f"Joint Gains: shoulder_pan Kp={Kp_gains['shoulder_pan']:.1f} | shoulder_lift Kp={Kp_gains['shoulder_lift']:.1f} | elbow_flex Kp={Kp_gains['elbow_flex']:.1f}"
+        )
         lines.append(
             f"Friction Comp: {'ON' if use_friction_comp else 'OFF'} | Viscous: {friction_viscous['shoulder_pan']:.3f} | Coulomb: {friction_coulomb['shoulder_pan']:.3f} (shoulder example)"
         )
+        lines.append(
+            f"Gravity Comp: {'ON' if use_gravity_comp else 'OFF'} | Inertia Comp: {'ON' if use_gravity_comp else 'OFF'} | (Both disabled for testing)"
+        )
 
         block = "\n".join(lines)
         if first_print:

src/lerobot/motors/feetech/tables.py

@@ -176,6 +176,7 @@ HLS_SERIES_CONTROL_TABLE = {
     "Minimum_Startup_Force": (24, 2),
     "CW_Dead_Zone": (26, 1),
     "CCW_Dead_Zone": (27, 1),
+    "Protection_Current": (28, 2),
     "Homing_Offset": (31, 2),
     "Operating_Mode": (33, 1),
     "P_Coefficient_Curr": (34, 1),
@@ -190,6 +191,7 @@ HLS_SERIES_CONTROL_TABLE = {
     "P_Coefficient_Ring": (50, 1),
     "D_Coefficient_Ring": (51, 1),
     "I_Coefficient_Ring": (52, 1),
+    "km": (53, 1),
     "Lock": (55, 1),
     # SRAM feedback
     "Present_Position": (56, 2),

src/lerobot/robots/so101_follower_torque/so101_follower_t.py

@@ -182,8 +182,6 @@ class SO101FollowerT(Robot):
         """
         Compute disturbance torques using direct model-based approach:
         τ_disturbance = τ_measured - τ_gravity - τ_inertia
-
-        This replaces the DOB and gives cleaner, delay-free disturbance estimation.
         """
         # Get gravity compensation
         tau_gravity = self._gravity_from_q(q_rad)
@@ -265,6 +263,22 @@ class SO101FollowerT(Robot):
                 self.bus.write("Torque_Limit", motor, 1000, num_retry=2)  # 100%
                 self.bus.write("Return_Delay_Time", motor, 0, num_retry=2)
 
+                # Disable interfering protection systems for better torque response
+                # Read current unloading condition and disable current/overload protection
+                current_unload = int(self.bus.read("Unloading_Condition", motor, normalize=False))
+
+                # Disable multiple protection systems that can interfere with torque control:
+                # Bit 32 (0x20): Load overload protection - causes torque limiting
+                # Bit 16 (0x10): Current protection - causes current limiting during high torque
+                # Bit 8 (0x08): Speed protection - can interfere with velocity control
+                safe_unload = current_unload & ~(32 | 16 | 8)  # Remove bits 32, 16, 8
+
+                self.bus.write("Unloading_Condition", motor, safe_unload, num_retry=2)
+                print(f"Motor {motor}: Unloading condition 0x{current_unload:02X} → 0x{safe_unload:02X}")
+                print(
+                    "  Disabled: Load overload (bit 32), Current protection (bit 16), Speed protection (bit 8)"
+                )
+
     def setup_motors(self) -> None:
         for motor in reversed(self.bus.motors):
             input(f"Connect the controller board to the '{motor}' motor only and press enter.")
@@ -281,15 +295,15 @@ class SO101FollowerT(Robot):
         pos_deg = self.bus.sync_read("Present_Position", num_retry=5)
         pos_rad = self._deg_to_rad(pos_deg)
 
-        # velocity (read directly from motor - more accurate than numerical differentiation)
-        # Present_Velocity is in units of 0.732 RPM (revolutions per minute)
-        vel_rpm_units = self.bus.sync_read("Present_Velocity", num_retry=5)
-        # Convert: raw_units → RPM → rad/s
-        # rpm = raw_value * 0.732
-        # rad/s = rpm * (2π / 60)
-        vel_rad = {m: v * 0.732 * (2 * math.pi / 60) for m, v in vel_rpm_units.items()}
+        # velocity - calculate from position differences (HLS3625 motors in torque mode don't respond to Present_Velocity sync_read)
+        if self._prev_pos_rad is None or self._prev_t is None:
+            vel_rad = dict.fromkeys(pos_rad, 0.0)
+        else:
+            dt = t_now - self._prev_t
+            dt = max(dt, 1e-4)  # Avoid division by zero
+            vel_rad = {m: (pos_rad[m] - self._prev_pos_rad[m]) / dt for m in pos_rad}
 
-        # acceleration - calculate from motor velocity (motor's Acceleration register is settings only)
+        # acceleration - calculate from velocity differences
         if self._prev_vel_rad is None or self._prev_t is None:
             acc_rad = dict.fromkeys(pos_rad, 0.0)
         else:

src/lerobot/utils/visualization_utils.py

@@ -28,19 +28,35 @@ def _init_rerun(session_name: str = "lerobot_control_loop") -> None:
     rr.spawn(memory_limit=memory_limit)
 
 
-def log_rerun_data(observation: dict[str | Any], action: dict[str | Any]):
+def log_rerun_data(observation: dict[str, Any], action: dict[str, Any]):
     for obs, val in observation.items():
         if isinstance(val, float):
             rr.log(f"observation.{obs}", rr.Scalar(val))
+        elif isinstance(val, dict):
+            # Handle dictionary of joint values
+            for joint_name, joint_val in val.items():
+                if isinstance(joint_val, (float, int)):
+                    rr.log(f"observation.{obs}.{joint_name}", rr.Scalar(float(joint_val)))
         elif isinstance(val, np.ndarray):
             if val.ndim == 1:
                 for i, v in enumerate(val):
                     rr.log(f"observation.{obs}_{i}", rr.Scalar(float(v)))
             else:
                 rr.log(f"observation.{obs}", rr.Image(val), static=True)
-        for act, val in action.items():
-            if isinstance(val, float):
-                rr.log(f"action.{act}", rr.Scalar(val))
-            elif isinstance(val, np.ndarray):
-                for i, v in enumerate(val):
+
+    for act, val in action.items():
+        if isinstance(val, float):
+            rr.log(f"action.{act}", rr.Scalar(val))
+        elif isinstance(val, dict):
+            # Handle dictionary of joint values
+            for joint_name, joint_val in val.items():
+                if isinstance(joint_val, (float, int)):
+                    rr.log(f"action.{act}.{joint_name}", rr.Scalar(float(joint_val)))
+        elif isinstance(val, np.ndarray):
+            for i, v in enumerate(val):
+                rr.log(f"action.{act}_{i}", rr.Scalar(float(v)))
+        elif isinstance(val, list):
+            # Handle list of values
+            for i, v in enumerate(val):
+                if isinstance(v, (float, int)):
                     rr.log(f"action.{act}_{i}", rr.Scalar(float(v)))