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 execute_policy(self, rgbd_image_state, grasping_policy,
grasp_pose_publisher, pose_frame):
""" Executes a grasping policy on an RgbdImageState
Parameters
----------
rgbd_image_state: :obj:`RgbdImageState`
RgbdImageState from perception module to encapsulate depth and color image along with camera intrinsics
grasping_policy: :obj:`GraspingPolicy`
grasping policy to use
grasp_pose_publisher: :obj:`Publisher`
ROS Publisher to publish planned grasp's ROS Pose only for visualization
pose_frame: :obj:`str`
frame of reference to publish pose alone in
"""
# execute the policy's action
rospy.loginfo('Planning Grasp')
grasp_planning_start_time = time.time()
grasp = grasping_policy(rgbd_image_state)
# create GQCNNGrasp return msg and populate it
gqcnn_grasp = GQCNNGrasp()
gqcnn_grasp.grasp_success_prob = grasp.q_value
gqcnn_grasp.pose = grasp.grasp.pose().pose_msg
# create and publish the pose alone for visualization ease of grasp pose in rviz
pose_stamped = PoseStamped()
pose_stamped.pose = grasp.grasp.pose().pose_msg
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = pose_frame
pose_stamped.header = header
grasp_pose_publisher.publish(pose_stamped)
# return GQCNNGrasp msg
rospy.loginfo('Total grasp planning time: ' +
str(time.time() - grasp_planning_start_time) + ' secs.')
if self.cfg['vis']['vis_final_grasp']:
vis.imshow(rgbd_image_state.rgbd_im)
vis.grasp(grasp.grasp,
scale=1.5,
show_center=False,
show_axis=True)
vis.show()
return gqcnn_grasp
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 visualize_observation_list(observation_list_file_name):
if('.log' in observation_list_file_name):
system_command = "grep 'Observation =' " + observation_list_file_name + " | cut -d'|' -f2"
content = subprocess.check_output(["bash", "-O", "extglob", "-c", system_command])
system_command = "grep 'Action =' " + observation_list_file_name + " | cut -d' ' -f6,7,8,9"
action_content = subprocess.check_output(["bash", "-O", "extglob", "-c", system_command])
content = content.strip()
action_content = action_content.strip()
content = content.split('\n')
action_content = action_content.split('\n')
else:
with open(observation_list_file_name) as f:
content = f.readlines()
actions_file_name = observation_list_file_name.replace('observations', 'actions')
action_content = []
if(os.path.exists(actions_file_name)):
with open(actions_file_name) as f:
action_content = f.readlines()
content = [x.strip() for x in content]
action_content = [x.strip() for x in action_content]
print content
print action_content
print len(content)
print len(action_content)
num_cols = math.ceil(math.sqrt(len(content)))
num_rows = math.ceil(len(content)*1.0/num_cols)
vis.figure()
for i in range(0,len(content)):
ax = plt.subplot(num_rows,num_cols,i+1)
filename_prefix = content[i]
camera_intr = perception.CameraIntrinsics.load(filename_prefix + '.intr')
color_im = perception.ColorImage.open(filename_prefix + '.npz', frame=camera_intr.frame)
vis.imshow(color_im)
if(i < len(action_content)):
ax.set_title(action_content[i])
vis.show()
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
# 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 plan_grasp(self, req):
""" Grasp planner request handler
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS ServiceRequest for grasp planner service
"""
rospy.loginfo('Planning Grasp')
# get the raw depth and color images as ROS Image objects
raw_color = req.color_image
raw_depth = req.depth_image
# get the raw camera info as ROS CameraInfo object
raw_camera_info = req.camera_info
# get the bounding box as a custom ROS BoundingBox msg
bounding_box = req.bounding_box
# wrap the camera info in a perception CameraIntrinsics object
camera_intrinsics = perception.CameraIntrinsics(raw_camera_info.header.frame_id, raw_camera_info.K[0], raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5], raw_camera_info.K[1], raw_camera_info.height, raw_camera_info.width)
### Create wrapped Perception RGB and Depth Images by unpacking the ROS Images using CVBridge ###
try:
color_image = perception.ColorImage(self.cv_bridge.imgmsg_to_cv2(raw_color, "rgb8"), frame=camera_intrinsics.frame)
depth_image = perception.DepthImage(self.cv_bridge.imgmsg_to_cv2(raw_depth, desired_encoding = "passthrough"), frame=camera_intrinsics.frame)
except CvBridgeError as cv_bridge_exception:
rospy.logerr(cv_bridge_exception)
# visualize
if self.cfg['vis']['vis_uncropped_color_image']:
vis.imshow(color_image)
vis.show()
if self.cfg['vis']['vis_uncropped_depth_image']:
vis.imshow(depth_image)
vis.show()
# aggregate color and depth images into a single perception rgbdimage
rgbd_image = perception.RgbdImage.from_color_and_depth(color_image, depth_image)
# calc crop parameters
minX = bounding_box.minX
minY = bounding_box.minY
maxX = bounding_box.maxX
maxY = bounding_box.maxY
# contain box to image->don't let it exceed image height/width bounds
no_pad = False
if minX < 0:
minX = 0
no_pad = True
if minY < 0:
minY = 0
no_pad = True
if maxX > rgbd_image.width:
maxX = rgbd_image.width
no_pad = True
if maxY > rgbd_image.height:
maxY = rgbd_image.height
no_pad = True
centroidX = (maxX + minX) / 2
centroidY = (maxY + minY) / 2
# add some padding to bounding box to prevent empty pixel regions when the image is rotated during grasp planning
if not no_pad:
width = (maxX - minX) + self.cfg['width_pad']
height = (maxY - minY) + self.cfg['height_pad']
else:
width = (maxX - minX)
height = (maxY - minY)
# crop camera intrinsics and rgbd image
cropped_camera_intrinsics = camera_intrinsics.crop(height, width, centroidY, centroidX)
cropped_rgbd_image = rgbd_image.crop(height, width, centroidY, centroidX)
# visualize
if self.cfg['vis']['vis_cropped_rgbd_image']:
vis.imshow(cropped_rgbd_image)
vis.show()
# create an RGBDImageState with the cropped RGBDImage and CameraIntrinsics
image_state = RgbdImageState(cropped_rgbd_image, cropped_camera_intrinsics)
# execute policy
try:
return self.execute_policy(image_state, self.grasping_policy, self.grasp_pose_publisher, cropped_camera_intrinsics.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!')
except NoAntipodalPairsFoundException:
rospy.logerr('While executing policy could not sample any antipodal point pairs from input image. Aborting Policy! Please check if there is an object in the workspace or if the output of the object detector is reasonable.')
raise rospy.ServiceException('While executing policy could not sample any antipodal point pairs from input image. Aborting Policy! Please check if there is an object in the workspace or if the output of the object detector is reasonable.')
config_filename)
# read 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)
action = ParallelJawGrasp.load(action_path)
if policy_config['vis']['input']:
vis.figure()
vis.subplot(1,2,1)
vis.imshow(state.rgbd_im.color)
vis.subplot(1,2,2)
vis.imshow(state.rgbd_im.depth)
vis.show()
# 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(state.rgbd_im.color)
os.path.join(config['calib_dir'], sensor_frame, tf_filename))
# 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)
# sample grasps
grasp_sampler = AntipodalDepthImageGraspSampler(sample_config,
gripper_width)
grasps = grasp_sampler.sample(rgbd_im,
camera_intr,
num_grasp_samples,
segmask=None,
seed=100,
visualize=visualize_sampling)
# visualize
vis.figure()
vis.imshow(depth_im)
for grasp in grasps:
vis.grasp(grasp, scale=1.5, show_center=False, show_axis=True)
vis.title('Sampled grasps')
vis.show()
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
"""
# take the greedy action with prob 1 - epsilon
if np.random.rand() > self.epsilon:
logging.debug('Taking greedy action')
return CrossEntropyAntipodalGraspingPolicy.action(self, state)
# otherwise take a random action
logging.debug('Taking random action')
# 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 random antipodal 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
# choose a grasp uniformly at random
grasp_ind = np.random.choice(num_grasps, size=1)[0]
grasp = grasps[grasp_ind]
depth = grasp.depth
# create transformed image
image_tensor, pose_tensor = self.grasps_to_tensors([grasp], state)
image = DepthImage(image_tensor[0, ...])
# predict prob success
output_arr = self.gqcnn.predict(image_tensor, pose_tensor)
q_value = output_arr[0, -1]
# visualize planned grasp
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)
vis_grasp = Grasp2D(Point(image.center),
0.0,
depth,
width=self._gripper_width,
camera_intr=scaled_camera_intr)
vis.figure()
vis.imshow(image)
vis.grasp(vis_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 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)
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)
# read config
config = YamlConfig(config_filename)
policy_config = config['policy']
# read image
depth_im_arr = pkl.load(open(depth_im_filename, 'r'))
depth_im = DepthImage(depth_im_arr)
color_im = ColorImage(
np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8))
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
segmask_arr = 255 * (depth_im_arr < 1.0)
segmask = BinaryImage(segmask_arr.astype(np.uint8))
camera_intr = CameraIntrinsics.load(camera_intr_filename)
state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# init policy
policy = UniformRandomGraspingPolicy(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.imshow(rgbd_im.depth,
vmin=policy_config['vis']['vmin'],
vmax=policy_config['vis']['vmax'])
vis.grasp(action.grasp, scale=1.5, show_center=False, show_axis=True)
vis.title('Antipodal grasp')
vis.show()
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")