332 lines
17 KiB
Python
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}
|