def _test_grasp(self):
"""Simple tests for grasping. Don't forget to process depth images.
Do this independently of any rollout ...
"""
print("\nNow in `test_grasp` to check grasping net...")
self.position_head()
time.sleep(3)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if np.isnan(np.sum(d_img)):
cv2.patchNaNs(d_img, 0.0)
d_img = depth_to_net_dim(d_img, robot='HSR')
pred = self.g_detector.predict(np.copy(d_img))
img = self.dp.draw_prediction(d_img, pred)
print("prediction: {}".format(pred))
caption = 'G Predicted: {} (ESC to abort, other key to proceed)'.format(
pred)
cv2.imshow(caption, img)
key = cv2.waitKey(0)
if key in ESC_KEYS:
print("Pressed ESC key. Terminating program...")
sys.exit()
def _test_success(self):
"""Simple tests for success net. Don't forget to process depth images.
Should be done after a grasp test since I don't re-position... Note: we
have access to `self.sn` but that isn't the actual net which has a
`predict`, but it's a wrapper (explained above), but we can access the
true network via `self.sn.sdect` and from there call `predict`.
"""
print("\nNow in `test_success` to check success net...")
time.sleep(3)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if np.isnan(np.sum(d_img)):
cv2.patchNaNs(d_img, 0.0)
d_img = depth_to_net_dim(d_img, robot='HSR')
result = self.sn.sdect.predict(np.copy(d_img))
result = np.squeeze(result)
print("s-net pred: {} (if [0]<[1] failure, else success...)".format(
result))
caption = 'S Predicted: {} (ESC to abort, other key to proceed)'.format(
result)
cv2.imshow(caption, d_img)
key = cv2.waitKey(0)
if key in ESC_KEYS:
print("Pressed ESC key. Terminating program...")
sys.exit()
def save_image(self, c_img, d_img, success_class=None):
"""Save images. Don't forget to process depth images.
For now I'm using a tuned cutoff like 1400, at least to _visualize_.
NOTE: since the cutoff for turning depth images into black may change,
it would be better to save the original d_img in a dictionary. Don't use
cv2.imwrite() as I know from experience that it won't work as desired.
"""
rc = str(self.r_count).zfill(3)
f_rc_grasp = 'frame_{}_{}.png'.format(rc,
str(self.grasp_count).zfill(2))
f_rc_success = 'frame_{}_{}_class_{}.png'.format(
rc,
str(self.success_count).zfill(2), success_class)
if np.isnan(np.sum(d_img)):
cv2.patchNaNs(d_img, 0.0)
d_img = depth_to_net_dim(d_img, cutoff=1400) # for visualization only
if self.side == "BOTTOM":
if self.grasp:
pth1 = join(FAST_PATH, 'b_grasp', 'rgb_' + f_rc_grasp)
pth2 = join(FAST_PATH, 'b_grasp', 'depth_' + f_rc_grasp)
else:
pth1 = join(FAST_PATH, 'b_success', 'rgb_' + f_rc_success)
pth2 = join(FAST_PATH, 'b_success', 'depth_' + f_rc_success)
else:
if self.grasp:
pth1 = join(FAST_PATH, 't_grasp', 'rgb_' + f_rc_grasp)
pth2 = join(FAST_PATH, 't_grasp', 'depth_' + f_rc_grasp)
else:
pth1 = join(FAST_PATH, 't_success', 'rgb_' + f_rc_success)
pth2 = join(FAST_PATH, 't_success', 'depth_' + f_rc_success)
cv2.imwrite(pth1, c_img)
cv2.imwrite(pth2, d_img)
def save_imgs(RES, pref):
"""I'm thinking we want to check the grasp img w/the subsequent success img.
"""
diffs = []
ssims = []
for ridx, res in enumerate(RES):
fidx = 0
with open(res, 'r') as f:
data = pickle.load(f)
num_grasp = int(len(data) / 2.0)
print(res, num_grasp)
previous_grasp_img = None
for item in data:
if 'type' not in item:
continue
dimg = item['d_img']
if np.isnan(np.sum(dimg)):
cv2.patchNaNs(dimg, 0.0)
dimg = depth_to_net_dim(dimg, robot='HSR')
# Record current grasp img so we can compare w/next success net img.
which_net = item['type'].lower()
if which_net == 'grasp':
previous_grasp_img = dimg
else:
diff = np.linalg.norm(previous_grasp_img - dimg)
diffs.append(diff)
gray1 = previous_grasp_img[:, :, 0]
gray2 = dimg[:, :, 0]
assert gray1.shape == gray2.shape == (480, 640)
# we can optinally return other stuff
# http://scikit-image.org/docs/dev/api/skimage.measure.html#skimage.measure.compare_ssim
#(score, difference) = compare_ssim(gray1, gray2)
score = compare_ssim(gray1, gray2)
ssims.append(score)
# Save image, put L2 in success net img name
side = item['side'].lower()
fname = '{}_roll_{}_side_{}_idx_{}_{}.png'.format(
pref, ridx, side, fidx, which_net)
fname = join(TEST_HEAD, fname)
if which_net == 'success':
fname = fname.replace('.png',
'_{:.3f}_{:.3f}.png'.format(diff, score))
fidx += 1
cv2.imwrite(fname, dimg)
return diffs, ssims
def nan_to_num(x, copy=True, nan=0.0):
if copy:
out = Mat(x.shape, dtype=x.dtype, UMat=x.UMat)
else:
out = x
if CV_[out.dtype.name] == cv.CV_32F:
cv.patchNaNs(out._, nan)
elif CV_[out.dtype.name] == cv.CV_64F:
mat = out.n
np.nan_to_num(mat, copy=False, nan=nan)
if out.UMat:
del out.u
del out._
out.u = cv.UMat(mat)
out._ = out.u
return out
def buildPyramidMask(pyramidDepth, minDepth, maxDepth, pyramidNormal):
pyramidMask = []
for i in range(len(pyramidDepth)):
levelDepth = pyramidDepth[i]
levelDepth = cv.patchNaNs(levelDepth, 0)
minMask = levelDepth > minDepth
maxMask = levelDepth < maxDepth
levelMask = np.logical_and(minMask, maxMask)
levelNormal = pyramidNormal[i]
validNormalMask = levelNormal == levelNormal
#channelMask_0, channelMask_1, channelMask_2 = cv.split(validNormalMask)
channelMask_0 = validNormalMask[:, :, 0]
channelMask_1 = validNormalMask[:, :, 1]
channelMask_2 = validNormalMask[:, :, 2]
channelMask_01 = np.logical_and(channelMask_0, channelMask_1)
validNormalMask = np.logical_and(channelMask_01, channelMask_2)
levelMask = np.logical_and(levelMask, validNormalMask)
pyramidMask.append(levelMask)
return pyramidMask
def bed_make(self):
"""Runs the pipeline for deployment, testing out bed-making.
"""
# Get the starting image (from USB webcam). Try a second as well.
cap = cv2.VideoCapture(0)
frame = None
while frame is None:
ret, frame = cap.read()
cv2.waitKey(50)
self.image_start = frame
cv2.imwrite('image_start.png', self.image_start)
_, frame = cap.read()
self.image_start2 = frame
cv2.imwrite('image_start2.png', self.image_start2)
cap.release()
print(
"NOTE! Recorded `image_start` for coverage evaluation. Was it set up?"
)
def get_pose(data_all):
# See `find_pick_region_labeler` in `p_pi/bed_making/gripper.py`.
# It's because from the web labeler, we get a bunch of objects.
# So we have to compute the pose (x,y) from it.
res = data_all['objects'][0]
x_min = float(res['box'][0])
y_min = float(res['box'][1])
x_max = float(res['box'][2])
y_max = float(res['box'][3])
x = (x_max - x_min) / 2.0 + x_min
y = (y_max - y_min) / 2.0 + y_min
return (x, y)
args = self.args
use_d = BED_CFG.GRASP_CONFIG.USE_DEPTH
self.get_new_grasp = True
self.new_grasp = True
self.rollout_stats = [] # What we actually save for analysis later
# Add to self.rollout_stats at the end for more timing info
self.g_time_stats = [] # for _execution_ of a grasp
self.move_time_stats = [] # for moving to the other side
while True:
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if (not c_img.all() == None and not d_img.all() == None):
if self.new_grasp:
self.position_head()
else:
self.new_grasp = True
time.sleep(3)
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
d_img_raw = np.copy(d_img) # Needed for determining grasp pose
# --------------------------------------------------------------
# Process depth images! Helps network, human, and (presumably) analytic.
# Obviously human can see the c_img as well ... hard to compare fairly.
# --------------------------------------------------------------
if use_d:
if np.isnan(np.sum(d_img)):
cv2.patchNaNs(d_img, 0.0)
d_img = depth_to_net_dim(d_img, robot='HSR')
policy_input = np.copy(d_img)
else:
policy_input = np.copy(c_img)
# --------------------------------------------------------------
# Run grasp detector to get data=(x,y) point for target, record stats.
# Note that the web labeler returns a dictionary like this:
# {'objects': [{'box': (155, 187, 165, 194), 'class': 0}], 'num_labels': 1}
# but we really want just the 2D grasping point. So use `get_pose()`.
# Also, for the analytic one, we'll pick the highest point ourselves.
# --------------------------------------------------------------
sgraspt = time.time()
if args.g_type == 'network':
data = self.g_detector.predict(policy_input)
elif args.g_type == 'analytic':
data_all = self.wl.label_image(policy_input)
data = get_pose(data_all)
elif args.g_type == 'human':
data_all = self.wl.label_image(policy_input)
data = get_pose(data_all)
egraspt = time.time()
g_predict_t = egraspt - sgraspt
print("Grasp predict time: {:.2f}".format(g_predict_t))
self.record_stats(c_img, d_img_raw, data, self.side,
g_predict_t, 'grasp')
# For safety, we can check image and abort as needed before execution.
if use_d:
img = self.dp.draw_prediction(d_img, data)
else:
img = self.dp.draw_prediction(c_img, data)
caption = 'G Predicted: {} (ESC to abort, other key to proceed)'.format(
data)
call_wait_key(cv2.imshow(caption, img))
# --------------------------------------------------------------
# Broadcast grasp pose, execute the grasp, check for success.
# We'll use the `find_pick_region_net` since the `data` is the
# (x,y) pose, and not `find_pick_region_labeler`.
# --------------------------------------------------------------
self.gripper.find_pick_region_net(
pose=data,
c_img=c_img,
d_img=d_img_raw,
count=self.grasp_count,
side=self.side,
apply_offset=self.apply_offset)
pick_found, bed_pick = self.check_card_found()
if self.side == "BOTTOM":
self.whole_body.move_to_go()
self.tt.move_to_pose(self.omni_base, 'lower_start')
tic = time.time()
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_down')
toc = time.time()
else:
self.whole_body.move_to_go()
self.tt.move_to_pose(self.omni_base, 'top_mid')
tic = time.time()
self.gripper.execute_grasp(bed_pick, self.whole_body,
'head_up')
toc = time.time()
self.g_time_stats.append(toc - tic)
self.check_success_state(policy_input)
datum = pickle.load(open(fname, 'rb'))
print("On {}, len {}".format(fname, len(datum)))
print("keys: {}".format(datum.keys()))
print("datum['class']: {} (_their_ format, 1=success)".format(
datum['class']))
assert 'c_img' in datum and 'd_img' in datum \
and 'class' in datum and 'type' in datum
assert datum['type'] == 'success'
assert datum['c_img'].shape == (480, 640, 3)
assert datum['d_img'].shape == (480, 640)
# Need to process it because it's their data.
if np.isnan(np.sum(datum['d_img'])):
print("patching NaNs to zero ...")
cv2.patchNaNs(datum['d_img'], 0.0)
assert not np.isnan(np.sum(datum['d_img']))
datum = datum_to_net_dim(datum, robot=ROBOT)
# NOTE Don't forget to change their labeling !!!!!!
assert datum['d_img'].shape == (480, 640, 3)
#if datum['class'] == 0:
# datum['class'] = 1
# num_failures += 1
#elif datum['class'] == 1:
# datum['class'] = 0
# num_successes += 1
#else:
# raise ValueError(datum['class'])
# UPDATE UPDATE: OK, their three failure cases here were actually successes...
labels = list()
userinput = 'a'
while userinput != 'q':
print("rollout", rollout_num)
# generate random numbers for top/bottom, success/failure of rollout
top = np.random.randint(2)
success = np.random.randint(2)
labels.append(str(success)) # classification labels
print("Top (1)/Bottom (0): ", top)
print("Success (1)/Failure (0): ", success)
userinput = raw_input(
"press enter to take the picture, or enter q to quit: ")
if userinput != 'q':
c_img = camera.read_color_data()
d_img = camera.read_depth_data()
# preprocess depth image
if np.isnan(np.sum(d_img)):
cv2.patchNaNs(d_img, 0.0)
d_img = depth_to_net_dim(d_img, robot="HSR")
# save images
cv2.imwrite(
os.path.join(SAVE_PATH, "rgb_" + str(rollout_num) + ".png"), c_img)
cv2.imwrite(
os.path.join(SAVE_PATH, "depth_" + str(rollout_num) + ".png"),
d_img)
rollout_num += 1
# write labels to output
with open(os.path.join(SAVE_PATH, "labels.txt"), 'w') as fh:
fh.write("\n".join(labels))
)
pkl_f = os.path.join(ff, 'rollout.pkl')
number = str(ff.split('_')[-1])
file_path = os.path.join(ROLLOUTS, pkl_f)
datum = pickle.load(open(file_path, 'rb'))
assert type(datum) is dict
# Debug and accumulate statistics for plotting later.
print("\nOn data: {}, number {}".format(file_path, number))
print(" data['pose']: {}".format(np.array(datum['pose'])))
print(" data['type']: {}".format(np.array(datum['type'])))
num = str(number).zfill(3)
assert datum['c_img'].shape == (480, 640, 3)
assert datum['d_img'].shape == (480, 640)
cv2.patchNaNs(datum['d_img'], 0) # NOTE the patching!
# As usual, datum to net dim must be done before data augmentation.
datum = datum_to_net_dim(datum, robot='Fetch') # NOTE robot!
assert datum['d_img'].shape == (480, 640, 3)
assert 'c_img' in datum.keys() and 'd_img' in datum.keys(
) and 'pose' in datum.keys()
# SKIP points that have pose[0] (i.e., x value) less than some threshold.
if datum['pose'][0] <= 30:
print(" NOTE: skipping this due to pose: {}".format(
datum['pose']))
num_skipped += 1
continue
if datum['type'] != 'grasp':
print(" NOTE: skipping, type: {}".format(datum['type']))
image_num = 1
for i in range(2, 304, 1):
data = pickle.load(open(dirPath + str(i) + '.pkl', 'rb'))
RGBImage = data["RGBImage"]
depthImage = data["depthImage"]
w, h = depthImage.shape
# NaNs=
# NaNs=np.isnan(depthImage).astype(np.uint8)
# num_of_NaNs=np.sum(NaNs.flat)
# print 'NaNs % in the image:',num_of_NaNs/w/h*100
# cv2.imshow('NaNs',NaNs*255)
# cv2.waitKey(1000)
# raw_input()
# continue
no_NaNs_image = cv2.patchNaNs(depthImage, 0)
# ones=np.ones((w,h)).astype(np.float32)
# notNaNs=~np.isnan(depthImage)*ones
# no_NaNs_image=notNaNs*depthImage
outData = {
"RGBImage": RGBImage,
"depthImage": no_NaNs_image,
"armState": data["armState"],
"headState": data["headState"],
"markerPos": data["markerPos"]
}
pickle.dump(outData, open(dstDirPath + str(image_num) + ".pkl", "wb"))
image_num = image_num + 1
# time.sleep(0.1)
print(str(image_num))
continue
