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

33 lines
2.3 KiB
Python

"""Offline start-contact escape in the display workcell with 5mm clearance."""
import json
from pathlib import Path
from types import SimpleNamespace
from dataclasses import asdict
import xml.etree.ElementTree as ET
import numpy as np
from table_escape_geometry import EscapeGeometry,HAND_URDF
from table_contact_escape import EscapePolicy,plan_escape
from finite_table_constraint import FiniteTable,FiniteContactEscapeChecker
ROOT=Path(__file__).resolve().parents[1]
def main():
meta=json.loads((ROOT/'scene/measured_layout.json').read_text());cfg=meta['layout'];work=json.loads((ROOT/'config/cup_workcell.json').read_text())
hand={j.get('name'):0. for j in ET.parse(HAND_URDF).getroot().findall('joint') if j.find('mimic') is None}
geo=EscapeGeometry(hand)
state=json.loads((ROOT / meta['snapshot_file']).read_text())['states']['left']['joint_state']
names=[x.upper() for x in state['name']];q=np.array([state['position'][names.index(f'L{i}_JOINT')] for i in range(1,8)])
measured=cfg['measured'];est=cfg['estimated_visual_only'];near=measured['column_xy_m'][0]/2+measured['column_front_to_table_near_m']
table=FiniteTable([near+est['table_depth_m']/2,0,measured['table_top_height_m']-meta['controller_base_height_m']],np.eye(3),[est['table_depth_m'],est['table_width_m'],measured['table_thickness_m']],uncertainty_m=0.,verified=True,source='exact simulated tabletop ONLY; NOT physical table certification')
# Separate simulation policy; do not lower shared real-motion guard defaults.
fields=asdict(EscapePolicy());fields['margin_m']=work['table_clearance_m'];fields['max_steps']=250;fields['max_total_joint_travel_rad']=1.5
policy=SimpleNamespace(**fields)
checker=FiniteContactEscapeChecker(geo,table,policy=policy)
report=plan_escape(checker,q)
report.update({'margin_m':policy.margin_m,'hardware_connected':False,'executable_on_hardware':False,
'scope':'continuous finite-table checks for fixed nominal hand; no self/cup/other-arm validation',
'snapshot':meta['snapshot_file'],'table_top_base_m':float(table.center[2]),'table_evidence':'simulation dimensions only'})
out=ROOT/'data/cup_workcell';out.mkdir(exist_ok=True)
(out/'escape_report.json').write_text(json.dumps(report,indent=2))
print(report['status']);print(report.get('initial'));print(out/'escape_report.json')
if __name__=='__main__':main()