Linslab's picture
Move frozen Astra policies to astra/<task>/ (BuildTent-v1 .. DualArmFoldHandKerchief-v1)
592ea41 verified
Raw History Blame Contribute Delete
5.97 kB
"""D20 geometric trajectory tracking with legal-image reference selection."""
from pathlib import Path
import importlib.util
import numpy as np
import cv2
_ROOT=Path(__file__).resolve().parent
for _name in ('features','kinematics','visibility','active_calibration'):
_spec=importlib.util.spec_from_file_location('_local_'+_name,_ROOT/(_name+'.py'))
_mod=importlib.util.module_from_spec(_spec);_spec.loader.exec_module(_mod)
globals()['_'+_name]=_mod
_memory=None;_poses=None;_initial=None;_left1=None;_hanger=None
_phase=0.;_last_step=0;_demo=None;_bad=0;_hanger=None;_vision=None;_hanger_offset=0.;_vest_offset=0.;_lost_checks=0;_misplaced_checks=0;_stop_reason=None;_probe=None;_depth_offset=0.;_calibration_report={}
GAIN=10.
DEPTH_OFFSET=0.
def reset_episode():
global _phase,_last_step,_demo,_bad,_hanger,_hanger_offset,_vest_offset,_lost_checks,_misplaced_checks,_stop_reason,_probe,_depth_offset,_calibration_report
_phase=0.;_last_step=0;_demo=None;_bad=0;_hanger=None;_hanger_offset=0.;_vest_offset=0.;_lost_checks=0;_misplaced_checks=0;_stop_reason=None;_probe=None;_depth_offset=0.;_calibration_report={}
def act(obs):
global _memory,_poses,_phase,_last_step,_demo,_bad,_initial,_left1,_hanger,_vision,_hanger_offset,_vest_offset,_lost_checks,_misplaced_checks,_stop_reason,_probe,_depth_offset,_calibration_report
step=int(obs['step_id'])
if step>=1500:return None
if step<=_last_step:raise ValueError('Nonincreasing step')
_last_step=step
if _memory is None:
with np.load(_ROOT/'memory.npz') as d:_memory={k:d[k] for k in d.files}
with np.load(_ROOT/'poses.npz') as d:_poses={k:d[k] for k in d.files}
with np.load(_ROOT/'initial.npz') as d:_initial={k:d[k] for k in d.files}
with np.load(_ROOT/'left1.npz') as d:_left1={k:d[k] for k in d.files}
with np.load(_ROOT/'vision_refs.npz') as d:_vision={k:d[k] for k in d.files}
q,v=_features.proprio_feature(obs['proprioception'])
if _demo is None:
im=_features.image_feature(obs['rgb']);idx=np.flatnonzero(_memory['frame']==0)
cost=np.mean((_memory['image'][idx]-im)**2,axis=1)
_demo=int(_memory['demo'][idx[np.argmin(cost)]])
left=cv2.resize(np.asarray(obs['rgb']['left_camera']).squeeze(),(160,120)).astype(np.float32)
hc=[np.mean((left[:12,90:98]-_left1[str(k)][:12,90:98].astype(np.float32))**2) for k in range(20)]
_hanger=int(np.argmin(hc))
def cap(im):
gray=im[:24,180:196].astype(np.float64).mean(2)
ink=np.clip((110-gray)/70,0,1).sum(1);w=np.maximum(ink-2.6,0)
return float((w*np.arange(24)).sum()/max(w.sum(),1e-8))
cy=np.array([cap(_vision[f'left_camera_{k}']) for k in range(20)])
yy=np.array([_kinematics.fk(_poses[f'qpos_{k}'][51,9:16])[1,3] for k in range(20)])
coeff=np.polyfit(cy,yy,1)
live=cv2.resize(np.asarray(obs['rgb']['left_camera']).squeeze(),(320,240),interpolation=cv2.INTER_AREA)
estimate=float(np.clip(np.polyval(coeff,cap(live)),yy.min(),yy.max()))
_hanger=int(np.argmin(abs(yy-estimate)))
_hanger_offset=float(estimate-yy[_hanger])
def vest_center(im):
r,g,b=im.astype(np.float32).transpose(2,0,1)
mask=((r>180)&(g>130)&(b>100)&(r-g>15)&(r-g<80)&(g-b>5)).astype(np.uint8)
n,labels,stats,centers=cv2.connectedComponentsWithStats(mask)
if n<=1:return np.nan
i=1+int(np.argmax(stats[1:,cv2.CC_STAT_AREA]))
return float(centers[i,0]) if stats[i,cv2.CC_STAT_AREA]>=500 else np.nan
centers=np.array([vest_center(_vision[f'robot_camera_0_{k}']) for k in range(20)])
anchors=np.array([int(np.flatnonzero(_poses[f'a_{k}'][:,3]>.09)[0]) for k in range(20)])
positions=np.array([_kinematics.fk(_poses[f'qpos_{k}'][anchors[k],:7])[1,3] for k in range(20)])
live_vest=cv2.resize(np.asarray(obs['rgb']['robot_camera_0']).squeeze(),(320,240),interpolation=cv2.INTER_AREA)
cx=vest_center(live_vest);valid=np.isfinite(centers)
if np.isfinite(cx) and valid.sum()>=3:
calibration=np.polyfit(centers[valid],positions[valid],1)
_vest_offset=float(np.clip(calibration[0]*(cx-centers[_demo]),-.015,.015))
if _probe is None:_probe=_active_calibration.Probe(_ROOT,_poses,_demo,_kinematics.fk)
probe_axis=_probe.observe(step,obs,q)
if _probe.accepted:_depth_offset=_probe.offset_x;_vest_offset=_probe.offset_y
_calibration_report=_probe.diagnostic.copy()
actions=_poses[f'a_{_demo}'];refs=_poses[f'qpos_{_demo}']
roll_start=int(np.flatnonzero(actions[:,3]>.09)[0])
rot_end=int(np.flatnonzero(np.any(np.abs(actions[:,3:6])>.001,axis=1))[-1])
t=step-1;_phase=t/len(actions)
release=int(np.flatnonzero(np.diff(actions[:,6])>0)[0]+1)
if rot_end+60<=t<=release+20 and step%5==0:
_lost_checks=_lost_checks+1 if _visibility.cloth_area(obs['rgb'])<20 else 0
if _lost_checks>=5:
_stop_reason='persistent_cloth_loss';return None
if release-5<=t<=release+25 and step%5==0:
_misplaced_checks=_misplaced_checks+1 if _visibility.carried_position_error(obs['rgb'],_demo)>25 else 0
if _misplaced_checks>=3:
_stop_reason='cloth_not_carried_to_center';return None
if t>=len(actions)+40:return None
ti=min(t,len(actions)-1)
a=actions[ti].copy() if t<len(actions) else np.zeros(14,dtype=np.float32)
if t>=len(actions):a[[6,13]]=actions[-1,[6,13]]
target=refs[min(t,len(refs)-1)].copy()
if t<220:
target[9:]=_poses[f'qpos_{_hanger}'][t,9:]
a[7:]=_poses[f'a_{_hanger}'][t,7:]
maxerr=0.
for j in range(2):
now=_kinematics.fk(q[j*9:j*9+7]);ref=_kinematics.fk(target[j*9:j*9+7])
if j==0:
weight=min(float(np.clip((t-roll_start+40)/20.,0,1)),float(np.clip((rot_end+100-t)/60.,0,1)))
ref[1,3]+=_vest_offset*weight
ref[0,3]+=_depth_offset*weight
if j==1 and 3<=t<120:
ref[1,3]+=_hanger_offset*min(1.,max(0.,(120-t)/35.))
err=ref[:3,3]-now[:3,3];maxerr=max(maxerr,float(np.linalg.norm(err)))
a[j*7:j*7+3]+=np.clip(GAIN*err,-.3,.3)
rot=_kinematics.rotation_error(ref[:3,:3],now[:3,:3])
a[j*7+3:j*7+6]+=np.clip(-2.*rot,-.15,.15)
if maxerr>.15:_bad+=1
else:_bad=0
if _bad>30:return None
if probe_axis is not None:a[probe_axis]+=.1
return np.clip(a,-1,1).astype(np.float32)