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

62 lines
3.4 KiB
Python

"""Offline hole-pattern evidence and conservative tabletop smoke checks."""
import json
import numpy as np
import trimesh
from scipy.sparse import coo_matrix
from scipy.sparse.csgraph import connected_components
from offline_table_guard import ROOT, TableGuard
def circles(path,z):
m=trimesh.load_mesh(path)
f=m.faces[np.all(abs(m.vertices[m.faces][:,:,2]-z)<1e-7,axis=1)]
edges=np.sort(np.concatenate([f[:,[0,1]],f[:,[1,2]],f[:,[2,0]]]),axis=1)
e,n=np.unique(edges,axis=0,return_counts=True);e=e[n==1]
ids=np.unique(e);ij=np.searchsorted(ids,e)
graph=coo_matrix((np.ones(len(e)),(ij[:,0],ij[:,1])),shape=(len(ids),len(ids)))
count,labels=connected_components(graph,directed=False);out=[]
for i in range(count):
v=m.vertices[ids[labels==i],:2]
c=np.linalg.lstsq(np.c_[2*v,np.ones(len(v))],(v*v).sum(1),rcond=None)[0][:2]
r=np.linalg.norm(v-c,axis=1)
if len(v)>=8 and np.ptp(r)<1e-7 and .001<r.mean()<.003:
out.append(dict(center_mm=(c*1000).tolist(),radius_mm=float(r.mean()*1000)))
return out
def main():
arm=circles(ROOT/'assets/controller_model/meshes/L7_Link.STL',-.0525)
hand=circles(ROOT/'assets/L20/LEFT/meshes/hand_base_link.STL',0.)
if len(arm)!=6 or len(hand)!=4:raise RuntimeError('Hole pattern mismatch')
center=np.mean([h['center_mm'] for h in arm],axis=0)
if not np.allclose(center,[-28,0],atol=.001):raise RuntimeError('Unexpected flange center')
cad=json.loads((ROOT/'assets/hand_adapter/cad_measurements.json').read_text())
holes=[c for c in cad['cylinders'] if abs(c['axis'][2])>.99 and 1.1<c['radius_mm']<5]
if sum(abs(c['radius_mm']-2.25)<1e-5 for c in holes)!=6:raise RuntimeError('STEP six-hole mismatch')
if sum(abs(c['radius_mm']-1.75)<1e-5 for c in holes)!=4:raise RuntimeError('STEP four-hole mismatch')
guard=TableGuard([0,0,1],[0,0,-2])
synthetic=guard.check_segment(np.zeros(7),np.ones(7)*.01)
meta=json.loads((ROOT/'data/left_cup_current/camera.json').read_text())['table_estimate']
n=np.asarray(meta['normal']);n=n if n[2]>0 else -n
guard.n=n/np.linalg.norm(n);guard.p=np.asarray(meta['point'])
gap,body=guard.clearance(np.zeros(7))
result=dict(status='offline_geometry_only_not_motion_authorization',
arm_mating_holes=arm,hand_mating_holes=hand,
adapter_outer_face_distance_mm=36.,
L7_to_Arm_Tip_translation_m=[-.02325,0,-.052],
L7_to_hand_base_translation_m=[-.028,0,-.0885],
Arm_Tip_to_hand_base_translation_m=[-.00475,0,-.0365],
translation_basis='Six-hole outer face z=0 mates L7 z=-52.5mm; four-hole outer face z=36mm mates hand z=0.',
orientation='User: same simulation reference pose, +45deg counterclockwise viewed from above. Not assumed flange-local Z.',
orientation_status='reference-relative; fixed mounting rotation not independently verified',
hand_all_postures_sphere_radius_m=guard.radius,
adapter_sphere_radius_m=guard.adapter_radius,
synthetic_path=synthetic,
saved_table_zero_joint_pose=dict(gap_m=gap,limiting_body=body,actual_current_pose=False),
scope='Left arm meshes + adapter/hand conservative spheres vs infinite plane. No self/other-object check. No hardware connection.')
path=ROOT/'assets/hand_adapter/assembly_audit.json'
path.write_text(json.dumps(result,indent=2));print(json.dumps(result,indent=2))
if __name__=='__main__':main()