33 lines
2.3 KiB
Python
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()
|