Linslab's picture
Move frozen Astra policies to astra/<task>/ (BuildTent-v1 .. DualArmFoldHandKerchief-v1)
592ea41 verified
Raw History Blame Contribute Delete
9.38 kB
"""Non-neural demonstration path tracking with legal RGB layout selection."""
from pathlib import Path
import numpy as np
import numpy as np
OFFSETS=[(0,0,.333),(0,0,0),(0,-.316,0),(.0825,0,0),(-.0825,.384,0),(0,0,0),(.088,0,0)]
RX=[0,-np.pi/2,np.pi/2,np.pi/2,-np.pi/2,np.pi/2,np.pi/2]
def fk(q):
q=np.asarray(q);q=q.reshape(-1,q.shape[-1]);T=np.tile(np.eye(4),(len(q),1,1))
for j,(off,rx) in enumerate(zip(OFFSETS,RX)):
c,s=np.cos(rx),np.sin(rx);F=np.eye(4);F[:3,3]=off;F[:3,:3]=[[1,0,0],[0,c,-s],[0,s,c]]
R=np.tile(np.eye(4),(len(q),1,1));c,s=np.cos(q[:,j]),np.sin(q[:,j]);R[:,0,0]=c;R[:,0,1]=-s;R[:,1,0]=s;R[:,1,1]=c
T=T@F@R
return T[:,:3,3]+.107*T[:,:3,2]
import cv2,numpy as np
import numpy as np,cv2
def locate(rgb):
im=cv2.resize(np.asarray(rgb),(240,320)).astype(np.float32);r,g,b=im.transpose(2,0,1)
masks={'green':(g>r*1.16)&(g>b*1.12)&(g>65),'yellow':(np.minimum(r,g)>160)&(b<np.minimum(r,g)-5)&(np.abs(r-g)<12),'cyan':(np.minimum(g,b)>160)&(r<np.minimum(g,b)-3)&(np.abs(g-b)<12),'pink':(np.minimum(r,b)>160)&(g<np.minimum(r,b)-3)&(np.abs(r-b)<12)}
out={}
for name,mask in masks.items():
mask[:70]=False;mask[:,:100]=False
n,labels,stats,cents=cv2.connectedComponentsWithStats(mask.astype('uint8'))
good=np.flatnonzero(stats[:,4]>20);good=good[good!=0]
if len(good):
biggest=good[np.argmax(stats[good,4])];dist=cents[good]-cents[biggest]
keep=(np.linalg.norm(dist,axis=1)<100) if name=='green' else ((np.abs(dist[:,0])<50)&(np.abs(dist[:,1])<22))
good=good[keep]
mask=np.isin(labels,good)
pts=np.column_stack(np.nonzero(mask))[:,::-1].astype(np.float32)
if len(pts):
rect=cv2.minAreaRect(pts);out[name]={'center':list(rect[0]),'size':list(rect[1]),'angle':rect[2],'box':cv2.boxPoints(rect).tolist(),'pixels':len(pts)}
return out
def orientation(loc):
vals=[]
for c in ['green','yellow','cyan','pink']:
d=loc[c];angle=np.deg2rad(d['angle']+(90 if d['size'][0]<d['size'][1] else 0))
vals.extend([np.cos(2*angle),np.sin(2*angle)])
return np.array(vals)
def strips(loc):
d=loc['green'];angle=np.deg2rad(d['angle']+(90 if d['size'][0]<d['size'][1] else 0));v=np.array([np.cos(angle),np.sin(angle)])
if v[1]<0:v=-v
return np.array(d['center'])+np.arange(-1,2)[:,None]*max(d['size'])/3*v
def finger_shape(hand):
angles=np.asarray(hand)[[1,2,3,5,6,7,9,10,11]].reshape(3,3).mean(axis=0)
cumulative=np.cumsum(angles);weights=np.array([.45,.30,.25])
return float(weights@np.cos(cumulative)),float(weights@np.sin(cumulative))
class Controller:
def __init__(self):
self.data=np.load(Path(__file__).with_name('paths.npz'),allow_pickle=False)
self.reset_episode()
def reset_episode(self):
self.phase=0.;self.demo=None;self.waits=0;self.tick=0
self.corrected=[False]*3;self.correction_steps=[0]*3
self.released=[False]*3;self.release_state=None
self.grasp_pose=[None]*3
def release_action(self,q,pos,obs):
state=self.release_state;state['steps']+=1
order=[4,0,8,12,5,1,9,13,6,2,10,14,3,7,11,15]
action=np.zeros(22,dtype=np.float32)
hand=q[7:][order];action[6:]=np.clip(-.35*hand,-1,1)
if state['stage']==0:
reach0,height0=state['shape0']
def movement(h):
reach,height=finger_shape(h)
return np.array([0,.05*np.clip((reach-reach0)/max(1-reach0,.05),0,1),.06*np.clip((height0-height)/max(height0,.05),0,1)])
now=movement(hand);future=movement(hand+.095*action[6:])
goal=state['origin']+now
action[:3]=np.clip((future-now)/.04+5*(goal-pos),-.15,.15)
action[:2]=(future-now)[:2]/.04
live=locate(np.asarray(obs['rgb']['top_camera']).squeeze()).get(['yellow','cyan','pink'][state['cloth']])
if live is not None and live['pixels']>=350 and min(live['size'])>=20:
error=(self.target_pixels[state['cloth']]-np.asarray(live['center']))@self.data['pixel_map']
action[:2]+=5*error
action[:2]=np.clip(action[:2],-.09,.09)
offset=pos[:2]-state['origin'][:2]
for axis,(lower,upper) in enumerate([(-.06,.06),(-.04,.16)]):
if (offset[axis]<=lower and action[axis]<0) or (offset[axis]>=upper and action[axis]>0):action[axis]=0
if state['steps']>=80:
extra=np.clip(pos[:2]-state['origin'][:2],[-.08,-.06],[.08,.18]);resume=state['resume'];j=state['cloth']
shift=np.zeros((len(self.p),2))
if j<2:
pickup=int(self.groups[j+1][0])-60
shift[resume:pickup]=np.linspace(extra,0,pickup-resume)
else:shift[resume:]=extra
self.p[:,:2]+=shift;self.a[:,:2]+=np.diff(shift,axis=0)/.04
state['stage']=1;state['steps']=0;state['goal']=pos.copy();state['goal'][2]=self.p[resume,2]
else:
action[:3]=np.clip(5*(state['goal']-pos),-.15,.15)
if np.linalg.norm(state['goal']-pos)<.008 or state['steps']>=40:
self.phase=float(state['resume']);self.release_state=None
return action
def act(self,obs):
self.tick+=1
if int(obs.get('step_id',self.tick))>=1500:return None
q=np.asarray(obs['proprioception']['agent/qpos']).reshape(-1)
if self.demo is None:
loc=locate(np.asarray(obs['rgb']['top_camera']).squeeze())
layout=np.array([v for c in ['green','yellow','cyan','pink'] for v in loc[c]['center']])
self.demo=int(np.argmin(np.mean((self.data['layouts']-layout)**2,axis=1)+12.5*np.sum((self.data['orientations']-orientation(loc))**2,axis=1)))
self.a=self.data[f'a{self.demo}'];self.q=self.data[f'q{self.demo}'];self.p=self.data[f'p{self.demo}'].copy()
self.a=self.a.copy()
delta=(layout-self.data['layouts'][self.demo]).reshape(4,2)@self.data['pixel_map']
pattern_delta=np.clip((strips(loc)-self.data['strips'][self.demo])@self.data['pixel_map'],-.06,.06)
delta=np.clip(delta,-.045,.045)
delta[1:,1]+=.03
hit=np.flatnonzero(self.a[:,1]>.02)
groups=[g for g in np.split(hit,np.flatnonzero(np.diff(hit)>10)+1) if len(g)>30][:3]
self.groups=groups;self.target_pixels=strips(loc)
knots=[0]; offsets=[np.zeros(2)]
for j,g in enumerate(groups):
start,end=int(g[0]),int(g[-1]+1)
knots.extend([start-60,start,end,end+70]);offsets.extend([delta[j+1],delta[j+1],pattern_delta[j],pattern_delta[j]])
knots.append(len(self.p)-1);offsets.append(pattern_delta[-1])
shift=np.stack([np.interp(np.arange(len(self.p)),knots,np.array(offsets)[:,axis]) for axis in range(2)],axis=1)
self.p[:,:2]+=shift
self.a[:,:2]+=np.diff(shift,axis=0)/.04
i=min(int(self.phase),len(self.a)-1)
if self.phase>=len(self.a)+40:return None
if self.phase>=len(self.a):
self.phase+=1;return np.zeros(22,dtype=np.float32)
pos=fk(q)[0]
if self.release_state is not None:return self.release_action(q,pos,obs)
order=[4,0,8,12,5,1,9,13,6,2,10,14,3,7,11,15]
for j,g in enumerate(self.groups):
end=int(g[-1]+1)
if not self.corrected[j] and end-1<=i<end+5:
live=locate(np.asarray(obs['rgb']['top_camera']).squeeze()).get(['yellow','cyan','pink'][j])
dy=(live['center'][1]-(self.target_pixels[j,1]+12)) if live is not None else 0
dx=(live['center'][0]-(self.target_pixels[j,0]+4)) if live is not None else 0
lateral=live is not None and live['pixels']>=650 and min(live['size'])>=20 and abs(dx)<30
ax=float(np.clip(-.024*dx,-.06,.06)) if lateral and abs(dx)>3 else 0.
if abs(pos[0]-self.p[i,0])>=.06 and ax*(pos[0]-self.p[i,0])>0:ax=0.
if live is not None and live['pixels']>=100 and -20<dy<65 and (dy>3 or ax!=0) and self.correction_steps[j]<64:
action=np.zeros(22,dtype=np.float32)
action[0]=ax
action[1]=np.clip(.025*dy,0,.09) if dy>3 else 0.
action[2]=np.clip(5*(self.p[i,2]-pos[2]),-.09,.09)
target=self.grasp_pose[j] if self.grasp_pose[j] is not None else self.data[f'hand_target{self.demo}'][i]
action[6:]=np.clip(.5*(target-q[7:][order]),-1,1)
self.correction_steps[j]+=1
return action
self.corrected[j]=True
extra=np.clip(pos[:2]-self.p[i,:2],[-.07,0],[.07,.24])
shift=np.zeros((len(self.p),2))
if j<2:
release=end+70;pickup=int(self.groups[j+1][0])-60
shift[i:release]=extra
shift[release:pickup]=np.linspace(extra,0,pickup-release)
else:shift[i:]=extra
self.p[:,:2]+=shift;self.a[:,:2]+=np.diff(shift,axis=0)/.04
if self.corrected[j] and not self.released[j] and end-1<=i<end+5:
self.released[j]=True
resume=min(end+80,len(self.a)-1)
self.release_state={'stage':0,'steps':0,'goal':pos.copy(),'origin':pos.copy(),'shape0':finger_shape(q[7:][order]),'resume':resume,'cloth':j}
return self.release_action(q,pos,obs)
err=self.p[i]-pos
speed=1.0
if self.a[i,1]<-.015 or self.a[i,2]>.015:speed=1.8
if i<75:speed=1.5
if any(int(g[0])-65<=i<=int(g[-1])+35 for g in self.groups):speed=1.0
a=self.a[i].copy()*speed
a[:3]=np.clip(a[:3]+5.*err,-.22,.22)
gain=float(self.data[f'hand_gain{self.demo}'][i])
if gain>.4:gain=1.0
a[6:]=np.clip(gain*(self.data[f'hand_target{self.demo}'][i]-q[7:][order]),-1,1)
for j,g in enumerate(self.groups):
if int(g[0])<=i<=int(g[-1]):
if self.grasp_pose[j] is None:self.grasp_pose[j]=q[7:][order].copy()
a[6:]=np.clip(.5*(self.grasp_pose[j]-q[7:][order]),-1,1)
# Bound both pose holds and image-guided extensions within the native horizon.
if np.linalg.norm(err)>.02 and self.waits<120:self.waits+=1
else:self.phase+=speed
return a.astype(np.float32)
_controller=None
def reset_episode():
global _controller
if _controller is None:_controller=Controller()
else:_controller.reset_episode()
def act(obs):
global _controller
if _controller is None:reset_episode()
return _controller.act(obs)