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

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')