172 lines
8.3 KiB
Python
172 lines
8.3 KiB
Python
"""Offline, conservative tabletop constraint with ONE observed near edge.
|
|
|
|
The forbidden volume is the intersection of two closed halfspaces::
|
|
|
|
top_normal . (x - top_point) <= 0
|
|
inward_normal . (x - edge_point) >= 0
|
|
|
|
The volume is deliberately unbounded sideways, away from the robot, and down.
|
|
It is NOT a reconstructed finite table and cannot certify absence of table legs
|
|
outside this volume or other obstacles. All inputs are in one frame, in metres.
|
|
|
|
For a convex body, max(min(top distances), min(outward distances)) is a
|
|
conservative separating-plane clearance bound. A positive bound proves the
|
|
WHOLE body lies outside at least one forbidden halfspace; a nonpositive bound
|
|
does not prove collision. In particular, do NOT take min(max(top,outward)) over
|
|
vertices: a triangle can cross the forbidden volume while every vertex is out.
|
|
|
|
There are no hardware imports, network operations, or command-sending hooks.
|
|
An unverified edge requires explicit diagnostic=True; constructing this class,
|
|
even with edge_verified=True, never constitutes a hardware safety approval.
|
|
"""
|
|
|
|
import math
|
|
import numpy as np
|
|
|
|
|
|
def _vector(value, name):
|
|
result = np.asarray(value, dtype=float)
|
|
if result.shape != (3,) or not np.isfinite(result).all():
|
|
raise ValueError(f'{name} must be a finite three-vector')
|
|
return result.copy()
|
|
|
|
|
|
def _nonnegative(value, name):
|
|
result = float(value)
|
|
if not math.isfinite(result) or result < 0:
|
|
raise ValueError(f'{name} must be finite and nonnegative')
|
|
return result
|
|
|
|
|
|
class TableEdgeConstraint:
|
|
"""Represent one near edge, without inventing any unseen table boundaries.
|
|
|
|
inward_normal points into the tabletop footprint and is orthogonal to the
|
|
upward top_normal. edge_point lies on the top plane. uncertainty_m expands
|
|
both forbidden halfspaces, not just the table's height, by that distance.
|
|
A separate physical clearance margin belongs in the path checker.
|
|
"""
|
|
|
|
def __init__(self, top_normal, top_point, inward_normal, edge_point, *,
|
|
edge_verified=False, diagnostic=False, units='m',
|
|
uncertainty_m=0.0):
|
|
if units != 'm':
|
|
raise ValueError('Explicit metre units (units="m") required')
|
|
if not isinstance(edge_verified, bool) or not isinstance(diagnostic, bool):
|
|
raise ValueError('edge_verified and diagnostic must be bool')
|
|
if not edge_verified and not diagnostic:
|
|
raise ValueError('Unverified near edge; diagnostic=True is offline only')
|
|
self.top_normal = _vector(top_normal, 'top_normal')
|
|
self.top_point = _vector(top_point, 'top_point')
|
|
self.inward_normal = _vector(inward_normal, 'inward_normal')
|
|
self.edge_point = _vector(edge_point, 'edge_point')
|
|
for name in ('top_normal', 'inward_normal'):
|
|
if abs(np.linalg.norm(getattr(self, name)) - 1.0) > 1e-6:
|
|
raise ValueError(f'{name} must already be unit length')
|
|
if self.top_normal[2] <= 0:
|
|
raise ValueError('top_normal must point upward in the robot base frame')
|
|
if abs(self.top_normal @ self.inward_normal) > 1e-6:
|
|
raise ValueError('inward_normal must lie within the top plane')
|
|
if abs(self.top_normal @ (self.edge_point-self.top_point)) > 1e-5:
|
|
raise ValueError('edge_point must lie on the top plane (10 um tolerance)')
|
|
self.outward_normal = -self.inward_normal
|
|
self.uncertainty_m = _nonnegative(uncertainty_m, 'uncertainty_m')
|
|
self.edge_verified = edge_verified
|
|
self.diagnostic = diagnostic
|
|
for name in ('top_normal', 'top_point', 'inward_normal', 'edge_point',
|
|
'outward_normal'):
|
|
getattr(self, name).flags.writeable = False
|
|
|
|
def hull_clearance(self, vertices):
|
|
"""Lower bound for the entire convex hull, including its faces/interior.
|
|
|
|
The vertices can be all vertices of a mesh; no explicit hull computation
|
|
is needed since extrema of these linear functions occur at vertices.
|
|
"""
|
|
vertices = np.asarray(vertices, dtype=float)
|
|
if (vertices.ndim != 2 or vertices.shape[1] != 3 or len(vertices) == 0
|
|
or not np.isfinite(vertices).all()):
|
|
raise ValueError('A nonempty finite N x 3 vertex array is required')
|
|
top = float(np.min((vertices-self.top_point) @ self.top_normal))
|
|
outward = float(np.min((vertices-self.edge_point) @ self.outward_normal))
|
|
return max(top, outward) - self.uncertainty_m
|
|
|
|
def sphere_clearance(self, center, radius):
|
|
"""Lower bound for a complete bounding sphere, not only its centre."""
|
|
center = _vector(center, 'sphere center')
|
|
radius = _nonnegative(radius, 'sphere radius')
|
|
top = float((center-self.top_point) @ self.top_normal) - radius
|
|
outward = float((center-self.edge_point) @ self.outward_normal) - radius
|
|
return max(top, outward) - self.uncertainty_m
|
|
|
|
|
|
class TableEdgeViolation(RuntimeError):
|
|
"""Rejected conservative clearance test; not proof of physical penetration."""
|
|
|
|
def __init__(self, report):
|
|
self.report = report
|
|
super().__init__(
|
|
'Table edge constraint: '
|
|
f'{report["body"]}, gap={report["clearance_lower_bound_m"]:.6f}m, '
|
|
f'required={report["required_m"]:.6f}m, '
|
|
f'fraction={report["fraction"]:.6f}')
|
|
|
|
|
|
def check_joint_linear_segment(start, end, clearance_fn, *, reach_bound_m,
|
|
margin_m=.02, max_sample_motion_m=.004,
|
|
max_steps=20000):
|
|
"""Check one joint-linear path with conservative between-sample padding.
|
|
|
|
clearance_fn(q) returns either a finite bound or (bound, body_name). Caller
|
|
must supply a proven reach_bound_m >= distance of EVERY checked body point
|
|
from EVERY contributing revolute joint axis, valid throughout this segment.
|
|
Then reach * sum(abs(delta_q)) bounds all point travel. No prismatic joints
|
|
are supported by that formula. Angles MUST be radians; changing hand joints
|
|
must be included or bounded separately. q(t) is joint-linear, not Cartesian.
|
|
|
|
Both endpoint-inclusive samples and a half-interval travel padding are
|
|
required: every intermediate pose is within that bound of its nearest
|
|
sample. This is NOT a certificate for controller-specific interpolation.
|
|
"""
|
|
start = np.asarray(start, dtype=float)
|
|
end = np.asarray(end, dtype=float)
|
|
if (start.ndim != 1 or len(start) == 0 or end.shape != start.shape
|
|
or not np.isfinite(start).all() or not np.isfinite(end).all()):
|
|
raise ValueError('Joint endpoints must be equal-size finite 1D vectors')
|
|
reach = _nonnegative(reach_bound_m, 'reach_bound_m')
|
|
spacing = _nonnegative(max_sample_motion_m, 'max_sample_motion_m')
|
|
margin = _nonnegative(margin_m, 'margin_m')
|
|
if reach == 0 or spacing == 0:
|
|
raise ValueError('Reach bound and sample-motion limit must be positive')
|
|
if isinstance(max_steps, bool) or not isinstance(max_steps, int) or max_steps < 1:
|
|
raise ValueError('max_steps must be a positive integer')
|
|
travel = reach * float(np.abs(end-start).sum())
|
|
if not math.isfinite(travel):
|
|
raise ValueError('Joint displacement bound overflowed')
|
|
steps = max(1, math.ceil(travel/spacing))
|
|
if steps > max_steps:
|
|
raise RuntimeError('Offline verification sample budget exceeded')
|
|
padding = travel / (2*steps)
|
|
minimum = math.inf
|
|
worst_body = ''
|
|
for fraction in np.linspace(0., 1., steps+1):
|
|
value = clearance_fn((1-fraction)*start+fraction*end)
|
|
if isinstance(value, tuple):
|
|
gap, body = value
|
|
else:
|
|
gap, body = value, 'unspecified'
|
|
gap = float(gap)
|
|
if not math.isfinite(gap):
|
|
raise ValueError('Clearance callback returned nonfinite value')
|
|
if gap < minimum:
|
|
minimum, worst_body = gap, str(body)
|
|
if gap < margin + padding:
|
|
raise TableEdgeViolation(dict(
|
|
body=str(body), clearance_lower_bound_m=gap,
|
|
required_m=margin+padding, fraction=float(fraction),
|
|
interval_bound_m=padding, planned_samples=steps+1))
|
|
return dict(minimum_gap_m=minimum, worst_body=worst_body, samples=steps+1,
|
|
interval_bound_m=padding,
|
|
certified_clearance_lower_bound_m=minimum-padding,
|
|
margin_m=margin, interpolation='joint_linear_offline_only')
|