Linslab's picture
Move frozen Astra policies to astra/<task>/ (DualArmHammerHandover-v1 .. DualArmPushBox-v1)
67e0b67 verified
Raw History Blame Contribute Delete
748 Bytes
import cv2
import numpy as np
HUES=(0,15,30,60,120,90,150,135)
def centers(rgb):
out=[]
for cam in ('top_camera','front_camera'):
im=np.asarray(rgb[cam]); im=im[0] if im.ndim==4 else im
hsv=cv2.cvtColor(im,cv2.COLOR_RGB2HSV)
for hue in HUES:
dh=np.abs(hsv[:,:,0].astype(float)-hue);dh=np.minimum(dh,180-dh)
m=((dh<7)&(hsv[:,:,1]>160)&(hsv[:,:,2]>90)).astype('uint8')
n,_,stats,cents=cv2.connectedComponentsWithStats(m)
candidates=[j for j in range(1,n) if stats[j,4]>=8 and max(stats[j,2:4])/max(1,min(stats[j,2:4]))<4]
if candidates:
j=max(candidates,key=lambda k:stats[k,4]);out.append([cents[j,0]/im.shape[1],cents[j,1]/im.shape[0],1])
else:out.append([0,0,0])
return np.array(out,dtype=np.float32).reshape(2,8,3)