Linslab's picture
Move frozen Astra policies to astra/<task>/ (BuildTent-v1 .. DualArmFoldHandKerchief-v1)
592ea41 verified
Raw History Blame Contribute Delete
11.2 kB
import cv2,numpy as np
def posts_hough(im):
im=cv2.resize(np.asarray(im).squeeze(),(480,640),interpolation=cv2.INTER_LINEAR)
hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV);g=cv2.cvtColor(im,cv2.COLOR_RGB2GRAY)
cs=cv2.HoughCircles(g,cv2.HOUGH_GRADIENT,1,9,param1=70,param2=8,minRadius=3,maxRadius=7)
out=[]
if cs is None:return out
for x,y,r in cs[0]:
if not 170<x<430 or not 170<y<470:continue
xx,yy=int(x),int(y)
if np.median(hsv[yy-1:yy+2,xx-1:xx+2,1])>80 or np.median(hsv[yy-1:yy+2,xx-1:xx+2,2])<165:continue
ang=np.arange(12)*2*np.pi/12;px=np.clip((x+np.cos(ang)*(r+3)).astype(int),0,479);py=np.clip((y+np.sin(ang)*(r+3)).astype(int),0,639)
if np.sum((hsv[py,px,1]>75)&(hsv[py,px,2]<220))<3:continue
out.append([float(x/2),float(y/2),float(np.pi*r*r/4)])
return out
def update_posts(state, grid, im):
groups=[[] for _ in range(4)]
wrist=wrist_hough(im)
for direct, points in [(True,posts(im)),(False,posts_hough(im))]:
for px,py,area in points:
if not 4<area<(25 if direct else 40):continue
xy=np.array([px,py])
if np.isfinite(wrist).all() and np.linalg.norm(xy-wrist)<18:continue
dist=np.linalg.norm(grid-xy,axis=1);j=int(np.argmin(dist))
if dist[j]>32 or np.sort(dist)[1]-dist[j]<5:continue
groups[j].append((xy,direct))
for j,group in enumerate(groups):
if not group:continue
anchor=state.post_positions[j] if state.post_counts[j] else grid[j]
preferred=[v for v in group if v[1]] or group
xy,direct=min(preferred,key=lambda v:np.linalg.norm(v[0]-anchor))
if state.post_counts[j]==0 or (state.post_counts[j]<5 and np.linalg.norm(xy-anchor)>2):
state.post_positions[j]=xy;state.post_counts[j]=1;state.direct_counts[j]=int(direct)
elif np.linalg.norm(xy-anchor)<=2:
state.post_positions[j]=.9*anchor+.1*xy;state.post_counts[j]+=1;state.direct_counts[j]+=int(direct)
known=np.flatnonzero(state.post_counts>=5)
best=None;best_score=-1
z=grid[:,0]+1j*grid[:,1]
for ii,j in enumerate(known):
for k in known[ii+1:]:
p=state.post_positions
c=complex(*(p[k]-p[j]))/(z[k]-z[j])
if not .92<abs(c)<1.08 or abs(np.angle(c))>.75:continue
zz=c*(z-z[j])+complex(*p[j]);pred=np.stack([zz.real,zz.imag],axis=1)
residual=np.linalg.norm(pred[known]-p[known],axis=1)
score=float(np.sum(state.post_counts[known][residual<3]))
if score>best_score:best=pred;best_score=score
if best is not None:
old_score=-1 if state.live_grid is None else np.sum(state.post_counts[known][np.linalg.norm(state.live_grid[known]-state.post_positions[known],axis=1)<3])
if best_score>=old_score:state.live_grid=best
def target_post_delta(state, grid, target):
if state.live_grid is not None:
delta=state.live_grid[target]-grid[target]
if np.linalg.norm(delta)<35:return delta,True
return np.zeros(2),False
import cv2,numpy as np
def posts(im):
im=cv2.resize(np.asarray(im).squeeze(),(240,320),interpolation=cv2.INTER_AREA)
hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV)
mask=((hsv[:,:,1]<70)&(hsv[:,:,2]>170)).astype(np.uint8)*255
cs,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
out=[]
for c in cs:
a=cv2.contourArea(c);p=cv2.arcLength(c,True)
if 2<a<55 and p>0 and 4*np.pi*a/p**2>.45:
m=cv2.moments(c);x,y=m['m10']/m['m00'],m['m01']/m['m00']
if 85<x<215 and 85<y<235:out.append([x,y,a])
return out
import numpy as np,cv2
def holes(im):
im=cv2.resize(np.asarray(im).squeeze(),(240,320),interpolation=cv2.INTER_AREA)
hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV)
white=((hsv[:,:,1]<75)&(hsv[:,:,2]>150)).astype(np.uint8)*255
cs,hh=cv2.findContours(white,cv2.RETR_CCOMP,cv2.CHAIN_APPROX_SIMPLE)
out=[]
if hh is None:return out
for c,h in zip(cs,hh[0]):
area=cv2.contourArea(c);per=cv2.arcLength(c,True);x,y,w,height=cv2.boundingRect(c)
if h[3]>=0 and 20<area<180 and per>0 and 4*np.pi*area/per**2>.65 and x>70:
m=cv2.moments(c);out.append([m['m10']/m['m00'],m['m01']/m['m00']])
return out
def wrist(im):
im=cv2.resize(np.asarray(im).squeeze(),(240,320),interpolation=cv2.INTER_AREA)
hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV)
mask=((hsv[:,:,1]<28)&(hsv[:,:,2]>45)&(hsv[:,:,2]<180)).astype(np.uint8)*255
cs,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
out=[]
for c in cs:
area=cv2.contourArea(c);per=cv2.arcLength(c,True)
if 80<area<900 and per>0 and 4*np.pi*area/per**2>.6:
(x,y),r=cv2.minEnclosingCircle(c);out.append((x,y,r,4*np.pi*area/per**2))
return out
def wrist_hough(im):
im=cv2.resize(np.asarray(im).squeeze(),(240,320),interpolation=cv2.INTER_AREA)
g=cv2.cvtColor(im,cv2.COLOR_RGB2GRAY);hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV)
cs=cv2.HoughCircles(g,cv2.HOUGH_GRADIENT,1,12,param1=80,param2=14,minRadius=6,maxRadius=13)
if cs is None:return np.array([np.nan,np.nan])
valid=[]
for x,y,r in cs[0]:
xx,yy=int(x),int(y)
if x>75 and 50<y<270 and np.median(hsv[max(0,yy-3):yy+4,max(0,xx-3):xx+4,1])<60 and 35<np.median(g[max(0,yy-3):yy+4,max(0,xx-3):xx+4])<200:valid.append((x,y))
return np.array(max(valid,key=lambda p:p[0]) if valid else [np.nan,np.nan])
"""Authorized-demo trajectory alignment with current-pose correction; no learned weights."""
from pathlib import Path
import numpy as np
import cv2
def descriptor(im):
im=np.asarray(im).squeeze()
im=cv2.resize(im,(120,160),interpolation=cv2.INTER_AREA)
hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV)
mask=((hsv[:,:,1]<55)&(hsv[:,:,2]>135)).astype(np.float32)
# Exclude the initial robot footprint from cloth localization.
mask[:,:50]=0;mask[:45]=0;mask[120:]=0
return cv2.resize(mask,(60,80),interpolation=cv2.INTER_AREA)
class Controller:
def __init__(self):
self.bank=None;self.reset_episode()
def reset_episode(self):
self.selected=None;self.t=1;self.calls=0;self.direct_counts=np.zeros(4,dtype=int);self.live_grid=None;self.last_target_delta=np.zeros(2);self.target_known=False;self.release_wait={};self.last_release_t=None;self.post_bank=None;self.post_positions=np.full((4,2),np.nan);self.post_counts=np.zeros(4,dtype=int);self.geometry=None;self.offset=np.zeros(2);self.approach_key=None;self.last_diagnostic={};self.previous=None
def act(self,obs):
self.calls+=1
step=int(obs.get('step_id',self.calls))
if step>=1000:return None
q=np.asarray(obs['proprioception']['agent/qpos']).reshape(-1)
if self.bank is None:
with np.load(Path(__file__).with_name('sequences.npz')) as z:self.bank={k:z[k] for k in z.files}
if step<25:
self.t=step+1
return np.array([0,0,0,0,0,0,1.],dtype=np.float32)
if self.post_bank is None:
with np.load(Path(__file__).with_name('posts_inferred.npz')) as z:self.post_bank={k:z[k] for k in z.files}
if self.geometry is None:
with np.load(Path(__file__).with_name('geometry_clean.npz')) as z:self.geometry={k:z[k] for k in z.files}
if self.selected is None:
f=descriptor(obs['rgb']['top_camera'])
scores=[np.mean((descriptor(self.bank[f'demo_{i:02d}_im'])-f)**2) for i in range(20)]
self.selected=f'demo_{np.argmin(scores):02d}';self.t=25
self.initial_score=float(min(scores))
a=self.bank[self.selected+'_a'];qs=self.bank[self.selected+'_q']
if self.t>=len(a):return None
# Re-align only locally by observed joint pose, with a one-frame timing prior.
lo=max(25,self.t-1);hi=min(len(a),self.t+4)
ix=np.arange(lo,hi)
costs=np.mean((qs[ix,:7]-q[:7])**2,axis=1)+.0004*(ix-self.t)**2
t=min(self.t,int(ix[np.argmin(costs)]))
transitions=np.flatnonzero(np.diff(a[:,6])!=0)+1
if np.any(np.abs(transitions-self.t)<=4):t=self.t
command=a[t].copy()
# A local finite-difference inverse maps joint tracking error to EE deltas.
lo=max(0,t-25);hi=min(len(a),t+25)
aa=a[lo:hi,:6].astype(float);dq=np.diff(qs[lo:hi+1,:7],axis=0)
J=np.linalg.solve(aa.T@aa+.002*np.eye(6),aa.T@dq)
err=qs[t,:7]-q[:7]
correction=np.linalg.solve(J@J.T+.02*np.eye(6),J@err)
command[:3]+=np.clip(correction[:3],-.005,.005)
# Keep demonstrated orientation deltas; position errors must not induce wrist rotation.
# Match the next grasp's reference eyelet to the current legal image.
ws=self.geometry[self.selected+'_w'];hs=self.geometry[self.selected+'_h']
visual_used=False
grid=self.post_bank[self.selected]
update_posts(self,grid,obs['rgb']['top_camera'])
target_delta=np.zeros(2)
future=np.flatnonzero((a[:,6]<-.5)&(np.r_[1,a[:-1,6]]>=-.5))
closes=future[future>=t]
if len(closes) and command[6]==0:
close=int(closes[0]);ww=ws[max(25,close-5):close+1];ww=ww[np.isfinite(ww).all(axis=1)]
ref=hs[t];ref=ref[np.isfinite(ref).all(axis=1)]
live=np.array(holes(obs['rgb']['top_camera']))
if len(ww) and len(ref) and len(live):
goal=ww[-1];target=ref[np.argmin(np.linalg.norm(ref-goal,axis=1))]
match=live[np.argmin(np.linalg.norm(live-target,axis=1))]
delta=match-target
if np.linalg.norm(delta)<25 and np.linalg.norm(target-goal)<40:
self.offset=.8*self.offset+.2*delta if self.approach_key==close else delta
self.approach_key=close
else:
past=future[future<=t]
dwell=(command[6]<-.5 and len(past)>0 and t-int(past[-1])<12)
if command[6]<-.5 and len(past):
self.target_known=False
close=int(past[-1]);release=np.flatnonzero(a[close:,6]>=0)
end=close+int(release[0]) if len(release) else len(a)-1
ww=ws[max(0,end-3):min(len(ws),end+2)];ww=ww[np.isfinite(ww).all(axis=1)]
if len(ww):
target=int(np.argmin(np.linalg.norm(grid-np.median(ww,axis=0),axis=1)))
target_delta,self.target_known=target_post_delta(self,grid,target)
self.last_target_delta=target_delta.copy()
if not dwell:self.offset=.92*self.offset+.08*target_delta
else:self.offset*=.85
is_release=t>0 and a[t,6]>.5 and a[t-1,6]<-.5
if is_release:self.last_release_t=t
if self.target_known and self.last_release_t is not None and 0<=t-self.last_release_t<8:
self.offset=self.last_target_delta.copy()
refws=ws[max(0,t-2):min(len(ws),t+3)]
validws=refws[np.isfinite(refws).all(axis=1)]
refw=np.median(validws,axis=0) if len(validws) else np.array([np.nan,np.nan])
w=wrist_hough(obs['rgb']['top_camera'])
if np.isfinite(w).all() and np.isfinite(refw).all():
error=refw+self.offset-w
if np.linalg.norm(refw-w)<35:
visual=np.clip(np.array([error[0],-error[1]])/75.,-0.06,0.06)
command[:2]=a[t,:2]+visual
visual_used=True
pause=False
if is_release and self.target_known and np.isfinite(w).all() and np.isfinite(refw).all():
error=refw+self.last_target_delta-w
count=self.release_wait.get(t,0)
if 3<np.linalg.norm(error)<35 and count<30 and (1000-step)>(len(a)-t):
command[:6]=0.;command[6]=-1.
command[:2]=np.clip(np.array([error[0],-error[1]])/75.,-0.06,0.06)
self.release_wait[t]=count+1;pause=True
if not pause:self.t+=1
self.last_diagnostic={'reason':'pose-aligned demonstration with bounded correction','demo':self.selected,'frame':t,'initial_mask_error':self.initial_score,'joint_error':float(np.linalg.norm(err)), 'visual_used':visual_used,'offset':self.offset.tolist(),'post_target_offset':target_delta.tolist(),'post_counts':self.post_counts.tolist(),'live_grid':None if self.live_grid is None else self.live_grid.tolist(),'release_pause':pause}
return np.clip(command,-1,1).astype(np.float32)
_controller=Controller()
def reset_episode():_controller.reset_episode()
def act(obs):return _controller.act(obs)