def predict(self): rot, trans = RigidTransform.rotation_and_translation_from_matrix( self.datapoint['obj_pose']) dataset_pose = RigidTransform(rot, trans, 'obj', 'world') dataset_final_poses = self.datapoint['final_poses'] num_vertices = self.datapoint['num_vertices'] final_poses = [] for i in range(20): final_pose_mat = dataset_final_poses[i * 4:(i + 1) * 4] if np.array_equal(final_pose_mat, np.zeros((4, 4))): break rot, trans = RigidTransform.rotation_and_translation_from_matrix( final_pose_mat) final_poses.append(RigidTransform(rot, trans, 'obj', 'world')) vertices = self.datapoint['vertices'][:num_vertices] normals = self.datapoint['normals'][:num_vertices] vertices = (self.obj.T_obj_world * dataset_pose.inverse() * PointCloud(vertices.T, 'world')).data.T normals = (self.obj.T_obj_world * dataset_pose.inverse() * PointCloud(normals.T, 'world')).data.T vertex_probs = self.datapoint[ 'vertex_probs'][:num_vertices, :len(final_poses)] required_forces = self.datapoint['min_required_forces'][:num_vertices] return vertices, normals, final_poses, vertex_probs, required_forces
def load_object(self, state): self.obj = state.obj self.com = self.obj.mesh.center_mass obj_dataset_name = self.obj.key.split('~')[0] dataset = self.datasets[obj_dataset_name] obj_ids = self.obj_ids[obj_dataset_name] self.datapoint = None similarity = np.inf # for i in range(dataset.num_datapoints): # for i in [101]: obj_specific_datapoints = self.id_to_datapoint[obj_ids[self.obj.key]] for _, i in obj_specific_datapoints: datapoint = dataset.datapoint(i) if obj_ids[self.obj.key] != datapoint['obj_id']: continue rot, trans = RigidTransform.rotation_and_translation_from_matrix( datapoint['obj_pose']) obj_pose = RigidTransform(rot, trans, 'obj', 'world') pose_diff_val = pose_diff(obj_pose, self.obj.T_obj_world) if pose_diff_val < similarity: self.datapoint = datapoint similarity = pose_diff_val if self.datapoint == None: raise ValueError('Object {} not in database'.format(self.obj.key))
CONFIG = YamlConfig(TEST_CONFIG_NAME) obj = GraspableObject3D(sdf, mesh) gripper = RobotGripper.load(GRIPPER_NAME, os.path.join(WORK_DIR, "foxarm/common")) ags = AntipodalGraspSampler(gripper, CONFIG) ##### Generate Grasps ##### unaligned_grasps = ags.generate_grasps(obj, target_num_grasps=100) print('### Generated %d unaligned grasps! ###' % len(unaligned_grasps)) grasps = {} stp_mats, stp_probs = mesh.compute_stable_poses(n_samples=1) stps = [] for stp_mat in stp_mats: r, t = RigidTransform.rotation_and_translation_from_matrix(stp_mat) stps.append(RigidTransform(rotation=r, translation=t)) print('### Generated %d stable poses! ###' % len(stps)) for stp_idx, stp in enumerate(stps): grasps[stp_idx] = [] for grasp in unaligned_grasps: aligned_grasp = grasp.perpendicular_table(stp) grasps[stp_idx].append(copy.deepcopy(aligned_grasp)) print('\tStable poses %s has %d grasps!' % (stp_idx, len(grasps[stp_idx]))) print('\n### Grasp Quality ###') for stp_idx, stp in enumerate(stps): grasps_test = grasps[stp_idx] print('### Stable pose %s (%-5.3f) ###' % (stp_idx, stp_probs[stp_idx])) l_fa1, l_fa2 = [], []
# ---------------------- print 'Computing Topples for obj', obj_id env.state.material_props._color = np.array([0.5] * 3) policy.set_environment(env.environment) obj_keys_to_id[env.state.obj.key] = deepcopy(obj_id) obj_config = config['state_space']['object'] stable_poses, _ = env.state.obj.mesh.compute_stable_poses( sigma=obj_config['stp_com_sigma'], n_samples=obj_config['stp_num_samples'], threshold=obj_config['stp_min_prob'] ) print 'num poses', len(stable_poses) for pose_num, pose in enumerate(stable_poses): print 'Computing Topples for pose', pose_num rot, trans = RigidTransform.rotation_and_translation_from_matrix(pose) env.state.obj.T_obj_world = RigidTransform(rot, trans, 'obj', 'world') # pose = env.state.obj.T_obj_world.matrix if args.before: env.render_3d_scene() vis3d.show(starting_camera_pose=CAMERA_POSE) skip = raw_input('skip?') if skip == 'y': continue vertices, normals, final_poses, vertex_probs, min_required_forces = policy.compute_topple(env.state) num_final_poses = len(final_poses) num_vertices = len(vertices) if num_final_poses > MAX_FINAL_POSES: print 'Too Many Poses! Skipping'
def to_rigid(mat): rot, trans = RigidTransform.rotation_and_translation_from_matrix(mat) return RigidTransform(rot, trans, 'obj', 'world')
def evaluate_models(models, datasets, obj_id_to_keys, env, use_sensitivity): y_true, y_pred = [], [[]] total_datapoints = 0 for dataset, obj_id_to_key in zip(datasets, obj_id_to_keys): total_datapoints += dataset.num_datapoints for i in range(dataset.num_datapoints): datapoint = dataset.datapoint(i) dataset_name, key = obj_id_to_key[str( datapoint['obj_id'])].split(KEY_SEP_TOKEN) obj = env._state_space._database.dataset(dataset_name)[key] orig_pose = to_rigid(datapoint['obj_pose']) obj.T_obj_world = orig_pose env.state.obj = obj model.load_object(env.state) predicted_poses, vertex_probs, _ = model.predict( [datapoint['vertex']], [datapoint['normal']], [-datapoint['normal']], # push dir use_sensitivity=use_sensitivity) vertex_probs = vertex_probs[0] y_pred.extend([1 - vertex_probs[0]] * NUM_PER_DATAPOINT) empirical_dist = [] for i in range(NUM_PER_DATAPOINT): actual_pose_mat = datapoint['actual_poses'][i * 4:(i + 1) * 4] rot, trans = RigidTransform.rotation_and_translation_from_matrix( actual_pose_mat) pose_to_add = RigidTransform(rot, trans, 'obj', 'world') y_true.append( 0 if is_equivalent_pose(pose_to_add, orig_pose) else 1) found = False for i in range(len(empirical_dist)): actual_pose, actual_prob = empirical_dist[i] if is_equivalent_pose(actual_pose, pose_to_add): empirical_dist[i][1] += .1 found = True break if not found: empirical_dist.append([pose_to_add, .1]) total_variation, l1 = 1, [] for empirical_pose, empirical_prob in empirical_dist: for predicted_pose, predicted_prob in zip( predicted_poses, vertex_probs): if is_equivalent_pose(empirical_pose, predicted_pose): total_variation = min( total_variation, abs(empirical_prob - predicted_prob)) l1.append(abs(empirical_prob - predicted_prob)) break l1 = np.mean(l1) if len(l1) > 0 else 0 # if i + total_datapoints in test_set: # test_tvs.append(total_variation) # test_l1s.append(l1) # else: # train_tvs.append(total_variation) # train_l1s.append(l1) combined_tvs.append(total_variation) combined_l1s.append(l1) # for i in range(10): # actual_pose_mat = datapoint['actual_poses'][i*4:(i+1)*4] # rot, trans = RigidTransform.rotation_and_translation_from_matrix(actual_pose_mat) # actual_pose = RigidTransform(rot, trans, 'obj', 'world') # y_true.append(0 if is_equivalent_pose(orig_pose, actual_pose) else 1) # y_pred.append(1-vertex_probs[0]) # # Cross Entropy # # q_x = 0 # # for predicted_pose, prob in zip(predicted_poses, vertex_probs): # # if is_equivalent_pose(actual_pose, predicted_pose): # # q_x = prob # # break # # if q_x == 0: # # counter += 1 # # # env.render_3d_scene() # # # vis3d.show(title='before', starting_camera_pose=CAMERA_POSE) # # # env.state.obj.T_obj_world = actual_pose # # # env.render_3d_scene() # # # vis3d.show(title='after', starting_camera_pose=CAMERA_POSE) # # # for predicted_pose, prob in zip(predicted_poses, vertex_probs): # # # env.state.obj.T_obj_world = predicted_pose # # # env.render_3d_scene() # # # title = '{}, pose diff: {}, angle diff: {}, prob: {}'.format( # # # is_equivalent_pose(actual_pose, predicted_pose), # # # pose_diff(actual_pose, predicted_pose), # # # pose_angle(actual_pose, predicted_pose), # # # prob # # # ) # # # vis3d.show(title=title, starting_camera_pose=CAMERA_POSE) # # # env.state.obj.T_obj_world = to_rigid(datapoint['obj_pose']) # # q_x = max(q_x, 1e-5) # # cross_entropy -= .1 * np.log(q_x) # p(x) * log(q(x)) # # n += .1 # logger.info('Mean Cross Entropy '+str(cross_entropy/float(n))) # logger.info('frac 0: {} {} {}'.format(counter, 10*total_datapoints, counter / float(10*total_datapoints))) # precision, recall, _ = metrics.precision_recall_curve(y_true, y_pred) # aucs.append(metrics.auc(recall, precision)) # prs.append((precision, recall, model)) # return np.mean(train_tvs), np.mean(test_tvs), np.mean(combined_tvs), np.mean(train_l1s), np.mean(test_l1s), np.mean(combined_l1s) avg_precision = metrics.average_precision_score(y_true, y_pred) return combined_tvs, combined_l1s, avg_precision, y_true, y_pred
def generate_dataset(self): # read parameters vis_params = self.config["vis"] camera_filename = vis_params["camera_filename"] save_dir = vis_params["save_dir"] coll_check_params = self.config['collision_checking'] approach_dist = coll_check_params['approach_dist'] delta_approach = coll_check_params['delta_approach'] table_offset = coll_check_params['table_offset'] gqcnn_params = self.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 image_samples_per_stable_pose = self.config['images_per_stable_pose'] table_mesh_filename = coll_check_params['table_mesh_filename'] table_alignment_params = self.config['table_alignment'] env_rv_params = self.config['env_rv_params'] max_grasp_approach_table_angle = np.deg2rad( table_alignment_params['max_approach_table_angle']) table_mesh = trimesh.load_mesh(table_mesh_filename) T_table_obj = RigidTransform(from_frame='table', to_frame='obj') scene_objs = {'table': SceneObject(table_mesh, T_table_obj)} img_idx = 0 quality_config = GraspQualityConfigFactory.create_config( CONFIG['metrics']['robust_ferrari_canny']) # rv: friction core params_rv = ParamsGaussianRV(quality_config, quality_config.params_uncertainty) for obj_idx, obj_path in enumerate(self.obj_paths): obj_id = self.obj_ids[obj_idx] result = {} result[obj_id] = {} # 1. construct the object sdf_path = obj_path[:-3] + "sdf" mesh = trimesh.load_mesh(obj_path) sdf = SdfFile(sdf_path).read() plot_scale = 2 * float(np.max(np.abs(mesh.vertices))) obj = GraspableObject3D(sdf, mesh) T_obj_world = RigidTransform(from_frame='obj', to_frame='world') graspable_rv = GraspableObjectPoseGaussianRV( obj, T_obj_world, quality_config.obj_uncertainty) result[obj_id]['mesh'] = {} result[obj_id]['mesh']['vertices'] = mesh.vertices result[obj_id]['mesh']['triangles'] = mesh.faces # 2. sample force closure grasps unaligned_fc_grasps = [] ags = AntipodalGraspSampler(gripper, CONFIG) unaligned_grasps = ags.generate_grasps(obj, target_num_grasps=100) for i, grasp in enumerate(unaligned_grasps): success, c = grasp.close_fingers(obj, check_approach=False) if success: c1, c2 = c if_force_closure = bool( PointGraspMetrics3D.force_closure( c1, c2, CONFIG['sampling_friction_coef'])) if if_force_closure: unaligned_fc_grasps.append(grasp) # 3. compute stable poses stp_mats, stp_probs = mesh.compute_stable_poses(n_samples=1) stp_mats = stp_mats[:10] stp_probs = stp_probs[:10] stps = [] for stp_mat in stp_mats: r, t = RigidTransform.rotation_and_translation_from_matrix( stp_mat) stps.append(RigidTransform(rotation=r, translation=t)) result[obj_id]['stable_poses'] = {} # for each stable pose grasps = {} for stp_idx, stp in enumerate(stps): result[obj_id]['stable_poses']['stp_0'] = {} grasp_idx = 0 # filter grasps and calculate quality grasps[stp_idx] = [] for grasp_idx, grasp in enumerate(unaligned_fc_grasps): # align the grasp aligned_grasp = grasp.perpendicular_table(stp) # check perpendicular with table _, grasp_approach_table_angle, _ = aligned_grasp.grasp_angles_from_stp_z( stp) perpendicular_table = (np.abs(grasp_approach_table_angle) < max_grasp_approach_table_angle) if not perpendicular_table: continue # check collision success, c = aligned_grasp.close_fingers(obj) if not success: continue # calculate grasp quality grasp_rv = ParallelJawGraspPoseGaussianRV( aligned_grasp, quality_config.grasp_uncertainty) mean_q, std_q = RobustPointGraspMetrics3D.expected_quality( grasp_rv, graspable_rv, params_rv, quality_config) grasps[stp_idx].append((aligned_grasp, mean_q)) # render depth images urv = UniformPlanarWorksurfaceImageRandomVariable( obj.mesh, render_modes, 'camera', env_rv_params, stable_pose=stp, scene_objs=scene_objs) image_samples_per_stable_pose = 1 render_samples = urv.rvs(size=image_samples_per_stable_pose) render_samples = [render_samples] # if debug: if False: for render_sample in render_samples: mlab.clf() camera_t = render_sample.camera.object_to_camera_pose Vis.plot_camera(camera_filename, camera_t) t_obj_stp = np.array( [0, 0, -stp.rotation.dot(stp.translation)[2]]) T_obj_stp = RigidTransform(rotation=stp.rotation, translation=stp.translation, from_frame='obj', to_frame='stp') stable_mesh = obj.mesh.copy() stable_mesh.apply_transform(T_obj_stp.matrix) # obj.mesh.apply_transform(T_obj_stp.matrix) mag = 2 * float(np.max(np.abs(stable_mesh.vertices))) Vis.plot_mesh(stable_mesh) Vis.plot_plane(4 * mag) Vis.plot_frame(mag) mlab.view(0, 0, 0.85) # https://stackoverflow.com/questions/16543634/mayavi-mlab-savefig-gives-an-empty-image mlab.savefig('%s/%d.jpg' % (save_dir, img_idx)) img_idx += 1 # mlab.show() for render_sample in render_samples: depth_im_table = render_sample.renders[ RenderMode.DEPTH_SCENE].image 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) T_obj_camera = T_stp_camera * stp.as_frames( 'obj', T_stp_camera.from_frame) for grasp_info in grasps[stp_idx]: grasp = grasp_info[0] grasp_quality = grasp_info[1] # get the gripper pose grasp_2d = grasp.project_camera( T_obj_camera, shifted_camera_intr) grasp_depth = grasp_2d.depth # 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]) # rotate, crop and resize depth_im_tf_table = depth_im_table.transform( translation, grasp_2d.angle) depth_im_tf_table = depth_im_tf_table.crop( im_crop_height, im_crop_width) depth_im_tf_table = depth_im_tf_table.resize( (im_final_height, im_final_width)) # one sample consists of depth_im_tf_table, grasp_quality, and grasp_depth # misc.imsave('generated_data/%f.jpg' % grasp_quality, depth_im_tf_table.data) grasp_key = 'grasp_%d' % grasp_idx result[obj_id]['stable_poses']['stp_0'][grasp_key] = {} result[obj_id]['stable_poses']['stp_0'][grasp_key][ 'depth_img'] = depth_im_tf_table.data result[obj_id]['stable_poses']['stp_0'][grasp_key][ 'grasp_depth'] = grasp_depth result[obj_id]['stable_poses']['stp_0'][grasp_key][ 'quality'] = grasp_quality grasp_idx += 1 if self.debug: break self.result_queue.put(result) break
def top_n_actions(self, mesh, obj_name, vis=True): """ Takes in a mesh, samples a bunch of grasps on the mesh, evaluates them using the metric given in the constructor, and returns the best grasps for the mesh. SHOULD RETURN GRASPS IN ORDER OF THEIR GRASP QUALITY. You should try to use mesh.mass to get the mass of the object. You should check the output of this, because from this https://github.com/BerkeleyAutomation/trimesh/blob/master/trimesh/base.py#L2203 it would appear that the mass is approximated using the volume of the object. If it is not returning reasonable results, you can manually weight the objects and store them in the dictionary at the top of the file. Parameters ---------- mesh : :obj:`Trimesh` vis : bool Whether or not to visualize the top grasps Returns ------- :obj:`list` of :obj:`autolab_core.RigidTransform` the matrices T_grasp_world, which represents the hand poses of the baxter / sawyer which would result in the fingers being placed at the vertices of the best grasps """ # Some objects have vertices in odd places, so you should sample evenly across # the mesh to get nicer candidate grasp points using trimesh.sample.sample_surface_even() points, face_indices = trimesh.sample.sample_surface_even( mesh, self.n_vert) # (frame: object) normals = mesh.face_normals[face_indices] # (frame: object) grasp_vertices, grasp_normals = self.sample_grasps( points, normals) # (frame: object) scores = self.score_grasps(grasp_vertices, grasp_normals, mesh.mass) top_k_scores_idx = sorted(zip(scores, range(len(scores))), key=lambda x: x[0], reverse=True) top_grasp_qualities = [s for s, i in top_k_scores_idx][:self.n_execute] top_idx = [i for s, i in top_k_scores_idx][:self.n_execute] top_grasp_vertices = np.array([grasp_vertices[i] for i in top_idx]) top_grasp_normals = np.array([grasp_normals[i] for i in top_idx]) # Get the hand poses poses = [] for grasp_verts, grasp_norm in zip(top_grasp_vertices, top_grasp_normals): # grasp_verts = np.array([gv - mesh.bounding_box.centroid for gv in grasp_verts]).reshape(grasp_verts.shape) poses.append( self.vertices_to_baxter_hand_pose(grasp_verts, grasp_norm)) # tf_inv = self.T_obj_world.inverse() # world_poses1 = [tf_inv.matrix.dot(pose.matrix) for pose in poses] # world_poses2 = [pose * tf_inv for pose in poses] # combos = {} # for pose_i, pose_option in enumerate([poses[0].matrix, poses[0].inverse().matrix]): # for t_obj_world_i, t_obj_world_option in enumerate([self.T_obj_world.matrix, self.T_obj_world.inverse().matrix]): # key0 = (pose_i, t_obj_world_i, 0) # combos[key0] = pose_option.dot(t_obj_world_option) # key1 = (pose_i, t_obj_world_i, 1) # combos[key1] = t_obj_world_option.dot(pose_option) # for key, value in combos.items(): # print key # print value # print '' adjusted_poses = [] world_poses = [] for pose in poses: rotation, translation = RigidTransform.rotation_and_translation_from_matrix( self.T_obj_world.inverse().matrix.dot(pose.inverse().matrix)) pose_world = RigidTransform(rotation=rotation, translation=translation) world_pose = get_approach_rt(pose_world) world_poses.append(world_pose) adjusted_rotation, adjusted_translation = RigidTransform.rotation_and_translation_from_matrix( world_pose.matrix.dot(self.T_obj_world.matrix)) adjusted_pose = RigidTransform(rotation=adjusted_rotation, translation=adjusted_translation) adjusted_poses.append(adjusted_pose) # Visualize the grasps if vis: self.vis(mesh, top_grasp_vertices, top_grasp_normals, top_grasp_qualities, poses) # import pdb; pdb.set_trace() return world_poses