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)