Files
a7l20-hug-sim/scripts/verify_saved_cup_offline.py
T

48 lines
2.9 KiB
Python

"""Recheck saved cup plan. Offline only: imports no robot or CAN clients."""
import json
import numpy as np
from offline_table_guard import ROOT, TableGuard
def main():
cap=ROOT/'data/left_cup_current'
plan=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
guard=TableGuard(n/np.linalg.norm(n),table['point'])
report={'status':'blocked','hardware_connected':False,'motion_sent':False,
'input':'saved cup plan, not a new camera observation',
'scope':'left arm meshes and conservative adapter/hand envelopes against saved infinite table plane',
'margin_m':guard.margin,'poses':{}}
for phase,key in [('pregrasp','pregrasp_joints'),('grasp','grasp_joints')]:
q=np.asarray(plan[key]);gap,body=guard.clearance(q)
# Evaluate the saved q with actual adapter translation added. The saved
# mount rotation is only a provisional orientation reference.
from scipy.spatial.transform import Rotation as R
T=np.eye(4)
for angle,(O,axis,v,lo,hi) in zip(q,guard.chain):
A=np.eye(4);A[:3,:3]=R.from_rotvec(axis*angle).as_matrix();T=T@O@A
hand=T[:3,:3]@guard.hand_center+T[:3,3]
target=np.asarray(plan['geometry']['pregrasp_position'] if phase=='pregrasp' else plan['grasp']['wrist_pos'])
report['poses'][phase]={'minimum_envelope_gap_m':float(gap),'limiting_body':body,
'corrected_hand_base_m':hand.tolist(),'planned_hand_base_m':target.tolist(),
'position_error_m':float(np.linalg.norm(hand-target))}
try:
report['segment']=guard.check_segment(plan['pregrasp_joints'],plan['grasp_joints'])
except (ValueError,RuntimeError) as e:report['segment_rejection']=str(e)
report['blockers']=[
'Saved IK targets predate adapter translation; regenerate IK and complete path.',
'Conservative hand sphere rejection is not proof of actual mesh penetration.',
'No fresh starting arm/hand state or fresh object observation; current-to-pregrasp not checked.',
'Actual controller interpolation and stopping distance not certified by offline linear interpolation.',
'Local SDK documents emergency stop only; retaining enable and canceling queued motion are undocumented.',
'Self collision and cup/other obstacles not covered by this table-only test.']
report['stop_evidence']={'sdk':'lbot_driver/lbot_api_cpp.h (SDK 1.0.5 header; external vendor source)',
'symbol':'lbot_emergency_stop','keeps_enable':'unknown',
'executor':'scripts/execute_left_cup.py exception handler calls arm.emergency(True)'}
path=cap/'offline_execution_verification.json'
path.write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2))
if __name__=='__main__':main()