103 lines
3.9 KiB
Python
103 lines
3.9 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
Live view + logger of the Inspire hand's 12 joints (rt/inspire/state).
|
|
|
|
conda activate g1_env
|
|
cd ~/Robotics_workspace/yslootahtech/G1/G1_Lootah/Manual_Recorder
|
|
python3 hand_monitor.py enp3s0 # live bars + auto CSV log
|
|
python3 hand_monitor.py enp3s0 --hz 30 # faster refresh/log
|
|
python3 hand_monitor.py enp3s0 --log run1.csv # choose log file
|
|
python3 hand_monitor.py enp3s0 --no-log # view only, no file
|
|
|
|
Needs inspire_g1 running on the robot (the Docker container).
|
|
q in [0,1]: 1.00 = open, 0.00 = closed. Ctrl+C to stop.
|
|
Idx 0-5 = right hand, 6-11 = left; per hand: pinky,ring,middle,index,thumb_bend,thumb_rot.
|
|
"""
|
|
import sys, time, argparse, csv
|
|
from datetime import datetime
|
|
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
|
from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorStates_
|
|
|
|
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"]
|
|
|
|
CLR = "\x1b[K" # clear to end of line (avoids redraw artifacts)
|
|
|
|
|
|
def bar(v, width=26):
|
|
v = 0.0 if v < 0 else (1.0 if v > 1 else v)
|
|
n = int(round(v * width))
|
|
return "█" * n + "·" * (width - n)
|
|
|
|
|
|
def main():
|
|
ap = argparse.ArgumentParser()
|
|
ap.add_argument("iface", nargs="?", default="enp3s0", help="network interface (default enp3s0)")
|
|
ap.add_argument("--hz", type=float, default=20.0, help="refresh + log rate (default 20)")
|
|
ap.add_argument("--log", default=None, help="CSV log path (default hand_log_<timestamp>.csv)")
|
|
ap.add_argument("--no-log", action="store_true", help="view only, do not write a log")
|
|
args = ap.parse_args()
|
|
|
|
ChannelFactoryInitialize(0, args.iface)
|
|
|
|
st = {"q": [0.0] * 12, "n": 0}
|
|
|
|
def on_state(msg):
|
|
try:
|
|
st["q"] = [float(msg.states[i].q) for i in range(12)]
|
|
st["n"] += 1
|
|
except Exception:
|
|
pass
|
|
|
|
sub = ChannelSubscriber("rt/inspire/state", MotorStates_)
|
|
sub.Init(on_state, 10)
|
|
|
|
writer = logf = log_path = None
|
|
if not args.no_log:
|
|
log_path = args.log or f"hand_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
|
|
logf = open(log_path, "w", newline="")
|
|
writer = csv.writer(logf)
|
|
writer.writerow(["t_epoch", "iso"] + NAMES)
|
|
|
|
dt = 1.0 / args.hz
|
|
rows = 0
|
|
print("\x1b[2J", end="") # clear once
|
|
try:
|
|
while True:
|
|
now = time.time()
|
|
q = st["q"]
|
|
live_r = any(abs(x) > 1e-3 for x in q[:6])
|
|
live_l = any(abs(x) > 1e-3 for x in q[6:])
|
|
|
|
lines = ["\x1b[H"] # cursor home; redraw in place
|
|
lines.append(f"Inspire hand — live joints iface={args.iface} msgs={st['n']:>6d} "
|
|
f"{datetime.now().strftime('%H:%M:%S')} (Ctrl+C to stop){CLR}")
|
|
lines.append(f"q: 1.00 open ░ 0.00 closed" + (f" log: {log_path}" if log_path else " (no log)") + CLR)
|
|
lines.append(CLR)
|
|
for i, nm in enumerate(NAMES):
|
|
if i == 6:
|
|
lines.append(CLR) # blank line between hands
|
|
lines.append(f" {nm:9s} [{bar(q[i])}] {q[i]:5.2f}{CLR}")
|
|
lines.append(CLR)
|
|
lines.append(f" RIGHT: {'● live' if live_r else '○ FLAT 0 (adapter dropped?)'} "
|
|
f"LEFT: {'● live' if live_l else '○ FLAT 0 (adapter dropped?)'}{CLR}")
|
|
if log_path:
|
|
lines.append(f" logged rows: {rows}{CLR}")
|
|
print("\n".join(lines), end="", flush=True)
|
|
|
|
if writer:
|
|
writer.writerow([f"{now:.3f}", datetime.now().isoformat()] + [f"{x:.4f}" for x in q])
|
|
rows += 1
|
|
|
|
time.sleep(dt)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
if logf:
|
|
logf.close()
|
|
print(f"\n\nstopped." + (f" Saved {rows} rows to {log_path}" if log_path else ""))
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|