90 lines
5.3 KiB
Python
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()
|