66 lines
3.7 KiB
Python
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()
|