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

107 lines
6.6 KiB
Python

"""Offline current-frame/start audit. No hardware control; never certifies motion.
Near-edge pixels are manually selected in the specific saved RGB image below.
The observed segment does not license extrapolation behind either robot arm.
"""
import json
from pathlib import Path
import cv2
import numpy as np
from scipy.spatial.transform import Rotation as R
from check_index_board_hover import hand_geometry
from audit_table_edge_start import EdgePoseGuard
from table_edge_constraint import TableEdgeConstraint
from offline_table_guard import ROOT
def main():
cap = ROOT/'data/start_evidence_20260927T053837893105Z'
ref = json.loads((ROOT/'data/real_calib_20260927_9jkoqN/camera.json').read_text())
cam = json.loads((cap/'camera.json').read_text())
K = np.array(cam['K']); transform = np.array(cam['T_world_cam_opencv'])
image = cv2.imread(str(cap/'rgb.png')); depth = cv2.imread(str(cap/'depth.png'), -1)*.001
ok, corners = cv2.findChessboardCornersSB(cv2.cvtColor(image, cv2.COLOR_BGR2GRAY), (8,5))
if not ok:
raise RuntimeError('Complete board not found')
obj = np.array(ref['board_objp'], np.float64).reshape(-1,3)
before = np.array(ref['T_cam_board'])
expected = cv2.projectPoints(obj, cv2.Rodrigues(before[:3,:3])[0], before[:3,3], K, None)[0].reshape(-1,2)
corners = corners.reshape(-1,2)
corners = min([corners, corners[::-1]], key=lambda c: np.linalg.norm(c-expected,axis=1).mean())
_, r, t = cv2.solvePnP(obj, corners, K, None)
current = np.eye(4); current[:3,:3] = cv2.Rodrigues(r)[0]; current[:3,3] = t[:,0]
delta = before @ np.linalg.inv(current)
# Visible white tabletop left of the cardboard; exclude robot and board.
polygon = np.array([[225,100],[305,100],[255,315],[200,310]], np.int32)
mask = np.zeros(depth.shape, np.uint8); cv2.fillPoly(mask,[polygon],1)
v,u = np.where((mask>0)&(depth>.15)&(depth<1.5))
z = depth[v,u]; camera_points = np.c_[(u-K[0,2])*z/K[0,0],(v-K[1,2])*z/K[1,1],z]
points = camera_points @ transform[:3,:3].T + transform[:3,3]
if len(points)<100:
raise RuntimeError('Insufficient table depth')
rng = np.random.default_rng(27); best = np.zeros(len(points),bool)
for _ in range(300):
p = points[rng.choice(len(points),3,replace=False)]
n = np.cross(p[1]-p[0],p[2]-p[0]); length=np.linalg.norm(n)
if length<1e-9: continue
n/=length
if abs(n[2])<.9: continue
inside = abs((points-p[0])@n)<.006
if inside.sum()>best.sum(): best=inside
if best.sum()<100: raise RuntimeError('Plane fit failed')
plane_point=points[best].mean(axis=0)
normal=np.linalg.svd(points[best]-plane_point)[2][-1]
if normal[2]<0: normal=-normal
endpoints=np.array([[195.,358.],[635.,447.]])
rays=np.c_[(endpoints[:,0]-K[0,2])/K[0,0],(endpoints[:,1]-K[1,2])/K[1,1],np.ones(2)]@transform[:3,:3].T
distance=((plane_point-transform[:3,3])@normal)/(rays@normal)
edgepoints=transform[:3,3]+rays*distance[:,None]
direction=edgepoints[1]-edgepoints[0];direction/=np.linalg.norm(direction)
inward=np.cross(normal,direction);inward/=np.linalg.norm(inward)
board_origin=(transform@current)[:3,3]
if (board_origin-edgepoints.mean(axis=0))@inward<0:inward=-inward
guard=EdgePoseGuard(normal,plane_point)
mount=json.loads((ROOT/'config/left_hand_mount.json').read_text())
guard.mount=np.array(mount['rotation_Arm_Tip_to_hand'])
raw=json.loads((ROOT/'data/left_hand_open_q10r1mak/result.json').read_text())['final_feedback']
_,guard.cloud,_=hand_geometry(raw)
guard.edge=TableEdgeConstraint(normal,plane_point,inward,edgepoints.mean(axis=0),diagnostic=True,uncertainty_m=.025)
guard.observed_y=[float(edgepoints[:,1].min()),float(edgepoints[:,1].max())]
q=np.array(json.loads((cap/'arm_snapshot.json').read_text())['states']['left']['joint_state']['position'])
bodies=[]
for name,p,radius in guard.geometry(q):
span=[float(p[:,1].min()),float(p[:,1].max())] if radius is None else [float(p[1]-radius),float(p[1]+radius)]
gap=guard.edge.hull_clearance(p) if radius is None else guard.edge.sphere_clearance(p,radius)
bodies.append(dict(name=name,y_range_m=span,extrapolation_required=span[0]<guard.observed_y[0] or span[1]>guard.observed_y[1],
extrapolated_edge_gap_m=gap))
candidates=json.loads((ROOT/'data/index_board_hover_qwt_6tmw/report.json').read_text())['candidates']
paths=[]
for candidate in candidates:
try: paths.append(dict(status='table_only_pass',result=guard.check_segment(q,np.array(candidate['joints_rad']))))
except (ValueError,RuntimeError) as exc: paths.append(dict(status='rejected',reason=str(exc)))
report=dict(executable=False,motion_sent=False,
board_relative_translation_mm=float(np.linalg.norm(delta[:3,3])*1000),
board_relative_rotation_deg=float(R.from_matrix(delta[:3,:3]).magnitude()*180/np.pi),
board_corner_shift_median_px=float(np.median(np.linalg.norm(corners-expected,axis=1))),
table_normal=normal.tolist(),table_point_m=plane_point.tolist(),
plane_inliers=int(best.sum()),plane_samples=len(points),
plane_residual_p95_m=float(np.quantile(abs((points[best]-plane_point)@normal),.95)),
edge_pixels=endpoints.tolist(),observed_edge_y_m=guard.observed_y,
start_clearance=guard.clearance(q),bodies=bodies,path_checks=paths,
limitations=['25mm provisional uncertainty is not an experimentally certified bound.',
'Selected tabletop ROI and manually selected edge; unseen edges remain unknown.',
'Saved hand feedback is not simultaneous with this RGB-D frame.',
'No self/other-arm collision or actual controller interpolation validation.',
'Rejection is not proof of physical penetration.'])
for p in endpoints.astype(int):cv2.circle(image,tuple(p),5,(0,0,255),-1)
cv2.line(image,tuple(endpoints[0].astype(int)),tuple(endpoints[1].astype(int)),(0,0,255),2)
cv2.polylines(image,[polygon],True,(0,255,0),1)
cv2.imwrite(str(cap/'index_hover_edge_overlay.png'),image)
(cap/'index_hover_validation.json').write_text(json.dumps(report,indent=2))
print(json.dumps({k:report[k] for k in ['board_relative_translation_mm','board_relative_rotation_deg','observed_edge_y_m','start_clearance']},indent=2))
print('Paths passed:',sum(p['status']=='table_only_pass' for p in paths),'/',len(paths))
print(cap/'index_hover_validation.json')
if __name__=='__main__':main()