Files
a7l20-hug-sim/scripts/read_arm_start_check.py

66 lines
3.7 KiB
Python

"""Only subscribe to arm states, disconnect, then check an offline path."""
import json
import time
from datetime import datetime,timezone
import numpy as np
import roslibpy
from offline_table_guard import ROOT,TableGuard
from search_cup_poses_offline import PoseGuard,hand_cloud
def main():
ros=roslibpy.Ros(host='192.168.10.21',port=9090)
topics=[];messages={'left':[],'right':[]}
try:
ros.run(timeout=8)
for side in messages:
topic=roslibpy.Topic(ros,f'/{side}/arm_state','lbot_arm_interfaces/msg/ArmState')
def receive(msg,side=side):
messages[side].append((time.monotonic(),dict(msg)))
messages[side][:]=messages[side][-10:]
topic.subscribe(receive);topics.append(topic)
deadline=time.monotonic()+8
while time.monotonic()<deadline:
if all(len(rows)>=5 for rows in messages.values()):break
time.sleep(.05)
now=time.monotonic()
if any(len(rows)<5 or now-rows[-1][0]>.5 for rows in messages.values()):
raise RuntimeError('No recent arm state messages')
states={side:rows[-1][1] for side,rows in messages.items()}
for state in states.values():
q=np.asarray(state['joint_state']['position'],float)
if q.shape!=(7,) or not np.isfinite(q).all():raise ValueError('Invalid joints')
finally:
for topic in topics:topic.unsubscribe()
ros.terminate()
cap=ROOT/'data/left_cup_current';stamp=datetime.now(timezone.utc).strftime('%Y%m%dT%H%M%S%fZ')
snapshot=cap/f'arm_snapshot_{stamp}.json'
snapshot.write_text(json.dumps({'states':states,'motion_sent':False,'received_at_utc':stamp},indent=2))
rows=json.loads((cap/'other_poses_offline_report.json').read_text())['candidates']
c=next(c for c in rows if c['raise_m']==.04 and c['yaw_relative_saved_deg']==0 and c['status']=='table_segment_pass')
saved=json.loads((cap/'cup_yaw_plan.json').read_text())['solutions'][0]
table=json.loads((cap/'camera.json').read_text())['table_estimate']
n=np.asarray(table['normal']);n=n if n[2]>0 else -n;n=n/np.linalg.norm(n)
guard=PoseGuard(n,table['point']);guard.mount=np.asarray(saved['mount_rotation'])
start=np.asarray(states['left']['joint_state']['position']);target=np.asarray(c['pregrasp_joints'])
T=guard.fk(start);model_tip=T[:3,:3]@np.array([-.02325,0,-.052])+T[:3,3]
measured=np.array([states['left']['pose']['position'][k] for k in 'xyz'])
error=float(np.linalg.norm(model_tip-measured))
report={'executable':False,'motion_sent':False,'snapshot':str(snapshot),'left_joints_rad':start.tolist(),
'model_vs_received_tip_error_m':error,'hand_state_read':False,'checks':{}}
if not np.isfinite(error) or error>.005:report['blocked']='Model/state FK mismatch'
else:
guard.cloud=hand_cloud(saved['grasp']['hand_pre'])
for name,checker in [('conditional_fixed_preshape',guard),('all_hand_postures_envelope',TableGuard(n,table['point']))]:
try:report['checks'][name]={'status':'pass',**checker.check_segment(start,target)}
except (RuntimeError,ValueError) as exc:report['checks'][name]={'status':'blocked','reason':str(exc)}
report['limitations']=['Preshape check assumes hand already in planned posture; hand not read.',
'No self/other-arm/cup collision or finger preshape transition check.',
'Saved mount assumption, old scene, actual controller interpolation remain unverified.',
'New received frames alone do not prove controller cache freshness. Motion gate stays locked.']
path=cap/f'start_path_check_{stamp}.json';path.write_text(json.dumps(report,indent=2))
print(json.dumps(report,indent=2));print(path)
if __name__=='__main__':main()