#!/usr/bin/env python3
"""
Does the RH56 become COMPLIANT at a minimum force threshold?

The claim: dropping FORCE_SET to its lowest value puts the hand in a "transparent / zero-G"
state where its internal hybrid force-position loop retreats from your hand, letting you
shape the fingers manually. Inspire's own host software is said to expose this.

This has NOT been tested here. Earlier measurements used force limits of 400-500g and found
a push produced only +-3g and 0.000 rotation -- but that is a different regime. At a near-zero
threshold the hand should stop driving the instant it feels anything, which is exactly the
condition the claim depends on.

The test sweeps FORCE_SET from high to minimum via the cmd kp relay (inspire_g1 applies it
live, no restart) and reports, at each level, whether pushing actually moved the joint.

    conda activate g1_env
    cd ~/Robotics_workspace/yslootahtech/G1/G1_Lootah/Manual_Recorder
    python3 hand_compliance_test.py enp3s0 R.index

PUSH THE NAMED FINGER steadily through the whole run. Each level lasts 8s and prints whether
that level yielded.
"""
import sys, time
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize
from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_
from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_

NAMES = ["R.pinky", "R.ring", "R.mid", "R.index", "R.thumbB", "R.thumbR",
         "L.pinky", "L.ring", "L.mid", "L.index", "L.thumbB", "L.thumbR"]
IFACE = sys.argv[1] if len(sys.argv) > 1 else "enp3s0"
TARGET = sys.argv[2] if len(sys.argv) > 2 else "R.index"
idx = [i for i, n in enumerate(NAMES) if n.lower() == TARGET.lower()]
if not idx:
    sys.exit(f"unknown finger {TARGET!r}. One of: {', '.join(NAMES)}")
IDX = idx[0]
HAND = 0 if IDX < 6 else 6          # which hand's kp field carries the force limit

ChannelFactoryInitialize(0, IFACE)
st = {"q": [1.0] * 12, "f": [0.0] * 12, "n": 0}
def on_state(m):
    st["q"] = [float(m.states[i].q) for i in range(12)]
    st["f"] = [float(m.states[i].tau_est) for i in range(12)]
    st["n"] += 1
ChannelSubscriber("rt/inspire/state", MotorStates_).Init(on_state, 10)
pub = ChannelPublisher("rt/inspire/cmd", MotorCmds_); pub.Init()
msg = MotorCmds_(); msg.cmds = [unitree_go_msg_dds__MotorCmd_() for _ in range(12)]

REST = [1.0] * 12; REST[5] = 0.0; REST[11] = 0.0

def send(force):
    for i in range(12):
        msg.cmds[i].q = REST[i]
        # inspire_g1 reads cmds[0].kp for the right hand and cmds[6].kp for the left
        msg.cmds[i].kp = float(force) if (i // 6) * 6 == HAND else 500.0
    pub.Write(msg)

time.sleep(1.0)
if st["n"] == 0:
    sys.exit("no hand state - is inspire_g1 running?")

print(f"\ntesting {NAMES[IDX]} — PUSH IT STEADILY for the whole run (~50s)\n")
print(f"{'FORCE_SET':>10s} {'q start':>8s} {'q end':>7s} {'moved':>7s} {'force seen':>11s}   verdict")
print("-" * 68)

results = []
for lim in (500, 300, 150, 80, 30, 10, 1):
    send(lim); time.sleep(2.0)              # settle at the new limit
    q0 = st["q"][IDX]
    qs, fs = [], []
    t0 = time.time()
    while time.time() - t0 < 6.0:
        send(lim)
        qs.append(st["q"][IDX]); fs.append(st["f"][IDX])
        time.sleep(0.03)
    moved = max(qs) - min(qs)
    frng = max(fs) - min(fs)
    yielded = moved > 0.05
    results.append((lim, moved, yielded))
    print(f"{lim:9d}g {q0:8.3f} {st['q'][IDX]:7.3f} {moved:7.3f} {frng:10.0f}g   "
          + ("YIELDED — compliant!" if yielded else "rigid"))

send(500); time.sleep(1.0)
print("-" * 68)
ok = [l for l, m, y in results if y]
print()
if ok:
    print(f"COMPLIANCE WORKS at FORCE_SET <= {max(ok)}g.")
    print("  -> manual finger shaping IS possible; record angles at that limit,")
    print("     then raise the limit for replay so it grips properly.")
else:
    print("NO compliance at any threshold down to 1g.")
    print("  -> the lead screw self-locks regardless of the force setting, so the")
    print("     hand genuinely cannot be shaped by hand. The claim does not hold here.")