Linslab's picture
Move frozen Astra policies to astra/<task>/ (DualArmHammerHandover-v1 .. DualArmPushBox-v1)
67e0b67 verified
Raw History Blame Contribute Delete
2.91 kB
"""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