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

58 lines
3.0 KiB
Python

"""Read-only cup approach search about the HUG contact centroid; no motion."""
import json
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation as R
from real_arm import RealArm
from hand_table_check import check_candidate
ROOT=Path(__file__).resolve().parents[1]
def main():
cap=ROOT/'data/left_cup_current'
report=json.loads((cap/'cup_plan_report.json').read_text())
audit=json.loads((ROOT/'data/left_ring_current/left_plan_audit.json').read_text())
mount=np.array(audit['mount_rotation']);data=np.load(cap/'hug_cup.npz')
arm=RealArm('left',table_z=report['table_z']);valid=[]
try:
state=arm.read()
for item in report['candidates']:
if 'geometry' not in item:continue
g=json.loads((cap/item['source']).read_text())
for field,name,old,new in item['numerical_limit_corrections']:g[field][name]=new
center=data['landmarks_world'][g['sample'],[4,8,12,16,20]].mean(0)
original_R=R.from_quat(np.roll(g['wrist_quat_wxyz'],-1)).as_matrix()
for yaw in (90,-90,180,45,-45,135,-135):
turn=R.from_euler('z',yaw,degrees=True).as_matrix()
pos=center+turn@(np.array(g['wrist_pos'])-center)
hand_R=turn@original_R; flange_R=R.from_matrix(hand_R@mount.T)
approach=turn@np.array(g['approach']);approach/=np.linalg.norm(approach)
jg=arm.ik(pos,flange_R,state['joints'])
if jg is None:continue
for distance in (.10,.07,.05,.03):
pre=pos-distance*approach;jp=arm.ik(pre,flange_R,jg)
if jp is None:continue
candidate=dict(g);candidate['wrist_pos']=pos.tolist()
candidate['wrist_quat_wxyz']=np.roll(R.from_matrix(hand_R).as_quat(),1).tolist()
candidate['approach']=approach.tolist()
geometry=check_candidate(candidate,'left',report['table_z'],distance,margin=.01)
result={'source':item['source'],'yaw_adjustment_deg':yaw,'clearance':distance,
'contact_centroid_base':center.tolist(),'grasp':candidate,'geometry':geometry,
'pregrasp_joints':jp.tolist(),'grasp_joints':jg.tolist(),
'mount_rotation':mount.tolist(),'executed':False,
'limitations':'mount assumption; no cup collision/contact or full arm transit check'}
fk=arm.fk(jp)
if fk is None or np.linalg.norm(fk-pre)>.005:continue
valid.append(result)
print('FOUND',item['source'],'yaw',yaw,'clearance',distance,'pre',pre.tolist(),flush=True)
break
if valid:break
if valid:break
finally:arm.close()
(cap/'cup_yaw_plan.json').write_text(json.dumps({'executed':False,'solutions':valid},indent=2))
if not valid:raise SystemExit('No reachable yaw-adjusted candidate')
if __name__=='__main__':main()