89 lines
5.8 KiB
Python
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()
|