Files

68 lines
3.6 KiB
Python

"""Cup candidate geometry + read-only controller IK. Never issues motion."""
import json
from pathlib import Path
import numpy as np
import mujoco
from scipy.spatial.transform import Rotation
from hand_table_check import check_candidate
from real_arm import RealArm
ROOT = Path(__file__).resolve().parents[1]
def main():
cap=ROOT/'data/left_cup_current'
camera=json.loads((cap/'camera.json').read_text())
table=float(camera['table_estimate']['z_at_target'])
audit=json.loads((ROOT/'data/left_ring_current/left_plan_audit.json').read_text())
mount=np.asarray(audit['mount_rotation'])
model=mujoco.MjModel.from_xml_path(str(ROOT/'assets/l20_left_mj.urdf'))
report={'executed':False,'table_z':table,'mount_status':'user-described 45deg with assumed comparison reference; not measured',
'scope':'sampled hand/table meshes and endpoint IK; no full arm path/contact verification',
'candidates':[]}
arm=RealArm('left',table_z=table)
try:
state=arm.read()
for path in sorted(cap.glob('grasp_l20_cup_*.json'),key=lambda p:int(p.stem.split('_')[-1])):
g=json.loads(path.read_text());entry={'source':path.name};report['candidates'].append(entry)
try:
corrections=[]
for field in ('hand_pre','hand_qpos'):
for name,value in g[field].items():
joint=model.joint(name);lo,hi=joint.range
if joint.limited[0] and (value<lo or value>hi):
new=float(np.clip(value,lo,hi))
if abs(new-value)>.0011:raise ValueError(f'joint limit {name}')
g[field][name]=new;corrections.append([field,name,value,new])
entry['numerical_limit_corrections']=corrections
R=Rotation.from_quat(np.roll(g['wrist_quat_wxyz'],-1)).as_matrix()
flange_R=Rotation.from_matrix(R@mount.T)
pos=np.array(g['wrist_pos']);approach=np.array(g['approach']);approach/=np.linalg.norm(approach)
# Check the whole hand at the actual grasp point and along the approach.
entry['geometry']=check_candidate(g,'left',table,.10,margin=.01)
jg=arm.ik(pos,flange_R,state['joints'])
entry['grasp_ik']=jg is not None
if jg is None:continue
entry['grasp_joints']=jg.tolist();entry['pregrasps']=[]
for distance in (.10,.07,.05,.03):
pre=pos-distance*approach
jp=arm.ik(pre,flange_R,jg)
item={'clearance':distance,'position':pre.tolist(),'ik':jp is not None}
entry['pregrasps'].append(item)
if jp is not None:
fk=arm.fk(jp);item['fk_error_m']=None if fk is None else float(np.linalg.norm(fk-pre))
item['joints']=jp.tolist()
entry['normalized_grasp']=g
except (ValueError,RuntimeError,KeyError) as exc:
entry['rejected']=str(exc)
print(path.name,'geometry',entry.get('rejected','pass'),'grasp_ik',entry.get('grasp_ik'),
'pregrasps',[(x['clearance'],x['ik']) for x in entry.get('pregrasps',[])],flush=True)
finally:arm.close()
(cap/'cup_plan_report.json').write_text(json.dumps(report,indent=2))
valid=[c for c in report['candidates'] if any(p['ik'] and p.get('fk_error_m',1)<.005 for p in c.get('pregrasps',[]))]
print('geometry + endpoint IK valid:',len(valid),'/',len(report['candidates']))
if not valid:raise SystemExit(2)
if __name__=='__main__':main()