def quality(self, state, actions, params):
"""Evaluate the quality of a set of actions according to a GQ-CNN.
Parameters
----------
state : :obj:`RgbdImageState`
State of the world described by an RGB-D image.
actions: :obj:`object`
Set of grasping actions to evaluate.
params: dict
Optional parameters for quality evaluation.
Returns
-------
:obj:`list` of float
Real-valued grasp quality predictions for each action, between 0
and 1.
"""
# Form tensors.
image_tensor, pose_tensor = self.grasps_to_tensors(actions, state)
if params is not None and params["vis"]["tf_images"]:
# Read vis params.
k = params["vis"]["k"]
d = utils.sqrt_ceil(k)
# Display grasp transformed images.
from visualization import Visualizer2D as vis2d
vis2d.figure(size=(GeneralConstants.FIGSIZE,
GeneralConstants.FIGSIZE))
for i, image_tf in enumerate(image_tensor[:k, ...]):
depth = pose_tensor[i][0]
vis2d.subplot(d, d, i + 1)
vis2d.imshow(DepthImage(image_tf))
vis2d.title("Image %d: d=%.3f" % (i, depth))
vis2d.show()
# Predict grasps.
num_actions = len(actions)
null_arr = -1 * np.ones(self._batch_size)
predict_start = time()
output_arr = np.zeros([num_actions, 2])
cur_i = 0
end_i = cur_i + min(self._batch_size, num_actions - cur_i)
while cur_i < num_actions:
output_arr[cur_i:end_i, :] = self.gqcnn(
image_tensor[cur_i:end_i, :, :, 0],
pose_tensor[cur_i:end_i, 0], null_arr)[0]
cur_i = end_i
end_i = cur_i + min(self._batch_size, num_actions - cur_i)
q_values = output_arr[:, -1]
self._logger.debug("Prediction took %.3f sec" %
(time() - predict_start))
return q_values.tolist()
def quality(self, state, actions, params):
""" Evaluate the quality of a set of actions according to a GQ-CNN.
Parameters
----------
state : :obj:`RgbdImageState`
state of the world described by an RGB-D image
actions: :obj:`object`
set of grasping actions to evaluate
params: dict
optional parameters for quality evaluation
Returns
-------
:obj:`list` of float
real-valued grasp quality predictions for each action, between 0 and 1
"""
# form tensors
image_tensor, pose_tensor = self.grasps_to_tensors(actions, state)
if params is not None and params['vis']['tf_images']:
# read vis params
k = params['vis']['k']
d = utils.sqrt_ceil(k)
# display grasp transformed images
from visualization import Visualizer2D as vis2d
vis2d.figure(size=(FIGSIZE, FIGSIZE))
for i, image_tf in enumerate(image_tensor[:k, ...]):
depth = pose_tensor[i][0]
vis2d.subplot(d, d, i + 1)
vis2d.imshow(GrayscaleImage(image_tf))
vis2d.title('Image %d: d=%.3f' % (i, depth))
vis2d.show()
# predict grasps
num_actions = len(actions)
null_arr = -1 * np.ones(self._batch_size)
predict_start = time()
output_arr = np.zeros([num_actions, 2])
cur_i = 0
end_i = cur_i + min(self._batch_size, num_actions - cur_i)
while cur_i < num_actions:
output_arr[cur_i:end_i, :] = self.gqcnn(
image_tensor[cur_i:end_i, :, :, 0],
pose_tensor[cur_i:end_i, 0], null_arr)[0]
cur_i = end_i
end_i = cur_i + min(self._batch_size, num_actions - cur_i)
q_values = output_arr[:, -1]
logging.debug('Prediction took %.3f sec' % (time() - predict_start))
return q_values.tolist()
def run():
""" Main run loop """
cv_bridge = CvBridge()
rospy.wait_for_service('plan_gqcnn_grasp')
plan_grasp = rospy.ServiceProxy('plan_gqcnn_grasp', GQCNNGraspPlanner)
while True:
raw_input("Press ENTER to proceed ...")
# get the images and camera intrinsics from the sensor
color_image, depth_image, _ = sensor.frames()
camera_intrinsics = sensor.ir_intrinsics
# inpaint to remove holes
inpainted_color_image = color_image.inpaint(
rescale_factor=config['inpaint_rescale_factor'])
inpainted_depth_image = depth_image.inpaint(
rescale_factor=config['inpaint_rescale_factor'])
detector = RgbdDetectorFactory.detector('point_cloud_box')
detection = detector.detect(inpainted_color_image,
inpainted_depth_image,
detector_cfg,
camera_intrinsics,
T_camera_world,
vis_foreground=False,
vis_segmentation=False)[0]
if config['vis']['vis_detector_output']:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(detection.color_thumbnail)
vis.subplot(1, 2, 2)
vis.imshow(detection.depth_thumbnail)
vis.show()
boundingBox = BoundingBox()
boundingBox.minY = detection.bounding_box.min_pt[0]
boundingBox.minX = detection.bounding_box.min_pt[1]
boundingBox.maxY = detection.bounding_box.max_pt[0]
boundingBox.maxX = detection.bounding_box.max_pt[1]
try:
planned_grasp_data = plan_grasp(inpainted_color_image.rosmsg,
inpainted_depth_image.rosmsg,
camera_intrinsics.rosmsg,
boundingBox)
process_GQCNNGrasp(planned_grasp_data)
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def quality(self, state, actions, params):
"""Evaluate the quality of a set of actions according to a GQ-CNN.
Parameters
----------
state : :obj:`RgbdImageState`
State of the world described by an RGB-D image.
actions: :obj:`object`
Set of grasping actions to evaluate.
params: dict
Optional parameters for quality evaluation.
Returns
-------
:obj:`list` of float
Real-valued grasp quality predictions for each
action, between 0 and 1.
"""
# Form tensors.
tensor_start = time()
image_tensor, pose_tensor = self.grasps_to_tensors(actions, state)
self._logger.info("Image transformation took %.3f sec" %
(time() - tensor_start))
if params is not None and params["vis"]["tf_images"]:
# Read vis params.
k = params["vis"]["k"]
d = utils.sqrt_ceil(k)
# Display grasp transformed images.
from visualization import Visualizer2D as vis2d
vis2d.figure(size=(GeneralConstants.FIGSIZE,
GeneralConstants.FIGSIZE))
for i, image_tf in enumerate(image_tensor[:k, ...]):
depth = pose_tensor[i][0]
vis2d.subplot(d, d, i + 1)
vis2d.imshow(DepthImage(image_tf))
vis2d.title("Image %d: d=%.3f" % (i, depth))
vis2d.show()
# Predict grasps.
predict_start = time()
output_arr = self.gqcnn.predict(image_tensor, pose_tensor)
q_values = output_arr[:, -1]
self._logger.info("Inference took %.3f sec" % (time() - predict_start))
return q_values.tolist()
def quality(self, state, actions, params):
""" Evaluate the quality of a set of actions according to a GQ-CNN.
Parameters
----------
state : :obj:`RgbdImageState`
state of the world described by an RGB-D image
actions: :obj:`object`
set of grasping actions to evaluate
params: dict
optional parameters for quality evaluation
Returns
-------
:obj:`list` of float
real-valued grasp quality predictions for each action, between 0 and 1
"""
# form tensors
tensor_start = time()
image_tensor, pose_tensor = self.grasps_to_tensors(actions, state)
logging.info('Image transformation took %.3f sec' %
(time() - tensor_start))
if params is not None and params['vis']['tf_images']:
# read vis params
k = params['vis']['k']
d = utils.sqrt_ceil(k)
# display grasp transformed images
from visualization import Visualizer2D as vis2d
vis2d.figure(size=(FIGSIZE, FIGSIZE))
for i, image_tf in enumerate(image_tensor[:k, ...]):
depth = pose_tensor[i][0]
vis2d.subplot(d, d, i + 1)
vis2d.imshow(DepthImage(image_tf))
vis2d.title('Image %d: d=%.3f' % (i, depth))
vis2d.show()
# predict grasps
predict_start = time()
output_arr = self.gqcnn.predict(image_tensor, pose_tensor)
q_values = output_arr[:, -1]
logging.info('Inference took %.3f sec' % (time() - predict_start))
return q_values.tolist()
def compute_object_pose(self):
camera_intrinsics = self.sensor.ir_intrinsics
# get the images from the sensor
color_image, depth_image, _ = self.sensor.frames()
# inpaint to remove holes
inpainted_color_image = color_image.inpaint(
rescale_factor=self.config['inpaint_rescale_factor'])
inpainted_depth_image = depth_image.inpaint(
rescale_factor=self.config['inpaint_rescale_factor'])
detector_cfg = self.config['detector']
detector_cfg['image_width'] = inpainted_depth_image.width // 3
detector_cfg['image_height'] = inpainted_depth_image.height // 3
detector = RgbdDetectorFactory.detector('point_cloud_box')
detection = detector.detect(inpainted_color_image,
inpainted_depth_image,
detector_cfg,
camera_intrinsics,
self.tf_camera_world,
vis_foreground=False,
vis_segmentation=False)[0]
if self.config['vis']['vis_detector_output']:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(detection.color_thumbnail)
vis.subplot(1, 2, 2)
vis.imshow(detection.depth_thumbnail)
vis.show()
boundingBox = BoundingBox()
boundingBox.minY = detection.bounding_box.min_pt[0]
boundingBox.minX = detection.bounding_box.min_pt[1]
boundingBox.maxY = detection.bounding_box.max_pt[0]
boundingBox.maxX = detection.bounding_box.max_pt[1]
try:
start_time = time.time()
planned_grasp_data = self.plan_grasp_client(
inpainted_color_image.rosmsg, inpainted_depth_image.rosmsg,
camera_intrinsics.rosmsg, boundingBox, None)
grasp_plan_time = time.time() - start_time
rospy.loginfo("Planning time: {}".format(grasp_plan_time))
rospy.loginfo("Camera CS planned_grasp_data:\n{}".format(
planned_grasp_data.grasp.pose))
if planned_grasp_data.grasp.q_value < 0.20:
rospy.loginfo("Q value is too small : {}".format(
planned_grasp_data.grasp.q_value))
return None
grasp_camera_pose = planned_grasp_data.grasp
rospy.loginfo('Processing Grasp')
self.broadcast_pose_as_transform(self.tf_camera_world.from_frame,
"object_link",
grasp_camera_pose.pose)
# create Stamped ROS Transform
camera_world_transform = TransformStamped()
camera_world_transform.header.stamp = rospy.Time.now()
camera_world_transform.header.frame_id = self.tf_camera_world.from_frame
camera_world_transform.child_frame_id = self.tf_camera_world.to_frame
camera_world_transform.transform.translation.x = self.tf_camera_world.translation[
0]
camera_world_transform.transform.translation.y = self.tf_camera_world.translation[
1]
camera_world_transform.transform.translation.z = self.tf_camera_world.translation[
2]
q = self.tf_camera_world.quaternion
camera_world_transform.transform.rotation.x = q[1]
camera_world_transform.transform.rotation.y = q[2]
camera_world_transform.transform.rotation.z = q[3]
camera_world_transform.transform.rotation.w = q[0]
grasp_world_pose = tf2_geometry_msgs.do_transform_pose(
grasp_camera_pose, camera_world_transform)
# if grasp_world_pose.pose.position.z < 0.05:
# rospy.logwarn("Predicted pose Z value is less than 5 cm -> bad pose")
# return None
# Hack z position
# grasp_world_pose.pose.position.z += 0.0075
# Hack orientation to 90 degree vertical pose
grasp_world_pose.pose.orientation.x = -0.718339806303
grasp_world_pose.pose.orientation.y = 0.00601026421019
grasp_world_pose.pose.orientation.z = 0.695637686512
grasp_world_pose.pose.orientation.w = 0.00632522789696
rospy.loginfo(
"World CS planned_grasp_data:\n{}".format(grasp_world_pose))
return grasp_world_pose.pose
except rospy.ServiceException as e:
rospy.logerr("Service call failed: \n %s" % e)
return None
def _run_prediction_single_model(self, model_dir, model_output_dir,
dataset_config):
""" Analyze the performance of a single model. """
# read in model config
model_config_filename = os.path.join(model_dir, 'config.json')
with open(model_config_filename) as data_file:
model_config = json.load(data_file)
# load model
logging.info('Loading model %s' % (model_dir))
gqcnn = GQCNN.load(model_dir)
gqcnn.open_session()
gripper_mode = gqcnn.gripper_mode
# read params from the config
if dataset_config is None:
dataset_dir = model_config['dataset_dir']
split_name = model_config['split_name']
image_field_name = model_config['image_field_name']
pose_field_name = model_config['pose_field_name']
metric_name = model_config['target_metric_name']
metric_thresh = model_config['metric_thresh']
else:
dataset_dir = dataset_config['dataset_dir']
split_name = dataset_config['split_name']
image_field_name = dataset_config['image_field_name']
pose_field_name = dataset_config['pose_field_name']
metric_name = dataset_config['target_metric_name']
metric_thresh = dataset_config['metric_thresh']
gripper_mode = dataset_config['gripper_mode']
logging.info('Loading dataset %s' % (dataset_dir))
dataset = TensorDataset.open(dataset_dir)
train_indices, val_indices, _ = dataset.split(split_name)
# visualize conv filters
conv1_filters = gqcnn.filters
num_filt = conv1_filters.shape[3]
d = utils.sqrt_ceil(num_filt)
vis2d.clf()
for k in range(num_filt):
filt = conv1_filters[:, :, 0, k]
vis2d.subplot(d, d, k + 1)
vis2d.imshow(DepthImage(filt))
figname = os.path.join(model_output_dir, 'conv1_filters.pdf')
vis2d.savefig(figname, dpi=self.dpi)
# aggregate training and validation true labels and predicted probabilities
all_predictions = []
all_labels = []
for i in range(dataset.num_tensors):
# log progress
if i % self.log_rate == 0:
logging.info('Predicting tensor %d of %d' %
(i + 1, dataset.num_tensors))
# read in data
image_arr = dataset.tensor(image_field_name, i).arr
pose_arr = read_pose_data(
dataset.tensor(pose_field_name, i).arr, gripper_mode)
metric_arr = dataset.tensor(metric_name, i).arr
label_arr = 1 * (metric_arr > metric_thresh)
label_arr = label_arr.astype(np.uint8)
# predict with GQ-CNN
predictions = gqcnn.predict(image_arr, pose_arr)
# aggregate
all_predictions.extend(predictions[:, 1].tolist())
all_labels.extend(label_arr.tolist())
# close session
gqcnn.close_session()
# create arrays
all_predictions = np.array(all_predictions)
all_labels = np.array(all_labels)
train_predictions = all_predictions[train_indices]
val_predictions = all_predictions[val_indices]
train_labels = all_labels[train_indices]
val_labels = all_labels[val_indices]
# aggregate results
train_result = BinaryClassificationResult(train_predictions,
train_labels)
val_result = BinaryClassificationResult(val_predictions, val_labels)
train_result.save(os.path.join(model_output_dir, 'train_result.cres'))
val_result.save(os.path.join(model_output_dir, 'val_result.cres'))
# get stats, plot curves
logging.info('Model %s training error rate: %.3f' %
(model_dir, train_result.error_rate))
logging.info('Model %s validation error rate: %.3f' %
(model_dir, val_result.error_rate))
# save images
vis2d.figure()
example_dir = os.path.join(model_output_dir, 'examples')
if not os.path.exists(example_dir):
os.mkdir(example_dir)
# train
logging.info('Saving training examples')
train_example_dir = os.path.join(example_dir, 'train')
if not os.path.exists(train_example_dir):
os.mkdir(train_example_dir)
# train TP
true_positive_indices = train_result.true_positive_indices
np.random.shuffle(true_positive_indices)
true_positive_indices = true_positive_indices[:self.num_vis]
for i, j in enumerate(true_positive_indices):
k = train_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title(
'Datapoint %d: Pred: %.3f Label: %.3f' %
(k, train_result.pred_probs[j], train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(train_example_dir,
'true_positive_%03d.png' % (i)))
# train FP
false_positive_indices = train_result.false_positive_indices
np.random.shuffle(false_positive_indices)
false_positive_indices = false_positive_indices[:self.num_vis]
for i, j in enumerate(false_positive_indices):
k = train_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title(
'Datapoint %d: Pred: %.3f Label: %.3f' %
(k, train_result.pred_probs[j], train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(train_example_dir,
'false_positive_%03d.png' % (i)))
# train TN
true_negative_indices = train_result.true_negative_indices
np.random.shuffle(true_negative_indices)
true_negative_indices = true_negative_indices[:self.num_vis]
for i, j in enumerate(true_negative_indices):
k = train_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title(
'Datapoint %d: Pred: %.3f Label: %.3f' %
(k, train_result.pred_probs[j], train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(train_example_dir,
'true_negative_%03d.png' % (i)))
# train TP
false_negative_indices = train_result.false_negative_indices
np.random.shuffle(false_negative_indices)
false_negative_indices = false_negative_indices[:self.num_vis]
for i, j in enumerate(false_negative_indices):
k = train_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title(
'Datapoint %d: Pred: %.3f Label: %.3f' %
(k, train_result.pred_probs[j], train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(train_example_dir,
'false_negative_%03d.png' % (i)))
# val
logging.info('Saving validation examples')
val_example_dir = os.path.join(example_dir, 'val')
if not os.path.exists(val_example_dir):
os.mkdir(val_example_dir)
# val TP
true_positive_indices = val_result.true_positive_indices
np.random.shuffle(true_positive_indices)
true_positive_indices = true_positive_indices[:self.num_vis]
for i, j in enumerate(true_positive_indices):
k = val_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %
(k, val_result.pred_probs[j], val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(val_example_dir, 'true_positive_%03d.png' % (i)))
# val FP
false_positive_indices = val_result.false_positive_indices
np.random.shuffle(false_positive_indices)
false_positive_indices = false_positive_indices[:self.num_vis]
for i, j in enumerate(false_positive_indices):
k = val_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %
(k, val_result.pred_probs[j], val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(val_example_dir, 'false_positive_%03d.png' % (i)))
# val TN
true_negative_indices = val_result.true_negative_indices
np.random.shuffle(true_negative_indices)
true_negative_indices = true_negative_indices[:self.num_vis]
for i, j in enumerate(true_negative_indices):
k = val_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %
(k, val_result.pred_probs[j], val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(val_example_dir, 'true_negative_%03d.png' % (i)))
# val TP
false_negative_indices = val_result.false_negative_indices
np.random.shuffle(false_negative_indices)
false_negative_indices = false_negative_indices[:self.num_vis]
for i, j in enumerate(false_negative_indices):
k = val_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %
(k, val_result.pred_probs[j], val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(val_example_dir, 'false_negative_%03d.png' % (i)))
# save summary stats
train_summary_stats = {
'error_rate': train_result.error_rate,
'ap_score': train_result.ap_score,
'auc_score': train_result.auc_score
}
train_stats_filename = os.path.join(model_output_dir,
'train_stats.json')
json.dump(train_summary_stats,
open(train_stats_filename, 'w'),
indent=JSON_INDENT,
sort_keys=True)
val_summary_stats = {
'error_rate': val_result.error_rate,
'ap_score': val_result.ap_score,
'auc_score': val_result.auc_score
}
val_stats_filename = os.path.join(model_output_dir, 'val_stats.json')
json.dump(val_summary_stats,
open(val_stats_filename, 'w'),
indent=JSON_INDENT,
sort_keys=True)
return train_result, val_result
segmask = segmask.resize(rescale_factor, interp='nearest')
# save segmask
segmask.save(os.path.join(save_dir, 'segmask_%d.png' %(i)))
# rescale images
if rescale_factor != 1.0:
color = color.resize(rescale_factor)
depth = depth.resize(rescale_factor, interp='nearest')
# save images
color.save(os.path.join(save_dir, 'color_%d.png' %(i)))
depth.save(os.path.join(save_dir, 'depth_%d.npy' %(i)))
if ir is not None:
ir.save(os.path.join(save_dir, 'ir_%d.npy' %(i)))
if vis:
from visualization import Visualizer2D as vis2d
num_plots = 3 if workspace is not None else 2
vis2d.figure()
vis2d.subplot(1,num_plots,1)
vis2d.imshow(color)
vis2d.subplot(1,num_plots,2)
vis2d.imshow(depth)
if workspace is not None:
vis2d.subplot(1,num_plots,3)
vis2d.imshow(segmask)
vis2d.show()
sensor.stop()
def _sample_suction_points(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False,
constraint_fn=None):
"""Sample a set of 2D suction point candidates from a depth image by
choosing points on an object surface uniformly at random
and then sampling around the surface normal.
Parameters
----------
depth_im : :obj:"perception.DepthImage"
Depth image to sample from.
camera_intr : :obj:`perception.CameraIntrinsics`
Intrinsics of the camera that captured the images.
num_samples : int
Number of grasps to sample.
segmask : :obj:`perception.BinaryImage`
Binary image segmenting out the object of interest.
visualize : bool
Whether or not to show intermediate samples (for debugging).
Returns
-------
:obj:`list` of :obj:`SuctionPoint2D`
List of 2D suction point candidates.
"""
# Compute edge pixels.
filter_start = time()
if self._depth_gaussian_sigma > 0:
depth_im_mask = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_gaussian_sigma)
else:
depth_im_mask = depth_im.copy()
if segmask is not None:
depth_im_mask = depth_im.mask_binary(segmask)
self._logger.debug("Filtering took %.3f sec" % (time() - filter_start))
if visualize:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(depth_im)
vis.subplot(1, 2, 2)
vis.imshow(depth_im_mask)
vis.show()
# Project to get the point cloud.
cloud_start = time()
point_cloud_im = camera_intr.deproject_to_image(depth_im_mask)
normal_cloud_im = point_cloud_im.normal_cloud_im()
nonzero_px = depth_im_mask.nonzero_pixels()
num_nonzero_px = nonzero_px.shape[0]
if num_nonzero_px == 0:
return []
self._logger.debug("Normal cloud took %.3f sec" %
(time() - cloud_start))
# Randomly sample points and add to image.
sample_start = time()
suction_points = []
k = 0
sample_size = min(self._max_num_samples, num_nonzero_px)
indices = np.random.choice(num_nonzero_px,
size=sample_size,
replace=False)
while k < sample_size and len(suction_points) < num_samples:
# Sample a point uniformly at random.
ind = indices[k]
center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]])
center = Point(center_px, frame=camera_intr.frame)
axis = -normal_cloud_im[center.y, center.x]
depth = point_cloud_im[center.y, center.x][2]
# Update number of tries.
k += 1
# Check center px dist from boundary.
if (center_px[0] < self._min_dist_from_boundary
or center_px[1] < self._min_dist_from_boundary
or center_px[1] >
depth_im.height - self._min_dist_from_boundary
or center_px[0] >
depth_im.width - self._min_dist_from_boundary):
continue
# Perturb depth.
delta_depth = self._depth_rv.rvs(size=1)[0]
depth = depth + delta_depth
# Keep if the angle between the camera optical axis and the suction
# direction is less than a threshold.
dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0)
psi = np.arccos(dot)
if psi < self._max_suction_dir_optical_axis_angle:
# Create candidate grasp.
candidate = SuctionPoint2D(center,
axis,
depth,
camera_intr=camera_intr)
# Check constraint satisfaction.
if constraint_fn is None or constraint_fn(candidate):
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.scatter(center.x, center.y)
vis.show()
suction_points.append(candidate)
self._logger.debug("Loop took %.3f sec" % (time() - sample_start))
return suction_points
valid_px_mask = depth_im.invalid_pixel_mask().inverse()
if segmask is None:
segmask = valid_px_mask
else:
segmask = segmask.mask_binary(valid_px_mask)
# Inpaint.
depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor)
if "input_images" in policy_config["vis"] and policy_config["vis"][
"input_images"]:
vis.figure(size=(10, 10))
num_plot = 1
if segmask is not None:
num_plot = 2
vis.subplot(1, num_plot, 1)
vis.imshow(depth_im)
if segmask is not None:
vis.subplot(1, num_plot, 2)
vis.imshow(segmask)
vis.show()
# Create state.
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# Set input sizes for fully-convolutional policy.
if fully_conv:
policy_config["metric"]["fully_conv_gqcnn_config"][
"im_height"] = depth_im.shape[0]
policy_config["metric"]["fully_conv_gqcnn_config"][
config = YamlConfig(config_filename)
policy_config = config['policy']
# load test case
state_path = os.path.join(test_case_path, 'state')
action_path = os.path.join(test_case_path, 'action')
state = RgbdImageState.load(state_path)
original_action = ParallelJawGrasp.load(action_path)
# init policy
policy = CrossEntropyRobustGraspingPolicy(policy_config)
if policy_config['vis']['input_images']:
vis2d.figure()
if state.segmask is None:
vis2d.subplot(1, 2, 1)
vis2d.imshow(state.rgbd_im.color)
vis2d.title('COLOR')
vis2d.subplot(1, 2, 2)
vis2d.imshow(state.rgbd_im.depth,
vmin=policy_config['vis']['vmin'],
vmax=policy_config['vis']['vmax'])
vis2d.title('DEPTH')
else:
vis2d.subplot(1, 3, 1)
vis2d.imshow(state.rgbd_im.color)
vis2d.title('COLOR')
vis2d.subplot(1, 3, 2)
vis2d.imshow(state.rgbd_im.depth,
vmin=policy_config['vis']['vmin'],
vmax=policy_config['vis']['vmax'])
def _plan_grasp(self,
color_im,
depth_im,
camera_intr,
bounding_box=None,
segmask=None):
"""Grasp planner request handler.
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS `ServiceRequest` for grasp planner service.
"""
rospy.loginfo("Planning Grasp")
# Inpaint images.
color_im = color_im.inpaint(
rescale_factor=self.cfg["inpaint_rescale_factor"])
depth_im = depth_im.inpaint(
rescale_factor=self.cfg["inpaint_rescale_factor"])
# Init segmask.
if segmask is None:
segmask = BinaryImage(255 *
np.ones(depth_im.shape).astype(np.uint8),
frame=color_im.frame)
# Visualize.
if self.cfg["vis"]["color_image"]:
vis.imshow(color_im)
vis.show()
if self.cfg["vis"]["depth_image"]:
vis.imshow(depth_im)
vis.show()
if self.cfg["vis"]["segmask"] and segmask is not None:
vis.imshow(segmask)
vis.show()
# Aggregate color and depth images into a single
# BerkeleyAutomation/perception `RgbdImage`.
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
# Mask bounding box.
if bounding_box is not None:
# Calc bb parameters.
min_x = bounding_box.minX
min_y = bounding_box.minY
max_x = bounding_box.maxX
max_y = bounding_box.maxY
# Contain box to image->don't let it exceed image height/width
# bounds.
if min_x < 0:
min_x = 0
if min_y < 0:
min_y = 0
if max_x > rgbd_im.width:
max_x = rgbd_im.width
if max_y > rgbd_im.height:
max_y = rgbd_im.height
# Mask.
bb_segmask_arr = np.zeros([rgbd_im.height, rgbd_im.width])
bb_segmask_arr[min_y:max_y, min_x:max_x] = 255
bb_segmask = BinaryImage(bb_segmask_arr.astype(np.uint8),
segmask.frame)
segmask = segmask.mask_binary(bb_segmask)
# Visualize.
if self.cfg["vis"]["rgbd_state"]:
masked_rgbd_im = rgbd_im.mask_binary(segmask)
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(masked_rgbd_im.color)
vis.subplot(1, 2, 2)
vis.imshow(masked_rgbd_im.depth)
vis.show()
# Create an `RgbdImageState` with the cropped `RgbdImage` and
# `CameraIntrinsics`.
rgbd_state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# Execute policy.
try:
return self.execute_policy(rgbd_state, self.grasping_policy,
self.grasp_pose_publisher,
camera_intr.frame)
except NoValidGraspsException:
rospy.logerr(
("While executing policy found no valid grasps from sampled"
" antipodal point pairs. Aborting Policy!"))
raise rospy.ServiceException(
("While executing policy found no valid grasps from sampled"
" antipodal point pairs. Aborting Policy!"))
def _run_prediction_single_model(self, model_dir,
model_output_dir,
dataset_config):
""" Analyze the performance of a single model. """
# read in model config
model_config_filename = os.path.join(model_dir, 'config.json')
with open(model_config_filename) as data_file:
model_config = json.load(data_file)
# load model
self.logger.info('Loading model %s' %(model_dir))
log_file = None
for handler in self.logger.handlers:
if isinstance(handler, logging.FileHandler):
log_file = handler.baseFilename
gqcnn = get_gqcnn_model(verbose=self.verbose).load(model_dir, verbose=self.verbose, log_file=log_file)
gqcnn.open_session()
gripper_mode = gqcnn.gripper_mode
angular_bins = gqcnn.angular_bins
# read params from the config
if dataset_config is None:
dataset_dir = model_config['dataset_dir']
split_name = model_config['split_name']
image_field_name = model_config['image_field_name']
pose_field_name = model_config['pose_field_name']
metric_name = model_config['target_metric_name']
metric_thresh = model_config['metric_thresh']
else:
dataset_dir = dataset_config['dataset_dir']
split_name = dataset_config['split_name']
image_field_name = dataset_config['image_field_name']
pose_field_name = dataset_config['pose_field_name']
metric_name = dataset_config['target_metric_name']
metric_thresh = dataset_config['metric_thresh']
gripper_mode = dataset_config['gripper_mode']
self.logger.info('Loading dataset %s' %(dataset_dir))
dataset = TensorDataset.open(dataset_dir)
train_indices, val_indices, _ = dataset.split(split_name)
# visualize conv filters
conv1_filters = gqcnn.filters
num_filt = conv1_filters.shape[3]
d = utils.sqrt_ceil(num_filt)
vis2d.clf()
for k in range(num_filt):
filt = conv1_filters[:,:,0,k]
vis2d.subplot(d,d,k+1)
vis2d.imshow(DepthImage(filt))
figname = os.path.join(model_output_dir, 'conv1_filters.pdf')
vis2d.savefig(figname, dpi=self.dpi)
# aggregate training and validation true labels and predicted probabilities
all_predictions = []
if angular_bins > 0:
all_predictions_raw = []
all_labels = []
for i in range(dataset.num_tensors):
# log progress
if i % self.log_rate == 0:
self.logger.info('Predicting tensor %d of %d' %(i+1, dataset.num_tensors))
# read in data
image_arr = dataset.tensor(image_field_name, i).arr
pose_arr = read_pose_data(dataset.tensor(pose_field_name, i).arr,
gripper_mode)
metric_arr = dataset.tensor(metric_name, i).arr
label_arr = 1 * (metric_arr > metric_thresh)
label_arr = label_arr.astype(np.uint8)
if angular_bins > 0:
# form mask to extract predictions from ground-truth angular bins
raw_poses = dataset.tensor(pose_field_name, i).arr
angles = raw_poses[:, 3]
neg_ind = np.where(angles < 0)
angles = np.abs(angles) % GeneralConstants.PI
angles[neg_ind] *= -1
g_90 = np.where(angles > (GeneralConstants.PI / 2))
l_neg_90 = np.where(angles < (-1 * (GeneralConstants.PI / 2)))
angles[g_90] -= GeneralConstants.PI
angles[l_neg_90] += GeneralConstants.PI
angles *= -1 # hack to fix reverse angle convention
angles += (GeneralConstants.PI / 2)
pred_mask = np.zeros((raw_poses.shape[0], angular_bins*2), dtype=bool)
bin_width = GeneralConstants.PI / angular_bins
for i in range(angles.shape[0]):
pred_mask[i, int((angles[i] // bin_width)*2)] = True
pred_mask[i, int((angles[i] // bin_width)*2 + 1)] = True
# predict with GQ-CNN
predictions = gqcnn.predict(image_arr, pose_arr)
if angular_bins > 0:
raw_predictions = np.array(predictions)
predictions = predictions[pred_mask].reshape((-1, 2))
# aggregate
all_predictions.extend(predictions[:,1].tolist())
if angular_bins > 0:
all_predictions_raw.extend(raw_predictions.tolist())
all_labels.extend(label_arr.tolist())
# close session
gqcnn.close_session()
# create arrays
all_predictions = np.array(all_predictions)
all_labels = np.array(all_labels)
train_predictions = all_predictions[train_indices]
val_predictions = all_predictions[val_indices]
train_labels = all_labels[train_indices]
val_labels = all_labels[val_indices]
if angular_bins > 0:
all_predictions_raw = np.array(all_predictions_raw)
train_predictions_raw = all_predictions_raw[train_indices]
val_predictions_raw = all_predictions_raw[val_indices]
# aggregate results
train_result = BinaryClassificationResult(train_predictions, train_labels)
val_result = BinaryClassificationResult(val_predictions, val_labels)
train_result.save(os.path.join(model_output_dir, 'train_result.cres'))
val_result.save(os.path.join(model_output_dir, 'val_result.cres'))
# get stats, plot curves
self.logger.info('Model %s training error rate: %.3f' %(model_dir, train_result.error_rate))
self.logger.info('Model %s validation error rate: %.3f' %(model_dir, val_result.error_rate))
self.logger.info('Model %s training loss: %.3f' %(model_dir, train_result.cross_entropy_loss))
self.logger.info('Model %s validation loss: %.3f' %(model_dir, val_result.cross_entropy_loss))
# save images
vis2d.figure()
example_dir = os.path.join(model_output_dir, 'examples')
if not os.path.exists(example_dir):
os.mkdir(example_dir)
# train
self.logger.info('Saving training examples')
train_example_dir = os.path.join(example_dir, 'train')
if not os.path.exists(train_example_dir):
os.mkdir(train_example_dir)
# train TP
true_positive_indices = train_result.true_positive_indices
np.random.shuffle(true_positive_indices)
true_positive_indices = true_positive_indices[:self.num_vis]
for i, j in enumerate(true_positive_indices):
k = train_indices[j]
datapoint = dataset.datapoint(k, field_names=[image_field_name,
pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k,
train_result.pred_probs[j],
train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(os.path.join(train_example_dir, 'true_positive_%03d.png' %(i)))
# train FP
false_positive_indices = train_result.false_positive_indices
np.random.shuffle(false_positive_indices)
false_positive_indices = false_positive_indices[:self.num_vis]
for i, j in enumerate(false_positive_indices):
k = train_indices[j]
datapoint = dataset.datapoint(k, field_names=[image_field_name,
pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k,
train_result.pred_probs[j],
train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(os.path.join(train_example_dir, 'false_positive_%03d.png' %(i)))
# train TN
true_negative_indices = train_result.true_negative_indices
np.random.shuffle(true_negative_indices)
true_negative_indices = true_negative_indices[:self.num_vis]
for i, j in enumerate(true_negative_indices):
k = train_indices[j]
datapoint = dataset.datapoint(k, field_names=[image_field_name,
pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k,
train_result.pred_probs[j],
train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(os.path.join(train_example_dir, 'true_negative_%03d.png' %(i)))
# train TP
false_negative_indices = train_result.false_negative_indices
np.random.shuffle(false_negative_indices)
false_negative_indices = false_negative_indices[:self.num_vis]
for i, j in enumerate(false_negative_indices):
k = train_indices[j]
datapoint = dataset.datapoint(k, field_names=[image_field_name,
pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k,
train_result.pred_probs[j],
train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(os.path.join(train_example_dir, 'false_negative_%03d.png' %(i)))
# val
self.logger.info('Saving validation examples')
val_example_dir = os.path.join(example_dir, 'val')
if not os.path.exists(val_example_dir):
os.mkdir(val_example_dir)
# val TP
true_positive_indices = val_result.true_positive_indices
np.random.shuffle(true_positive_indices)
true_positive_indices = true_positive_indices[:self.num_vis]
for i, j in enumerate(true_positive_indices):
k = val_indices[j]
datapoint = dataset.datapoint(k, field_names=[image_field_name,
pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k,
val_result.pred_probs[j],
val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(os.path.join(val_example_dir, 'true_positive_%03d.png' %(i)))
# val FP
false_positive_indices = val_result.false_positive_indices
np.random.shuffle(false_positive_indices)
false_positive_indices = false_positive_indices[:self.num_vis]
for i, j in enumerate(false_positive_indices):
k = val_indices[j]
datapoint = dataset.datapoint(k, field_names=[image_field_name,
pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k,
val_result.pred_probs[j],
val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(os.path.join(val_example_dir, 'false_positive_%03d.png' %(i)))
# val TN
true_negative_indices = val_result.true_negative_indices
np.random.shuffle(true_negative_indices)
true_negative_indices = true_negative_indices[:self.num_vis]
for i, j in enumerate(true_negative_indices):
k = val_indices[j]
datapoint = dataset.datapoint(k, field_names=[image_field_name,
pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k,
val_result.pred_probs[j],
val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(os.path.join(val_example_dir, 'true_negative_%03d.png' %(i)))
# val TP
false_negative_indices = val_result.false_negative_indices
np.random.shuffle(false_negative_indices)
false_negative_indices = false_negative_indices[:self.num_vis]
for i, j in enumerate(false_negative_indices):
k = val_indices[j]
datapoint = dataset.datapoint(k, field_names=[image_field_name,
pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k,
val_result.pred_probs[j],
val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(os.path.join(val_example_dir, 'false_negative_%03d.png' %(i)))
# save summary stats
train_summary_stats = {
'error_rate': train_result.error_rate,
'ap_score': train_result.ap_score,
'auc_score': train_result.auc_score,
'loss': train_result.cross_entropy_loss
}
train_stats_filename = os.path.join(model_output_dir, 'train_stats.json')
json.dump(train_summary_stats, open(train_stats_filename, 'w'),
indent=JSON_INDENT,
sort_keys=True)
val_summary_stats = {
'error_rate': val_result.error_rate,
'ap_score': val_result.ap_score,
'auc_score': val_result.auc_score,
'loss': val_result.cross_entropy_loss
}
val_stats_filename = os.path.join(model_output_dir, 'val_stats.json')
json.dump(val_summary_stats, open(val_stats_filename, 'w'),
indent=JSON_INDENT,
sort_keys=True)
return train_result, val_result
sensor.start()
camera_intr = sensor.ir_intrinsics
n = 15
frame_rates = []
for i in range(n):
logging.info("Reading frame %d of %d" % (i + 1, n))
read_start = time.time()
color_im, depth_im, _ = sensor.frames()
read_stop = time.time()
frame_rates.append(1.0 / (read_stop - read_start))
logging.info("Avg fps: %.3f" % (np.mean(frame_rates)))
color_im = color_im.inpaint(rescale_factor=0.5)
depth_im = depth_im.inpaint(rescale_factor=0.5)
point_cloud = camera_intr.deproject(depth_im)
vis3d.figure()
vis3d.points(point_cloud, subsample=15)
vis3d.show()
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(color_im)
vis.subplot(1, 2, 2)
vis.imshow(depth_im)
vis.show()
sensor.stop()
[0, 0, -1]]),
translation=[0, 0, cam_dist],
from_frame=camera_intr.frame,
to_frame='world')
T_obj_camera = T_camera_world.inverse() * T_obj_world
# show mesh
if False:
vis3d.figure()
vis3d.mesh(mesh, T_obj_camera)
vis3d.pose(RigidTransform(), alpha=0.1)
vis3d.pose(T_obj_camera, alpha=0.1)
vis3d.show()
# render depth image
render_start = time.time()
IPython.embed()
renders = virtual_camera.wrapped_images(mesh, [T_obj_camera],
RenderMode.RGBD_SCENE,
mat_props=mat_props,
light_props=light_props,
debug=False)
render_stop = time.time()
logging.info('Render took %.3f sec' % (render_stop - render_start))
vis.subplot(d, d, k + 1)
vis.imshow(renders[0].image.color)
#vis.imshow(renders[0].image.depth)
vis.show()
def _sample_suction_points(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False):
"""
Sample a set of 2D suction point candidates from a depth image by
choosing points on an object surface uniformly at random
and then sampling around the surface normal
Parameters
----------
depth_im : :obj:'perception.DepthImage'
Depth image to sample from
camera_intr : :obj:`perception.CameraIntrinsics`
intrinsics of the camera that captured the images
num_samples : int
number of grasps to sample
segmask : :obj:`perception.BinaryImage`
binary image segmenting out the object of interest
visualize : bool
whether or not to show intermediate samples (for debugging)
Returns
-------
:obj:`list` of :obj:`SuctionPoint2D`
list of 2D suction point candidates
"""
# compute edge pixels
filter_start = time()
depth_im_mask = depth_im.copy()
if segmask is not None:
depth_im_mask = depth_im.mask_binary(segmask)
logging.debug('Filtering took %.3f sec' % (time() - filter_start))
if visualize:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(depth_im)
vis.subplot(1, 2, 2)
vis.imshow(depth_im_mask)
vis.show()
# project to get the point cloud
cloud_start = time()
point_cloud_im = camera_intr.deproject_to_image(depth_im_mask)
normal_cloud_im = point_cloud_im.normal_cloud_im()
nonzero_px = depth_im_mask.nonzero_pixels()
num_nonzero_px = nonzero_px.shape[0]
if num_nonzero_px == 0:
return []
logging.debug('Normal cloud took %.3f sec' % (time() - cloud_start))
# randomly sample points and add to image
sample_start = time()
suction_points = []
k = 0
sample_size = min(self._max_num_samples, num_nonzero_px)
indices = np.random.choice(num_nonzero_px,
size=sample_size,
replace=False)
while k < sample_size and len(suction_points) < num_samples:
# sample a point uniformly at random
ind = indices[k]
center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]])
center = Point(center_px, frame=camera_intr.frame)
axis = -normal_cloud_im[center.y, center.x]
depth = point_cloud_im[center.y, center.x][2]
# update number of tries
k += 1
# check center px dist from boundary
if center_px[0] < self._min_dist_from_boundary or \
center_px[1] < self._min_dist_from_boundary or \
center_px[1] > depth_im.height - self._min_dist_from_boundary or \
center_px[0] > depth_im.width - self._min_dist_from_boundary:
continue
# perturb depth
delta_depth = self._depth_rv.rvs(size=1)[0]
depth = depth + delta_depth
# keep if the angle between the camera optical axis and the suction direction is less than a threshold
dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0)
psi = np.arccos(dot)
if psi < self._max_suction_dir_optical_axis_angle:
# check distance to ensure sample diversity
candidate = SuctionPoint2D(center,
axis,
depth,
camera_intr=camera_intr)
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.scatter(center.x, center.y)
vis.show()
suction_points.append(candidate)
logging.debug('Loop took %.3f sec' % (time() - sample_start))
return suction_points
def _sample_antipodal_grasps(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False):
"""
Sample a set of 2D grasp candidates from a depth image by finding depth
edges, then uniformly sampling point pairs and keeping only antipodal
grasps with width less than the maximum allowable.
Parameters
----------
depth_im : :obj:'perception.DepthImage'
Depth image to sample from
camera_intr : :obj:`perception.CameraIntrinsics`
intrinsics of the camera that captured the images
num_samples : int
number of grasps to sample
segmask : :obj:`perception.BinaryImage`
binary image segmenting out the object of interest
visualize : bool
whether or not to show intermediate samples (for debugging)
Returns
-------
:obj:`list` of :obj:`Grasp2D`
list of 2D grasp candidates
"""
# compute edge pixels
edge_start = time()
depth_im = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_grad_gaussian_sigma)
scale_factor = self._rescale_factor
depth_im_downsampled = depth_im.resize(scale_factor)
depth_im_threshed = depth_im_downsampled.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = (1.0 / scale_factor) * depth_im_threshed.zero_pixels()
edge_pixels = edge_pixels.astype(np.int16)
depth_im_mask = depth_im.copy()
if segmask is not None:
edge_pixels = np.array(
[p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)])
depth_im_mask = depth_im.mask_binary(segmask)
# re-threshold edges if there are too few
if edge_pixels.shape[0] < self._min_num_edge_pixels:
logging.info('Too few edge pixels!')
depth_im_threshed = depth_im.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = depth_im_threshed.zero_pixels()
edge_pixels = edge_pixels.astype(np.int16)
depth_im_mask = depth_im.copy()
if segmask is not None:
edge_pixels = np.array([
p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)
])
depth_im_mask = depth_im.mask_binary(segmask)
num_pixels = edge_pixels.shape[0]
logging.debug('Depth edge detection took %.3f sec' %
(time() - edge_start))
logging.debug('Found %d edge pixels' % (num_pixels))
# exit if no edge pixels
if num_pixels == 0:
return []
# compute_max_depth
min_depth = np.min(depth_im_mask.data[
depth_im_mask.data > 0]) + self._min_depth_offset
max_depth = np.max(depth_im_mask.data[
depth_im_mask.data > 0]) + self._max_depth_offset
# compute surface normals
normal_start = time()
edge_normals = self._surface_normals(depth_im, edge_pixels)
logging.debug('Normal computation took %.3f sec' %
(time() - normal_start))
if visualize:
vis.figure()
vis.subplot(1, 3, 1)
vis.imshow(depth_im)
if num_pixels > 0:
vis.scatter(edge_pixels[:, 1], edge_pixels[:, 0], s=10, c='b')
X = [pix[1] for pix in edge_pixels]
Y = [pix[0] for pix in edge_pixels]
U = [10 * pix[1] for pix in edge_normals]
V = [-10 * pix[0] for pix in edge_normals]
plt.quiver(X, Y, U, V, units='x', scale=0.5, zorder=2, color='g')
vis.title('Edge pixels and normals')
vis.subplot(1, 3, 2)
vis.imshow(depth_im_threshed)
vis.title('Edge map')
vis.subplot(1, 3, 3)
vis.imshow(segmask)
vis.title('Segmask')
vis.show()
# form set of valid candidate point pairs
pruning_start = time()
max_grasp_width_px = Grasp2D(Point(np.zeros(2)),
0.0,
min_depth,
width=self._gripper_width,
camera_intr=camera_intr).width_px
normal_ip = edge_normals.dot(edge_normals.T)
dists = ssd.squareform(ssd.pdist(edge_pixels))
valid_indices = np.where(
(normal_ip < -np.cos(np.arctan(self._friction_coef)))
& (dists < max_grasp_width_px) & (dists > 0.0))
valid_indices = np.c_[valid_indices[0], valid_indices[1]]
logging.debug('Normal pruning %.3f sec' % (time() - pruning_start))
# raise exception if no antipodal pairs
num_pairs = valid_indices.shape[0]
if num_pairs == 0:
return []
# prune out grasps
contact_points1 = edge_pixels[valid_indices[:, 0], :]
contact_points2 = edge_pixels[valid_indices[:, 1], :]
contact_normals1 = edge_normals[valid_indices[:, 0], :]
contact_normals2 = edge_normals[valid_indices[:, 1], :]
v = contact_points1 - contact_points2
v_norm = np.linalg.norm(v, axis=1)
v = v / np.tile(v_norm[:, np.newaxis], [1, 2])
ip1 = np.sum(contact_normals1 * v, axis=1)
ip2 = np.sum(contact_normals2 * (-v), axis=1)
ip1[ip1 > 1.0] = 1.0
ip1[ip1 < -1.0] = -1.0
ip2[ip2 > 1.0] = 1.0
ip2[ip2 < -1.0] = -1.0
beta1 = np.arccos(ip1)
beta2 = np.arccos(ip2)
alpha = np.arctan(self._friction_coef)
antipodal_indices = np.where((beta1 < alpha) & (beta2 < alpha))[0]
# raise exception if no antipodal pairs
num_pairs = antipodal_indices.shape[0]
if num_pairs == 0:
return []
sample_size = min(self._max_rejection_samples, num_pairs)
grasp_indices = np.random.choice(antipodal_indices,
size=sample_size,
replace=False)
logging.debug('Grasp comp took %.3f sec' % (time() - pruning_start))
# compute grasps
sample_start = time()
k = 0
grasps = []
while k < sample_size and len(grasps) < num_samples:
grasp_ind = grasp_indices[k]
p1 = contact_points1[grasp_ind, :]
p2 = contact_points2[grasp_ind, :]
n1 = contact_normals1[grasp_ind, :]
n2 = contact_normals2[grasp_ind, :]
width = np.linalg.norm(p1 - p2)
k += 1
# compute center and axis
grasp_center = (p1 + p2) / 2
grasp_axis = p2 - p1
grasp_axis = grasp_axis / np.linalg.norm(grasp_axis)
grasp_theta = np.pi / 2
if grasp_axis[1] != 0:
grasp_theta = np.arctan(grasp_axis[0] / grasp_axis[1])
grasp_center_pt = Point(
np.array([grasp_center[1], grasp_center[0]]))
# check center px dist from boundary
if grasp_center[0] < self._min_dist_from_boundary or \
grasp_center[1] < self._min_dist_from_boundary or \
grasp_center[0] > depth_im.height - self._min_dist_from_boundary or \
grasp_center[1] > depth_im.width - self._min_dist_from_boundary:
continue
# sample depths
for i in range(self._depth_samples_per_grasp):
# get depth in the neighborhood of the center pixel
depth_win = depth_im.data[grasp_center[0] -
self._h:grasp_center[0] + self._h,
grasp_center[1] -
self._w:grasp_center[1] + self._w]
center_depth = np.min(depth_win)
if center_depth == 0 or np.isnan(center_depth):
continue
# sample depth between the min and max
min_depth = np.min(center_depth) + self._min_depth_offset
max_depth = np.max(center_depth) + self._max_depth_offset
sample_depth = min_depth + (max_depth -
min_depth) * np.random.rand()
candidate_grasp = Grasp2D(grasp_center_pt,
grasp_theta,
sample_depth,
width=self._gripper_width,
camera_intr=camera_intr,
contact_points=[p1, p2],
contact_normals=[n1, n2])
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.grasp(candidate_grasp)
vis.scatter(p1[1], p1[0], c='b', s=25)
vis.scatter(p2[1], p2[0], c='b', s=25)
vis.show()
grasps.append(candidate_grasp)
# return sampled grasps
logging.debug('Loop took %.3f sec' % (time() - sample_start))
return grasps
interp='nearest')
# preprocess
color_im, depth_im, segmask = preprocess_images(raw_color_im,
raw_depth_im,
camera_intr,
T_camera_world,
workspace_box,
workspace_im,
image_proc_config)
# visualize
if vis:
gui = plt.figure(0)
plt.clf()
vis2d.subplot(2,3,1)
vis2d.imshow(raw_color_im)
vis2d.title('RAW COLOR')
vis2d.subplot(2,3,2)
vis2d.imshow(raw_depth_im)
vis2d.title('RAW DEPTH')
vis2d.subplot(2,3,4)
vis2d.imshow(color_im)
vis2d.title('COLOR')
vis2d.subplot(2,3,5)
vis2d.imshow(depth_im)
vis2d.title('DEPTH')
vis2d.subplot(2,3,6)
vis2d.imshow(segmask)
vis2d.title('SEGMASK')
plt.draw()
def run_experiment():
""" Run the experiment """
print("run_experiment")
rospy.loginfo('Wait for the service...')
# wait for Grasp Planning Service and create Service Proxy
rospy.wait_for_service('grasping_policy')
plan_grasp = rospy.ServiceProxy('grasping_policy', GQCNNGraspPlanner)
# create ROS CVBridge
# cv_bridge = CvBridge()
# get camera intrinsics
# camera_intrinsics = sensor.color_intrinsics
camera_intrinsics = sensor.ir_intrinsics
rospy.loginfo('camera_intrinsics: {}'.format(camera_intrinsics.rosmsg))
rospy.loginfo('Beginning experiment')
# get the images from the sensor
color_image, depth_image, _ = sensor.frames()
# inpaint to remove holes
inpainted_color_image = color_image.inpaint(
rescale_factor=config['inpaint_rescale_factor'])
inpainted_depth_image = depth_image.inpaint(
rescale_factor=config['inpaint_rescale_factor'])
print("inpainted_color_image: shape", inpainted_depth_image.shape)
detector_cfg['image_width'] = inpainted_depth_image.width // 3
detector_cfg['image_height'] = inpainted_depth_image.height // 3
detector = RgbdDetectorFactory.detector('point_cloud_box')
rospy.loginfo("Detect bbox")
detection = detector.detect(inpainted_color_image,
inpainted_depth_image,
detector_cfg,
camera_intrinsics,
T_camera_world,
vis_foreground=False,
vis_segmentation=False)[0]
if config['vis']['vis_detector_output']:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(detection.color_thumbnail)
vis.subplot(1, 2, 2)
vis.imshow(detection.depth_thumbnail)
vis.show()
boundingBox = BoundingBox()
boundingBox.minY = detection.bounding_box.min_pt[0]
boundingBox.minX = detection.bounding_box.min_pt[1]
boundingBox.maxY = detection.bounding_box.max_pt[0]
boundingBox.maxX = detection.bounding_box.max_pt[1]
try:
rospy.loginfo("Start planning")
start_time = time.time()
planned_grasp_data = plan_grasp(inpainted_color_image.rosmsg,
inpainted_depth_image.rosmsg,
camera_intrinsics.rosmsg, boundingBox,
None)
grasp_plan_time = time.time() - start_time
rospy.loginfo("Planning time: {}".format(grasp_plan_time))
rospy.loginfo("planned_grasp_data:\n{}".format(
planned_grasp_data.grasp.pose))
grasp_camera_pose = planned_grasp_data.grasp
# create Stamped ROS Transform
camera_world_transform = TransformStamped()
camera_world_transform.header.stamp = rospy.Time.now()
camera_world_transform.header.frame_id = T_camera_world.from_frame
camera_world_transform.child_frame_id = T_camera_world.to_frame
camera_world_transform.transform.translation.x = T_camera_world.translation[
0]
camera_world_transform.transform.translation.y = T_camera_world.translation[
1]
camera_world_transform.transform.translation.z = T_camera_world.translation[
2]
q = T_camera_world.quaternion
camera_world_transform.transform.rotation.x = q[1]
camera_world_transform.transform.rotation.y = q[2]
camera_world_transform.transform.rotation.z = q[3]
camera_world_transform.transform.rotation.w = q[0]
grasp_world_pose = tf2_geometry_msgs.do_transform_pose(
grasp_camera_pose, camera_world_transform)
rospy.loginfo(
"World CS planned_grasp_data:\n{}".format(grasp_world_pose))
except rospy.ServiceException as e:
rospy.logerr("Service call failed: \n %s" % e)
def _run_prediction_single_model(self, model_dir, model_output_dir,
dataset_config):
"""Analyze the performance of a single model."""
# Read in model config.
model_config_filename = os.path.join(model_dir,
GQCNNFilenames.SAVED_CFG)
with open(model_config_filename) as data_file:
model_config = json.load(data_file)
# Load model.
self.logger.info("Loading model %s" % (model_dir))
log_file = None
for handler in self.logger.handlers:
if isinstance(handler, logging.FileHandler):
log_file = handler.baseFilename
gqcnn = get_gqcnn_model(verbose=self.verbose).load(
model_dir, verbose=self.verbose, log_file=log_file)
gqcnn.open_session()
gripper_mode = gqcnn.gripper_mode
angular_bins = gqcnn.angular_bins
# Read params from the config.
if dataset_config is None:
dataset_dir = model_config["dataset_dir"]
split_name = model_config["split_name"]
image_field_name = model_config["image_field_name"]
pose_field_name = model_config["pose_field_name"]
metric_name = model_config["target_metric_name"]
metric_thresh = model_config["metric_thresh"]
else:
dataset_dir = dataset_config["dataset_dir"]
split_name = dataset_config["split_name"]
image_field_name = dataset_config["image_field_name"]
pose_field_name = dataset_config["pose_field_name"]
metric_name = dataset_config["target_metric_name"]
metric_thresh = dataset_config["metric_thresh"]
gripper_mode = dataset_config["gripper_mode"]
self.logger.info("Loading dataset %s" % (dataset_dir))
dataset = TensorDataset.open(dataset_dir)
train_indices, val_indices, _ = dataset.split(split_name)
# Visualize conv filters.
conv1_filters = gqcnn.filters
num_filt = conv1_filters.shape[3]
d = utils.sqrt_ceil(num_filt)
vis2d.clf()
for k in range(num_filt):
filt = conv1_filters[:, :, 0, k]
vis2d.subplot(d, d, k + 1)
vis2d.imshow(DepthImage(filt))
figname = os.path.join(model_output_dir, "conv1_filters.pdf")
vis2d.savefig(figname, dpi=self.dpi)
# Aggregate training and validation true labels and predicted
# probabilities.
all_predictions = []
if angular_bins > 0:
all_predictions_raw = []
all_labels = []
for i in range(dataset.num_tensors):
# Log progress.
if i % self.log_rate == 0:
self.logger.info("Predicting tensor %d of %d" %
(i + 1, dataset.num_tensors))
# Read in data.
image_arr = dataset.tensor(image_field_name, i).arr
pose_arr = read_pose_data(
dataset.tensor(pose_field_name, i).arr, gripper_mode)
metric_arr = dataset.tensor(metric_name, i).arr
label_arr = 1 * (metric_arr > metric_thresh)
label_arr = label_arr.astype(np.uint8)
if angular_bins > 0:
# Form mask to extract predictions from ground-truth angular
# bins.
raw_poses = dataset.tensor(pose_field_name, i).arr
angles = raw_poses[:, 3]
neg_ind = np.where(angles < 0)
# TODO(vsatish): These should use the max angle instead.
angles = np.abs(angles) % GeneralConstants.PI
angles[neg_ind] *= -1
g_90 = np.where(angles > (GeneralConstants.PI / 2))
l_neg_90 = np.where(angles < (-1 * (GeneralConstants.PI / 2)))
angles[g_90] -= GeneralConstants.PI
angles[l_neg_90] += GeneralConstants.PI
# TODO(vsatish): Fix this along with the others.
angles *= -1 # Hack to fix reverse angle convention.
angles += (GeneralConstants.PI / 2)
pred_mask = np.zeros((raw_poses.shape[0], angular_bins * 2),
dtype=bool)
bin_width = GeneralConstants.PI / angular_bins
for i in range(angles.shape[0]):
pred_mask[i, int((angles[i] // bin_width) * 2)] = True
pred_mask[i, int((angles[i] // bin_width) * 2 + 1)] = True
# Predict with GQ-CNN.
predictions = gqcnn.predict(image_arr, pose_arr)
if angular_bins > 0:
raw_predictions = np.array(predictions)
predictions = predictions[pred_mask].reshape((-1, 2))
# Aggregate.
all_predictions.extend(predictions[:, 1].tolist())
if angular_bins > 0:
all_predictions_raw.extend(raw_predictions.tolist())
all_labels.extend(label_arr.tolist())
# Close session.
gqcnn.close_session()
# Create arrays.
all_predictions = np.array(all_predictions)
all_labels = np.array(all_labels)
train_predictions = all_predictions[train_indices]
val_predictions = all_predictions[val_indices]
train_labels = all_labels[train_indices]
val_labels = all_labels[val_indices]
if angular_bins > 0:
all_predictions_raw = np.array(all_predictions_raw)
train_predictions_raw = all_predictions_raw[train_indices]
val_predictions_raw = all_predictions_raw[val_indices]
# Aggregate results.
train_result = BinaryClassificationResult(train_predictions,
train_labels)
val_result = BinaryClassificationResult(val_predictions, val_labels)
train_result.save(os.path.join(model_output_dir, "train_result.cres"))
val_result.save(os.path.join(model_output_dir, "val_result.cres"))
# Get stats, plot curves.
self.logger.info("Model %s training error rate: %.3f" %
(model_dir, train_result.error_rate))
self.logger.info("Model %s validation error rate: %.3f" %
(model_dir, val_result.error_rate))
self.logger.info("Model %s training loss: %.3f" %
(model_dir, train_result.cross_entropy_loss))
self.logger.info("Model %s validation loss: %.3f" %
(model_dir, val_result.cross_entropy_loss))
# Save images.
vis2d.figure()
example_dir = os.path.join(model_output_dir, "examples")
if not os.path.exists(example_dir):
os.mkdir(example_dir)
# Train.
self.logger.info("Saving training examples")
train_example_dir = os.path.join(example_dir, "train")
if not os.path.exists(train_example_dir):
os.mkdir(train_example_dir)
# Train TP.
true_positive_indices = train_result.true_positive_indices
np.random.shuffle(true_positive_indices)
true_positive_indices = true_positive_indices[:self.num_vis]
for i, j in enumerate(true_positive_indices):
k = train_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint,
image_field_name,
pose_field_name,
gripper_mode,
angular_preds=train_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title(
"Datapoint %d: Pred: %.3f Label: %.3f" %
(k, train_result.pred_probs[j], train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(train_example_dir,
"true_positive_%03d.png" % (i)))
# Train FP.
false_positive_indices = train_result.false_positive_indices
np.random.shuffle(false_positive_indices)
false_positive_indices = false_positive_indices[:self.num_vis]
for i, j in enumerate(false_positive_indices):
k = train_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint,
image_field_name,
pose_field_name,
gripper_mode,
angular_preds=train_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title(
"Datapoint %d: Pred: %.3f Label: %.3f" %
(k, train_result.pred_probs[j], train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(train_example_dir,
"false_positive_%03d.png" % (i)))
# Train TN.
true_negative_indices = train_result.true_negative_indices
np.random.shuffle(true_negative_indices)
true_negative_indices = true_negative_indices[:self.num_vis]
for i, j in enumerate(true_negative_indices):
k = train_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint,
image_field_name,
pose_field_name,
gripper_mode,
angular_preds=train_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title(
"Datapoint %d: Pred: %.3f Label: %.3f" %
(k, train_result.pred_probs[j], train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(train_example_dir,
"true_negative_%03d.png" % (i)))
# Train TP.
false_negative_indices = train_result.false_negative_indices
np.random.shuffle(false_negative_indices)
false_negative_indices = false_negative_indices[:self.num_vis]
for i, j in enumerate(false_negative_indices):
k = train_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint,
image_field_name,
pose_field_name,
gripper_mode,
angular_preds=train_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title(
"Datapoint %d: Pred: %.3f Label: %.3f" %
(k, train_result.pred_probs[j], train_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(train_example_dir,
"false_negative_%03d.png" % (i)))
# Val.
self.logger.info("Saving validation examples")
val_example_dir = os.path.join(example_dir, "val")
if not os.path.exists(val_example_dir):
os.mkdir(val_example_dir)
# Val TP.
true_positive_indices = val_result.true_positive_indices
np.random.shuffle(true_positive_indices)
true_positive_indices = true_positive_indices[:self.num_vis]
for i, j in enumerate(true_positive_indices):
k = val_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint,
image_field_name,
pose_field_name,
gripper_mode,
angular_preds=val_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %
(k, val_result.pred_probs[j], val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(val_example_dir, "true_positive_%03d.png" % (i)))
# Val FP.
false_positive_indices = val_result.false_positive_indices
np.random.shuffle(false_positive_indices)
false_positive_indices = false_positive_indices[:self.num_vis]
for i, j in enumerate(false_positive_indices):
k = val_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint,
image_field_name,
pose_field_name,
gripper_mode,
angular_preds=val_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %
(k, val_result.pred_probs[j], val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(val_example_dir, "false_positive_%03d.png" % (i)))
# Val TN.
true_negative_indices = val_result.true_negative_indices
np.random.shuffle(true_negative_indices)
true_negative_indices = true_negative_indices[:self.num_vis]
for i, j in enumerate(true_negative_indices):
k = val_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint,
image_field_name,
pose_field_name,
gripper_mode,
angular_preds=val_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %
(k, val_result.pred_probs[j], val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(val_example_dir, "true_negative_%03d.png" % (i)))
# Val TP.
false_negative_indices = val_result.false_negative_indices
np.random.shuffle(false_negative_indices)
false_negative_indices = false_negative_indices[:self.num_vis]
for i, j in enumerate(false_negative_indices):
k = val_indices[j]
datapoint = dataset.datapoint(
k, field_names=[image_field_name, pose_field_name])
vis2d.clf()
if angular_bins > 0:
self._plot_grasp(datapoint,
image_field_name,
pose_field_name,
gripper_mode,
angular_preds=val_predictions_raw[j])
else:
self._plot_grasp(datapoint, image_field_name, pose_field_name,
gripper_mode)
vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %
(k, val_result.pred_probs[j], val_result.labels[j]),
fontsize=self.font_size)
vis2d.savefig(
os.path.join(val_example_dir, "false_negative_%03d.png" % (i)))
# Save summary stats.
train_summary_stats = {
"error_rate": train_result.error_rate,
"ap_score": train_result.ap_score,
"auc_score": train_result.auc_score,
"loss": train_result.cross_entropy_loss
}
train_stats_filename = os.path.join(model_output_dir,
"train_stats.json")
json.dump(train_summary_stats,
open(train_stats_filename, "w"),
indent=JSON_INDENT,
sort_keys=True)
val_summary_stats = {
"error_rate": val_result.error_rate,
"ap_score": val_result.ap_score,
"auc_score": val_result.auc_score,
"loss": val_result.cross_entropy_loss
}
val_stats_filename = os.path.join(model_output_dir, "val_stats.json")
json.dump(val_summary_stats,
open(val_stats_filename, "w"),
indent=JSON_INDENT,
sort_keys=True)
return train_result, val_result
def generate_gqcnn_dataset(dataset_path, database, target_object_keys,
env_rv_params, gripper_name, config):
"""
Generates a GQ-CNN TensorDataset for training models with new grippers, quality metrics, objects, and cameras.
Parameters
----------
dataset_path : str
path to save the dataset to
database : :obj:`Hdf5Database`
Dex-Net database containing the 3D meshes, grasps, and grasp metrics
target_object_keys : :obj:`OrderedDict`
dictionary mapping dataset names to target objects (either 'all' or a list of specific object keys)
env_rv_params : :obj:`OrderedDict`
parameters of the camera and object random variables used in sampling (see meshpy.UniformPlanarWorksurfaceImageRandomVariable for more info)
gripper_name : str
name of the gripper to use
config : :obj:`autolab_core.YamlConfig`
other parameters for dataset generation
Notes
-----
Required parameters of config are specified in Other Parameters
Other Parameters
----------------
images_per_stable_pose : int
number of object and camera poses to sample for each stable pose
stable_pose_min_p : float
minimum probability of occurrence for a stable pose to be used in data generation (used to prune bad stable poses
gqcnn/crop_width : int
width, in pixels, of crop region around each grasp center, before resize (changes the size of the region seen by the GQ-CNN)
gqcnn/crop_height : int
height, in pixels, of crop region around each grasp center, before resize (changes the size of the region seen by the GQ-CNN)
gqcnn/final_width : int
width, in pixels, of final transformed grasp image for input to the GQ-CNN (defaults to 32)
gqcnn/final_height : int
height, in pixels, of final transformed grasp image for input to the GQ-CNN (defaults to 32)
table_alignment/max_approach_table_angle : float
max angle between the grasp axis and the table normal when the grasp approach is maximally aligned with the table normal
table_alignment/max_approach_offset : float
max deviation from perpendicular approach direction to use in grasp collision checking
table_alignment/num_approach_offset_samples : int
number of approach samples to use in collision checking
collision_checking/table_offset : float
max allowable interpenetration between the gripper and table to be considered collision free
collision_checking/table_mesh_filename : str
path to a table mesh for collision checking (default data/meshes/table.obj)
collision_checking/approach_dist : float
distance, in meters, between the approach pose and final grasp pose along the grasp axis
collision_checking/delta_approach : float
amount, in meters, to discretize the straight-line path from the gripper approach pose to the final grasp pose
tensors/datapoints_per_file : int
number of datapoints to store in each unique tensor file on disk
tensors/fields : :obj:`dict`
dictionary mapping field names to dictionaries specifying the data type, height, width, and number of channels for each tensor
debug : bool
True (or 1) if the random seed should be set to enforce deterministic behavior, False (0) otherwise
vis/candidate_grasps : bool
True (or 1) if the collision free candidate grasps should be displayed in 3D (for debugging)
vis/rendered_images : bool
True (or 1) if the rendered images for each stable pose should be displayed (for debugging)
vis/grasp_images : bool
True (or 1) if the transformed grasp images should be displayed (for debugging)
"""
# read data gen params
output_dir = dataset_path
gripper = RobotGripper.load(gripper_name)
image_samples_per_stable_pose = config['images_per_stable_pose']
stable_pose_min_p = config['stable_pose_min_p']
# read gqcnn params
gqcnn_params = config['gqcnn']
im_crop_height = gqcnn_params['crop_height']
im_crop_width = gqcnn_params['crop_width']
im_final_height = gqcnn_params['final_height']
im_final_width = gqcnn_params['final_width']
cx_crop = float(im_crop_width) / 2
cy_crop = float(im_crop_height) / 2
# open database
dataset_names = target_object_keys.keys()
datasets = [database.dataset(dn) for dn in dataset_names]
if CREATE_SUBSET:
datasets = [
dataset.subset(INDEX_START, INDEX_END) for dataset in datasets
]
# set target objects
for dataset in datasets:
if target_object_keys[dataset.name] == 'all':
target_object_keys[dataset.name] = dataset.object_keys
# setup grasp params
table_alignment_params = config['table_alignment']
min_grasp_approach_offset = -np.deg2rad(
table_alignment_params['max_approach_offset'])
max_grasp_approach_offset = np.deg2rad(
table_alignment_params['max_approach_offset'])
max_grasp_approach_table_angle = np.deg2rad(
table_alignment_params['max_approach_table_angle'])
num_grasp_approach_samples = table_alignment_params[
'num_approach_offset_samples']
phi_offsets = []
if max_grasp_approach_offset == min_grasp_approach_offset:
phi_inc = 1
elif num_grasp_approach_samples == 1:
phi_inc = max_grasp_approach_offset - min_grasp_approach_offset + 1
else:
phi_inc = (max_grasp_approach_offset - min_grasp_approach_offset) / (
num_grasp_approach_samples - 1)
phi = min_grasp_approach_offset
while phi <= max_grasp_approach_offset:
phi_offsets.append(phi)
phi += phi_inc
# setup collision checking
coll_check_params = config['collision_checking']
approach_dist = coll_check_params['approach_dist']
delta_approach = coll_check_params['delta_approach']
table_offset = coll_check_params['table_offset']
table_mesh_filename = coll_check_params['table_mesh_filename']
if not os.path.isabs(table_mesh_filename):
table_mesh_filename = os.path.join(
os.path.dirname(os.path.realpath(__file__)), '..',
table_mesh_filename)
table_mesh = ObjFile(table_mesh_filename).read()
# set tensor dataset config
tensor_config = config['tensors']
tensor_config['fields']['depth_ims_tf_table']['height'] = im_final_height
tensor_config['fields']['depth_ims_tf_table']['width'] = im_final_width
tensor_config['fields']['obj_masks']['height'] = im_final_height
tensor_config['fields']['obj_masks']['width'] = im_final_width
# add available metrics (assuming same are computed for all objects)
metric_names = []
dataset = datasets[0]
obj_keys = dataset.object_keys
if len(obj_keys) == 0:
raise ValueError('No valid objects in dataset %s' % (dataset.name))
obj = dataset[obj_keys[0]]
grasps = dataset.grasps(obj.key, gripper=gripper.name)
grasp_metrics = dataset.grasp_metrics(obj.key,
grasps,
gripper=gripper.name)
metric_names = grasp_metrics[grasp_metrics.keys()[0]].keys()
for metric_name in metric_names:
tensor_config['fields'][metric_name] = {}
tensor_config['fields'][metric_name]['dtype'] = 'float32'
# init tensor dataset
tensor_dataset = TensorDataset(output_dir, tensor_config)
tensor_datapoint = tensor_dataset.datapoint_template
# setup log file
experiment_log_filename = os.path.join(output_dir,
'dataset_generation.log')
formatter = logging.Formatter('%(asctime)s %(levelname)s: %(message)s')
hdlr = logging.FileHandler(experiment_log_filename)
hdlr.setFormatter(formatter)
logging.getLogger().addHandler(hdlr)
root_logger = logging.getLogger()
# copy config
out_config_filename = os.path.join(output_dir, 'dataset_generation.json')
ordered_dict_config = collections.OrderedDict()
for key in config.keys():
ordered_dict_config[key] = config[key]
with open(out_config_filename, 'w') as outfile:
json.dump(ordered_dict_config, outfile)
# 1. Precompute the set of valid grasps for each stable pose:
# i) Perpendicular to the table
# ii) Collision-free along the approach direction
# load grasps if they already exist
grasp_cache_filename = os.path.join(output_dir, CACHE_FILENAME)
if os.path.exists(grasp_cache_filename):
logging.info('Loading grasp candidates from file')
candidate_grasps_dict = pkl.load(open(grasp_cache_filename, 'rb'))
# otherwise re-compute by reading from the database and enforcing constraints
else:
# create grasps dict
candidate_grasps_dict = {}
# loop through datasets and objects
for dataset in datasets:
logging.info('Reading dataset %s' % (dataset.name))
for obj in dataset:
if obj.key not in target_object_keys[dataset.name]:
continue
# init candidate grasp storage
candidate_grasps_dict[obj.key] = {}
# setup collision checker
collision_checker = GraspCollisionChecker(gripper)
collision_checker.set_graspable_object(obj)
# read in the stable poses of the mesh
stable_poses = dataset.stable_poses(obj.key)
for i, stable_pose in enumerate(stable_poses):
# render images if stable pose is valid
if stable_pose.p > stable_pose_min_p:
candidate_grasps_dict[obj.key][stable_pose.id] = []
# setup table in collision checker
T_obj_stp = stable_pose.T_obj_table.as_frames(
'obj', 'stp')
T_obj_table = obj.mesh.get_T_surface_obj(
T_obj_stp,
delta=table_offset).as_frames('obj', 'table')
T_table_obj = T_obj_table.inverse()
collision_checker.set_table(table_mesh_filename,
T_table_obj)
# read grasp and metrics
grasps = dataset.grasps(obj.key, gripper=gripper.name)
logging.info(
'Aligning %d grasps for object %s in stable %s' %
(len(grasps), obj.key, stable_pose.id))
# align grasps with the table
aligned_grasps = [
grasp.perpendicular_table(stable_pose)
for grasp in grasps
]
# check grasp validity
logging.info(
'Checking collisions for %d grasps for object %s in stable %s'
% (len(grasps), obj.key, stable_pose.id))
for aligned_grasp in aligned_grasps:
# check angle with table plane and skip unaligned grasps
_, grasp_approach_table_angle, _ = aligned_grasp.grasp_angles_from_stp_z(
stable_pose)
perpendicular_table = (
np.abs(grasp_approach_table_angle) <
max_grasp_approach_table_angle)
if not perpendicular_table:
continue
# check whether any valid approach directions are collision free
collision_free = False
for phi_offset in phi_offsets:
rotated_grasp = aligned_grasp.grasp_y_axis_offset(
phi_offset)
collides = collision_checker.collides_along_approach(
rotated_grasp, approach_dist,
delta_approach)
if not collides:
collision_free = True
break
# store if aligned to table
candidate_grasps_dict[obj.key][
stable_pose.id].append(
GraspInfo(aligned_grasp, collision_free))
# visualize if specified
if collision_free and config['vis'][
'candidate_grasps']:
logging.info('Grasp %d' % (aligned_grasp.id))
vis.figure()
vis.gripper_on_object(gripper, aligned_grasp,
obj,
stable_pose.T_obj_world)
vis.show()
# save to file
logging.info('Saving to file')
pkl.dump(candidate_grasps_dict, open(grasp_cache_filename, 'wb'))
# 2. Render a dataset of images and associate the gripper pose with image coordinates for each grasp in the Dex-Net database
# setup variables
obj_category_map = {}
pose_category_map = {}
cur_pose_label = 0
cur_obj_label = 0
cur_image_label = 0
# render images for each stable pose of each object in the dataset
render_modes = [RenderMode.SEGMASK, RenderMode.DEPTH_SCENE]
for dataset in datasets:
logging.info('Generating data for dataset %s' % (dataset.name))
# iterate through all object keys
object_keys = dataset.object_keys
for obj_key in object_keys:
obj = dataset[obj_key]
obj.mesh.rescale(RESCALING_FACTOR)
if obj.key not in target_object_keys[dataset.name]:
continue
# read in the stable poses of the mesh
stable_poses = dataset.stable_poses(obj.key)
if SAVE_ONE_OBJECT:
# Save object mesh
savefile = ObjFile("./data/meshes/dexnet/" + obj.key + ".obj")
savefile.write(obj.mesh)
# Save stable poses
save_stp = StablePoseFile("./data/meshes/dexnet/" + obj.key +
".stp")
save_stp.write(stable_poses)
candidate_grasp_info = candidate_grasps_dict[obj.key][
stable_poses[0].id]
print("Stable pose id:", stable_poses[0].id)
candidate_grasps = [g.grasp for g in candidate_grasp_info]
# Save candidate grasp info
pkl.dump(
candidate_grasps_dict[obj.key],
open("./data/meshes/dexnet/" + obj.key + ".pkl", 'wb'))
# Save grasp metrics
grasp_metrics = dataset.grasp_metrics(obj.key,
candidate_grasps,
gripper=gripper.name)
write_metrics = json.dumps(grasp_metrics)
f = open("./data/meshes/dexnet/" + obj.key + ".json", "w")
f.write(write_metrics)
f.close()
for i, stable_pose in enumerate(stable_poses):
# render images if stable pose is valid
if stable_pose.p > stable_pose_min_p:
# log progress
logging.info('Rendering images for object %s in %s' %
(obj.key, stable_pose.id))
# add to category maps
if obj.key not in obj_category_map.keys():
obj_category_map[obj.key] = cur_obj_label
pose_category_map['%s_%s' %
(obj.key,
stable_pose.id)] = cur_pose_label
# read in candidate grasps and metrics
candidate_grasp_info = candidate_grasps_dict[obj.key][
stable_pose.id]
candidate_grasps = [g.grasp for g in candidate_grasp_info]
grasp_metrics = dataset.grasp_metrics(obj.key,
candidate_grasps,
gripper=gripper.name)
# compute object pose relative to the table
T_obj_stp = stable_pose.T_obj_table.as_frames('obj', 'stp')
T_obj_stp = obj.mesh.get_T_surface_obj(T_obj_stp)
# sample images from random variable
T_table_obj = RigidTransform(from_frame='table',
to_frame='obj')
scene_objs = {
'table': SceneObject(table_mesh, T_table_obj)
}
urv = UniformPlanarWorksurfaceImageRandomVariable(
obj.mesh,
render_modes,
'camera',
env_rv_params,
stable_pose=stable_pose,
scene_objs=scene_objs)
render_start = time.time()
render_samples = urv.rvs(
size=image_samples_per_stable_pose)
render_stop = time.time()
logging.info('Rendering images took %.3f sec' %
(render_stop - render_start))
# visualize
if config['vis']['rendered_images']:
d = int(np.ceil(
np.sqrt(image_samples_per_stable_pose)))
#segmask
vis2d.figure()
for j, render_sample in enumerate(render_samples):
vis2d.subplot(d, d, j + 1)
vis2d.imshow(render_sample.renders[
RenderMode.SEGMASK].image)
# depth table
vis2d.figure()
for j, render_sample in enumerate(render_samples):
vis2d.subplot(d, d, j + 1)
vis2d.imshow(render_sample.renders[
RenderMode.DEPTH_SCENE].image)
vis2d.show()
if False:
# binary
fig = plt.figure(figsize=(8, 8))
fig.suptitle('SEGMASK')
for j, render_sample in enumerate(render_samples):
plt.subplot(d, d, j + 1)
plt.imshow(render_sample.renders[
RenderMode.SEGMASK].image.data)
# depth table
fig = plt.figure(figsize=(8, 8))
fig.suptitle('DEPTH SCENE')
for j, render_sample in enumerate(render_samples):
plt.subplot(d, d, j + 1)
plt.imshow(render_sample.renders[
RenderMode.DEPTH_SCENE].image.data)
plt.show()
# tally total amount of data
num_grasps = len(candidate_grasps)
num_images = image_samples_per_stable_pose
num_save = num_images * num_grasps
logging.info('Saving %d datapoints' % (num_save))
# for each candidate grasp on the object compute the projection
# of the grasp into image space
for render_sample in render_samples:
# read images
binary_im = render_sample.renders[
RenderMode.SEGMASK].image
depth_im_table = render_sample.renders[
RenderMode.DEPTH_SCENE].image
# read camera params
T_stp_camera = render_sample.camera.object_to_camera_pose
shifted_camera_intr = render_sample.camera.camera_intr
# read pixel offsets
cx = depth_im_table.center[1]
cy = depth_im_table.center[0]
# compute intrinsics for virtual camera of the final
# cropped and rescaled images
camera_intr_scale = float(im_final_height) / float(
im_crop_height)
cropped_camera_intr = shifted_camera_intr.crop(
im_crop_height, im_crop_width, cy, cx)
final_camera_intr = cropped_camera_intr.resize(
camera_intr_scale)
# create a thumbnail for each grasp
for grasp_info in candidate_grasp_info:
# read info
grasp = grasp_info.grasp
collision_free = grasp_info.collision_free
# get the gripper pose
T_obj_camera = T_stp_camera * T_obj_stp.as_frames(
'obj', T_stp_camera.from_frame)
grasp_2d = grasp.project_camera(
T_obj_camera, shifted_camera_intr)
# center images on the grasp, rotate to image x axis
dx = cx - grasp_2d.center.x
dy = cy - grasp_2d.center.y
translation = np.array([dy, dx])
binary_im_tf = binary_im.transform(
translation, grasp_2d.angle)
depth_im_tf_table = depth_im_table.transform(
translation, grasp_2d.angle)
# crop to image size
binary_im_tf = binary_im_tf.crop(
im_crop_height, im_crop_width)
depth_im_tf_table = depth_im_tf_table.crop(
im_crop_height, im_crop_width)
# resize to image size
binary_im_tf = binary_im_tf.resize(
(im_final_height, im_final_width),
interp='nearest')
depth_im_tf_table = depth_im_tf_table.resize(
(im_final_height, im_final_width))
# visualize the transformed images
if config['vis']['grasp_images']:
grasp_center = Point(
depth_im_tf_table.center,
frame=final_camera_intr.frame)
# plot 2D grasp image
grasp_center = Point(
depth_im_tf_table.center,
frame=final_camera_intr.frame)
tf_grasp_2d = Grasp2D(
grasp_center,
0,
grasp_2d.depth,
width=gripper.max_width,
camera_intr=final_camera_intr)
vis2d.figure()
vis2d.subplot(2, 2, 1)
vis2d.imshow(binary_im)
vis2d.grasp(grasp_2d)
vis2d.subplot(2, 2, 2)
vis2d.imshow(depth_im_table)
vis2d.grasp(grasp_2d)
vis2d.subplot(2, 2, 3)
vis2d.imshow(binary_im_tf)
vis2d.grasp(tf_grasp_2d)
vis2d.subplot(2, 2, 4)
vis2d.imshow(depth_im_tf_table)
vis2d.grasp(tf_grasp_2d)
vis2d.title('Coll Free? %d' %
(grasp_info.collision_free))
vis2d.show()
if False:
plt.figure(figsize=(8, 8))
plt.subplot(2, 2, 1)
plt.imshow(binary_im.data)
plt.title('Binary Image')
plt.subplot(2, 2, 2)
plt.imshow(depth_im_table.data)
plt.subplot(2, 2, 3)
plt.imshow(binary_im_tf.data)
plt.subplot(2, 2, 4)
plt.imshow(depth_im_tf_table.data)
plt.title('Coll Free? %d' %
(grasp_info.collision_free))
plt.show()
plt.close()
# plot 3D visualization
# Whooza
fig = plt.figure()
ax = fig.add_subplot(111,
projection='3d',
transform=T_obj_camera)
object_vertices = obj.mesh.trimesh.vertices
table_vertices = table_mesh.trimesh.vertices
# ax.plot_trisurf(table_vertices[:,0],table_vertices[:,1],triangles=table_mesh.trimesh.faces,Z=table_vertices[:,2],color='g')
ax.plot_trisurf(
object_vertices[:, 0],
object_vertices[:, 1],
triangles=obj.mesh.trimesh.faces,
Z=object_vertices[:, 2],
color='b')
# Axes3D(fig,rect=(-0.2,-0.2,0.4,0.4),transform=T_obj_camera)
plt.show()
if False:
vis.figure()
T_obj_world = vis.mesh_stable_pose(
obj.mesh.trimesh,
stable_pose.T_obj_world,
style='surface',
dim=0.5)
vis.gripper(gripper,
grasp,
T_obj_world,
color=(0.3, 0.3, 0.3))
vis.show()
# form hand pose array
hand_pose = np.r_[grasp_2d.center.y,
grasp_2d.center.x,
grasp_2d.depth, grasp_2d.angle,
grasp_2d.center.y -
shifted_camera_intr.cy,
grasp_2d.center.x -
shifted_camera_intr.cx,
grasp_2d.width_px]
# store to data buffers
tensor_datapoint[
'depth_ims_tf_table'] = depth_im_tf_table.raw_data
tensor_datapoint[
'obj_masks'] = binary_im_tf.raw_data
tensor_datapoint['hand_poses'] = hand_pose
tensor_datapoint['collision_free'] = collision_free
tensor_datapoint['obj_labels'] = cur_obj_label
tensor_datapoint['pose_labels'] = cur_pose_label
tensor_datapoint['image_labels'] = cur_image_label
for metric_name, metric_val in grasp_metrics[
grasp.id].iteritems():
coll_free_metric = (
1 * collision_free) * metric_val
tensor_datapoint[
metric_name] = coll_free_metric
tensor_dataset.add(tensor_datapoint)
# update image label
cur_image_label += 1
# update pose label
cur_pose_label += 1
# force clean up
gc.collect()
# update object label
cur_obj_label += 1
# force clean up
gc.collect()
# save last file
tensor_dataset.flush()
# save category mappings
obj_cat_filename = os.path.join(output_dir, 'object_category_map.json')
json.dump(obj_category_map, open(obj_cat_filename, 'w'))
pose_cat_filename = os.path.join(output_dir, 'pose_category_map.json')
json.dump(pose_category_map, open(pose_cat_filename, 'w'))
def _sample_antipodal_grasps(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False,
constraint_fn=None):
"""Sample a set of 2D grasp candidates from a depth image by finding
depth edges, then uniformly sampling point pairs and keeping only
antipodal grasps with width less than the maximum allowable.
Parameters
----------
depth_im : :obj:"perception.DepthImage"
Depth image to sample from.
camera_intr : :obj:`perception.CameraIntrinsics`
Intrinsics of the camera that captured the images.
num_samples : int
Number of grasps to sample.
segmask : :obj:`perception.BinaryImage`
Binary image segmenting out the object of interest.
visualize : bool
Whether or not to show intermediate samples (for debugging).
constraint_fn : :obj:`GraspConstraintFn`
Constraint function to apply to grasps.
Returns
-------
:obj:`list` of :obj:`Grasp2D`
List of 2D grasp candidates.
"""
# Compute edge pixels.
edge_start = time()
depth_im = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_grad_gaussian_sigma)
scale_factor = self._rescale_factor
depth_im_downsampled = depth_im.resize(scale_factor)
depth_im_threshed = depth_im_downsampled.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = (1.0 / scale_factor) * depth_im_threshed.zero_pixels()
edge_pixels = edge_pixels.astype(np.int16)
depth_im_mask = depth_im.copy()
if segmask is not None:
edge_pixels = np.array(
[p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)])
depth_im_mask = depth_im.mask_binary(segmask)
# Re-threshold edges if there are too few.
if edge_pixels.shape[0] < self._min_num_edge_pixels:
self._logger.info("Too few edge pixels!")
depth_im_threshed = depth_im.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = depth_im_threshed.zero_pixels()
edge_pixels = edge_pixels.astype(np.int16)
depth_im_mask = depth_im.copy()
if segmask is not None:
edge_pixels = np.array([
p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)
])
depth_im_mask = depth_im.mask_binary(segmask)
num_pixels = edge_pixels.shape[0]
self._logger.debug("Depth edge detection took %.3f sec" %
(time() - edge_start))
self._logger.debug("Found %d edge pixels" % (num_pixels))
# Compute point cloud.
point_cloud_im = camera_intr.deproject_to_image(depth_im_mask)
# Compute_max_depth.
depth_data = depth_im_mask.data[depth_im_mask.data > 0]
if depth_data.shape[0] == 0:
return []
min_depth = np.min(depth_data) + self._min_depth_offset
max_depth = np.max(depth_data) + self._max_depth_offset
# Compute surface normals.
normal_start = time()
edge_normals = self._surface_normals(depth_im, edge_pixels)
self._logger.debug("Normal computation took %.3f sec" %
(time() - normal_start))
if visualize:
edge_pixels = edge_pixels[::2, :]
edge_normals = edge_normals[::2, :]
vis.figure()
vis.subplot(1, 3, 1)
vis.imshow(depth_im)
if num_pixels > 0:
vis.scatter(edge_pixels[:, 1], edge_pixels[:, 0], s=2, c="b")
X = [pix[1] for pix in edge_pixels]
Y = [pix[0] for pix in edge_pixels]
U = [3 * pix[1] for pix in edge_normals]
V = [-3 * pix[0] for pix in edge_normals]
plt.quiver(X,
Y,
U,
V,
units="x",
scale=0.25,
width=0.5,
zorder=2,
color="r")
vis.title("Edge pixels and normals")
vis.subplot(1, 3, 2)
vis.imshow(depth_im_threshed)
vis.title("Edge map")
vis.subplot(1, 3, 3)
vis.imshow(segmask)
vis.title("Segmask")
vis.show()
# Exit if no edge pixels.
if num_pixels == 0:
return []
# Form set of valid candidate point pairs.
pruning_start = time()
max_grasp_width_px = Grasp2D(Point(np.zeros(2)),
0.0,
min_depth,
width=self._gripper_width,
camera_intr=camera_intr).width_px
normal_ip = edge_normals.dot(edge_normals.T)
dists = ssd.squareform(ssd.pdist(edge_pixels))
valid_indices = np.where(
(normal_ip < -np.cos(np.arctan(self._friction_coef)))
& (dists < max_grasp_width_px) & (dists > 0.0))
valid_indices = np.c_[valid_indices[0], valid_indices[1]]
self._logger.debug("Normal pruning %.3f sec" %
(time() - pruning_start))
# Raise exception if no antipodal pairs.
num_pairs = valid_indices.shape[0]
if num_pairs == 0:
return []
# Prune out grasps.
contact_points1 = edge_pixels[valid_indices[:, 0], :]
contact_points2 = edge_pixels[valid_indices[:, 1], :]
contact_normals1 = edge_normals[valid_indices[:, 0], :]
contact_normals2 = edge_normals[valid_indices[:, 1], :]
v = contact_points1 - contact_points2
v_norm = np.linalg.norm(v, axis=1)
v = v / np.tile(v_norm[:, np.newaxis], [1, 2])
ip1 = np.sum(contact_normals1 * v, axis=1)
ip2 = np.sum(contact_normals2 * (-v), axis=1)
ip1[ip1 > 1.0] = 1.0
ip1[ip1 < -1.0] = -1.0
ip2[ip2 > 1.0] = 1.0
ip2[ip2 < -1.0] = -1.0
beta1 = np.arccos(ip1)
beta2 = np.arccos(ip2)
alpha = np.arctan(self._friction_coef)
antipodal_indices = np.where((beta1 < alpha) & (beta2 < alpha))[0]
# Raise exception if no antipodal pairs.
num_pairs = antipodal_indices.shape[0]
if num_pairs == 0:
return []
sample_size = min(self._max_rejection_samples, num_pairs)
grasp_indices = np.random.choice(antipodal_indices,
size=sample_size,
replace=False)
self._logger.debug("Grasp comp took %.3f sec" %
(time() - pruning_start))
# Compute grasps.
sample_start = time()
k = 0
grasps = []
while k < sample_size and len(grasps) < num_samples:
grasp_ind = grasp_indices[k]
p1 = contact_points1[grasp_ind, :]
p2 = contact_points2[grasp_ind, :]
n1 = contact_normals1[grasp_ind, :]
n2 = contact_normals2[grasp_ind, :]
# width = np.linalg.norm(p1 - p2)
k += 1
# Compute center and axis.
grasp_center = (p1 + p2) // 2
grasp_axis = p2 - p1
grasp_axis = grasp_axis / np.linalg.norm(grasp_axis)
grasp_theta = np.pi / 2
if grasp_axis[1] != 0:
grasp_theta = np.arctan2(grasp_axis[0], grasp_axis[1])
grasp_center_pt = Point(np.array(
[grasp_center[1], grasp_center[0]]),
frame=camera_intr.frame)
# Compute grasp points in 3D.
x1 = point_cloud_im[p1[0], p1[1]]
x2 = point_cloud_im[p2[0], p2[1]]
if np.linalg.norm(x2 - x1) > self._gripper_width:
continue
# Perturb.
if self._grasp_center_sigma > 0.0:
grasp_center_pt = grasp_center_pt + ss.multivariate_normal.rvs(
cov=self._grasp_center_sigma * np.diag(np.ones(2)))
if self._grasp_angle_sigma > 0.0:
grasp_theta = grasp_theta + ss.norm.rvs(
scale=self._grasp_angle_sigma)
# Check center px dist from boundary.
if (grasp_center[0] < self._min_dist_from_boundary
or grasp_center[1] < self._min_dist_from_boundary
or grasp_center[0] >
depth_im.height - self._min_dist_from_boundary
or grasp_center[1] >
depth_im.width - self._min_dist_from_boundary):
continue
# Sample depths.
for i in range(self._depth_samples_per_grasp):
# Get depth in the neighborhood of the center pixel.
depth_win = depth_im.data[grasp_center[0] -
self._h:grasp_center[0] +
self._h, grasp_center[1] -
self._w:grasp_center[1] + self._w]
center_depth = np.min(depth_win)
if center_depth == 0 or np.isnan(center_depth):
continue
# Sample depth between the min and max.
min_depth = center_depth + self._min_depth_offset
max_depth = center_depth + self._max_depth_offset
sample_depth = min_depth + (max_depth -
min_depth) * np.random.rand()
candidate_grasp = Grasp2D(grasp_center_pt,
grasp_theta,
sample_depth,
width=self._gripper_width,
camera_intr=camera_intr,
contact_points=[p1, p2],
contact_normals=[n1, n2])
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.grasp(candidate_grasp)
vis.scatter(p1[1], p1[0], c="b", s=25)
vis.scatter(p2[1], p2[0], c="b", s=25)
vis.show()
grasps.append(candidate_grasp)
# Return sampled grasps.
self._logger.debug("Loop took %.3f sec" % (time() - sample_start))
return grasps
#depth_im_noise = small_camera_intr.project_to_image(point_cloud_cam)
depth_im_filtered = small_camera_intr.project_to_image(point_cloud_cam)
#depth_im_filtered = depth_im_filtered.resize(1.0/rescale_factor)#, interp='nearest')
noise_mask = depth_im_filtered.to_binary()
#depth_im_filtered = depth_im_filtered.inpaint(rescale_factor)
#depth_im_noise = depth_im_noise.resize(1.0/rescale_factor)
#noise_mask = depth_im_noise.to_binary()
logging.info('Project took %.3f sec' % (time.time() - project_start))
#depth_im_filtered = depth_im.mask_binary(noise_mask)
#depth_im_filtered = depth_im_filtered.mask_binary(noise_mask.inverse())
depth_im_filtered = depth_im_filtered.inpaint(0.5)
filter_stop = time.time()
logging.info('Filtering took %.3f sec' % (filter_stop - filter_start))
if vis_final_images:
vis2d.figure()
vis2d.subplot(2, 2, 1)
vis2d.imshow(depth_im)
vis2d.title('Orig')
vis2d.subplot(2, 2, 2)
vis2d.imshow(depth_im_orig)
vis2d.title('Inpainted')
vis2d.subplot(2, 2, 3)
vis2d.imshow(noise_mask)
vis2d.title('Mask')
vis2d.subplot(2, 2, 4)
vis2d.imshow(depth_im_filtered)
vis2d.title('Filtered')
vis2d.show()
def _sample_antipodal_grasps(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False,
constraint_fn=None):
"""Sample a set of 2D grasp candidates from a depth image by finding
depth edges, then uniformly sampling point pairs and keeping only
antipodal grasps with width less than the maximum allowable.
Parameters
----------
depth_im : :obj:"perception.DepthImage"
Depth image to sample from.
camera_intr : :obj:`autolab.CameraIntrinsics`
Intrinsics of the camera that captured the images.
num_samples : int
Number of grasps to sample.
segmask : :obj:`perception.BinaryImage`
Binary image segmenting out the object of interest.
visualize : bool
Whether or not to show intermediate samples (for debugging).
constraint_fn : :obj:`GraspConstraintFn`
Constraint function to apply to grasps.
Returns
-------
:obj:`list` of :obj:`Grasp2D`
List of 2D grasp candidates.
"""
# Compute edge pixels.
edge_start = time()
depth_im = depth_im.apply(
snf.
gaussian_filter, #Create a new image by applying a function to this image's data.
sigma=self._depth_grad_gaussian_sigma) #这里会造成失真，最好用copy
scale_factor = self._rescale_factor
depth_im_downsampled = depth_im.resize(
scale_factor) #Resize the image.#这里会造成失真，最好用copy
depth_im_threshed = depth_im_downsampled.threshold_gradients( #Creates a new DepthImage by zeroing out all depths where the magnitude of the gradient at that point is greater than grad_thresh.
self._depth_grad_thresh)
edge_pixels = (1.0 / scale_factor) * depth_im_threshed.zero_pixels(
) #Return an array of the zero pixels.
edge_pixels = edge_pixels.astype(
np.int16) #找到所有的大于_depth_grad_thresh的像素点。
depth_im_mask = depth_im.copy()
if segmask is not None:
edge_pixels = np.array(
[p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)])
depth_im_mask = depth_im.mask_binary(segmask)
# Re-threshold edges if there are too few.
if edge_pixels.shape[0] < self._min_num_edge_pixels:
self._logger.info("Too few edge pixels!")
depth_im_threshed = depth_im.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = depth_im_threshed.zero_pixels(
) #Return an array of the zero pixels的坐标.
edge_pixels = edge_pixels.astype(
np.int16) #找到所有的大于_depth_grad_thresh的像素点。
depth_im_mask = depth_im.copy() # depth_im.resize(scale_factor)
if segmask is not None: #如果有segmask，就去除segmask外的edge_pixels，以及深度信息。
edge_pixels = np.array([
p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)
])
depth_im_mask = depth_im.mask_binary(segmask)
num_pixels = edge_pixels.shape[0] #边缘像素的数量
self._logger.debug("Depth edge detection took %.3f sec" %
(time() - edge_start))
self._logger.debug("Found %d edge pixels" % (num_pixels))
# Compute point cloud.
point_cloud_im = camera_intr.deproject_to_image(
depth_im_mask) # Deprojects a DepthImage into a PointCloudImage.
# Compute_max_depth.
depth_data = depth_im_mask.data[
depth_im_mask.data > 0] #depth_im_mask是指经过mask后的深度图（如果有mask）
if depth_data.shape[0] == 0: #失败
return []
min_depth = np.min(
depth_data
) + self._min_depth_offset # Offset from the minimum depth at the grasp center pixel to use in depth sampling
max_depth = np.max(depth_data) + self._max_depth_offset
# Compute surface normals.
normal_start = time()
edge_normals = self._surface_normals(
depth_im, edge_pixels
) # Return an array of the surface normals at the edge pixels.
self._logger.debug("Normal computation took %.3f sec" %
(time() - normal_start))
if visualize:
edge_pixels = edge_pixels[::2, :]
edge_normals = edge_normals[::2, :]
vis.figure()
vis.subplot(1, 3, 1)
vis.imshow(depth_im)
if num_pixels > 0:
vis.scatter(edge_pixels[:, 1], edge_pixels[:, 0], s=2, c="b")
X = [pix[1] for pix in edge_pixels]
Y = [pix[0] for pix in edge_pixels]
U = [3 * pix[1] for pix in edge_normals]
V = [-3 * pix[0] for pix in edge_normals]
plt.quiver(X,
Y,
U,
V,
units="x",
scale=0.25,
width=0.5,
zorder=2,
color="r")
vis.title("Edge pixels and normals")
vis.subplot(1, 3, 2)
vis.imshow(depth_im_threshed)
vis.title("Edge map")
vis.subplot(1, 3, 3)
vis.imshow(segmask)
vis.title("Segmask")
vis.show()
# Exit if no edge pixels.
if num_pixels == 0:
return []
# Form set of valid candidate point pairs. #先找到一部分符合要求的抓取点。
pruning_start = time()
max_grasp_width_px = Grasp2D(
Point(np.zeros(2)),
0.0,
min_depth,
width=self._gripper_width,
camera_intr=camera_intr).width_px #计算Returns the width in pixels.
normal_ip = edge_normals.dot(edge_normals.T) #ab cos（θ）是一个对称矩阵
dists = ssd.squareform(ssd.pdist(edge_pixels)) # 是一个对称矩阵
#ssd.pdist(edge_pixels)计算每个点距离其他点之间的距离。ssd.squareform 用来把一个向量格式的距离向量转换成一个方阵格式的距离矩阵，反之亦然。
#ssd.pdist先形成距离函数，再由squareform形成一个距离方阵
#https://blog.csdn.net/qq_20135597/article/details/94212816?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-3.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-3.control
valid_indices = np.where(
(normal_ip < -np.cos(np.arctan(self._friction_coef)))
& (dists < max_grasp_width_px) & (dists > 0.0)
) #两个点之间符合force closure，距离小于max_grasp_width，同时两点距离大于零。np.where对于二维方阵生成的是两个array，是两个配对点的索引
valid_indices = np.c_[valid_indices[0], valid_indices[
1]] #Translates slice objects to concatenation along the second axis.即将valid_indices[1]并到valid_indices[0]后面。
self._logger.debug("Normal pruning %.3f sec" %
(time() - pruning_start))
# Raise exception if no antipodal pairs.
num_pairs = valid_indices.shape[0]
if num_pairs == 0:
return []
# Prune out grasps.
contact_points1 = edge_pixels[
valid_indices[:,
0], :] # valid_indices：[ [c1x,c1y] [c2x,c2y].. ] valid_indices所有的cnx所对应的那一行 也就是第一个点的坐标
contact_points2 = edge_pixels[
valid_indices[:,
1], :] # valid_indices：[ [c1x,c1y] [c2x,c2y].. ] valid_indices所有的cny所对应的那一行 也就是第二个点的坐标
contact_normals1 = edge_normals[valid_indices[:, 0], :] #
contact_normals2 = edge_normals[valid_indices[:, 1], :] #
v = contact_points1 - contact_points2
v_norm = np.linalg.norm(v, axis=1)
v = v / np.tile(v_norm[:, np.newaxis], [1, 2])
ip1 = np.sum(contact_normals1 * v, axis=1)
ip2 = np.sum(contact_normals2 * (-v), axis=1)
ip1[ip1 > 1.0] = 1.0
ip1[ip1 < -1.0] = -1.0
ip2[ip2 > 1.0] = 1.0
ip2[ip2 < -1.0] = -1.0
beta1 = np.arccos(ip1)
beta2 = np.arccos(ip2)
alpha = np.arctan(self._friction_coef)
antipodal_indices = np.where((beta1 < alpha) & (beta2 < alpha))[0]
# Raise exception if no antipodal pairs.
num_pairs = antipodal_indices.shape[0]
if num_pairs == 0:
return []
sample_size = min(self._max_rejection_samples,
num_pairs) #防止采样个数过多造成计算负担，设置一个最大的采样量。
grasp_indices = np.random.choice(
antipodal_indices, #随机选择一部分抓取
size=sample_size,
replace=False)
self._logger.debug("Grasp comp took %.3f sec" %
(time() - pruning_start))
# Compute grasps.
sample_start = time()
k = 0
grasps = []
while k < sample_size and len(grasps) < num_samples:
grasp_ind = grasp_indices[k]
p1 = contact_points1[grasp_ind, :] #第一个点的坐标
p2 = contact_points2[grasp_ind, :] #第二个点的坐标
n1 = contact_normals1[grasp_ind, :]
n2 = contact_normals2[grasp_ind, :]
# width = np.linalg.norm(p1 - p2)
k += 1
depth_p1 = depth_im[p1[0], p1[1]]
depth_p2 = depth_im[p2[0], p2[1]]
# Compute center and axis.
grasp_center = (p1 + p2) // 2 #一个抓取的中心
grasp_axis = p2 - p1
grasp_axis = grasp_axis / np.linalg.norm(grasp_axis)
grasp_theta = np.pi / 2
if grasp_axis[1] != 0:
grasp_theta = np.arctan2(grasp_axis[0],
grasp_axis[1]) #一个抓取的轴角
grasp_center_pt = Point(np.array(
[grasp_center[1], grasp_center[0]]),
frame=camera_intr.frame)
# Compute grasp points in 3D.只是为了确定两点距离小于夹爪宽度
x1 = point_cloud_im[p1[0], p1[1]]
x2 = point_cloud_im[p2[0], p2[1]]
if np.linalg.norm(x2 - x1) > self._gripper_width:
continue
# Perturb.
if self._grasp_center_sigma > 0.0:
grasp_center_pt = grasp_center_pt + ss.multivariate_normal.rvs(
cov=self._grasp_center_sigma * np.diag(np.ones(2)))
if self._grasp_angle_sigma > 0.0:
grasp_theta = grasp_theta + ss.norm.rvs(
scale=self._grasp_angle_sigma)
# Check center px dist from boundary. 检查抓取中心是否在工作空间内
if (grasp_center[0] < self._min_dist_from_boundary
or grasp_center[1] < self._min_dist_from_boundary
or grasp_center[0] >
depth_im.height - self._min_dist_from_boundary
or grasp_center[1] >
depth_im.width - self._min_dist_from_boundary):
continue
depth_p1 = depth_im[p1[0], p1[1]]
depth_p2 = depth_im[p2[0], p2[1]]
depth_grasp = (depth_p1 + depth_p2) / 2
depth_grasp = depth_grasp[0]
candidate_grasp = Grasp2D(grasp_center_pt,
grasp_theta,
depth_grasp,
width=self._gripper_width,
camera_intr=camera_intr,
contact_points=[p1, p2],
contact_normals=[n1, n2])
grasps.append(candidate_grasp)
'''
for i in range(self._depth_samples_per_grasp):
# Get depth in the neighborhood of the center pixel.
depth_win = depth_im.data[grasp_center[0] -
self._h:grasp_center[0] +
self._h, grasp_center[1] -
self._w:grasp_center[1] + self._w]
center_depth = np.min(depth_win)
if center_depth == 0 or np.isnan(center_depth):
continue
# Sample depth between the min and max.
min_depth = center_depth + self._min_depth_offset
max_depth = center_depth + self._max_depth_offset
sample_depth = min_depth + (max_depth -
min_depth) * np.random.rand()
candidate_grasp = Grasp2D(grasp_center_pt,
grasp_theta,
sample_depth,
width=self._gripper_width,
camera_intr=camera_intr,
contact_points=[p1, p2],
contact_normals=[n1, n2])
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.grasp(candidate_grasp)
vis.scatter(p1[1], p1[0], c="b", s=25)
vis.scatter(p2[1], p2[0], c="b", s=25)
vis.show()
grasps.append(candidate_grasp)
'''
# Return sampled grasps.
self._logger.debug("Loop took %.3f sec" % (time() - sample_start))
return grasps
def _plan_grasp(self,
color_im,
depth_im,
camera_intr,
bounding_box=None,
segmask=None):
""" Grasp planner request handler .
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS ServiceRequest for grasp planner service
"""
rospy.loginfo('Planning Grasp')
# inpaint images
color_im = color_im.inpaint(
rescale_factor=self.cfg['inpaint_rescale_factor'])
depth_im = depth_im.inpaint(
rescale_factor=self.cfg['inpaint_rescale_factor'])
# init segmask
if segmask is None:
segmask = BinaryImage(255 *
np.ones(depth_im.shape).astype(np.uint8),
frame=color_im.frame)
# visualize
if self.cfg['vis']['color_image']:
vis.imshow(color_im)
vis.show()
if self.cfg['vis']['depth_image']:
vis.imshow(depth_im)
vis.show()
if self.cfg['vis']['segmask'] and segmask is not None:
vis.imshow(segmask)
vis.show()
# aggregate color and depth images into a single perception rgbdimage
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
# mask bounding box
if bounding_box is not None:
# calc bb parameters
min_x = bounding_box.minX
min_y = bounding_box.minY
max_x = bounding_box.maxX
max_y = bounding_box.maxY
# contain box to image->don't let it exceed image height/width bounds
if min_x < 0:
min_x = 0
if min_y < 0:
min_y = 0
if max_x > rgbd_im.width:
max_x = rgbd_im.width
if max_y > rgbd_im.height:
max_y = rgbd_im.height
# mask
bb_segmask_arr = np.zeros([rgbd_image.height, rgbd_image.width])
bb_segmask_arr[min_y:max_y, min_x:max_x] = 255
bb_segmask = BinaryImage(bb_segmask_arr.astype(np.uint8),
segmask.frame)
segmask = segmask.mask_binary(bb_segmask)
# visualize
if self.cfg['vis']['rgbd_state']:
masked_rgbd_im = rgbd_im.mask_binary(segmask)
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(masked_rgbd_im.color)
vis.subplot(1, 2, 2)
vis.imshow(masked_rgbd_im.depth)
vis.show()
# create an RGBDImageState with the cropped RGBDImage and CameraIntrinsics
rgbd_state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# execute policy
try:
return self.execute_policy(rgbd_state, self.grasping_policy,
self.grasp_pose_publisher,
camera_intr.frame)
except NoValidGraspsException:
rospy.logerr(
'While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!'
)
raise rospy.ServiceException(
'While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!'
)
def _sample_suction_points(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False,
constraint_fn=None):
"""Sample a set of 2D suction point candidates from a depth image by
choosing points on an object surface uniformly at random
and then sampling around the surface normal.
Parameters
----------
depth_im : :obj:"perception.DepthImage"
Depth image to sample from.
camera_intr : :obj:`perception.CameraIntrinsics`
Intrinsics of the camera that captured the images.
num_samples : int
Number of grasps to sample.
segmask : :obj:`perception.BinaryImage`
Binary image segmenting out the object of interest.
visualize : bool
Whether or not to show intermediate samples (for debugging).
constraint_fn : :obj:`GraspConstraintFn`
Constraint function to apply to grasps.
Returns
-------
:obj:`list` of :obj:`SuctionPoint2D`
List of 2D suction point candidates.
"""
# Compute edge pixels.
filter_start = time()
if self._depth_gaussian_sigma > 0:
depth_im_mask = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_gaussian_sigma)
else:
depth_im_mask = depth_im.copy()
if segmask is not None:
depth_im_mask = depth_im.mask_binary(segmask)
self._logger.debug("Filtering took %.3f sec" % (time() - filter_start))
if visualize:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(depth_im)
vis.subplot(1, 2, 2)
vis.imshow(depth_im_mask)
vis.show()
# Project to get the point cloud.
cloud_start = time()
point_cloud_im = camera_intr.deproject_to_image(depth_im_mask)
normal_cloud_im = point_cloud_im.normal_cloud_im()
nonzero_px = depth_im_mask.nonzero_pixels()
num_nonzero_px = nonzero_px.shape[0]
if num_nonzero_px == 0:
return []
self._logger.debug("Normal cloud took %.3f sec" %
(time() - cloud_start))
# Randomly sample points and add to image.
sample_start = time()
suction_points = []
k = 0
sample_size = min(self._max_num_samples, num_nonzero_px)
indices = np.random.choice(num_nonzero_px,
size=sample_size,
replace=False)
while k < sample_size and len(suction_points) < num_samples:
# Sample a point uniformly at random.
ind = indices[k]
center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]])
center = Point(center_px, frame=camera_intr.frame)
axis = -normal_cloud_im[center.y, center.x]
# depth = point_cloud_im[center.y, center.x][2]
orientation = 2 * np.pi * np.random.rand()
# Update number of tries.
k += 1
# Skip bad axes.
if np.linalg.norm(axis) == 0:
continue
# Rotation matrix.
x_axis = axis
y_axis = np.array([axis[1], -axis[0], 0])
if np.linalg.norm(y_axis) == 0:
y_axis = np.array([1, 0, 0])
y_axis = y_axis / np.linalg.norm(y_axis)
z_axis = np.cross(x_axis, y_axis)
R = np.array([x_axis, y_axis, z_axis]).T
# R_orig = np.copy(R)
R = R.dot(RigidTransform.x_axis_rotation(orientation))
t = point_cloud_im[center.y, center.x]
pose = RigidTransform(rotation=R,
translation=t,
from_frame="grasp",
to_frame=camera_intr.frame)
# Check center px dist from boundary.
if (center_px[0] < self._min_dist_from_boundary
or center_px[1] < self._min_dist_from_boundary
or center_px[1] >
depth_im.height - self._min_dist_from_boundary
or center_px[0] >
depth_im.width - self._min_dist_from_boundary):
continue
# Keep if the angle between the camera optical axis and the suction
# direction is less than a threshold.
dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0)
psi = np.arccos(dot)
if psi < self._max_suction_dir_optical_axis_angle:
# Check distance to ensure sample diversity.
candidate = MultiSuctionPoint2D(pose, camera_intr=camera_intr)
# Check constraint satisfaction.
if constraint_fn is None or constraint_fn(candidate):
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.scatter(center.x, center.y)
vis.show()
suction_points.append(candidate)
self._logger.debug("Loop took %.3f sec" % (time() - sample_start))
return suction_points
def run_experiment():
""" Run the experiment """
if not config['robot_off']:
rospy.loginfo('Initializing YuMi')
robot, subscriber, left_arm, right_arm, home_pose = init_robot(config)
# create ROS CVBridge
cv_bridge = CvBridge()
# wait for Grasp Planning Service and create Service Proxy
rospy.wait_for_service('plan_gqcnn_grasp')
plan_grasp = rospy.ServiceProxy('plan_gqcnn_grasp', GQCNNGraspPlanner)
# get camera intrinsics
camera_intrinsics = sensor.ir_intrinsics
# setup experiment logger
experiment_logger = GraspIsolatedObjectExperimentLogger(config['experiment_dir'],
config['supervisor'],
camera_intrinsics,
T_camera_world,
'/home/autolab/Workspace/vishal_working/catkin_ws/src/gqcnn/cfg/ros_nodes/yumi_control_node.yaml',
planner_type=config['planner_type'])
logging.info('Saving experiment to %s' %(experiment_logger.experiment_path))
object_keys = config['test_object_keys']
trial_number = 1
re_try = False
logging.info('Beginning experiment')
while True:
if not re_try:
experiment_logger.start_trial()
obj = np.random.choice(object_keys, size=1)[0]
else:
re_try = False
rospy.loginfo('Please place object: ' + obj + ' on the workspace.')
raw_input("Press ENTER when ready ...")
# start the next trial
rospy.loginfo('Trial %d' % (trial_number))
# get the images from the sensor
color_image, depth_image, _ = sensor.frames()
# log some trial info
experiment_logger.update_trial_attribute('trial_num', trial_number)
experiment_logger.update_trial_attribute('color_im', color_image)
experiment_logger.update_trial_attribute('depth_im', depth_image)
# inpaint to remove holes
inpainted_color_image = color_image.inpaint(rescale_factor=config['inpaint_rescale_factor'])
inpainted_depth_image = depth_image.inpaint(rescale_factor=config['inpaint_rescale_factor'])
detector = RgbdDetectorFactory.detector('point_cloud_box')
detection = detector.detect(inpainted_color_image, inpainted_depth_image, detector_cfg, camera_intrinsics, T_camera_world, vis_foreground=False, vis_segmentation=False
)[0]
if config['vis']['vis_detector_output']:
vis.figure()
vis.subplot(1,2,1)
vis.imshow(detection.color_thumbnail)
vis.subplot(1,2,2)
vis.imshow(detection.depth_thumbnail)
vis.show()
boundingBox = BoundingBox()
boundingBox.minY = detection.bounding_box.min_pt[0]
boundingBox.minX = detection.bounding_box.min_pt[1]
boundingBox.maxY = detection.bounding_box.max_pt[0]
boundingBox.maxX = detection.bounding_box.max_pt[1]
try:
start_time = time.time()
planned_grasp_data = plan_grasp(inpainted_color_image.rosmsg, inpainted_depth_image.rosmsg, camera_intrinsics.rosmsg, boundingBox)
grasp_plan_time = time.time() - start_time
lift_gripper_width, T_gripper_world = process_GQCNNGrasp(planned_grasp_data, robot, left_arm, right_arm, subscriber, home_pose, config)
# get human label
human_input = raw_input('Grasp success, or grasp failure? [s/f] ')
while human_input.lower() != 's' and human_input.lower() != 'f':
logging.info('Did not understand input. Please answer \'s\' or \'f\'')
human_input = raw_input('Grasp success, or grasp failure? [s/f] ')
if human_input.lower() == 's':
experiment_logger.update_trial_attribute('human_label', 1)
else:
experiment_logger.update_trial_attribute('human_label', 0)
# log result
experiment_logger.update_trial_attribute('gripper_pose', T_gripper_world)
experiment_logger.update_trial_attribute('planning_time', grasp_plan_time)
experiment_logger.update_trial_attribute('gripper_width', lift_gripper_width)
experiment_logger.update_trial_attribute('found_grasp', 1)
experiment_logger.update_trial_attribute('completed', True)
experiment_logger.update_trial_attribute('object_key', obj)
trial_number += 1
except rospy.ServiceException as e:
rospy.logerr("Service call failed: \n %s" % e)
experiment_logger.update_trial_attribute('found_grasp', 0)
experiment_logger.update_trial_attribute('completed', True)
experiment_logger.update_trial_attribute('object_key', obj)
trial_number += 1
except (YuMiCommException, YuMiControlException) as yce:
rospy.logerr(str(yce))
if sensor is not None:
sensor.stop()
if robot is not None:
robot.stop()
if subscriber is not None and subscriber._started:
subscriber.stop()
rospy.loginfo("Re-trying")
re_try = True
robot, subscriber, left_arm, right_arm, home_pose = init_robot(config)
