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

95 lines
5.5 KiB
Python

"""Finite oriented tabletop and explicit evidence states; offline only.
The box includes the slab only, not legs or unobserved objects. Negative
separating-plane bounds are inconclusive, never proof of physical collision.
"""
import numpy as np
from table_contact_escape import ContactEscapeChecker,EscapeRejected,EPS
class FiniteTable:
def __init__(self,top_center,rotation,size,*,uncertainty_m,verified,source):
self.center=np.array(top_center,float);self.rotation=np.array(rotation,float);self.size=np.array(size,float)
self.uncertainty=float(uncertainty_m);self.verified=verified;self.source=source
if (self.center.shape!=(3,) or self.rotation.shape!=(3,3) or self.size.shape!=(3,) or
not np.isfinite(np.r_[self.center,self.rotation.ravel(),self.size,self.uncertainty]).all() or
np.any(self.size<=0) or self.uncertainty<0 or type(verified) is not bool or not source or
not np.allclose(self.rotation.T@self.rotation,np.eye(3),atol=1e-8) or
not np.isclose(np.linalg.det(self.rotation),1,atol=1e-8) or self.rotation[2,2]<.9):
raise ValueError('Invalid finite tabletop geometry/evidence')
self.planes=[]
for axis,half in [(0,self.size[0]/2),(1,self.size[1]/2)]:
for sign in (-1,1):
n=sign*self.rotation[:,axis]
self.planes.append((n,float(n@self.center+half+self.uncertainty),f'side_{axis}_{sign}'))
n=self.rotation[:,2]
self.planes.extend([(n.copy(),float(n@self.center+self.uncertainty),'top'),
(-n.copy(),float(-n@self.center+self.size[2]+self.uncertainty),'bottom')])
def bounds(self,points):
points=np.asarray(points,float)
if points.ndim!=2 or points.shape[1]!=3 or not len(points) or not np.isfinite(points).all():
raise ValueError('Nonempty finite Nx3 points required')
return [(float((points@n-offset).min()),label) for n,offset,label in self.planes]
def classify(self,points,margin=.02):
if not np.isfinite(margin) or margin<0:raise ValueError('Invalid margin')
gap,label=max(self.bounds(points))
if not self.verified:status='unknown_geometry'
elif gap>=margin:status='clear'
elif gap>=-EPS:status='contact_or_near_requires_escape'
else:status='separation_not_proven'
return dict(status=status,clearance_lower_bound_m=gap,separating_face=label,
physical_collision_proven=False,executable=False)
class NearEdgeSlab(FiniteTable):
"""Known near edge and thickness; conservatively unbounded sideways/far.
No visible-span extrapolation is inferred here: the caller must explicitly
supply the straight-edge model and its provenance. Slab only, not legs.
"""
def __init__(self,normal,point,outward,edge,thickness,*,uncertainty_m,verified,source):
normal=np.asarray(normal,float);point=np.asarray(point,float)
outward=np.asarray(outward,float);edge=np.asarray(edge,float)
if any(v.shape!=(3,) for v in (normal,point,outward,edge)):
raise ValueError('Expected 3-vectors')
if (not np.isfinite(np.r_[normal,point,outward,edge,thickness,uncertainty_m]).all() or
abs(np.linalg.norm(normal)-1)>1e-6 or abs(np.linalg.norm(outward)-1)>1e-6 or
abs(normal@outward)>1e-6 or abs((edge-point)@normal)>1e-6 or normal[2]<.9 or
thickness<=0 or uncertainty_m<0 or type(verified) is not bool or not source):
raise ValueError('Invalid near-edge slab')
self.verified=verified;self.source=source;self.uncertainty=float(uncertainty_m)
self.planes=[(normal.copy(),float(normal@point+uncertainty_m),'top'),
(-normal.copy(),float(-normal@point+thickness+uncertainty_m),'bottom'),
(outward.copy(),float(outward@edge+uncertainty_m),'near_side')]
class FiniteContactEscapeChecker(ContactEscapeChecker):
"""Reuse continuous contact-escape proof, with six fixed slab faces.
Start contact is accepted only at a nonnegative conservative bound;
near points must monotonically separate, others retain normal margin.
No measured/verified finite bounds -> explicit unknown, not a fake table.
"""
def bind(self,start):
if not self.table.verified:raise EscapeRejected('unknown_geometry',source=self.table.source)
self.planes=None;self.shapes=None
rows=self.sample(start);self.shapes={name:r['points'].shape for name,r in rows.items()}
self.planes={};report=[]
for name,row in rows.items():
candidates=[(float((row['points']@n-offset).min()),n,offset,label) for n,offset,label in self.table.planes]
gap,n,offset,label=max(candidates,key=lambda r:r[0])
self.planes[name]=(n.copy(),offset,label)
report.append(dict(body=name,plane=label,clearance_lower_bound_m=gap))
minimum=min(r['clearance_lower_bound_m'] for r in report)
self.initial=dict(status='unresolved_initial_geometry' if minimum<-EPS else
'normal_clearance' if minimum>=self.policy.margin_m else 'escape_required',
minimum_clearance_m=minimum,bodies=report,
negative_bound_is_not_proof_of_physical_collision=True)
return self.initial
def normal_clearance(self,q):
if not self.table.verified:raise EscapeRejected('unknown_geometry',source=self.table.source)
return min((max(self.table.bounds(row['points']))[0],name) for name,row in self.sample(q).items())