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

122 lines
6.5 KiB
Python

"""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 gap<worst[0]:worst=(gap,f'L{i}_Link')
for name,c,r in [('hand_envelope',self.hand_center,self.radius),('adapter_envelope',self.adapter_center,self.adapter_radius)]:
gap=float((T[:3,:3]@c+T[:3,3]-self.p)@self.n)-r
if gap<worst[0]:worst=(gap,name)
return worst
def check_segment(self,start,end):
start,end=np.asarray(start,float),np.asarray(end,float)
if start.shape!=(7,) or end.shape!=(7,) or not np.isfinite([start,end]).all():
raise ValueError('Seven finite joint angles required')
for i,(_,_,_,lo,hi) in enumerate(self.chain):
if not lo<=start[i]<=hi or not lo<=end[i]<=hi:raise ValueError('Joint limit violation')
travel=self.reach*float(np.abs(end-start).sum());steps=max(1,int(np.ceil(travel/.004)))
if steps>20000: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<self.margin+padding:
raise RuntimeError(f'Table constraint: {body}, gap={gap:.4f}m, required={self.margin+padding:.4f}m, fraction={t:.4f}')
return dict(minimum_gap_m=minimum,samples=steps+1,interval_bound_m=padding)