95 lines
4.0 KiB
Python
95 lines
4.0 KiB
Python
#!/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.")
|