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

89 lines
5.8 KiB
Python

"""Offline rerun using user 20mm slab /105mm black-cell-edge measurement.
Nominal and provisional uncertainty scenarios; not physical authorization.
"""
import json
import tempfile
from pathlib import Path
import cv2
import numpy as np
from offline_table_guard import ROOT
from finite_table_constraint import NearEdgeSlab,FiniteContactEscapeChecker
from audit_finite_table_path import JsonHandGeometry
from table_edge_constraint import check_joint_linear_segment
from left_index_pointing_pose import pointing_target
from g20_left_lookup import LeftLookup
def main():
cap=ROOT/'data/start_evidence_20260927T053837893105Z'
measured=json.loads((cap/'index_hover_validation.json').read_text())
camera=json.loads((cap/'camera.json').read_text());T=np.array(camera['T_world_cam_opencv']);K=np.array(camera['K'])
board=np.array(json.loads((ROOT/'data/real_calib_20260927_9jkoqN/camera.json').read_text())['T_base_board'])
n=np.array(measured['table_normal']);p=np.array(measured['table_point_m']);uv=np.array(measured['edge_pixels'])
rays=np.c_[(uv[:,0]-K[0,2])/K[0,0],(uv[:,1]-K[1,2])/K[1,1],np.ones(2)]@T[:3,:3].T
ep=T[:3,3]+rays*(((p-T[:3,3])@n)/(rays@n))[:,None]
outward=np.cross(n,ep[1]-ep[0]);outward/=np.linalg.norm(outward)
if (board[:3,3]-ep.mean(axis=0))@outward>0:outward=-outward
# 8x5 inner corners ->9x6 cells. Bottom row leftmost black is column1.
# Board origin is the first inner corner; square length27mm.
square=.027;cell_center=np.array([square/2,4.5*square,0.])
border=np.array([square/2,5*square,0.])
anchor=board[:3,:3]@border+board[:3,3]
anchor=anchor-n*((anchor-p)@n)
edge=anchor+.105*outward
raw=json.loads((ROOT/'data/left_hand_open_q10r1mak/result.json').read_text())['final_feedback']
opened=JsonHandGeometry(raw);closed=JsonHandGeometry(pointing_target(raw,LeftLookup()))
elbow=json.loads((cap/'elbow_raise_proposal.json').read_text())
stages=json.loads((ROOT/'data/fresh_start_path_cdh8ypbu/lift_rotate_point_validation.json').read_text())
def path_audit(path,geometry,table):
path=np.array(path,float);checker=FiniteContactEscapeChecker(geometry,table);initial=checker.bind(path[0])
phase='escape' if initial['status']=='escape_required' else 'normal';rows=[]
for a,b in zip(path,path[1:]):
row={'phase':phase}
try:
if phase=='escape':
row['check']=checker.check_segment(a,b)
if min(checker.current_gaps(b).values())>=.02:phase='normal'
else:row['check']=check_joint_linear_segment(a,b,checker.normal_clearance,reach_bound_m=geometry.reach,margin_m=.02)
row['status']='table_pass_offline'
except (RuntimeError,ValueError) as exc:row.update(status='not_verified',reason=str(exc))
rows.append(row)
return dict(initial=initial,segments=rows,all_segments_pass=all(r['status']=='table_pass_offline' for r in rows),
end_clearance=checker.normal_clearance(path[-1]))
scenarios=[]
for uncertainty in (0.,.01,.025):
table=NearEdgeSlab(n,p,outward,edge,.02,uncertainty_m=uncertainty,verified=True,
source='User dimensions plus saved RGB-D edge direction; conditional diagnostic model, not calibration approval')
row=dict(uncertainty_m=uncertainty,elbow_raise=path_audit(elbow['waypoints'],opened,table),candidates=[])
for candidate in stages['candidates']:
if 'final_joints' not in candidate:continue
row['candidates'].append(dict(lift_m=candidate['lift_m'],yaw=candidate['palm_down_yaw_deg'],
lift=path_audit(candidate['lift_joints'],opened,table),
rotation=path_audit(candidate['rotate_joints'],opened,table),
transit=path_audit([candidate['rotate_joints'][-1],candidate['final_joints']],closed,table)))
scenarios.append(row)
print('Uncertainty',uncertainty,'elbow initial',row['elbow_raise']['initial']['status'],
'elbow segments pass',sum(s['status']=='table_pass_offline' for s in row['elbow_raise']['segments']),flush=True)
out=Path(tempfile.mkdtemp(prefix='measured_near_edge_',dir=ROOT/'data'))
report=dict(executable=False,motion_sent=False,thickness_m=.02,black_edge_distance_m=.105,
black_cell_center_board_m=cell_center.tolist(),black_near_border_board_m=border.tolist(),
top_normal=n.tolist(),top_point=p.tolist(),near_edge_base_m=edge.tolist(),outward_base=outward.tolist(),
edge_offset_from_rgbd_m=float((edge-ep.mean(axis=0))@outward),scenarios=scenarios,
assumptions=['Bottommost-row leftmost black cell; distance perpendicular to near table edge.',
'Straight near edge direction from saved RGB-D; extends sideways as user requested.',
'Lateral/far table limits unbounded; underside excludes table legs.',
'0/10/25mm uncertainty are sensitivity scenarios, not measured calibration error bounds.',
'Open and pointing hand states from saved data; closure at new poses not continuously certified.',
'Separate elbow and prior staged paths are NOT concatenated: their endpoints differ.'])
(out/'report.json').write_text(json.dumps(report,indent=2))
image=cv2.imread(str(cap/'rgb.png'));base_to_cam=np.linalg.inv(T)
for name,bp in [('measured black edge',board[:3,:3]@border+board[:3,3]),('near table edge',edge)]:
cp=base_to_cam[:3,:3]@bp+base_to_cam[:3,3];pixel=K@cp;pixel=np.rint(pixel[:2]/pixel[2]).astype(int)
cv2.circle(image,tuple(pixel),5,(0,0,255),-1);cv2.putText(image,name,tuple(pixel+[8,0]),cv2.FONT_HERSHEY_SIMPLEX,.4,(0,0,255),1)
cv2.imwrite(str(out/'measurement_overlay.png'),image)
print(out/'report.json')
if __name__=='__main__':main()