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

332 lines
17 KiB
Python

"""Offline two-phase TABLE-ONLY start-contact escape; no hardware interfaces.
Normal motion keeps 20mm clearance. Escape may start at zero *conservative*
clearance, but all initially near-table vertices must move away monotonically.
Other vertices keep normal clearance. Negative initial bounds are unresolved,
not waived. Planes are locked over the entire escape and hand joints are fixed.
Continuous checks use Taylor bounds for a revolute chain q(s)=q0+s*dq:
|d''(s)| <= reach_bound * sum(abs(dq))**2.
The bound follows by differentiating each rotated link-translation product;
reach_bound bounds the sum of translation norms plus each local point radius.
Thus no unsafe interval can be hidden just by safe endpoint samples.
This does NOT certify contact forces, other obstacles or controller motion.
"""
from __future__ import annotations
from dataclasses import dataclass
import math
import numpy as np
from scipy.optimize import linprog
from table_edge_constraint import TableEdgeConstraint, check_joint_linear_segment
EPS = 1e-10 # numerical arithmetic tolerance, NOT a penetration allowance
@dataclass(frozen=True)
class EscapePolicy:
margin_m: float = .02
max_joint_step_rad: float = .01
max_sample_motion_m: float = .001
max_total_joint_travel_rad: float = .7
max_steps: int = 100
max_interval_checks: int = 4096
# Proposed ceilings only; this module never sends a command.
speed_rad_s: float = .03
acceleration_rad_s2: float = .05
def __post_init__(self):
for name in ("margin_m", "max_joint_step_rad", "max_sample_motion_m",
"max_total_joint_travel_rad", "speed_rad_s", "acceleration_rad_s2"):
value = getattr(self, name)
if not np.isfinite(value) or value <= 0:
raise ValueError(f"{name} must be finite and positive")
if self.margin_m < .01 or self.max_joint_step_rad > .02:
raise ValueError("Margin >=10mm and per-joint step <=0.02rad required")
if self.speed_rad_s > .05 or self.acceleration_rad_s2 > .1:
raise ValueError("Escape proposal exceeds slow-motion ceilings")
for name in ("max_steps", "max_interval_checks"):
value = getattr(self, name)
if isinstance(value, bool) or not isinstance(value, int) or value < 1:
raise ValueError(f"{name} must be a positive integer")
class EscapeRejected(RuntimeError):
def __init__(self, reason, **details):
self.report = {"reason": reason, **details}
super().__init__(f"{reason}: {details}")
class ContactEscapeChecker:
"""Geometry protocol: .sample(q), .body_names, .lo/.hi, .reach.
sample returns {name: {'points': Nx3, 'jacobian': Nx3xnq}} in one base frame,
with a FIXED vertex set/order and hand configuration throughout the path.
The caller is responsible for a valid global reach bound for that geometry.
"""
def __init__(self, geometry, table: TableEdgeConstraint, *, observed_y=None,
policy=None):
self.geometry, self.table = geometry, table
self.policy = policy or EscapePolicy()
self.lo = np.array(geometry.lo, dtype=float)
self.hi = np.array(geometry.hi, dtype=float)
if (self.lo.ndim != 1 or not len(self.lo) or self.hi.shape != self.lo.shape
or not np.isfinite([self.lo, self.hi]).all() or np.any(self.lo >= self.hi)):
raise ValueError("Finite, ordered joint limits required")
self.reach = float(geometry.reach)
if not np.isfinite(self.reach) or self.reach <= 0:
raise ValueError("A positive global reach bound is required")
self.observed_y = None
if observed_y is not None:
span = np.array(observed_y, dtype=float)
if span.shape != (2,) or not np.isfinite(span).all() or span[0] >= span[1]:
raise ValueError("Observed edge y range must be finite and increasing")
# Erode the trusted span by the same positional uncertainty.
span += [table.uncertainty_m, -table.uncertainty_m]
if span[0] >= span[1]:
raise ValueError("Uncertainty consumes the observed edge span")
self.observed_y = span
self.planes = None
self.shapes = None
self.initial = None
def sample(self, q):
q = np.asarray(q, dtype=float)
if q.shape != self.lo.shape or not np.isfinite(q).all():
raise ValueError("Wrong-size or nonfinite joint vector")
if np.any(q < self.lo) or np.any(q > self.hi):
raise ValueError("Joint limit violation")
rows = self.geometry.sample(q)
if not rows or set(rows) != set(self.geometry.body_names):
raise ValueError("Geometry omitted/added a body")
for name, row in rows.items():
points = np.asarray(row["points"], dtype=float)
jac = np.asarray(row["jacobian"], dtype=float)
if (points.ndim != 2 or points.shape[1] != 3 or not len(points)
or jac.shape != (len(points), 3, len(q))
or not np.isfinite(points).all() or not np.isfinite(jac).all()):
raise ValueError("Invalid points/Jacobian")
if self.shapes is not None and points.shape != self.shapes[name]:
raise ValueError("Geometry changed during escape")
return rows
def _distances(self, name, row):
normal, offset, _ = self.planes[name]
return row["points"] @ normal - offset
def bind(self, start):
"""Fix one WHOLE-BODY separation plane per body at the initial pose."""
self.planes, self.shapes = None, None
rows = self.sample(start)
table = self.table
top = (table.top_normal.copy(), float(table.top_normal @ table.top_point + table.uncertainty_m), "top")
outside = (table.outward_normal.copy(), float(table.outward_normal @ table.edge_point + table.uncertainty_m), "near_edge")
self.planes = {}
self.shapes = {name: row["points"].shape for name, row in rows.items()}
report = []
for name, row in rows.items():
points = row["points"]
plane = top
top_gap = float(np.min(points @ top[0] - top[1]))
gap = top_gap
within = self.observed_y is not None and bool(
np.all(points[:, 1] >= self.observed_y[0]) and np.all(points[:, 1] <= self.observed_y[1]))
edge_gap = float(np.min(points @ outside[0] - outside[1]))
if within and edge_gap > top_gap:
plane, gap = outside, edge_gap
self.planes[name] = plane
report.append({"body": name, "plane": plane[2], "clearance_lower_bound_m": gap,
"edge_span_observed": within, "top_bound_m": top_gap,
"near_edge_bound_m": edge_gap})
minimum = min(row["clearance_lower_bound_m"] for row in report)
status = ("unresolved_initial_geometry" if minimum < -EPS else
"normal_clearance" if minimum >= self.policy.margin_m else "escape_required")
self.initial = {"status": status, "minimum_clearance_m": minimum, "bodies": report,
"negative_bound_is_not_proof_of_physical_collision": True}
return self.initial
def current_gaps(self, q):
if self.planes is None:
raise ValueError("Bind the start pose first")
return {name: float(self._distances(name, row).min()) for name, row in self.sample(q).items()}
def check_segment(self, start, end):
"""Continuous table-only proof for a fixed-hand, joint-linear segment."""
if self.planes is None:
self.bind(start)
start, end = np.asarray(start, float), np.asarray(end, float)
first, last = self.sample(start), self.sample(end)
dq = end - start
if np.max(np.abs(dq)) > self.policy.max_joint_step_rad + EPS:
raise EscapeRejected("escape_step_too_large")
travel = self.reach * float(np.abs(dq).sum())
count = max(1, math.ceil(travel / self.policy.max_sample_motion_m))
if count > self.policy.max_interval_checks:
raise EscapeRejected("interval_check_budget_exceeded")
h = 1. / count
curvature = self.reach * float(np.abs(dq).sum()) ** 2
near, floors = {}, {}
for name, row in first.items():
d0 = self._distances(name, row)
if d0.min() < -EPS:
raise EscapeRejected("unresolved_initial_geometry", body=name, bound_m=float(d0.min()))
near[name] = d0 < self.policy.margin_m
floors[name] = np.minimum(d0, self.policy.margin_m)
minimum = math.inf
for k in range(count):
fraction = k / count
rows = first if k == 0 else self.sample(start + fraction * dq)
for name, row in rows.items():
normal, _, label = self.planes[name]
d = self._distances(name, row)
velocity = np.einsum("nij,j->ni", row["jacobian"], dq)
derivative = velocity @ normal
# Concave quadratic is minimized at an endpoint of [0,h].
lower = np.minimum(d, d + derivative * h - .5 * curvature * h * h)
minimum = min(minimum, float(lower.min()))
if np.any(lower < floors[name] - EPS):
raise EscapeRejected("clearance_decreases_or_normal_margin_lost", body=name,
fraction=fraction, bound_m=float(lower.min()))
if np.any(derivative[near[name]] - curvature * h < -EPS):
raise EscapeRejected("contact_motion_not_proven_outward", body=name, fraction=fraction)
if label == "near_edge":
# The whole swept body must remain inside the observed span.
for sign, offset in ((1., self.observed_y[0]), (-1., -self.observed_y[1])):
yd = sign * row["points"][:, 1] - offset
ylower = np.minimum(yd, yd + sign * velocity[:, 1] * h - .5 * curvature * h*h)
if ylower.min() < -EPS:
raise EscapeRejected("path_leaves_observed_edge_span", body=name, fraction=fraction)
endpoint = {name: float(self._distances(name, row).min()) for name, row in last.items()}
return {"status": "continuous_table_segment_pass", "intervals": count,
"minimum_certified_bound_m": minimum, "endpoint_body_gaps_m": endpoint,
"interpolation": "joint_linear_fixed_hand_offline_only"}
def check_path(self, waypoints):
points = np.asarray(waypoints, dtype=float)
if points.ndim != 2 or points.shape[1:] != self.lo.shape or not len(points):
raise ValueError("Nonempty waypoint matrix required")
initial = self.bind(points[0])
if initial["status"] == "unresolved_initial_geometry":
raise EscapeRejected("unresolved_initial_geometry", initial=initial)
travel = float(np.abs(np.diff(points, axis=0)).sum())
if len(points)-1 > self.policy.max_steps or travel > self.policy.max_total_joint_travel_rad + EPS:
raise EscapeRejected("escape_path_budget_exceeded")
segments = [self.check_segment(a, b) for a, b in zip(points, points[1:])]
gaps = self.current_gaps(points[-1])
if min(gaps.values()) < self.policy.margin_m:
raise EscapeRejected("normal_margin_not_restored", endpoint_body_gaps_m=gaps)
return {"status": "escape_path_table_verified_offline", "segments": segments,
"endpoint_body_gaps_m": gaps, "total_joint_travel_rad": travel,
"next_phase": "normal_motion_requires_separate_path_validation",
"executable": False, "motion_sent": False}
def normal_clearance(self, q):
"""20mm phase: no escape exemptions and no discontinuous edge fallback.
The outward branch is limited by distance to BOTH observed-span bounds.
min/max of unit-plane distances remains 1-Lipschitz, so the existing
between-sample displacement bound also covers leaving the visible edge.
This may conservatively require extra clearance to the span boundaries.
"""
table = self.table
result = []
for name, row in self.sample(q).items():
points = row["points"]
top = float(((points-table.top_point) @ table.top_normal).min()) - table.uncertainty_m
gap = top
if self.observed_y is not None:
outside = float(((points-table.edge_point) @ table.outward_normal).min()) - table.uncertainty_m
span = min(float(points[:, 1].min()-self.observed_y[0]),
float(self.observed_y[1]-points[:, 1].max()))
gap = max(top, min(outside, span))
result.append((gap, name))
return min(result)
def check_normal_segment(self, start, end):
"""Separate normal-margin validation after escape; no start bypass."""
# Validate full vectors and limits before the interpolated checker.
self.sample(start)
self.sample(end)
return check_joint_linear_segment(start, end, self.normal_clearance,
reach_bound_m=self.reach, margin_m=self.policy.margin_m,
max_sample_motion_m=self.policy.max_sample_motion_m)
def plan_escape(checker, start):
"""Find small outward joint steps with LP, then independently check each.
Objective: grow the currently smallest locked-plane distance, with all
near-table vertex derivatives positive. A failed local search is NOT proof
that no escape exists (e.g. tangential, second-order escapes may be rejected).
"""
start = np.asarray(start, float)
initial = checker.bind(start)
report = {"executable": False, "hardware_connected": False, "motion_sent": False,
"initial": initial, "waypoints_rad": [start.tolist()],
"speed_ceiling_rad_s": checker.policy.speed_rad_s,
"acceleration_ceiling_rad_s2": checker.policy.acceleration_rad_s2}
if initial["status"] == "unresolved_initial_geometry":
return {**report, "status": "unresolved_initial_geometry",
"reason": "Negative conservative start bounds cannot be waived as contact."}
q = start.copy()
traveled = 0.
for _ in range(checker.policy.max_steps + 1):
rows = checker.sample(q)
minimum = min(float(checker._distances(name, row).min()) for name, row in rows.items())
if minimum >= checker.policy.margin_m:
checked = checker.check_path(report["waypoints_rad"])
return {**report, "status": "normal_clearance" if len(report["waypoints_rad"]) == 1 else "escape_found_offline",
"verification": checked, "steps": len(report["waypoints_rad"])-1}
if len(report["waypoints_rad"])-1 >= checker.policy.max_steps:
break
matrix, rhs = [], []
for name, row in rows.items():
normal, _, label = checker.planes[name]
d = checker._distances(name, row)
jac = np.einsum("nij,i->nj", row["jacobian"], normal)
for distance, gradient in zip(d, jac):
if distance < checker.policy.margin_m:
# g is a shared strictly outward increment for all near vertices.
matrix.append(np.r_[-gradient, 1.]); rhs.append(0.)
else:
matrix.append(np.r_[-gradient, 0.]); rhs.append(distance-checker.policy.margin_m)
matrix.append(np.r_[-gradient, 1.]); rhs.append(distance-minimum)
if label == "near_edge":
for point, jacobian in zip(row["points"], row["jacobian"]):
matrix.append(np.r_[-jacobian[1], 0.]); rhs.append(point[1]-checker.observed_y[0])
matrix.append(np.r_[jacobian[1], 0.]); rhs.append(checker.observed_y[1]-point[1])
radius = checker.policy.max_joint_step_rad
bounds = [(max(lo-value, -radius), min(hi-value, radius))
for value, lo, hi in zip(q, checker.lo, checker.hi)] + [(0., .002)]
fit = linprog(np.r_[np.zeros(len(q)), -1.], A_ub=np.asarray(matrix), b_ub=np.asarray(rhs),
bounds=bounds, method="highs")
if not fit.success or fit.x[-1] < 1e-7:
report["last_search_reason"] = "No positive first-order outward LP direction"
break
accepted = False
for scale in (1., .5, .25, .125, .0625, .03125):
candidate = q + scale * fit.x[:-1]
length = float(np.abs(candidate-q).sum())
if traveled + length > checker.policy.max_total_joint_travel_rad:
continue
try:
checker.check_segment(q, candidate)
except EscapeRejected as exc:
report["last_search_reason"] = exc.report
continue
actual_minimum = min(checker.current_gaps(candidate).values())
if actual_minimum <= minimum + 1e-8:
continue
traveled += length
q = candidate
report["waypoints_rad"].append(q.tolist())
accepted = True
break
if not accepted:
break
return {**report, "status": "no_certified_escape_found",
"reason": "Local/budget-limited search failure, not global unreachability. Partial path NOT released.",
"partial_waypoints_are_not_a_motion_plan": True}