def register_point_cloud(self):
registration_result_array = []
depth_im_seg, camera_intr = self.get_object_point_cloud_from_sensor(
) #self.database_objects[1]#
source_point_cloud, source_normal_cloud = self.get_point_normal_cloud(
depth_im_seg, camera_intr)
source_sample_size = int(source_point_cloud.shape[1])
if self.debug:
print source_sample_size
vis.figure()
vis.subplot(1, 1, 1)
vis.imshow(depth_im_seg)
vis.show()
source_sample_size = 1000
p2pis = PointToPlaneICPSolver(sample_size=source_sample_size)
p2pfm = PointToPlaneFeatureMatcher()
for objectFileName in self.obj_filenames:
self.load_object_point_clouds([objectFileName])
for (target_depth_im, target_camera_intr) in self.database_objects:
if self.debug:
vis.figure()
vis.subplot(1, 1, 1)
vis.imshow(target_depth_im)
vis.show()
target_point_cloud, target_normal_cloud = self.get_point_normal_cloud(
target_depth_im, target_camera_intr)
registration_result = p2pis.register(source_point_cloud,
target_point_cloud,
source_normal_cloud,
target_normal_cloud,
p2pfm,
num_iterations=10)
registration_result_array.append(registration_result)
return registration_result_array
def get_current_point_cloud(debug=False, start_node=True, env='simulator'):
giob = GetInitialObjectBelief(None, debug, start_node, env)
(depth_im_seg, _) = giob.get_object_point_cloud_from_sensor()
if debug:
vis.figure()
vis.subplot(1, 1, 1)
vis.imshow(depth_im_seg)
vis.show()
def save_point_cloud(self, filename_prefix, depth_im_seg, camera_intr):
if self.debug:
vis.figure()
vis.subplot(1, 1, 1)
vis.imshow(depth_im_seg)
vis.show()
depth_im_seg.save(filename_prefix + '.npy')
camera_intr.save(filename_prefix + '.intr')
def get_current_rgb_image(debug=False, start_node=True, env='simulator'):
giob = GetInitialObjectBelief(None, debug, start_node, env)
(color_im_seg, camera_intr) = giob.get_world_color_image()
(depth_im_seg,
camera_intr) = giob.get_object_point_cloud_from_sensor(None, False)
if debug:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(depth_im_seg)
vis.subplot(1, 2, 2)
vis.imshow(color_im_seg)
vis.show()
return (depth_im_seg, color_im_seg, camera_intr)
def load_object_point_clouds(self, obj_filenames=None):
if obj_filenames is None:
obj_filenames = self.obj_filenames
ans = []
for filename in obj_filenames:
print "Loading " + filename
depth_im, camera_intr = self.load_saved_point_cloud(filename)
ans.append((depth_im, camera_intr))
if self.debug:
vis.figure()
vis.subplot(1, 1, 1)
vis.imshow(depth_im)
vis.show()
self.database_objects = ans
return ans
def get_current_point_cloud_for_movement(min_x,
debug=False,
start_node=True,
env='simulator',
min_z=None):
giob = GetInitialObjectBelief(None, debug, start_node, env)
(camera_intr, point_cloud_world,
T_camera_world) = giob.get_world_point_cloud()
cfg = copy.deepcopy(giob.detector_cfg)
cfg['min_pt'][2] = cfg['min_z_for_movement']
if min_z is not None:
cfg['min_pt'][2] = min_z
if min_x > cfg['min_pt'][0]:
cfg['min_pt'][0] = min_x
seg_point_cloud_world = giob.get_segmented_point_cloud_world(
cfg, point_cloud_world)
"""
import sensor_msgs.point_cloud2 as pcl2
from sensor_msgs.msg import Image, PointCloud2, PointField
print point_cloud_world.data.shape
pc2 = PointCloud2()
pc2.header.frame_id = giob.WORLD_FRAME
segmented_pc = pcl2.create_cloud_xyz32(pc2.header, np.transpose(seg_point_cloud_world.data))
pcl_pub = rospy.Publisher('mico_node/pointcloud', PointCloud2, queue_size=10)
i = 0
while not rospy.is_shutdown():
#hello_str = "hello world %s" % rospy.get_time()
#rospy.loginfo(hello_str)
pcl_pub.publish(segmented_pc)
#depth_im_pub.publish(depth_im)
rospy.sleep(5)
i = i+1
if i > 5:
break
#return copy.deepcopy(point_cloud_world)
"""
if debug:
seg_point_cloud_cam = T_camera_world.inverse() * seg_point_cloud_world
depth_im_seg = camera_intr.project_to_image(seg_point_cloud_cam)
vis.figure()
vis.subplot(1, 1, 1)
vis.imshow(depth_im_seg)
vis.show()
return seg_point_cloud_world
def main():
delete_files = False
directory_name = None
out_directory_name = './'
num_days = 5
opts, args = getopt.getopt(sys.argv[1:], "d:o:", ["dir=", "out="])
for opt, arg in opts:
if opt in ("-d", "--dir"):
directory_name = arg
if opt in ("-o", "--out"):
out_directory_name = arg
if directory_name is None:
print "Please specify directory name"
return
num_saved = 0
for root, dir, files in os.walk(directory_name):
#print "Root " + root
for file in files:
filename = os.path.join(root, file)
#print filename
if filename.endswith('.npz') and "depth" not in filename:
rgb_image = perception.ColorImage.open(filename)
vis.figure()
vis.subplot(1, 1, 1)
vis.imshow(rgb_image)
vis.show(block=False)
save_ans = raw_input("Shall I save[y|n]?")
if save_ans == 'y':
png_file_name = os.path.basename(filename).replace(
'.npz', '.png')
new_file_name = os.path.join(out_directory_name,
png_file_name)
rgb_image.save(new_file_name)
num_saved = num_saved + 1
print num_saved
plt.close()
def get_depth_image_thumbmail(depth_im, size_x=16, size_y=8, debug=False):
depth_im_centered, _ = depth_im.center_nonzero()
#if crop_point is None:
# crop_point = depth_im_centered.center
#check
depth_im_focus = depth_im_centered.focus(size_x, size_y)
depth_im_focus_zero_pixels = depth_im_focus.zero_pixels()
depth_im_centered_zero_pixels = depth_im_centered.zero_pixels()
clipped = False
try:
assert np.array_equal(depth_im_focus_zero_pixels,
depth_im_centered_zero_pixels)
except:
clipped = True
#debug = True
"""
print depth_im_focus_zero_pixels[0]
print depth_im_centered_zero_pixels[0]
a = np.append(depth_im_focus_zero_pixels,depth_im_centered_zero_pixels, axis=0)
b,counts = np.unique(["{}-{}".format(i, j) for i, j in a], return_counts=True)
#c = np.where(counts==1)
print len(b)
print len(counts)
c = np.array([map(int,x.split('-')) for x in b[np.where(counts==1)]])
print c
print depth_im_focus.center
print depth_im_centered[c[:,0],c[:,1]]
"""
depth_im_crop = depth_im_centered.crop(size_x, size_y)
depth_im_crop_thumbnail = depth_im_crop.resize(0.25)
if debug:
vis.figure()
vis.subplot(1, 4, 1)
vis.imshow(depth_im_crop)
vis.subplot(1, 4, 2)
vis.imshow(depth_im_centered)
vis.subplot(1, 4, 3)
vis.imshow(depth_im)
vis.subplot(1, 4, 4)
vis.imshow(depth_im_crop_thumbnail)
vis.show()
return depth_im_crop_thumbnail, clipped
def action(self, state):
""" Plans the grasp with the highest probability of success on
the given RGB-D image.
Attributes
----------
state : :obj:`RgbdImageState`
image to plan grasps on
Returns
-------
:obj:`ParallelJawGrasp`
grasp to execute
"""
# check valid input
if not isinstance(state, RgbdImageState):
raise ValueError('Must provide an RGB-D image state.')
# parse state
rgbd_im = state.rgbd_im
camera_intr = state.camera_intr
segmask = state.segmask
# sample grasps
grasps = self._grasp_sampler.sample(
rgbd_im,
camera_intr,
self._num_grasp_samples,
segmask=segmask,
visualize=self.config['vis']['grasp_sampling'],
seed=None)
num_grasps = len(grasps)
# form tensors
image_tensor, pose_tensor = self.grasps_to_tensors(grasps, state)
if self.config['vis']['tf_images']:
# read vis params
k = self.config['vis']['k']
d = utils.sqrt_ceil(k)
# display grasp transformed images
vis.figure(size=(FIGSIZE, FIGSIZE))
for i, image_tf in enumerate(image_tensor[:k, ...]):
depth = pose_tensor[i][0]
vis.subplot(d, d, i + 1)
vis.imshow(DepthImage(image_tf))
vis.title('Image %d: d=%.3f' % (i, depth))
vis.show()
# predict grasps
predict_start = time()
output_arr = self.gqcnn.predict(image_tensor, pose_tensor)
q_values = output_arr[:, -1]
logging.debug('Prediction took %.3f sec' % (time() - predict_start))
if self.config['vis']['grasp_candidates']:
# display each grasp on the original image, colored by predicted success
vis.figure(size=(FIGSIZE, FIGSIZE))
vis.imshow(rgbd_im.depth)
for grasp, q in zip(grasps, q_values):
vis.grasp(grasp,
scale=1.5,
show_center=False,
show_axis=True,
color=plt.cm.RdYlBu(q))
vis.title('Sampled grasps')
vis.show()
if self.config['vis']['grasp_ranking']:
# read vis params
k = self.config['vis']['k']
d = utils.sqrt_ceil(k)
# form camera intr for the thumbnail (to compute gripper width)
scale_factor = float(self.gqcnn.im_width) / float(self._crop_width)
scaled_camera_intr = camera_intr.resize(scale_factor)
# sort grasps
q_values_and_indices = zip(q_values, np.arange(num_grasps))
q_values_and_indices.sort(key=lambda x: x[0], reverse=True)
vis.figure(size=(FIGSIZE, FIGSIZE))
for i, p in enumerate(q_values_and_indices[:k]):
# read stats for grasp
q_value = p[0]
ind = p[1]
depth = pose_tensor[ind][0]
image = DepthImage(image_tensor[ind, ...])
grasp = Grasp2D(Point(image.center),
0.0,
depth,
width=self._gripper_width,
camera_intr=scaled_camera_intr)
# plot
vis.subplot(d, d, i + 1)
vis.imshow(image)
vis.grasp(grasp, scale=1.5)
vis.title('K=%d: d=%.3f, q=%.3f' % (i, depth, q_value))
vis.show()
# select grasp
index = self.select(grasps, q_values)
grasp = grasps[index]
q_value = q_values[index]
image = DepthImage(image_tensor[index, ...])
pose = pose_tensor[index, ...]
depth = pose[0]
if self.config['vis']['grasp_plan']:
scale_factor = float(self.gqcnn.im_width) / float(self._crop_width)
scaled_camera_intr = camera_intr.resize(scale_factor)
grasp = Grasp2D(Point(image.center),
0.0,
pose[0],
width=self._gripper_width,
camera_intr=scaled_camera_intr)
vis.figure()
vis.imshow(image)
vis.grasp(grasp, scale=1.5, show_center=False, show_axis=True)
vis.title('Best Grasp: d=%.3f, q=%.3f' % (depth, q_value))
vis.show()
# return action
return ParallelJawGrasp(grasp, q_value, image)
# setup sensor
sensor = RgbdSensorFactory.sensor(sensor_type, config['sensor'])
sensor.start()
camera_intr = sensor.ir_intrinsics
# read images
color_im, depth_im, _ = sensor.frames()
color_im = color_im.inpaint(rescale_factor=inpaint_rescale_factor)
depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor)
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
state = RgbdImageState(rgbd_im, camera_intr)
# init policy
policy = CrossEntropyAntipodalGraspingPolicy(policy_config)
policy_start = time.time()
action = policy(state)
logging.info('Planning took %.3f sec' % (time.time() - policy_start))
# vis final grasp
if policy_config['vis']['final_grasp']:
vis.figure(size=(10, 10))
vis.subplot(1, 2, 1)
vis.imshow(rgbd_im.color)
vis.grasp(action.grasp, scale=1.5, show_center=False, show_axis=True)
vis.title('Planned grasp on color (Q=%.3f)' % (action.q_value))
vis.subplot(1, 2, 2)
vis.imshow(rgbd_im.depth)
vis.grasp(action.grasp, scale=1.5, show_center=False, show_axis=True)
vis.title('Planned grasp on depth (Q=%.3f)' % (action.q_value))
vis.show()
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_berkeley.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]
# 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]
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)
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)))
# binary
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()
# 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)
tf_grasp_2d = Grasp2D(
grasp_center,
0,
grasp_2d.depth,
width=gripper.max_width,
camera_intr=final_camera_intr)
# plot 2D grasp image
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()
# plot 3D visualization
vis.figure()
T_obj_world = vis.mesh_stable_pose(
obj.mesh,
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 visualize_tensor_dataset(dataset, config):
"""
Visualizes a Tensor dataset.
Parameters
----------
dataset : :obj:`TensorDataset`
dataset to visualize
config : :obj:`autolab_core.YamlConfig`
parameters for visualization
Notes
-----
Required parameters of config are specified in Other Parameters
Other Parameters
----------------
field_name : str
name of the field in the TensorDataset to visualize (defaults to depth_ims_tf_table, which is a single view point cloud of the object on a table)
field_type : str
type of image that the field name correspondes to (defaults to depth, can also be `segmask` if using the field `object_masks`)
print_fields : :obj:`list` of str
names of additiona fields to print to the command line
filter : :obj:`dict` mapping str to :obj:`dict`
contraints that all displayed datapoints must satisfy (supports any univariate field name as a key and numeric thresholds)
gripper_width_px : float
width of the gripper to plot in pixels
font_size : int
size of font on the rendered images
"""
# shuffle the tensor indices
indices = dataset.datapoint_indices
np.random.shuffle(indices)
# read config
field_name = config['field_name']
field_type = config['field_type']
font_size = config['font_size']
print_fields = config['print_fields']
gripper_width_px = config['gripper_width_px']
num = 0
for i, ind in enumerate(indices):
datapoint = dataset[ind]
data = datapoint[field_name]
if field_type == RenderMode.SEGMASK:
image = BinaryImage(data)
elif field_type == RenderMode.DEPTH:
image = DepthImage(data)
else:
raise ValueError('Field type %s not supported!' % (field_type))
skip_datapoint = False
for f, filter_cfg in config['filter'].iteritems():
data = datapoint[f]
if 'greater_than' in filter_cfg.keys(
) and data < filter_cfg['greater_than']:
skip_datapoint = True
break
elif 'less_than' in filter_cfg.keys(
) and data > filter_cfg['less_than']:
skip_datapoint = True
break
if skip_datapoint:
continue
logging.info('DATAPOINT %d' % (num))
for f in print_fields:
data = datapoint[f]
logging.info('Field %s:' % (f))
print data
grasp_2d = Grasp2D(Point(image.center), 0, datapoint['hand_poses'][2])
vis2d.figure()
if field_type == RenderMode.RGBD:
vis2d.subplot(1, 2, 1)
vis2d.imshow(image.color)
vis2d.grasp(grasp_2d, width=gripper_width_px)
vis2d.subplot(1, 2, 2)
vis2d.imshow(image.depth)
vis2d.grasp(grasp_2d, width=gripper_width_px)
elif field_type == RenderMode.GD:
vis2d.subplot(1, 2, 1)
vis2d.imshow(image.gray)
vis2d.grasp(grasp_2d, width=gripper_width_px)
vis2d.subplot(1, 2, 2)
vis2d.imshow(image.depth)
vis2d.grasp(grasp_2d, width=gripper_width_px)
else:
vis2d.imshow(image)
vis2d.grasp(grasp_2d, width=gripper_width_px)
vis2d.title('Datapoint %d: %s' % (ind, field_type))
vis2d.show()
num += 1
def _sample_antipodal_grasps(self,
rgbd_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
----------
rgbd_im : :obj:`perception.RgbdImage`
RGB-D 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 = rgbd_im.depth
depth_im = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_grad_gaussian_sigma)
depth_im_downsampled = depth_im.resize(self._rescale_factor)
depth_im_threshed = depth_im_downsampled.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = self._downsample_rate * depth_im_threshed.zero_pixels()
if segmask is not None:
edge_pixels = np.array(
[p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)])
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.data) + self._min_depth_offset
max_depth = np.max(depth_im.data) + 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, 2, 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=1, zorder=2, color='g')
vis.title('Edge pixels and normals')
vis.subplot(1, 2, 2)
vis.imshow(depth_im_threshed)
vis.title('Edge map')
vis.show()
# form set of valid candidate point pairs
sample_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]]
num_pairs = valid_indices.shape[0]
logging.debug('Normal pruning %.3f sec' % (time() - sample_start))
# raise exception if no antipodal pairs
if num_pairs == 0:
return []
# iteratively sample grasps
k = 0
grasps = []
sample_size = min(self._max_rejection_samples, num_pairs)
candidate_pair_indices = np.random.choice(num_pairs,
size=sample_size,
replace=False)
while k < sample_size and len(grasps) < num_samples:
# sample a random pair without replacement
j = candidate_pair_indices[k]
pair_ind = valid_indices[j, :]
p1 = edge_pixels[pair_ind[0], :]
p2 = edge_pixels[pair_ind[1], :]
n1 = edge_normals[pair_ind[0], :]
n2 = edge_normals[pair_ind[1], :]
width = np.linalg.norm(p1 - p2)
k += 1
# check force closure
if force_closure(p1, p2, n1, n2, self._friction_coef):
# compute grasp parameters
grasp_center = (p1 + p2) / 2
grasp_axis = p2 - p1
grasp_axis = grasp_axis / np.linalg.norm(grasp_axis)
grasp_theta = 0
if grasp_axis[1] != 0:
grasp_theta = np.arctan(grasp_axis[0] / grasp_axis[1])
# compute distance from image center
dist_from_center = np.linalg.norm(grasp_center -
depth_im.center)
dist_from_boundary = min(
np.abs(depth_im.height - grasp_center[0]),
np.abs(depth_im.width - grasp_center[1]), grasp_center[0],
grasp_center[1])
if dist_from_center < self._max_dist_from_center and \
dist_from_boundary > self._min_dist_from_boundary:
# form grasp object
grasp_center_pt = Point(
np.array([grasp_center[1], grasp_center[0]]))
grasp = Grasp2D(grasp_center_pt, grasp_theta, 0.0)
# check grasp dists
grasp_dists = [
Grasp2D.image_dist(grasp,
candidate,
alpha=self._angle_dist_weight)
for candidate in grasps
]
if len(grasps) == 0 or np.min(
grasp_dists) > self._min_grasp_dist:
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.scatter(p1[1], p1[0])
vis.scatter(p2[1], p2[0])
vis.title('Grasp candidate %d' % (len(grasps)))
vis.show()
# 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)
grasps.append(candidate_grasp)
# return sampled grasps
return grasps
def get_data_from_source(object_type,
use_kmeans=False,
kmeans_label='',
for_test=False):
object_labels = get_baseline_labels(use_kmeans=use_kmeans,
kmeans_label=kmeans_label,
object_type=object_type)
if object_type == 'g3db_instances':
object_name = 'g3db_instances_non_test_version7'
if for_test:
object_name = 'g3db_instances_version7'
#object_name = '39_beerbottle_final-13-Nov-2015-09-07-13_instance0'
#object_name = '1_Coffeecup_final-10-Dec-2015-06-58-01_instance0'
#object_name = 'Cylinder_7'
#object_name = '39_beerbottle_final-13-Nov-2015-09-07-13_instance0'
object_file_dir = '../grasping_ros_mico/point_clouds_for_classification'
if object_type == 'g3db_instances_for_classification':
object_name = 'g3db_instances_for_classification'
#object_name = '39_beerbottle_final-13-Nov-2015-09-07-13_instance0'
object_file_dir = '../grasping_ros_mico/point_clouds_for_classification/additional_g3db_objects_for_classification'
object_file_names = giob.get_object_filenames(object_name, object_file_dir)
X = []
Y = []
object_names = []
clipped_objects = []
outfile_name = object_file_dir + "/clipped_object_list.txt"
with open(outfile_name, 'r') as f:
for line in f:
clipped_objects.append(line.strip())
for object_file_name_ in object_file_names:
object_instance_name = object_file_name_.replace('.yaml',
'').split('/')[-1]
for i in range(0, 81):
if for_test:
if object_instance_name in object_labels.keys():
Y.append(object_labels[object_instance_name])
else:
Y.append(1000)
else:
Y.append(object_labels[object_instance_name])
object_names.append(object_instance_name + "/" + repr(i))
object_file_name = object_file_name_.replace('.yaml',
'') + "/" + repr(i)
thumbnail_object_file_name = object_file_name + "_thumbnail.npy"
if os.path.exists(thumbnail_object_file_name):
print "Loading " + thumbnail_object_file_name
depth_im_cropped = perception.DepthImage.open(
thumbnail_object_file_name)
if False:
vis.figure()
vis.subplot(1, 1, 1)
vis.imshow(depth_im_cropped)
vis.show()
else:
print "Creating " + thumbnail_object_file_name
object_list = giob.load_object_file([object_file_name])
(depth_im_cropped,
clipped) = get_depth_image_thumbmail(object_list[0][0], 200,
160, False)
depth_im_cropped.save(thumbnail_object_file_name)
if clipped:
clipped_objects.append(object_file_name)
X.append(depth_im_cropped.data)
#print clipped_objects
outfile_name = object_file_dir + "/clipped_object_list.txt"
with open(outfile_name, 'w') as f:
f.write("\n".join(sorted(clipped_objects)))
f.write("\n")
assert len(X) == len(Y)
return (X, Y, object_names)
def action(self, state):
""" Plans the grasp with the highest probability of success on
the given RGB-D image.
Attributes
----------
state : :obj:`RgbdImageState`
image to plan grasps on
Returns
-------
:obj:`ParallelJawGrasp`
grasp to execute
"""
# check valid input
if not isinstance(state, RgbdImageState):
raise ValueError('Must provide an RGB-D image state.')
# parse state
rgbd_im = state.rgbd_im
camera_intr = state.camera_intr
segmask = state.segmask
# sample grasps
grasps = self._grasp_sampler.sample(
rgbd_im,
camera_intr,
self._num_seed_samples,
segmask=segmask,
visualize=self.config['vis']['grasp_sampling'],
seed=self._seed)
num_grasps = len(grasps)
if num_grasps == 0:
logging.warning('No valid grasps could be found')
return None
# form tensors
image_tensor, pose_tensor = self.grasps_to_tensors(grasps, state)
if self.config['vis']['tf_images']:
# read vis params
k = self.config['vis']['k']
d = utils.sqrt_ceil(k)
# display grasp transformed images
vis.figure(size=(FIGSIZE, FIGSIZE))
for i, image_tf in enumerate(image_tensor[:k, ...]):
depth = pose_tensor[i][0]
vis.subplot(d, d, i + 1)
vis.imshow(DepthImage(image_tf))
vis.title('Image %d: d=%.3f' % (i, depth))
# display grasp transformed images
vis.figure(size=(FIGSIZE, FIGSIZE))
for i in range(d):
image_tf = image_tensor[i, ...]
depth = pose_tensor[i][0]
grasp = grasps[i]
vis.subplot(d, 2, 2 * i + 1)
vis.imshow(rgbd_im.depth)
vis.grasp(grasp, scale=1.5, show_center=False, show_axis=True)
vis.title('Grasp %d: d=%.3f' % (i, depth))
vis.subplot(d, 2, 2 * i + 2)
vis.imshow(DepthImage(image_tf))
vis.title('TF image %d: d=%.3f' % (i, depth))
vis.show()
# iteratively refit and sample
for j in range(self._num_iters):
logging.debug('CEM iter %d' % (j))
# predict grasps
predict_start = time()
output_arr = self.gqcnn.predict(image_tensor, pose_tensor)
q_values = output_arr[:, -1]
logging.debug('Prediction took %.3f sec' %
(time() - predict_start))
# sort grasps
q_values_and_indices = zip(q_values, np.arange(num_grasps))
q_values_and_indices.sort(key=lambda x: x[0], reverse=True)
if self.config['vis']['grasp_candidates']:
# display each grasp on the original image, colored by predicted success
vis.figure(size=(FIGSIZE, FIGSIZE))
vis.imshow(rgbd_im.depth)
for grasp, q in zip(grasps, q_values):
vis.grasp(grasp,
scale=1.5,
show_center=False,
show_axis=True,
color=plt.cm.RdYlBu(q))
vis.title('Sampled grasps iter %d' % (j))
vis.show()
if self.config['vis']['grasp_ranking']:
# read vis params
k = self.config['vis']['k']
d = utils.sqrt_ceil(k)
# form camera intr for the thumbnail (to compute gripper width)
scale_factor = float(self.gqcnn.im_width) / float(
self._crop_width)
scaled_camera_intr = camera_intr.resize(scale_factor)
vis.figure(size=(FIGSIZE, FIGSIZE))
for i, p in enumerate(q_values_and_indices[:k]):
# read stats for grasp
q_value = p[0]
ind = p[1]
depth = pose_tensor[ind][0]
image = DepthImage(image_tensor[ind, ...])
grasp = Grasp2D(Point(image.center),
0.0,
depth,
width=self._gripper_width,
camera_intr=scaled_camera_intr)
# plot
vis.subplot(d, d, i + 1)
vis.imshow(image)
vis.grasp(grasp, scale=1.5)
vis.title('K=%d: d=%.3f, q=%.3f' % (i, depth, q_value))
vis.show()
# fit elite set
num_refit = max(int(np.ceil(self._gmm_refit_p * num_grasps)), 1)
elite_q_values = [i[0] for i in q_values_and_indices[:num_refit]]
elite_grasp_indices = [
i[1] for i in q_values_and_indices[:num_refit]
]
elite_grasps = [grasps[i] for i in elite_grasp_indices]
elite_grasp_arr = np.array([g.feature_vec for g in elite_grasps])
if self.config['vis']['elite_grasps']:
# display each grasp on the original image, colored by predicted success
vis.figure(size=(FIGSIZE, FIGSIZE))
vis.imshow(rgbd_im.depth)
for grasp, q in zip(elite_grasps, elite_q_values):
vis.grasp(grasp,
scale=1.5,
show_center=False,
show_axis=True,
color=plt.cm.RdYlBu(q))
vis.title('Elite grasps iter %d' % (j))
vis.show()
# normalize elite set
elite_grasp_mean = np.mean(elite_grasp_arr, axis=0)
elite_grasp_std = np.std(elite_grasp_arr, axis=0)
elite_grasp_std[elite_grasp_std == 0] = 1.0
elite_grasp_arr = (elite_grasp_arr -
elite_grasp_mean) / elite_grasp_std
# fit a GMM to the top samples
num_components = max(
int(np.ceil(self._gmm_component_frac * num_refit)), 1)
uniform_weights = (1.0 / num_components) * np.ones(num_components)
gmm = GaussianMixture(n_components=num_components,
weights_init=uniform_weights,
reg_covar=self._gmm_reg_covar)
train_start = time()
gmm.fit(elite_grasp_arr)
train_duration = time() - train_start
logging.debug('GMM fitting with %d components took %.3f sec' %
(num_components, train_duration))
# sample the next grasps
sample_start = time()
grasp_vecs, _ = gmm.sample(n_samples=self._num_gmm_samples)
grasp_vecs = elite_grasp_std * grasp_vecs + elite_grasp_mean
sample_duration = time() - sample_start
logging.debug('GMM sampling took %.3f sec' % (sample_duration))
# convert features to grasps
grasps = []
for grasp_vec in grasp_vecs:
grasps.append(
Grasp2D.from_feature_vec(grasp_vec,
width=self._gripper_width,
camera_intr=camera_intr))
num_grasps = len(grasps)
if num_grasps == 0:
logging.warning('No valid grasps could be found')
return None
# form tensors
image_tensor, pose_tensor = self.grasps_to_tensors(grasps, state)
if self.config['vis']['tf_images']:
# read vis params
k = self.config['vis']['k']
d = utils.sqrt_ceil(k)
# display grasp transformed images
vis.figure(size=(FIGSIZE, FIGSIZE))
for i, image_tf in enumerate(image_tensor[:k, ...]):
depth = pose_tensor[i][0]
vis.subplot(d, d, i + 1)
vis.imshow(DepthImage(image_tf))
vis.title('Image %d: d=%.3f' % (i, depth))
vis.show()
# predict final set of grasps
predict_start = time()
output_arr = self.gqcnn.predict(image_tensor, pose_tensor)
q_values = output_arr[:, -1]
logging.debug('Final prediction took %.3f sec' %
(time() - predict_start))
if self.config['vis']['grasp_candidates']:
# display each grasp on the original image, colored by predicted success
vis.figure(size=(FIGSIZE, FIGSIZE))
vis.imshow(rgbd_im.depth)
for grasp, q in zip(grasps, q_values):
vis.grasp(grasp,
scale=1.5,
show_center=False,
show_axis=True,
color=plt.cm.RdYlBu(q))
vis.title('Final sampled grasps')
vis.show()
# select grasp
index = self.select(grasps, q_values)
grasp = grasps[index]
q_value = q_values[index]
image = DepthImage(image_tensor[index, ...])
pose = pose_tensor[index, ...]
depth = pose[0]
if self.config['vis']['grasp_plan']:
scale_factor = float(self.gqcnn.im_width) / float(self._crop_width)
scaled_camera_intr = camera_intr.resize(scale_factor)
grasp = Grasp2D(Point(image.center),
0.0,
pose[0],
width=self._gripper_width,
camera_intr=scaled_camera_intr)
vis.figure()
vis.imshow(image)
vis.grasp(grasp, scale=1.5, show_center=False, show_axis=True)
vis.title('Best Grasp: d=%.3f, q=%.3f' % (depth, q_value))
vis.show()
# return action
return ParallelJawGrasp(grasp, q_value, image)
def visualize(self):
""" Visualize predictions """
logging.info('Visualizing ' + self.datapoint_type)
# iterate through shuffled file indices
for i in self.indices:
im_filename = self.im_filenames[i]
pose_filename = self.pose_filenames[i]
label_filename = self.label_filenames[i]
logging.info('Loading Image File: ' + im_filename +
' Pose File: ' + pose_filename + ' Label File: ' +
label_filename)
# load tensors from files
metric_tensor = np.load(os.path.join(self.data_dir,
label_filename))['arr_0']
label_tensor = 1 * (metric_tensor > self.metric_thresh)
image_tensor = np.load(os.path.join(self.data_dir,
im_filename))['arr_0']
hand_poses_tensor = np.load(
os.path.join(self.data_dir, pose_filename))['arr_0']
pose_tensor = self._read_pose_data(hand_poses_tensor,
self.input_data_mode)
# score with neural network
pred_p_success_tensor = self._gqcnn.predict(
image_tensor, pose_tensor)
# compute results
classification_result = ClassificationResult(
[pred_p_success_tensor], [label_tensor])
logging.info('Error rate on files: %.3f' %
(classification_result.error_rate))
logging.info('Precision on files: %.3f' %
(classification_result.precision))
logging.info('Recall on files: %.3f' %
(classification_result.recall))
mispred_ind = classification_result.mispredicted_indices()
correct_ind = classification_result.correct_indices()
# IPython.embed()
if self.datapoint_type == 'true_positive' or self.datapoint_type == 'true_negative':
vis_ind = correct_ind
else:
vis_ind = mispred_ind
num_visualized = 0
# visualize
for ind in vis_ind:
# limit the number of sampled datapoints displayed per object
if num_visualized >= self.samples_per_object:
break
num_visualized += 1
# don't visualize the datapoints that we don't want
if self.datapoint_type == 'true_positive':
if classification_result.labels[ind] == 0:
continue
elif self.datapoint_type == 'true_negative':
if classification_result.labels[ind] == 1:
continue
elif self.datapoint_type == 'false_positive':
if classification_result.labels[ind] == 0:
continue
elif self.datapoint_type == 'false_negative':
if classification_result.labels[ind] == 1:
continue
logging.info('Datapoint %d of files for %s' %
(ind, im_filename))
logging.info('Depth: %.3f' % (hand_poses_tensor[ind, 2]))
data = image_tensor[ind, ...]
if self.display_image_type == RenderMode.SEGMASK:
image = BinaryImage(data)
elif self.display_image_type == RenderMode.GRAYSCALE:
image = GrayscaleImage(data)
elif self.display_image_type == RenderMode.COLOR:
image = ColorImage(data)
elif self.display_image_type == RenderMode.DEPTH:
image = DepthImage(data)
elif self.display_image_type == RenderMode.RGBD:
image = RgbdImage(data)
elif self.display_image_type == RenderMode.GD:
image = GdImage(data)
vis2d.figure()
if self.display_image_type == RenderMode.RGBD:
vis2d.subplot(1, 2, 1)
vis2d.imshow(image.color)
grasp = Grasp2D(Point(image.center,
'img'), 0, hand_poses_tensor[ind, 2],
self.gripper_width_m)
grasp.camera_intr = grasp.camera_intr.resize(1.0 / 3.0)
vis2d.grasp(grasp)
vis2d.subplot(1, 2, 2)
vis2d.imshow(image.depth)
vis2d.grasp(grasp)
elif self.display_image_type == RenderMode.GD:
vis2d.subplot(1, 2, 1)
vis2d.imshow(image.gray)
grasp = Grasp2D(Point(image.center,
'img'), 0, hand_poses_tensor[ind, 2],
self.gripper_width_m)
grasp.camera_intr = grasp.camera_intr.resize(1.0 / 3.0)
vis2d.grasp(grasp)
vis2d.subplot(1, 2, 2)
vis2d.imshow(image.depth)
vis2d.grasp(grasp)
else:
vis2d.imshow(image)
grasp = Grasp2D(Point(image.center,
'img'), 0, hand_poses_tensor[ind, 2],
self.gripper_width_m)
grasp.camera_intr = grasp.camera_intr.resize(1.0 / 3.0)
vis2d.grasp(grasp)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %
(ind, classification_result.pred_probs[ind, 1],
classification_result.labels[ind]))
vis2d.show()
# cleanup
self._cleanup()
def run_experiment():
""" Run the experiment """
# get the images from the sensor
previous_grasp = None
while True:
rospy.loginfo("Waiting for images")
start_time = time.time()
raw_color = rospy.wait_for_message("/camera/rgb/image_color", Image)
raw_depth = rospy.wait_for_message("/camera/depth_registered/image",
Image)
image_load_time = time.time() - start_time
rospy.loginfo('Images loaded in: ' + str(image_load_time) + ' secs.')
### Create wrapped Perception RGB and Depth Images by unpacking the ROS Images using CVBridge ###
try:
color_image = perception.ColorImage(
cv_bridge.imgmsg_to_cv2(raw_color, "rgb8"),
frame=T_camera_world.from_frame)
depth_image = perception.DepthImage(
cv_bridge.imgmsg_to_cv2(raw_depth,
desired_encoding="passthrough"),
frame=T_camera_world.from_frame)
except CvBridgeError as cv_bridge_exception:
rospy.logerr(cv_bridge_exception)
# inpaint to remove holes
inpainted_color_image = color_image.inpaint(
rescale_factor=INPAINT_RESCALE_FACTOR)
inpainted_depth_image = depth_image.inpaint(
rescale_factor=INPAINT_RESCALE_FACTOR)
if DETECT_OBJECTS:
detector = RgbdDetectorFactory.detector('point_cloud_box')
detections = detector.detect(inpainted_color_image,
inpainted_depth_image,
detector_cfg,
camera_intrinsics,
T_camera_world,
vis_foreground=False,
vis_segmentation=False)
detected_obj = None
if previous_grasp is not None:
position = previous_grasp.pose.position
position = np.array([position.x, position.y, position.z])
center_point = Point(position, camera_intrinsics.frame)
center_pixel = camera_intrinsics.project(center_point,
camera_intrinsics.frame)
i, j = center_pixel.y, center_pixel.x
if DETECT_OBJECTS:
for detection in detections:
binaryIm = detection.binary_im
if binaryIm[i, j]:
segmask = binaryIm
detected_obj = detection
break
else:
# Generate an ellipse inverse segmask centered on previous grasp
y, x = np.ogrid[-i:IMAGE_HEIGHT - i, -j:IMAGE_WIDTH - j]
circlemask = x * x + y * y <= CIRCLE_RAD * CIRCLE_RAD
segmask_data = np.ones(
(IMAGE_HEIGHT, IMAGE_WIDTH), dtype=np.uint8) * 255
segmask_data[circlemask] = 0
segmask = BinaryImage(segmask_data, camera_intrinsics.frame)
else:
segmask = BinaryImage(
np.ones((IMAGE_HEIGHT, IMAGE_WIDTH), dtype=np.uint8) * 255,
camera_intrinsics.frame)
segmask._encoding = 'mono8'
if VISUALIZE_DETECTOR_OUTPUT:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(detected_obj.color_thumbnail)
vis.subplot(1, 2, 2)
vis.imshow(detected_obj.depth_thumbnail)
vis.show()
try:
rospy.loginfo('Planning Grasp')
start_time = time.time()
planned_grasp_data = plan_grasp(inpainted_color_image.rosmsg,
inpainted_depth_image.rosmsg,
segmask.rosmsg, raw_camera_info,
boundingBox)
grasp_plan_time = time.time() - start_time
rospy.loginfo('Total grasp planning time: ' +
str(grasp_plan_time) + ' secs.')
rospy.loginfo('Queueing Grasp')
previous_grasp = planned_grasp_data.grasp
execute_grasp(previous_grasp)
# raw_input("Press ENTER to resume")
except rospy.ServiceException as e:
rospy.logerr(e)
previous_grasp = None
raw_input("Press ENTER to resume")
