Files

116 lines
6.5 KiB
Python

"""Offline start-contact planning entry point. NEVER connects to a robot.
Default: diagnose a saved real-arm snapshot against saved table evidence with
an explicitly labelled saved planned hand preshape (NOT a live hand reading).
--demo: use the real assembly geometry with a synthetic touching table, to test
the escape logic independently of unknown real table extent/hand feedback.
"""
from __future__ import annotations
import argparse
from dataclasses import asdict
import hashlib
import json
from pathlib import Path
import xml.etree.ElementTree as ET
import numpy as np
from table_contact_escape import ContactEscapeChecker, EscapePolicy, plan_escape
from table_edge_constraint import TableEdgeConstraint
from table_escape_geometry import EscapeGeometry, ROOT, HAND_URDF
DEFAULT_SNAPSHOT = ROOT / "data/left_cup_current/arm_snapshot_20260924T084830651325Z.json"
DEFAULT_EDGE = ROOT / "data/table_edge_check_20260924_01/start_edge_audit.json"
def read_left_joints(snapshot):
state = snapshot["states"]["left"]["joint_state"]
names = [name.upper() for name in state["name"]]
values = state["position"]
if len(names) != len(values) or len(set(names)) != len(names):
raise ValueError("Ambiguous snapshot joint names")
return np.array([values[names.index(f"L{i}_JOINT")] for i in range(1, 8)], dtype=float)
def main(argv=None):
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--snapshot", type=Path, default=DEFAULT_SNAPSHOT)
parser.add_argument("--edge-json", type=Path, default=DEFAULT_EDGE)
parser.add_argument("--hand-json", type=Path,
help="Explicit active hand joint mapping; provenance is not assumed measured")
parser.add_argument("--demo", action="store_true", help="Synthetic touching table; NOT the real table")
parser.add_argument("--out", type=Path)
args = parser.parse_args(argv)
if args.demo and args.hand_json:
parser.error("--demo uses a fixed nominal-zero hand; do not mix with --hand-json")
if args.demo:
hand = {j.get("name"): 0. for j in ET.parse(HAND_URDF).getroot().findall("joint")
if j.find("mimic") is None}
hand_source = "synthetic nominal-zero hand; not real feedback"
elif args.hand_json:
hand = json.loads(args.hand_json.read_text())
hand_source = str(args.hand_json.resolve()) + "; supplied, not certified live feedback"
else:
hand = json.loads((ROOT / "data/left_cup_current/cup_yaw_plan.json").read_text())["solutions"][0]["grasp"]["hand_pre"]
hand_source = "OLD saved planned hand_pre; conditional diagnosis only, NOT current hand posture"
geometry = EscapeGeometry(hand)
policy = EscapePolicy()
if args.demo:
q = read_left_joints(json.loads(args.snapshot.read_text()))
rows = geometry.sample(q)
height = min(float(row["points"][:, 2].min()) for row in rows.values())
# Do not alter any measured plane/calibration: this table exists only in the demo.
table = TableEdgeConstraint([0, 0, 1], [0, 0, height], [1, 0, 0], [0, 0, height], diagnostic=True)
observed = None # conservative infinite top, no invented free sides
source = {"mode": "synthetic_contact_demo_NOT_REAL_TABLE", "table_height_m": height,
"pose": "saved arm joint angles, NOT live feedback", "hand_source": hand_source,
"snapshot": str(args.snapshot.resolve()),
"snapshot_sha256": hashlib.sha256(args.snapshot.read_bytes()).hexdigest(),
"synthetic_table_does_not_modify_measured_table": True}
else:
q = read_left_joints(json.loads(args.snapshot.read_text()))
evidence = json.loads(args.edge_json.read_text())
evidence = evidence.get("edge_evidence", evidence)
table = TableEdgeConstraint(evidence["top_normal_base"], evidence["top_point_base_m"],
evidence["inward_normal_base"], evidence["edge_point_base_m"], diagnostic=True,
uncertainty_m=evidence["uncertainty_m"])
observed = evidence["observed_y_range_m"]
source = {"mode": "saved_snapshot_conditional_diagnostic", "snapshot": str(args.snapshot.resolve()),
"table_evidence": str(args.edge_json.resolve()), "hand_source": hand_source,
"snapshot_sha256": hashlib.sha256(args.snapshot.read_bytes()).hexdigest(),
"table_sha256": hashlib.sha256(args.edge_json.read_bytes()).hexdigest(),
"snapshot_is_live": False, "hand_is_measured": False,
"uncertainty_m": table.uncertainty_m,
"table_edge_verified": evidence.get("edge_verified", False)}
checker = ContactEscapeChecker(geometry, table, observed_y=observed, policy=policy)
print("OFFLINE ONLY: no controller / CAN / enable / movement.", flush=True)
print("Source:", source["mode"], flush=True)
report = plan_escape(checker, q)
report.update({"sources": source, "geometry": geometry.sources, "policy": asdict(policy),
"scope": "LEFT arm + CAD adapter + fixed hand versus table halfspaces only",
"limitations": ["Saved inputs are not live hardware state or contact-force measurements.",
"Self/other-arm/cup/other-object collisions and contact sliding/friction are not certified.",
"Unknown table sides, underside/legs and calibration uncertainty are not assumed free.",
"Proof applies only to fixed-hand joint-linear segments, not actual controller interpolation.",
"RealArm safety gate stays locked; report is NOT an execution authorization.",
"Partial/search-failed paths must never be sent to a robot."]})
output = args.out or ROOT / "data/left_cup_current" / (
"table_escape_synthetic_demo.json" if args.demo else "table_escape_start_report.json")
output.parent.mkdir(parents=True, exist_ok=True)
output.write_text(json.dumps(report, indent=2, ensure_ascii=False, allow_nan=False) + "\n")
print("status:", report["status"])
for body in report["initial"]["bodies"]:
print(f" {body['body']}: {1000*body['clearance_lower_bound_m']:+.2f} mm ({body['plane']})")
if "verification" in report:
print("endpoint minimum:", 1000*min(report["verification"]["endpoint_body_gaps_m"].values()), "mm")
print("checked steps:", report.get("steps", 0))
print(output)
# All statuses are successful diagnostics. A blocked motion proposal does
# not prevent the program starting or saving actionable evidence.
return report
if __name__ == "__main__":
main()