91 lines
5.4 KiB
Python
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()
|