Files
a7l20-hug-sim/scripts/check_hover_observation_coverage.py
T

66 lines
3.7 KiB
Python

"""Project saved left-arm geometry onto RGB; diagnostic only, no hardware.
Convex outlines are not a depth renderer and cannot prove collision or alignment.
"""
import json
import cv2
import numpy as np
from scipy.spatial.transform import Rotation as R
from offline_table_guard import ROOT
from search_cup_poses_offline import PoseGuard
from check_index_board_hover import hand_geometry
def main():
cap=ROOT/'data/start_evidence_20260927T053837893105Z'
cam=json.loads((cap/'camera.json').read_text())
state=json.loads((cap/'arm_snapshot.json').read_text())['states']['left']
audit=json.loads((cap/'index_hover_validation.json').read_text())
K=np.array(cam['K']);base_to_cam=np.linalg.inv(np.array(cam['T_world_cam_opencv']))
image=cv2.imread(str(cap/'rgb.png'));h,w=image.shape[:2]
guard=PoseGuard(audit['table_normal'],audit['table_point_m'])
mount=json.loads((ROOT/'config/left_hand_mount.json').read_text())
raw=json.loads((ROOT/'data/left_hand_open_q10r1mak/result.json').read_text())['final_feedback']
tip,cloud,_=hand_geometry(raw)
rows=[]
def project(name,vertices,color):
c=vertices@base_to_cam[:3,:3].T+base_to_cam[:3,3]
c=c[c[:,2]>.01]
if not len(c):return
uv=(c@K.T);uv=uv[:,:2]/uv[:,2:]
inside=(uv[:,0]>=0)&(uv[:,0]<w)&(uv[:,1]>=0)&(uv[:,1]<h)
rows.append(dict(name=name,projected_bounds_px=[uv.min(axis=0).tolist(),uv.max(axis=0).tolist()],
vertices_visible_fraction=float(inside.mean())))
hull=cv2.convexHull(np.round(np.clip(uv,-10000,10000)).astype(np.int32))
cv2.polylines(image,[hull],True,color,1)
anchor=np.round(uv.mean(axis=0)).astype(int)
anchor=np.clip(anchor,[0,15],[w-100,h-10])
cv2.putText(image,name,tuple(anchor),cv2.FONT_HERSHEY_SIMPLEX,.45,color,1)
T=np.eye(4)
for i,(angle,(O,axis,v,lo,hi)) in enumerate(zip(state['joint_state']['position'],guard.chain),1):
A=np.eye(4);A[:3,:3]=R.from_rotvec(axis*angle).as_matrix();T=T@O@A
project(f'L{i}',v@T[:3,:3].T+T[:3,3],(255,0,255))
hand_in_l7=cloud@np.array(mount['rotation_Arm_Tip_to_hand']).T+mount['translation_L7_to_hand_m']
project('hand saved shape',hand_in_l7@T[:3,:3].T+T[:3,3],(0,150,255))
fk_tip=T[:3,:3]@np.array([-.02325,0,-.052])+T[:3,3]
reported=np.array([state['pose']['position'][k] for k in 'xyz'])
pose_rotation=R.from_quat([state['pose']['orientation'][k] for k in ['x','y','z','w']]).as_matrix()
edge=np.array(audit['edge_pixels'],np.int32)
cv2.line(image,tuple(edge[0]),tuple(edge[1]),(0,255,0),3)
cv2.putText(image,'Green: OBSERVED edge only; outlines: MODEL, not real contours',(12,22),cv2.FONT_HERSHEY_SIMPLEX,.43,(0,255,255),1)
report=dict(executable=False,motion_sent=False,coverage=rows,
fk_controller_position_error_m=float(np.linalg.norm(fk_tip-reported)),
fk_controller_rotation_error_deg=float(R.from_matrix(T[:3,:3].T@pose_rotation).magnitude()*180/np.pi),
observed_edge_y_m=audit['observed_edge_y_m'],
unresolved_bodies=[b for b in audit['bodies'] if b['extrapolation_required']],
limitations=['FK agreement checks internal convention, not camera-to-base accuracy.',
'Projected convex outlines do not account for occlusion or depth.',
'Unobserved tabletop boundaries cannot be recovered from this frame.',
'No table extrapolation or execution authorization is applied.'])
cv2.imwrite(str(cap/'hover_model_coverage.png'),image)
(cap/'hover_observation_coverage.json').write_text(json.dumps(report,indent=2))
print(json.dumps(report,indent=2))
if __name__=='__main__':main()