"""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