File size: 6,862 Bytes
8e97cab | 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 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 | """Task-local demo-reference tracking with visible selection and bounded feedback."""
from pathlib import Path
import json,numpy as np,cv2
from visual_features import marker_features,marker_distance
ROOT=Path(__file__).parent
CONFIG=json.loads((ROOT/'settings.json').read_text())
ROBOTS=('panda_leaphand-1','panda_leaphand_left-0')
_STATE={};_BANK=None
def _load():
global _BANK
if _BANK is None:
with np.load(ROOT/'reference_bank.npz') as f:b={k:f[k] for k in f.files}
with np.load(ROOT/'numeric_demos.npz') as f:b['numeric']={k:f[k] for k in f.files}
b['hand_map']=np.load(ROOT/'hand_map.npy');b['hand_sign']=np.load(ROOT/'hand_sign.npy')
b['table_features']=np.load(ROOT/'table_features.npy')
b['markers']=np.load(ROOT/'marker_bank.npy');b['feedback']=np.load(ROOT/'pose_feedback.npy')
with np.load(ROOT/'geometry_feedback.npz') as f:b['geometry']={k:f[k] for k in f.files}
b['indices']=[np.flatnonzero(b['demo_index']==i) for i in range(20)]
b['flip']=[int(np.flatnonzero((np.linalg.norm(b['numeric']['a'+str(i)][:,3:6],axis=1)>.25)&(np.arange(len(b['numeric']['a'+str(i)]))>len(b['numeric']['a'+str(i)])*.65))[0]) for i in range(20)]
b['gate']=[int(np.flatnonzero(b['numeric']['a'+str(i)][:400,23]<-.04)[-1]) for i in range(20)]
_BANK=b
return _BANK
def reset_episode():
_STATE.clear();_STATE.update(last_step=0,t=-1.,was_recovering=False,demo=None,gate_done=False,recovery=0,bad=0,reason='reset',phase=0.)
def image_features(rgb):
return np.concatenate([cv2.resize(np.asarray(rgb[c]).squeeze(),(16,12),interpolation=cv2.INTER_AREA).reshape(-1).astype(np.float32)/255 for c in ['front_camera','left_camera']])
def scene_features(m):
out=[]
for st in [8,20]:
p=m[st:st+4].reshape(2,2)
if np.any(p<0):return None
p=p[np.argsort(p[:,1 if st==20 else 0])];out.extend(p.mean(0));out.extend(p[1]-p[0])
return np.array(out)
def observe_scene(m,reference,previous):
result=np.asarray(previous if previous is not None else reference,np.float32).copy();visible=0
for cam,st in enumerate([8,20]):
p=m[st:st+4].reshape(2,2);p=p[p[:,0]>=0];visible+=len(p);base=result[cam*4:cam*4+4]
if len(p)==2:
p=p[np.argsort(p[:,cam])];result[cam*4:cam*4+4]=np.r_[p.mean(0),p[1]-p[0]]
elif len(p)==1:
endpoints=np.stack([base[:2]-.5*base[2:],base[:2]+.5*base[2:]])
side=int(np.argmin(np.sum((endpoints-p[0])**2,axis=1)))
result[cam*4:cam*4+2]=p[0]+(.5 if side==0 else -.5)*base[2:]
return result,visible
def leg_error(m):
red=m[:8].reshape(4,2);blue=m[8:12].reshape(2,2)
if np.any(red[:,0]<0) or np.any(blue[:,0]<0):return None
upper=red[np.argsort(red[:,1])[:2]]
return float(np.min(upper[:,0])-np.min(blue[:,0]))
def act(obs):
if not _STATE:reset_episode()
step=int(obs['step_id']);elapsed=step-_STATE['last_step']
if step>=min(2400,CONFIG.get('stop_step',2400)):return None
if elapsed<=0:raise ValueError('Non-increasing step')
_STATE['last_step']=step;b=_load();p=obs['proprioception']
q=np.concatenate([np.asarray(p['agent/'+r+'/qpos']).reshape(-1) for r in ROBOTS])
if q.shape!=(46,) or not np.isfinite(q).all():return None
im=image_features(obs['rgb']);marks=marker_features(obs['rgb'])
if _STATE['demo'] is None or step<=9:
ids=np.array([x[min(int((step-1)//8),len(x)-1)] for x in b['indices']])
ds=marker_distance(b['markers'][ids],marks)*CONFIG.get('marker_weight',.1)+np.mean(((b['image'][ids]-im)/.15)**2,1)
_STATE['demo']=int(np.argmin(ds));_STATE['selection_distance']=float(ds.min())
did=_STATE['demo'];a=b['numeric']['a'+str(did)];qref=b['numeric']['q'+str(did)];n=len(a)
if not _STATE['was_recovering']:_STATE['t']+=elapsed*CONFIG.get('speed',1.)
t=int(round(_STATE['t']));gate=b['gate'][did];err=leg_error(marks);_STATE['leg_error']=err
recovering=False
if CONFIG.get('leg_gate',False) and not _STATE['gate_done'] and t>=gate:
if err is not None and err<=CONFIG.get('leg_threshold',.025):_STATE['gate_done']=True
elif _STATE['recovery']>=CONFIG.get('recovery_limit',80):
_STATE['reason']='first-leg recovery exhausted; failed attempt';return None
else:
recovering=True;_STATE['recovery']+=elapsed;t=gate-20
if t>=n:
_STATE['reason']='reference endpoint reached; not a success assertion';return None
j=b['indices'][did][min(t//8,len(b['indices'][did])-1)]
action=a[t].copy();error=qref[t]-q
flipping=t>=b['flip'][did]-4
_STATE['flipping']=bool(flipping)
_STATE['geometry_offset_norm']=0.
geometry_joint_offset=np.zeros(14,np.float32)
if CONFIG.get('geometry_gain',0.) and t>n*.02 and not flipping:
reference=b['table_features'][j]
current,visible=observe_scene(marks,reference,_STATE.get('table_geometry'))
_STATE['table_geometry']=current.tolist();_STATE['geometry_visible_points']=int(visible)
previous=np.asarray(_STATE.get('geometry_offset',[0.]*14),np.float32)
offset=previous
if current is not None and reference is not None:
g=b['geometry'];k=int(np.argmin(abs(g['grid']-g['aligned_phase'][j])))
if g['valid'][k]>=8:
target=CONFIG['geometry_gain']*((current-reference)@g['jac'][k])
target=np.clip(target,-CONFIG.get('geometry_limit',.1),CONFIG.get('geometry_limit',.1))
alpha=1.-.9**elapsed;offset=previous+alpha*(target-previous)
_STATE['geometry_offset']=offset.tolist()
geometry_joint_offset=offset
_STATE['geometry_offset_norm']=float(np.linalg.norm(offset))
for r in range(2):
qs=slice(r*23,r*23+7);aa=slice(r*22,r*22+6)
gain=0. if (r==0 and flipping and CONFIG.get('disable_flip_feedback',True)) else CONFIG.get('pose_gain',.1)
correction=gain*(error[qs]@b['feedback'][j,r])
correction[3:]=0. # Preserve demonstrated wrist orientation commands.
correction[:3]+=gain*(geometry_joint_offset[r*7:r*7+7]@b['feedback'][j,r])[:3]
action[aa]+=np.clip(correction,-CONFIG.get('pose_limit',.02),CONFIG.get('pose_limit',.02))
hs=slice(r*23+7,r*23+23);ha=slice(r*22+6,r*22+22)
action[ha]+=np.clip(CONFIG.get('hand_gain',.1)*error[hs][b['hand_map'][r]]*b['hand_sign'][r]/.1,-CONFIG.get('hand_limit',.05),CONFIG.get('hand_limit',.05))
if recovering:
action[:22]=0
# Continue the demonstrated pull, without pulling back toward an earlier pose.
action[22:25]=a[max(gate-30,0),22:25]
if recovering:_STATE['t']=float(gate)
_STATE['was_recovering']=recovering
_STATE['phase']=float(_STATE['t']/n);_STATE['frame']=t
_STATE['arm_distance']=float(np.mean((error[:7]/b['q_scale'][:7])**2))
support_error=error.copy()
if flipping and CONFIG.get('disable_flip_feedback',True):support_error[:7]=0
_STATE['distance']=float(np.mean((support_error/b['q_scale'])**2))
_STATE['bad']=_STATE['bad']+elapsed if _STATE['distance']>CONFIG.get('pose_abort',3.) else 0
if _STATE['bad']>40:
_STATE['reason']='sustained pose deviation; failed attempt';return None
_STATE['reason']='visible leg recovery' if recovering else 'reference tracking with joint feedback'
return np.clip(action,-1,1).astype(np.float32)
|