"""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.15: a[i*7]=0. a[i*7+2]=0. return np.clip(a,-1,1)