File size: 2,950 Bytes
c158cda | 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 | """D20 action interpolation with E20-tested wrist lateral alignment.
No environment imports, kinematics, hidden state, seed lookup or neural weights.
"""
from pathlib import Path
import importlib.util
import numpy as np
_spec=importlib.util.spec_from_file_location('_d20_features',Path(__file__).with_name('features.py'))
_features=importlib.util.module_from_spec(_spec)
_spec.loader.exec_module(_features)
_library=None
_cache={}
def reset_episode():
global _cache
_cache={}
def _hold(q):
a=np.zeros(14,np.float32)
for i in range(2):
g=2*(float(q[i][7])+0.01)/0.05-1
if not np.isfinite(g) or not -1<=g<=1:
return None # Never clip an invalid hold into another command.
a[i*7+6]=g
return a
def act(obs):
global _library
step=int(obs['step_id'])
if step < 1 or step >= 250: return None
if _library is None:
with np.load(Path(__file__).with_name('library.npz'),allow_pickle=False) as f:
_library={k:f[k] for k in ('q','v','targets','wrist','actions')}
p=obs['proprioception']
try:
q=[np.asarray(p[f'agent/panda-{i}/qpos'],dtype=np.float32).reshape(-1) for i in range(2)]
v=[np.asarray(p[f'agent/panda-{i}/qvel'],dtype=np.float32).reshape(-1) for i in range(2)]
if any(len(x)!=9 or not np.isfinite(x).all() for x in q+v):return None
targets,wrist=_features.visual(obs['rgb'],_cache)
except (KeyError,ValueError,TypeError):return None
if not np.isfinite(targets).all():return _hold(q)
db=_library
# Baseline demonstration distances, plus the E20 wrist alignment below.
dt=np.mean(((db['targets']-targets)/0.04)**2,axis=1)
result=[]
for i in range(2):
dq=np.mean(((db['q'][:,i]-q[i][:7])/0.18)**2,axis=1)
dv=np.mean(((db['v'][:,i]-v[i][:7])/1.0)**2,axis=1)
w=db['wrist'][:,i]; live=wrist[i]
both=(w[:,3]>.5)&(live[3]>.5)
dw=np.where(both,np.mean(((w[:,:3]-live[:3])/np.array([.12,.12,.15]))**2,axis=1),0)
dw+=2.0*(w[:,3]-live[3])**2
distance=dq+dt+0.15*dv+0.5*dw
# Refuse to extrapolate far outside observed joint/target support.
if np.min(dq+dt)>30:return _hold(q)
idx=np.argpartition(distance,15)[:16]
weight=np.exp(-(distance[idx]-distance[idx].min())/0.6)
weight/=weight.sum()
result.extend(weight@db['actions'][idx,i*7:(i+1)*7])
a=np.asarray(result,np.float32)
if a.shape!=(14,) or not np.isfinite(a).all():return None
# Legal demo finite differences show positive root-y commands move the
# button right in the wrist image. Correct lateral alignment before descent.
for i in range(2):
u,v,area,present=wrist[i]
if present>.5 and area>.02 and .15<v<.8:
error=float(u)-.5
a[i*7+1]+=np.clip(-.2*error,-.08,.08)
if abs(error)>.15:
a[i*7]=0.
a[i*7+2]=0.
return np.clip(a,-1,1)
|