62 lines
3.4 KiB
Python
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()
|