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

91 lines
5.4 KiB
Python

"""Offline near-edge diagnostic from measured edge + saved robot state.
No controller import/connection. Does not change the execution safety gate.
"""
import argparse
import json
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation as R
from search_cup_poses_offline import ROOT,PoseGuard,hand_cloud
from table_edge_constraint import TableEdgeConstraint
class EdgePoseGuard(PoseGuard):
def geometry(self,q):
T=np.eye(4)
for i,(a,(O,axis,v,lo,hi)) in enumerate(zip(q,self.chain),1):
if not lo<=a<=hi:raise ValueError('Arm joint limit violation')
A=np.eye(4);A[:3,:3]=R.from_rotvec(axis*a).as_matrix();T=T@O@A
yield f'L{i}_Link',v@T[:3,:3].T+T[:3,3],None
yield 'adapter_envelope',T[:3,:3]@self.adapter_center+T[:3,3],self.adapter_radius
yield 'conditional_hand_preshape',(self.cloud@self.mount.T+self.hand_center)@T[:3,:3].T+T[:3,3],None
def clearance(self,q):
worst=(float('inf'),'')
for name,p,r in self.geometry(q):
gap=self.edge.hull_clearance(p) if r is None else self.edge.sphere_clearance(p,r)
ymin,ymax=(p[:,1].min(),p[:,1].max()) if r is None else (p[1]-r,p[1]+r)
# Outside the observed edge span, do not infer that below-table
# space is free. Revert to the conservative top-plane constraint.
if ymin<self.observed_y[0] or ymax>self.observed_y[1]:
gap=(float(((p-self.edge.top_point)@self.edge.top_normal).min()) if r is None
else float((p-self.edge.top_point)@self.edge.top_normal)-r)-self.edge.uncertainty_m
if gap<worst[0]:worst=(gap,name)
return worst
def main():
ap=argparse.ArgumentParser();ap.add_argument('--edge-json',type=Path,required=True)
ap.add_argument('--snapshot',type=Path,required=True);a=ap.parse_args()
e=json.loads(a.edge_json.read_text());state=json.loads(a.snapshot.read_text())['states']['left']
q=np.asarray(state['joint_state']['position']);cap=ROOT/'data/left_cup_current'
saved=json.loads((cap/'cup_yaw_plan.json').read_text())['solutions'][0]
guard=EdgePoseGuard(e['top_normal_base'],e['top_point_base_m'])
guard.mount=np.asarray(saved['mount_rotation']);guard.cloud=hand_cloud(saved['grasp']['hand_pre'])
guard.edge=TableEdgeConstraint(e['top_normal_base'],e['top_point_base_m'],e['inward_normal_base'],e['edge_point_base_m'],
diagnostic=True,uncertainty_m=e.get('uncertainty_m',.01))
guard.observed_y=e['observed_y_range_m']
report={'executable':False,'motion_sent':False,'snapshot':str(a.snapshot),'edge_evidence':e,
'scope':'one observed near-edge; unknown sideways/far/bottom extents remain conservative',
'hand_assumption':'planned preshape, actual hand state not read','initial_bodies':[]}
for name,p,r in guard.geometry(q):
if r is None:
top=(p-guard.edge.top_point)@guard.edge.top_normal
outward=(p-guard.edge.edge_point)@guard.edge.outward_normal
report['initial_bodies'].append(dict(name=name,plane_min_m=float(top.min()),
edge_clearance_bound_m=guard.edge.hull_clearance(p),
edge_extrapolation_required=bool(p[:,1].min()<guard.observed_y[0] or p[:,1].max()>guard.observed_y[1]),
y_range_m=[float(p[:,1].min()),float(p[:,1].max())],
vertices_inside_extrapolated_forbidden_volume=int(np.sum((top<0)&(outward<0)))))
else:
report['initial_bodies'].append(dict(name=name,edge_clearance_bound_m=guard.edge.sphere_clearance(p,r)))
rows=json.loads((cap/'other_poses_offline_report.json').read_text())['candidates']
chosen=next(c for c in rows if c['raise_m']==.04 and c['yaw_relative_saved_deg']==0 and c['status']=='table_segment_pass')
target=np.asarray(chosen['pregrasp_joints']);report['lift_candidates']=[]
try:report['direct_transit']=guard.check_segment(q,target)
except (RuntimeError,ValueError) as exc:report['direct_transit']={'status':'rejected','reason':str(exc)}
gap,body=guard.clearance(q)
if gap<guard.margin:
report['lift_search']='not_run: initial model clearance unresolved; cannot validate path by ignoring its start'
else:
T=guard.fk(q);p=T[:3,:3]@guard.hand_center+T[:3,3]
for dz in (.08,.12,.16):
row={'lift_m':dz};way=guard.solve(p+[0,0,dz],T[:3,:3],[q,target])
if way is None:row['status']='local_ik_not_found'
else:
try:
row.update(up=guard.check_segment(q,way),to_pregrasp=guard.check_segment(way,target),
status='conditional_table_pass',waypoint_joints=way.tolist())
except (RuntimeError,ValueError) as exc:row.update(status='rejected',reason=str(exc))
report['lift_candidates'].append(row)
report['limitations']=['Negative lower bound need not mean physical collision.',
'Observed near-edge segment does not establish invisible side/far/bottom table geometry.',
'Saved calibration, mount orientation and current hand posture still need consistency.',
'No self/other-arm/scene collision or actual controller trajectory check. Execution remains disabled.']
out=ROOT/'data/table_edge_check_20260924_01/start_edge_audit.json'
out.write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2))
if __name__=='__main__':main()