fix gravity compensation

lerobot/common/motors/feetech/tables.py

@@ -275,7 +275,7 @@ MODEL_ENCODING_TABLE = {
     "sm8512bl": STS_SMS_SERIES_ENCODINGS_TABLE,
     "scs0009": {},
     "hls3625": {
-        "Homing_Offset": 11,
+        "Homing_Offset": 15,
         "Goal_Velocity": 15,
         "Present_Velocity": 15,
         "Target_Torque": 10,

lerobot/common/robots/so101_follower_torque/so101_follower_t.py

@@ -40,7 +40,7 @@ class SO101FollowerT(Robot):
     """
 
     config_class = SO101FollowerTConfig
-    name = "so101_follower"
+    name = "so101_follower_t"
 
     _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

lerobot/common/robots/utils.py

@@ -37,6 +37,10 @@ def make_robot_from_config(config: RobotConfig) -> Robot:
         from .so101_follower import SO101Follower
 
         return SO101Follower(config)
+    elif config.type == "so101_follower_t":
+        from .so101_follower_torque import SO101FollowerT
+
+        return SO101FollowerT(config)
     elif config.type == "lekiwi":
         from .lekiwi import LeKiwi
 

lerobot/grav_comp.py

@@ -1,3 +1,4 @@
+import sys
 import time
 
 import numpy as np
@@ -15,6 +16,20 @@ MOTORS = [
     "gripper",
 ]
 
+FLIP_TORQUE = np.array(
+    [
+        True,  # shoulder_pan
+        True,  # shoulder_lift   t
+        True,  # elbow_flex  t
+        True,  # wrist_flex  t
+        True,  # wrist_roll
+        False,  # gripper
+    ],
+    dtype=bool,
+)
+
+SIGN = np.where(FLIP_TORQUE, -1.0, 1.0)
+
 IDX = {name: i for i, name in enumerate(MOTORS)}
 
 
@@ -28,6 +43,7 @@ def obs_to_q(obs):
 
 def tau_to_action(tau):
     """Convert τ array (Nm) to action dict understood by send_action()."""
+    tau = SIGN * tau
     return {f"{m}.effort": float(tau[IDX[m]]) for m in MOTORS}
 
 
@@ -40,10 +56,12 @@ pin_robot.initViewer(open=True)
 pin_robot.loadViewerModel()
 pin_robot.display(pin_robot.q0)
 
-cfg = SO101FollowerTConfig(port="/dev/tty.usbmodem58760431551", id="follower_t")
+cfg = SO101FollowerTConfig(port="/dev/tty.usbmodem58760431551", id="follower_torque6")
 real = SO101FollowerT(cfg)
 real.connect()
 
+ESC_CLR_EOL = "\x1b[K"
+
 print("Running gravity compensation")
 try:
     while True:
@@ -52,10 +70,20 @@ try:
 
         tau = pin.computeGeneralizedGravity(pin_robot.model, pin_robot.data, q)  # τ in [Nm]
 
-        # real.send_action(tau_to_action(tau)) # apply τ
+        # build compact single-line status
+        status = " | ".join(
+            f"{m}: {np.degrees(q[i]):+5.1f}° Δτ {(abs(obs[f'{m}.effort']) - abs(tau[i])):+5.1f}"
+            for i, m in enumerate(MOTORS)
+        )
+
+        # overwrite previous status
+        sys.stdout.write("\r" + ESC_CLR_EOL + status)
+        sys.stdout.flush()
+
+        real.send_action(tau_to_action(tau))  # apply τ
 
         pin_robot.display(q)
-        time.sleep(0.02)  # Run at 50 Hz
+        time.sleep(0.002)  # Run at 500 Hz
 except KeyboardInterrupt:
     print("Stopping")
 finally:

lerobot/setup_motors.py

@@ -35,6 +35,7 @@ from .common.robots import (  # noqa: F401
     make_robot_from_config,
     so100_follower,
     so101_follower,
+    so101_follower_torque,
 )
 from .common.teleoperators import (  # noqa: F401
     TeleoperatorConfig,
@@ -52,6 +53,7 @@ COMPATIBLE_DEVICES = [
     "so101_follower",
     "so101_leader",
     "lekiwi",
+    "so101_follower_t",
 ]