95 lines
5.5 KiB
Python
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())
|