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