107 lines
6.6 KiB
Python
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()
|