File size: 3,106 Bytes
e25638c | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 | """Geometric feedback control compiled from this task's D20 demonstrations."""
from pathlib import Path
import importlib.util
import numpy as np
import cv2
_spec=importlib.util.spec_from_file_location('_kin',Path(__file__).with_name('kinematics.py'));_k=importlib.util.module_from_spec(_spec);_spec.loader.exec_module(_k)
_phase=0;_ticks=0;_calls=0;_progress=0;_grasp=None;_place=None
SCALE=396.5
def reset_episode():
global _phase,_ticks,_calls,_progress,_grasp,_place
_phase=0;_ticks=0;_calls=0;_progress=0;_grasp=None;_place=None
def locate(rgb):
a=cv2.resize(np.asarray(rgb).squeeze(),(480,640)).astype(float)
cyan=((a[:,:,1]-a[:,:,0]>25)&(a[:,:,2]-a[:,:,0]>25)).astype('uint8');cyan[:210]=0;cyan[440:]=0
n,l,stats,cent=cv2.connectedComponentsWithStats(cyan)
i=1+np.argmax(stats[1:,4]);ys,xs=np.where(l==i);boundary=np.array([(xs.min()+xs.max())/2,(ys.min()+ys.max())/2])
white=((a.min(2)>180)&(a.max(2)-a.min(2)<50)).astype('uint8');white[:210]=0;white[440:]=0
n,l,stats,cent=cv2.connectedComponentsWithStats(white);i=1+np.argmax(stats[1:,4]);ys,xs=np.where(l==i)
rect=cv2.minAreaRect(np.column_stack([xs,ys]).astype('float32'));center=np.array(rect[0]);angle=rect[2]
if angle>45:angle-=90
return center,boundary,np.deg2rad(angle)
def offset(pos,delta):
p=pos.copy();p[0,0]+=delta[1]/SCALE;p[1,0]-=delta[1]/SCALE;p[0,1]+=delta[0]/SCALE;p[1,1]-=delta[0]/SCALE
return p
def act(obs):
global _phase,_ticks,_calls,_progress,_grasp,_place
if int(obs.get('step_id',_calls+1))>=200:return None
_calls+=1
try:
cur=np.array([_k.fk(np.asarray(obs['proprioception'][f'agent/panda-{i}/qpos']).reshape(-1)) for i in range(2)])
if _grasp is None:
center,boundary,theta=locate(obs['rgb']['top_camera'])
with np.load(Path(__file__).with_name('pose_bank.npz')) as b:
gs=np.array([offset(p,center-c) for p,c in zip(b['place'],b['boundary'])]);ps=np.array([offset(p,boundary-c) for p,c in zip(b['place'],b['boundary'])])
_grasp=np.median(gs,axis=0);_place=np.median(ps,axis=0)
for i,dy in enumerate([-.08,.08]):
delta=np.array([-np.sin(theta)*dy,(np.cos(theta)-1)*dy])*SCALE
tmp=offset(_grasp,delta);_grasp[i]=tmp[i]
_grasp[:,2]=.124;_place[:,2]=.124
except (KeyError,ValueError,IndexError):return None
_ticks+=1;_progress=float(_phase*20+_ticks)
target=_grasp.copy();grip=1.;limit=.15
if _phase==0:target[:,2]=.24
elif _phase==1:pass
elif _phase==2:grip=-1
elif _phase==3:target[:,2]=.20;grip=-1;limit=.18
elif _phase==4:target=_place.copy();target[:,2]=.20;grip=-1;limit=.18
elif _phase==5:target=_place.copy();grip=-1;limit=.095
elif _phase==6:target=_place.copy();limit=.095
elif _phase==7:target=_place.copy();target[:,2]=.30;limit=.12
else:return None
err=target-cur
a=np.zeros(14,dtype=np.float32)
for i in range(2):
e=err[i];d=np.array([e[1],-e[0],e[2]]) if i==0 else np.array([-e[1],e[0],e[2]])
a[i*7:i*7+3]=np.clip(d*5,-limit,limit);a[i*7+6]=grip
# Pose convergence, with bounded dwell and phase timeout.
done=np.max(np.abs(err))<.006
if _phase in (2,6):done=_ticks>=10
if (done and _ticks>=3) or _ticks>65:
_phase+=1;_ticks=0
return a
|