diff --git a/lerobot/common/motors/feetech/tables.py b/lerobot/common/motors/feetech/tables.py index 3b6f67e0a..d711c354a 100644 --- a/lerobot/common/motors/feetech/tables.py +++ b/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, diff --git a/lerobot/common/robots/so101_follower_torque/so101_follower_t.py b/lerobot/common/robots/so101_follower_torque/so101_follower_t.py index ef964a0b5..e9c5ff144 100644 --- a/lerobot/common/robots/so101_follower_torque/so101_follower_t.py +++ b/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 diff --git a/lerobot/common/robots/utils.py b/lerobot/common/robots/utils.py index ccc1c58e8..517718f68 100644 --- a/lerobot/common/robots/utils.py +++ b/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 diff --git a/lerobot/grav_comp.py b/lerobot/grav_comp.py index 6ab5b8104..bc70eb8b9 100644 --- a/lerobot/grav_comp.py +++ b/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: diff --git a/lerobot/setup_motors.py b/lerobot/setup_motors.py index 7909dc68d..e48dac1e2 100644 --- a/lerobot/setup_motors.py +++ b/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", ]