66 lines
3.7 KiB
Python
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()
|