58 lines
3.0 KiB
Python
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()
|