File size: 11,225 Bytes
4cfe1b1 | 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 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 | 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)
|