48 lines
2.9 KiB
Python
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()
|