From 05a2316a63d2565deceee739d8b3966edeecc7ad Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Wed, 16 Jul 2025 14:26:29 +0200
Subject: [PATCH] modify gains

---
 src/lerobot/bi_teleoperate.py                 | 15 +---------
 .../so101_follower_torque/so101_follower_t.py | 30 +++++++++----------
 src/lerobot/teleoperators/utils.py            |  6 ++++
 3 files changed, 22 insertions(+), 29 deletions(-)

diff --git a/src/lerobot/bi_teleoperate.py b/src/lerobot/bi_teleoperate.py
index c76dbce58..377521fba 100644
--- a/src/lerobot/bi_teleoperate.py
+++ b/src/lerobot/bi_teleoperate.py
@@ -7,7 +7,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 = 200
+FRQ = 100
 PRINT_HZ = 10
 RERUN_HZ = 100
 ESC_CLR_EOL = "\x1b[K"
@@ -37,10 +37,6 @@ first_print = True
 loop_count = 0
 tic_prev = time.perf_counter()
 
-# Initialize previous positions for velocity calculation
-pos_f_prev = None
-pos_l_prev = None
-
 while True:
     tic = time.perf_counter()
 
@@ -55,10 +51,6 @@ while True:
     tau_cmd_f, tau_cmd_l = [], []
     debug_info_f, debug_info_l = {}, {}
 
-    # Collect torques with deterministic components removed for model
-    tau_interaction_l = {}
-    tau_interaction_f = {}
-
     # 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}
@@ -70,10 +62,6 @@ while True:
     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}
 
-    # Update previous positions for next iteration
-    pos_f_prev = pos_f.copy()
-    pos_l_prev = pos_l.copy()
-
     # Compute reaction torques using model-based approach
     tau_reaction_f = follower._compute_model_based_disturbance(pos_f, vel_f, acc_f, tau_meas_f)
     tau_reaction_l = leader._compute_model_based_disturbance(pos_l, vel_l, acc_l, tau_meas_l)
@@ -122,7 +110,6 @@ while True:
     follower.send_action({f"{m}.effort": tau_cmd_f[i] for i, m in enumerate(follower.bus.motors)})
     leader.send_action({f"{m}.effort": tau_cmd_l[i] for i, m in enumerate(leader.bus.motors)})
 
-    # Create structured observation and action for rerun
     # Observation: follower side only (θ_f, ω_f, τ_ext)
     observation = {
         "follower_joint_angles": pos_f,  # θ_f: current angles
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 469282e6c..1eb3a85ec 100644
--- a/src/lerobot/robots/so101_follower_torque/so101_follower_t.py
+++ b/src/lerobot/robots/so101_follower_torque/so101_follower_t.py
@@ -48,26 +48,26 @@ 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
+    _MAX_CURRENT_A: float = 2.0  # Safe driver limit
 
     # Position gains [Nm/rad]
     _KP_GAINS = {
-        "shoulder_pan": 6.0,
+        "shoulder_pan": 5.0,
         "shoulder_lift": 7.0,
         "elbow_flex": 7.0,
         "wrist_flex": 5.0,
-        "wrist_roll": 4.0,
-        "gripper": 4.0,
+        "wrist_roll": 5.0,
+        "gripper": 5.0,
     }
 
     # Velocity gains [Nm⋅s/rad]
     _KD_GAINS = {
         "shoulder_pan": 0.4,
-        "shoulder_lift": 0.8,
-        "elbow_flex": 0.7,
+        "shoulder_lift": 0.6,
+        "elbow_flex": 0.6,
         "wrist_flex": 0.4,
-        "wrist_roll": 0.3,
-        "gripper": 0.3,
+        "wrist_roll": 0.4,
+        "gripper": 0.4,
     }
 
     # Force gains
@@ -82,22 +82,22 @@ class SO101FollowerT(Robot):
 
     # Viscous friction coefficient [Nm⋅s/rad] per joint
     _FRICTION_VISCOUS = {
-        "shoulder_pan": 0.02,
+        "shoulder_pan": 0.05,
         "shoulder_lift": 0.08,
         "elbow_flex": 0.05,
-        "wrist_flex": 0.02,
-        "wrist_roll": 0.015,
-        "gripper": 0.01,
+        "wrist_flex": 0.05,
+        "wrist_roll": 0.05,
+        "gripper": 0.05,
     }
 
     # Coulomb/static friction [Nm] per joint
     _FRICTION_COULOMB = {
         "shoulder_pan": 0.15,
         "shoulder_lift": 0.25,
-        "elbow_flex": 0.20,
+        "elbow_flex": 0.25,
         "wrist_flex": 0.20,
-        "wrist_roll": 0.15,
-        "gripper": 0.15,
+        "wrist_roll": 0.20,
+        "gripper": 0.20,
     }
 
     def __init__(self, config: SO101FollowerTConfig):
diff --git a/src/lerobot/teleoperators/utils.py b/src/lerobot/teleoperators/utils.py
index 8a667fd41..2db08ecb1 100644
--- a/src/lerobot/teleoperators/utils.py
+++ b/src/lerobot/teleoperators/utils.py
@@ -33,6 +33,12 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> Teleoperator:
         from .so101_leader import SO101Leader
 
         return SO101Leader(config)
+    elif config.type == "so101_follower_t":
+        # For bilateral teleoperation, SO101FollowerT is used as a robot, not a teleoperator
+        # This should be handled in the record.py file instead
+        raise ValueError(
+            "so101_follower_t should be created as a robot instance for bilateral teleoperation, not as a teleoperator"
+        )
     elif config.type == "stretch3":
         from .stretch3_gamepad import Stretch3GamePad