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

90 lines
5.3 KiB
Python

"""Audit saved fixed-hand paths against an evidence-backed finite slab.
No hardware. Missing dimensions -> unknown_geometry, NOT collision or pass.
Every segment gets a report even if the start cannot be certified.
"""
import argparse
import json
import tempfile
from pathlib import Path
import numpy as np
import trimesh
from scipy.spatial import ConvexHull
from finite_table_constraint import FiniteTable,FiniteContactEscapeChecker
from table_contact_escape import EscapeRejected
from table_edge_constraint import check_joint_linear_segment
from table_escape_geometry import EscapeGeometry,ADAPTER_MESH
from offline_table_guard import ROOT,TableGuard
from check_index_board_hover import hand_geometry
class JsonHandGeometry(EscapeGeometry):
"""Reuse analytic arm Jacobians, with all21 JSON-derived fixed hand angles."""
def __init__(self,raw):
template=TableGuard([0,0,1],[0,0,0]);self.chain=template.chain
self.lo=np.array([j[3] for j in self.chain]);self.hi=np.array([j[4] for j in self.chain])
mount=json.loads((ROOT/'config/left_hand_mount.json').read_text())
rotation=np.array(mount['rotation_Arm_Tip_to_hand'])
self._local_points={f'L{i}_Link':j[2].copy() for i,j in enumerate(self.chain,1)}
_,hand,_=hand_geometry(raw);hand=hand[ConvexHull(hand).vertices]
self._local_points['hand']=hand@rotation.T+mount['translation_L7_to_hand_m']
adapter=np.asarray(trimesh.load_mesh(ADAPTER_MESH,process=False).vertices)
adapter=adapter[ConvexHull(adapter).vertices]
self._local_points['adapter']=adapter@rotation.T+[-.028,0,-.0525]
self.reach=max(template.reach,sum(np.linalg.norm(j[0][:3,3]) for j in self.chain)+
max(float(np.linalg.norm(p,axis=1).max()) for p in self._local_points.values()))
def audit(plan,spec,geometry=None):
path=np.array(plan['waypoints'],float)
if path.ndim!=2 or path.shape[1]!=7 or len(path)<2 or not np.isfinite(path).all():raise ValueError('Invalid Nx7 path')
fields=['top_center_base_m','rotation_table_to_base','size_xyz_m','uncertainty_m']
missing=[key for key in fields if spec.get(key) is None]
report=dict(executable=False,motion_sent=False,model='finite_slab',missing_measurements=missing,
segments=[],physical_collision_proven=False)
if missing or spec.get('evidence_verified') is not True:
report['status']='unknown_geometry'
report['segments']=[dict(index=i,status='unknown_geometry') for i in range(len(path)-1)]
return report
if spec.get('units')!='m':raise ValueError('Only metres supported')
table=FiniteTable(spec[fields[0]],spec[fields[1]],spec[fields[2]],uncertainty_m=spec[fields[3]],
verified=True,source=spec['source'])
if geometry is None:raise ValueError('Fixed-hand geometry required')
checker=FiniteContactEscapeChecker(geometry,table)
initial=checker.bind(path[0]);report['initial']=initial
report['initial_bodies']={name:table.classify(row['points']) for name,row in geometry.sample(path[0]).items()}
phase='escape' if initial['status']=='escape_required' else 'normal'
for i,(start,end) in enumerate(zip(path,path[1:])):
row=dict(index=i,phase=phase)
try:
if phase=='escape':
row['check']=checker.check_segment(start,end)
if min(checker.current_gaps(end).values())>=checker.policy.margin_m:phase='normal'
else:
row['check']=check_joint_linear_segment(start,end,checker.normal_clearance,
reach_bound_m=geometry.reach,margin_m=checker.policy.margin_m)
row['status']='table_segment_pass_offline'
except (RuntimeError,ValueError) as exc:row.update(status='not_verified',reason=str(exc))
report['segments'].append(row)
report['normal_margin_restored']=checker.normal_clearance(path[-1])[0]>=checker.policy.margin_m
report['status']='table_path_pass_offline' if all(r['status']=='table_segment_pass_offline' for r in report['segments']) and report['normal_margin_restored'] else 'not_verified'
report['scope']='Fixed hand, tabletop slab, joint-linear segments only; no controller or other obstacles.'
return report
def main():
parser=argparse.ArgumentParser(description=__doc__)
parser.add_argument('--table',type=Path,default=ROOT/'config/finite_table_geometry.json')
parser.add_argument('--plan',type=Path,default=ROOT/'data/start_evidence_20260927T053837893105Z/elbow_raise_proposal.json')
args=parser.parse_args();spec=json.loads(args.table.read_text());plan=json.loads(args.plan.read_text())
geometry=None
if spec.get('evidence_verified') is True and all(spec.get(k) is not None for k in ['top_center_base_m','rotation_table_to_base','size_xyz_m','uncertainty_m']):
raw=json.loads((ROOT/'data/left_hand_open_q10r1mak/result.json').read_text())['final_feedback']
geometry=JsonHandGeometry(raw)
report=audit(plan,spec,geometry);report.update(table_source=str(args.table),plan_source=str(args.plan))
out=Path(tempfile.mkdtemp(prefix='finite_table_audit_',dir=ROOT/'data'))/'report.json'
out.write_text(json.dumps(report,indent=2));print(report['status']);print('Missing:',report['missing_measurements']);print(out)
if __name__=='__main__':main()