68 lines
3.6 KiB
Python
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()
|