Linslab's picture
Move frozen Astra policies to astra/<task>/ (BuildTent-v1 .. DualArmFoldHandKerchief-v1)
592ea41 verified
Raw History Blame Contribute Delete
1.56 kB
"""Deterministic RGB features; no learned or pretrained weights."""
import cv2
import numpy as np
CAMERAS = ('front_camera','top_camera','robot_camera_0','robot_camera_1')
def rgb_image(value):
a = np.asarray(value)
if a.ndim == 4 and a.shape[0] == 1: a = a[0]
if a.ndim != 3 or a.shape[2] != 3 or a.dtype != np.uint8:
raise ValueError('Expected uint8 HWC or 1HWC RGB')
return a
def marker(image, color):
a = rgb_image(image).astype(np.float32)
r,g,b = a[:,:,0],a[:,:,1],a[:,:,2]
mask = ((r > 90)&(r > 1.5*g)&(r > 1.4*b)) if color == 'red' else ((b > 80)&(b > 1.35*r)&(b > 1.15*g))
n,labels,stats,centers = cv2.connectedComponentsWithStats(mask.astype(np.uint8),8)
if n <= 1: return np.zeros(3,np.float32),False
k = 1+int(np.argmax(stats[1:,cv2.CC_STAT_AREA]));area=stats[k,cv2.CC_STAT_AREA]
if area < 4: return np.zeros(3,np.float32),False
h,w=mask.shape
return np.array([centers[k,0]/w,centers[k,1]/h,area/(h*w)],np.float32),True
def visual(rgb, cache):
# Cache only fixed-view target centroids observed in this episode.
targets=[]; wrist=[]
for cam in CAMERAS[:2]:
for color in ('red','blue'):
v,ok = marker(rgb[cam],color)
key=(cam,color)
if ok: cache[key]=v[:2].copy()
targets.extend(cache.get(key,[np.nan,np.nan]))
for i,color in enumerate(('blue','red')):
v,ok=marker(rgb['robot_camera_'+str(i)],color)
wrist.append(np.r_[v,float(ok)])
return np.array(targets,np.float32),np.array(wrist,np.float32)