116 lines
6.5 KiB
Python
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()
|