122 lines
6.5 KiB
Python
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)
|