"""No hardware imports: full arm + orientation-independent hand envelope/table. Infinite plane is deliberately conservative. Covers joint-linear segments with a between-sample displacement bound, NOT controller trajectory certification. """ import json from pathlib import Path import xml.etree.ElementTree as ET import numpy as np import trimesh from scipy.spatial import ConvexHull from scipy.spatial.transform import Rotation as R ROOT=Path(__file__).resolve().parents[1] def origin(e): T=np.eye(4) if e is not None: T[:3,3]=np.fromstring(e.get('xyz','0 0 0'),sep=' ') T[:3,:3]=R.from_euler('xyz',np.fromstring(e.get('rpy','0 0 0'),sep=' ')).as_matrix() return T def hand_radius(): """Triangle inequality along every joint chain: valid for any joint angle.""" path=ROOT/'assets/l20_left_mj.urdf';root=ET.parse(path).getroot() distance={'hand_base_link':0.};pending=list(root.findall('joint')) while pending: progress=False for j in pending[:]: parent=j.find('parent').get('link') if parent not in distance:continue if j.get('type') not in ('fixed','revolute','continuous'): raise ValueError('Unsupported hand joint') distance[j.find('child').get('link')]=distance[parent]+np.linalg.norm(origin(j.find('origin'))[:3,3]) pending.remove(j);progress=True if not progress:raise ValueError('Unresolved hand tree') radius=0.;count=0 for link in root.findall('link'): geometries=link.findall('collision')+link.findall('visual') if not geometries:raise ValueError('Missing hand geometry') for geom in geometries: mesh=geom.find('geometry/mesh') if mesh is None:raise ValueError('Unsupported hand geometry') file=Path(mesh.get('filename')) if not file.is_absolute(): compiler=root.find('mujoco/compiler') meshdir=Path(compiler.get('meshdir','.')) if compiler is not None else Path('.') if not meshdir.is_absolute():meshdir=path.parent/meshdir file=meshdir/file v=np.asarray(trimesh.load_mesh(file,process=False).vertices) v*=np.fromstring(mesh.get('scale','1 1 1'),sep=' ') T=origin(geom.find('origin'));v=v@T[:3,:3].T+T[:3,3] radius=max(radius,distance[link.get('name')]+float(np.linalg.norm(v,axis=1).max()));count+=1 if count==0:raise ValueError('Empty hand') return radius class TableGuard: def __init__(self,normal,point,margin=.02): self.n=np.asarray(normal,float);self.p=np.asarray(point,float);self.margin=float(margin) if (self.n.shape!=(3,) or self.p.shape!=(3,) or not np.isfinite(np.r_[self.n,self.p,self.margin]).all() or abs(np.linalg.norm(self.n)-1)>1e-5 or self.n[2]<.9 or self.margin<.01): raise ValueError('Invalid table plane/margin') # CAD fit: L7 mating center (-28,0,-52.5) mm; adapter length 36 mm. self.hand_center=np.array([-.028,0,-.0885]) self.radius=hand_radius() root=ET.parse(ROOT/'assets/controller_model/lingzuv2_description.urdf').getroot() self.chain=[];lengths=0.;rmax=0. for i in range(1,8): j=root.find(f"joint[@name='L{i}_JOINT']") if j is None or j.get('type')!='revolute':raise ValueError('Unexpected arm joint') if j.find('parent').get('link')!=('base_link' if i==1 else f'L{i-1}_Link'): raise ValueError('Unexpected arm chain') T=origin(j.find('origin'));lengths+=np.linalg.norm(T[:3,3]) axis=np.fromstring(j.find('axis').get('xyz'),sep=' ') if abs(np.linalg.norm(axis)-1)>1e-6:raise ValueError('Invalid axis') link=root.find(f"link[@name='L{i}_Link']");vv=[] for geom in link.findall('collision'): mesh=geom.find('geometry/mesh') if mesh is None:raise ValueError('Unsupported arm mesh') v=np.asarray(trimesh.load_mesh(ROOT/'assets/controller_model/meshes'/Path(mesh.get('filename')).name,process=False).vertices) v*=np.fromstring(mesh.get('scale','1 1 1'),sep=' ') C=origin(geom.find('origin'));vv.append(v@C[:3,:3].T+C[:3,3]) if not vv:raise ValueError('Missing arm mesh') v=np.concatenate(vv);v=v[ConvexHull(v).vertices] rmax=max(rmax,float(np.linalg.norm(v,axis=1).max())) lim=j.find('limit');self.chain.append((T,axis,v,float(lim.get('lower')),float(lim.get('upper')))) # Adapter sphere encloses all yaw variants, centered halfway along axis. self.adapter_center=np.array([-.028,0,-.0705]);self.adapter_radius=np.hypot(.030,.018) self.reach=lengths+max(rmax,np.linalg.norm(self.hand_center)+self.radius, np.linalg.norm(self.adapter_center)+self.adapter_radius) def clearance(self,q): T=np.eye(4);worst=(float('inf'),'') for i,(a,(O,axis,v,lo,hi)) in enumerate(zip(q,self.chain),1): if not lo<=a<=hi:raise ValueError('Joint limit violation') A=np.eye(4);A[:3,:3]=R.from_rotvec(axis*a).as_matrix();T=T@O@A gap=float(((v@T[:3,:3].T+T[:3,3]-self.p)@self.n).min()) if gap20000:raise RuntimeError('Verification budget exceeded') padding=travel/(2*steps);minimum=float('inf') for t in np.linspace(0,1,steps+1): gap,body=self.clearance((1-t)*start+t*end);minimum=min(minimum,gap) if gap