"""D20 geometric visual localization and proprioceptive pose servo, offline only.""" from pathlib import Path import importlib.util import numpy as np _ROOT=Path(__file__).resolve().parent _s=importlib.util.spec_from_file_location('_geom',_ROOT/'geometry.py');g=importlib.util.module_from_spec(_s);_s.loader.exec_module(g) with np.load(_ROOT/'geometry.npz') as z:B=z['pixel_to_fk'];P=z['poses'];A=z['actions'] _last_phase=None def reset_episode(): global state,age,objects,pick,idx,last_step,last_action,_last_phase,retries,end_age state=0;age=0;objects=None;pick=0;idx=275;retries=0;end_age=0;last_step=None;last_action=None;_last_phase=0 reset_episode() def act(obs): global state,age,objects,pick,idx,last_step,last_action,_last_phase,retries,end_age step=int(obs['step_id']) if step>=700:return None if step==last_step:return last_action.copy() q=[np.asarray(obs['proprioception']['agent/panda-%d/qpos'%r]).reshape(-1) for r in range(2)] cur=[g.fk(x) for x in q] if objects is None: objects=sorted(g.detect(obs['rgb']['top_camera'],B),key=lambda o:o[0][0]) if len(objects)<2: objects=[(np.array([.5,.1]),0.),(np.array([.7,-.1]),0.)] objects=objects[:2] a=np.zeros(14,dtype=np.float32);a[[6,13]]=1 if state<9: xy,yaw=objects[pick];dest=.55 if pick==0 else .48 place_yaw=yaw+((.785-yaw+np.pi/2)%np.pi-np.pi/2) if state==0 and age==0: seen=g.detect(obs['rgb']['top_camera'],B) if seen: nearest=min(seen,key=lambda o:np.linalg.norm(o[0]-xy)) if np.linalg.norm(nearest[0]-xy)<.055: objects[pick]=nearest;xy,yaw=nearest grasp_z=.123-.006*retries if state==0:target=g.pose([*xy,.27],yaw) elif state in [1,2]:target=g.pose([*xy,grasp_z],yaw) elif state==3:target=g.pose([*xy,.27],yaw) elif state==4:target=g.pose([dest,0,.27],yaw) elif state==5:target=g.pose([dest,0,.27],place_yaw) elif state in [6,7]:target=g.pose([dest,0,.201],place_yaw) else:target=g.pose([dest,0,.36],place_yaw if pick==0 else -.785) u,dist,ang=g.servo(cur[1],target);a[7:13]=u a[13]=-1 if 2<=state<=6 else 1 age+=1 dwell=14 if state in [2,7] else 2 reached=dist<.008 and ang<.065 if (reached and age>=dwell) or age>=75: if state==2 and min(q[1][7:])<.012 and retries<1: retries+=1;state=0;age=0 else: state+=1;age=0 if state==9 and pick==0:pick=1;state=0;retries=0 _last_phase=pick*9+state else: # Advance the final demonstrated bilateral maneuver using current pose feedback. if idx>=len(P)-1: end_age+=1 if end_age>=35:return None target=P[min(idx+2,len(P)-1)] errors=[] for r in range(2): u,d,ang=g.servo(cur[r],target[r]); if r==0:u[[0,1,3,4]]*=-1 a[r*7:r*7+6]=u;a[r*7+6]=A[min(idx,len(A)-1),r*7+6];errors.append((d,ang)) age+=1 if max(e[0] for e in errors)<.018 and max(e[1] for e in errors)<.18 or age>=8: idx=min(idx+2,len(P)-1);age=0 _last_phase=100+idx last_step=step;last_action=a.copy();return a