def showgraspfrompickle(self, object_name,metric_name,onlynagative,onlypositive,openravechecker): object = self.dataset[object_name] labelgrasps,labelmetrics=self.readdatafrompickle(object_name) vis.figure() vis.mesh(object.mesh.trimesh ,style='surface') config=self._get_config(None) low =0.0 high = np.max([labelmetrics[i][metric_name] for i in range(len(labelmetrics))]) # print 'high',high,config['quality_scale'] if low == high: q_to_c = lambda quality: config['quality_scale'] else: q_to_c = lambda quality: config['quality_scale'] * (quality - low) / (high - low) recalculate_grasp=[] for i in range(len(labelgrasps)): if labelmetrics[i][metric_name]==0 : if not onlypositive: vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=(1,0,0), grasp_axis_color=(1,0,0) ) elif labelmetrics[i][metric_name]==-1 : if not onlypositive: vis.shownormals(labelgrasps[i][1],labelgrasps[i][0],color=(1,0,0) ) recalculate_grasp.append(labelgrasps[i]) elif labelmetrics[i][metric_name]>0 : if not onlynagative: color = plt.get_cmap('hsv')(q_to_c(labelmetrics[i][metric_name]))[:-1] vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=color, grasp_axis_color=color ) recalculate_grasp.append(labelgrasps[i]) print i # ,high vis.pose(RigidTransform(), alpha=0.1) vis.show(animate=False)
def vis_topple_probs(vertices, topple_probs): vis3d.figure() env.render_3d_scene() for vertex, prob in zip(vertices, topple_probs): color = [min(1, 2*(1-prob)), min(2*prob, 1), 0] vis3d.points(Point(vertex, 'world'), scale=.001, color=color) for bottom_point in policy.toppling_model.bottom_points: vis3d.points(Point(bottom_point, 'world'), scale=.001, color=[0,0,0]) vis3d.show(starting_camera_pose=CAMERA_POSE)
def save_samples(self, sample, grasps, T_obj_stp, collision_checker, grasp_metrics, stable_pose): T_stp_camera = sample.camera.object_to_camera_pose self.T_obj_camera = T_stp_camera * T_obj_stp.as_frames('obj', T_stp_camera.from_frame) aligned_grasps = self.align_grasps_with_camera(grasps) depth_im_table = sample.renders[RenderMode.DEPTH_SCENE].image camera_pose = self.get_camera_pose(sample) self.tensor_datapoint['camera_poses'] = camera_pose shifted_camera_intr = sample.camera.camera_intr for cnt, aligned_grasp in enumerate(aligned_grasps): collision_free = self.is_grasp_collision_free(aligned_grasp, collision_checker) # Project grasp coordinates in image grasp_2d = aligned_grasp.project_camera(self.T_obj_camera, shifted_camera_intr) grasp_point = aligned_grasp.close_fingers(self.obj, vis=False, check_approach=False) if not grasp_point[0]: Warning("Could not find contact points") continue # Get grasp width in pixel from endpoints p_1 = np.dot(self.T_obj_camera.matrix, np.append(grasp_point[1][0].point, 1).T).T[:3] p_2 = np.dot(self.T_obj_camera.matrix, np.append(grasp_point[1][1].point, 1).T).T[:3] grasp_width_px = self.width_px(shifted_camera_intr, p_1, p_2) grasp_width = aligned_grasp.width_from_endpoints(grasp_point[1][0].point, grasp_point[1][1].point) depth_im_no_rot = self.prepare_images(depth_im_table, grasp_2d) T_grasp_camera = aligned_grasp.gripper_pose(self.gripper).inverse() * self.T_obj_camera.inverse() if VISUALISE_3D: z_axis = T_grasp_camera.z_axis theta = np.rad2deg(np.arccos((z_axis[2]) / (np.sqrt((z_axis[0] ** 2 + z_axis[1] ** 2 + z_axis[2] ** 2))))) vis.figure() T_obj_world = vis.mesh_stable_pose(self.obj.mesh.trimesh, stable_pose.T_obj_world, style='surface', dim=0.5, plot_table=True) T_camera_world = T_obj_world * self.T_obj_camera.inverse() vis.gripper(self.gripper, aligned_grasp, T_obj_world, color=(0.3, 0.3, 0.3), T_camera_world=T_camera_world) print(theta) vis.show() hand_pose = self.get_hand_pose(grasp_2d, T_grasp_camera, grasp_width, grasp_width_px) self.add_datapoint(depth_im_no_rot, hand_pose, collision_free, aligned_grasp, grasp_metrics) self.cur_image_label += 1
def test_multi_push(env): env = GraspingEnv(config, config['vis']) policy = MultiTopplePolicy(config, use_sensitivity=True) config['model']['load'] = 0 rand_policy = RandomTopplePolicy(config, use_sensitivity=True) with open("/nfs/diskstation/db/toppling/tuned_params/obj_ids.json", "r") as read_file: obj_ids = json.load(read_file) while True: env.reset() if env.state.obj.key not in obj_ids: continue env.state.material_props._color = np.array([0.5] * 3) policy.set_environment(env.environment) rand_policy.set_environment(env.environment) if args.before: vis3d.figure() env.render_3d_scene() vis3d.show(starting_camera_pose=CAMERA_POSE) planning_time = policy.action(env.state) logger.info(env.state.obj.key) logger.info('Value Iteration') path = forward_sim(policy.G, 'Value Iteration', 'value') logger.info('Value Iteration Path: ' + str(path)) logger.info('Value Iteration Path Length: ' + str(len(path) - 1)) logger.info('Value Iteration Planning Time: ' + str(planning_time)) logger.info('Value Iteration No Actions: ' + str(len(policy.G.nodes()) == 1)) logger.info('Value Iteration Already Best: ' + str(len(policy.G.nodes()) > 1 and len(path) == 1)) logger.info('Greedy') path = forward_sim(policy.G, 'Greedy', 'single_push_q') logger.info('Greedy Path: ' + str(path)) logger.info('Greedy Path Length: ' + str(len(path) - 1)) logger.info('Greedy Planning Time: ' + str( np.sum( [policy.G.nodes[node_id]['planning_time'] for node_id in path]))) logger.info('Greedy No Actions: ' + str(len(policy.G.nodes()) == 1)) logger.info('Greedy Already Best: ' + str((len(policy.G.nodes()) > 1 and len(path) == 1))) a = time() path_length, planning_time = forward_sim_random(env, rand_policy) logger.info('Random Path Length: ' + str(path_length)) logger.info('Random Planning Time: ' + str(planning_time)) logger.info('\n')
def display_stable_poses(self, object_name, config=None): """Display an object's stable poses Parameters ---------- object_name : :obj:`str` Object to display. config : :obj:`dict` Configuration dict for visualization. Parameters are in Other parameters. Values from self.default_config are used for keys not provided. Other Parameters ---------------- animate Whether or not to animate the displayed object Raises ------ ValueError invalid object name RuntimeError Database or dataset not opened. """ self._check_opens() config = self._get_config(config) if object_name not in self.dataset.object_keys: raise ValueError( "{} is not a valid object name".format(object_name)) logger.info('Displaying stable poses for'.format(object_name)) obj = self.dataset[object_name] stable_poses = self.dataset.stable_poses(object_name) for stable_pose in stable_poses: print 'Stable pose %s with p=%.3f' % (stable_pose.id, stable_pose.p) vis.figure() vis.mesh_stable_pose(obj.mesh, stable_pose, color=(0.5, 0.5, 0.5), style='surface') vis.pose(RigidTransform(), alpha=0.15) vis.show(animate=config['animate'])
def sim_to_real_tf(sim_point_cloud_masked, vis=False): # Capture point cloud from physical depth camera _, depth_im, _ = phoxi_sensor.frames() phys_point_cloud = phoxi_tf*phoxi_sensor.ir_intrinsics.deproject(depth_im) phys_point_cloud_masked, _ = phys_point_cloud.box_mask(mask_box) if phys_point_cloud_masked.num_points == 0: logging.warn('Object not found! Skipping...') quit() pcs_config = {'overlap': 0.6, 'accuracy': 0.001, 'samples': 500, 'timeout': 10, 'cache_dir': '/home/chriscorrea14/Super4PCS/cache'} pcs_aligner = Super4PCSAligner(pcs_config) sim_to_real_tf = pcs_aligner.align(phys_point_cloud_masked, sim_point_cloud_masked) if vis: vis3d.figure() vis3d.points(phys_point_cloud_masked.data.T, color=(1,0,0), scale=.001) vis3d.points((sim_to_real_tf*sim_point_cloud_masked).data.T, color=(0,1,0), scale=.001) vis3d.show(title='Predicted pose from Sim 2 Real') return sim_to_real_tf
def antipodal_grasp_sampler(self): of = ObjFile(OBJ_FILENAME) sf = SdfFile(SDF_FILENAME) mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh) gripper = RobotGripper.load(GRIPPER_NAME) ags = AntipodalGraspSampler(gripper, CONFIG) grasps = ags.generate_grasps(obj, target_num_grasps=10) quality_config = GraspQualityConfigFactory.create_config( CONFIG['metrics']['force_closure']) quality_fn = GraspQualityFunctionFactory.create_quality_function( obj, quality_config) q_to_c = lambda quality: CONFIG['quality_scale'] i = 0 vis.figure() vis.mesh(obj.mesh.trimesh, style='surface') for grasp in grasps: print(grasp.frame) success, c = grasp.close_fingers(obj) if success: c1, c2 = c fn_fc = quality_fn(grasp).quality true_fc = PointGraspMetrics3D.force_closure( c1, c2, quality_config.friction_coef) #print(grasp) T_obj_world = RigidTransform(from_frame='obj', to_frame='world') color = plt.get_cmap('hsv')(q_to_c(CONFIG['metrics']))[:-1] T_obj_gripper = grasp.gripper_pose(gripper) #vis.grasp(grasp,grasp_axis_color=color,endpoint_color=color) i += 1 #T_obj_world = RigidTransform(from_frame='obj',to_frame='world') vis.show(False)
def display_object(self, object_name, config=None): """Display an object Parameters ---------- object_name : :obj:`str` Ob ject to display. config : :obj:`dict` Configuration dict for visualization. Parameters are in Other parameters. Values from self.default_config are used for keys not provided. Other Parameters ---------------- animate Whether or not to animate the displayed object Raises ------ ValueError invalid object name RuntimeError Database or dataset not opened. """ self._check_opens() config = self._get_config(config) if object_name not in self.dataset.object_keys: raise ValueError( "{} is not a valid object name".format(object_name)) logger.info('Displaying {}'.format(object_name)) obj = self.dataset[object_name] vis.figure(bgcolor=(1, 1, 1), size=(1000, 1000)) vis.mesh(obj.mesh, color=(0.5, 0.5, 0.5), style='surface') vis.show(animate=config['animate'])
def benchmark_bin_picking_policy(policy, # input_dataset_path, # heap_ids, # timesteps, # output_dataset_path, config, # excluded_heaps_file): ): """ Benchmark a bin picking policy. Parameters ---------- policy : :obj:`Policy` policy to roll out input_dataset_path : str path to the input dataset heap_ids : list integer identifiers for the heaps to re-run timesteps : list integer timesteps to seed the simulation from output_dataset_path : str path to store the results config : dict dictionary-like objects containing parameters of the simulator and visualization """ # read subconfigs vis_config = config['vis'] dataset_config = config['dataset'] # read parameters fully_observed = config['fully_observed'] steps_per_test_case = config['steps_per_test_case'] rollouts_per_garbage_collect = config['rollouts_per_garbage_collect'] debug = config['debug'] im_height = config['state_space']['camera']['im_height'] im_width = config['state_space']['camera']['im_width'] max_obj_per_pile = config['state_space']['object']['max_obj_per_pile'] if debug: random.seed(SEED) np.random.seed(SEED) # read ids # if len(heap_ids) != len(timesteps): # raise ValueError('Must provide same number of heap ids and timesteps') # num_rollouts = len(heap_ids) num_rollouts = 1 # set dataset params tensor_config = dataset_config['tensors'] fields_config = tensor_config['fields'] # fields_config['color_ims']['height'] = im_height # fields_config['color_ims']['width'] = im_width # fields_config['depth_ims']['height'] = im_height # fields_config['depth_ims']['width'] = im_width fields_config['obj_poses']['height'] = POSE_DIM * max_obj_per_pile fields_config['obj_coms']['height'] = POINT_DIM * max_obj_per_pile fields_config['obj_ids']['height'] = max_obj_per_pile fields_config['bin_distances']['height'] = max_obj_per_pile # matrix has (n choose 2) elements in it max_distance_matrix_length = int(comb(max_obj_per_pile, 2)) fields_config['distance_matrix']['height'] = max_distance_matrix_length # sample a process id proc_id = utils.gen_experiment_id() # if not os.path.exists(output_dataset_path): # try: # os.mkdir(output_dataset_path) # except: # logging.warning('Failed to create %s. The dataset path may have been created simultaneously by another process' %(dataset_path)) # proc_id = 'clustering_2' # output_dataset_path = os.path.join(output_dataset_path, 'dataset_%s' %(proc_id)) # open input dataset # logging.info('Opening input dataset: %s' % input_dataset_path) # input_dataset = TensorDataset.open(input_dataset_path) # open output_dataset # logging.info('Opening output dataset: %s' % output_dataset_path) # dataset = TensorDataset(output_dataset_path, tensor_config) # datapoint = dataset.datapoint_template # setup logging # experiment_log_filename = os.path.join(output_dataset_path, '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) # config.save(os.path.join(output_dataset_path, 'dataset_generation_params.yaml')) # key mappings # we add the empty string as a mapping because if you don't evaluate dexnet on the 'before' state of the push obj_id = 1 obj_ids = {'': 0} action_ids = { 'ParallelJawGraspAction': 0, 'SuctionGraspAction': 1, 'LinearPushAction': 2 } # add action ids reverse_action_ids = utils.reverse_dictionary(action_ids) # dataset.add_metadata('action_ids', reverse_action_ids) # perform rollouts n = 0 rollout_start = time.time() current_heap_id = None while n < num_rollouts: # create env create_start = time.time() bin_picking_env = GraspingEnv(config, vis_config) create_stop = time.time() logging.info('Creating env took %.3f sec' %(create_stop-create_start)) # perform rollouts rollouts_remaining = num_rollouts - n for i in range(min(rollouts_per_garbage_collect, rollouts_remaining)): # log current rollout logging.info('\n') if n % vis_config['log_rate'] == 0: logging.info('Rollout: %03d' %(n)) try: # mark rollout status data_saved = False num_steps = 0 # read heap id # heap_id = heap_ids[n] # timestep = timesteps[n] # while heap_id == current_heap_id:# or heap_id < 81:#[226, 287, 325, 453, 469, 577, 601, 894, 921]: 26 # n += 1 # heap_id = heap_ids[n] # timestep = timesteps[n] push_logger = logging.getLogger('push') # push_logger.info('~') # push_logger.info('Heap ID %d' % heap_id) # current_heap_id = heap_id # reset env reset_start = time.time() # bin_picking_env.reset_from_dataset(input_dataset, # heap_id, # timestep) bin_picking_env.reset() state = bin_picking_env.state environment = bin_picking_env.environment if fully_observed: observation = None else: observation = bin_picking_env.observation policy.set_environment(environment) reset_stop = time.time() # add objects to mapping for obj_key in state.obj_keys: if obj_key not in obj_ids.keys(): obj_ids[obj_key] = obj_id obj_id += 1 push_logger.info(obj_key) # save id mappings reverse_obj_ids = utils.reverse_dictionary(obj_ids) # dataset.add_metadata('obj_ids', reverse_obj_ids) # store datapoint env params # datapoint['heap_ids'] = current_heap_id # datapoint['camera_poses'] = environment.camera.T_camera_world.vec # datapoint['camera_intrs'] = environment.camera.intrinsics.vec # datapoint['robot_poses'] = environment.robot.T_robot_world.vec # render if vis_config['initial_state']: vis3d.figure() bin_picking_env.render_3d_scene() vis3d.pose(environment.robot.T_robot_world) vis3d.show(starting_camera_pose=CAMERA_POSE) # observe if vis_config['initial_obs']: vis2d.figure() vis2d.imshow(observation, auto_subplot=True) vis2d.show() # rollout on current satte done = False failed = False # if isinstance(policy, SingulationFullRolloutPolicy): # policy.reset_num_failed_grasps() while not done: if vis_config['step_stats']: logging.info('Heap ID: %s' % heap_id) logging.info('Timestep: %s' % bin_picking_env.timestep) # get action policy_start = time.time() if fully_observed: action = policy.action(state) else: action = policy.action(observation) policy_stop = time.time() logging.info('Composite Policy took %.3f sec' %(policy_stop-policy_start)) # render scene before if vis_config['action']: #gripper = bin_picking_env.gripper(action) vis3d.figure() # GRASPINGENV # bin_picking_env.render_3d_scene(render_camera=False, workspace_objs_wireframe=False) bin_picking_env.render_3d_scene() if isinstance(action, GraspAction): vis3d.gripper(gripper, action.grasp(gripper)) #if isinstance(action, LinearPushAction): else: # # T_start_world = action.T_begin_world * gripper.T_mesh_grasp # # T_end_world = action.T_end_world * gripper.T_mesh_grasp # #start_point = action.T_begin_world.translation # start_point = action['start'] # #end_point = action.T_end_world.translation # end_point = action['end'] # vec = (end_point - start_point) / np.linalg.norm(end_point-start_point) if np.linalg.norm(end_point-start_point) > 0 else end_point-start_point # #h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(vec) # #h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(vec) # arrow_len = np.linalg.norm(start_point - end_point) # h1 = (end_point - start_point + np.array([0,0,arrow_len])) / (arrow_len*math.sqrt(2)) # h2 = (end_point - start_point - np.array([0,0,arrow_len])) / (arrow_len*math.sqrt(2)) # shaft_points = [start_point, end_point] # head_points = [end_point - 0.03*h2, end_point, end_point - 0.03*h1] # #vis3d.plot3d(shaft_points, color=[0,0,1]) # #vis3d.plot3d(head_points, color=[0,0,1]) # Displaying all potential topple points for vertex, prob in zip(action['vertices'], action['probabilities']): color = np.array([min(1, 2*(1-prob)), min(2*prob, 1), 0]) vis3d.points(Point(vertex, 'world'), scale=.0005, color=color) for vertex in action['bottom_points']: color = np.array([0,0,1]) vis3d.points(Point(vertex, 'world'), scale=.0005, color=color) vis3d.points(Point(action['com'], 'world'), scale=.005, color=np.array([0,0,1])) vis3d.points(Point(np.array([0,0,0]), 'world'), scale=.005, color=np.array([0,1,0])) #set_of_lines = action['set_of_lines'] #for i, line in enumerate(set_of_lines): # color = str(bin(i+1))[2:].zfill(3) # color = np.array([color[2], color[1], color[0]]) # vis3d.plot3d(line, color=color) vis3d.show(starting_camera_pose=CAMERA_POSE) # Show vis3d.figure() bin_picking_env.render_3d_scene() final_pose_ind = action['final_pose_ind'] / np.amax(action['final_pose_ind']) for vertex, final_pose_ind in zip(action['vertices'], final_pose_ind): color = np.array([0, min(1, 2*(1-prob)), min(2*prob, 1)]) vis3d.points(Point(vertex, 'world'), scale=.0005, color=color) vis3d.show(starting_camera_pose=CAMERA_POSE) color=np.array([0,0,1]) original_pose = state.obj.T_obj_world pose_num = 0 for pose, edge_point1, edge_point2 in zip(action['final_poses'], action['bottom_points'], np.roll(action['bottom_points'],-1,axis=0)): print 'Pose:', pose_num pose_num += 1 pose = pose.T_obj_table vis3d.figure() state.obj.T_obj_world = original_pose bin_picking_env.render_3d_scene() vis3d.points(Point(edge_point1, 'world'), scale=.0005, color=color) vis3d.points(Point(edge_point2, 'world'), scale=.0005, color=color) vis3d.show(starting_camera_pose=CAMERA_POSE) vis3d.figure() state.obj.T_obj_world = pose bin_picking_env.render_3d_scene() vis3d.points(Point(edge_point1, 'world'), scale=.0005, color=color) vis3d.points(Point(edge_point2, 'world'), scale=.0005, color=color) vis3d.show(starting_camera_pose=CAMERA_POSE) #vis3d.save('/home/mjd3/Pictures/weird_pics/%d_%d_before.png' % (heap_id, bin_picking_env.timestep), starting_camera_pose=CAMERA_POSE) # store datapoint pre-step data j = 0 obj_poses = np.zeros(fields_config['obj_poses']['height']) obj_coms = np.zeros(fields_config['obj_coms']['height']) obj_ids_vec = np.iinfo(np.uint32).max * np.ones(fields_config['obj_ids']['height']) for obj_state in state.obj_states: obj_poses[j*POSE_DIM:(j+1)*POSE_DIM] = obj_state.T_obj_world.vec obj_coms[j*POINT_DIM:(j+1)*POINT_DIM] = obj_state.center_of_mass obj_ids_vec[j] = obj_ids[obj_state.key] j += 1 action_poses = np.zeros(fields_config['action_poses']['height']) #if isinstance(action, GraspAction): # action_poses[:7] = action.T_grasp_world.vec #else: # action_poses[:7] = action.T_begin_world.vec # action_poses[7:] = action.T_end_world.vec # if isinstance(policy, SingulationMetricsCompositePolicy): # actual_distance_matrix_length = int(comb(len(state.objs), 2)) # bin_distances = np.append(action.metadata['bin_distances'], # np.zeros(max_obj_per_pile-len(state.objs)) # ) # distance_matrix = np.append(action.metadata['distance_matrix'], # np.zeros(max_distance_matrix_length - actual_distance_matrix_length) # ) # datapoint['bin_distances'] = bin_distances # datapoint['distance_matrix'] = distance_matrix # datapoint['T_begin_world'] = action.T_begin_world.matrix # datapoint['T_end_world'] = action.T_end_world.matrix # datapoint['parallel_jaw_best_q_value'] = action.metadata['parallel_jaw_best_q_value'] # # datapoint['parallel_jaw_mean_q_value'] = action.metadata['parallel_jaw_mean_q_value'] # # datapoint['parallel_jaw_num_grasps'] = action.metadata['parallel_jaw_num_grasps'] # datapoint['suction_best_q_value'] = action.metadata['suction_best_q_value'] # # datapoint['suction_mean_q_value'] = action.metadata['suction_mean_q_value'] # # datapoint['suction_num_grasps'] = action.metadata['suction_num_grasps'] # # logging.info('Suction Q: %f, PJ Q: %f' % (action.metadata['suction_q_value'], action.metadata['parallel_jaw_q_value'])) # # datapoint['obj_index'] = action.metadata['obj_index'] # # datapoint['parallel_jaw_best_q_value_single'] = action.metadata['parallel_jaw_best_q_value_single'] # # datapoint['suction_best_q_value_single'] = action.metadata['suction_best_q_value_single'] # datapoint['singulated_obj_index'] = action.metadata['singulated_obj_index'] # datapoint['parallel_jaw_grasped_obj_index'] = obj_ids[action.metadata['parallel_jaw_grasped_obj_key']] # datapoint['suction_grasped_obj_index'] = obj_ids[action.metadata['suction_grasped_obj_key']] # else: # datapoint['bin_distances'] = np.zeros(max_obj_per_pile) # datapoint['distance_matrix'] = np.zeros(max_distance_matrix_length) # datapoint['T_begin_world'] = np.zeros((4,4)) # datapoint['T_end_world'] = np.zeros((4,4)) # datapoint['parallel_jaw_best_q_value'] = -1 # datapoint['suction_best_q_value'] = -1 # datapoint['singulated_obj_index'] = -1 # datapoint['parallel_jaw_grasped_obj_index'] = -1 # datapoint['suction_grasped_obj_index'] = -1 # policy_id = 0 # if 'policy_id' in action.metadata.keys(): # policy_id = action.metadata['policy_id'] # greedy_q_value = 0 # if 'greedy_q_value' in action.metadata.keys(): # greedy_q_value = action.metadata['greedy_q_value'] # datapoint['timesteps'] = bin_picking_env.timestep # datapoint['obj_poses'] = obj_poses # datapoint['obj_coms'] = obj_coms # datapoint['obj_ids'] = obj_ids_vec # # if bin_picking_env.render_mode == RenderMode.RGBD: # # color_data = observation.color.raw_data # # depth_data = observation.depth.raw_data # # elif bin_picking_env.render_mode == RenderMode.DEPTH: # # color_data = np.zeros(observation.shape).astype(np.uint8) # # depth_data = observation.raw_data # # elif bin_picking_env.render_mode == RenderMode.COLOR: # # color_data = observation.raw_data # # depth_data = np.zeros(observation.shape) # # datapoint['color_ims'] = color_data # # datapoint['depth_ims'] = depth_data # datapoint['action_ids'] = action_ids[type(action).__name__] # datapoint['action_poses'] = action_poses # datapoint['policy_ids'] = policy_id # datapoint['greedy_q_values'] = greedy_q_value # datapoint['pred_q_values'] = action.q_value # step the policy #observation, reward, done, info = bin_picking_env.step(action) #state = bin_picking_env.state state.objs[0].T_obj_world = action['final_state'] # if isinstance(policy, SingulationFullRolloutPolicy): # policy.grasp_succeeds(info['grasp_succeeds']) # debugging info if vis_config['step_stats']: logging.info('Action type: %s' %(type(action).__name__)) logging.info('Action Q-value: %.3f' %(action.q_value)) logging.info('Reward: %d' %(reward)) logging.info('Policy took %.3f sec' %(policy_stop-policy_start)) logging.info('Num objects remaining: %d' %(bin_picking_env.num_objects)) if info['cleared_pile']: logging.info('Cleared pile!') # # store datapoint post-step data # datapoint['rewards'] = reward # datapoint['grasp_metrics'] = info['grasp_metric'] # datapoint['collisions'] = 1 * info['collides'] # datapoint['collisions_with_env'] = 1 * info['collides_with_static_obstacles'] # datapoint['grasped_obj_ids'] = obj_ids[info['grasped_obj_key']] # datapoint['cleared_pile'] = 1 * info['cleared_pile'] # # store datapoint # # dataset.add(datapoint) # data_saved = True # render observation if vis_config['obs']: vis2d.figure() vis2d.imshow(observation, auto_subplot=True) vis2d.show() # render scene after if vis_config['state']: vis3d.figure() bin_picking_env.render_3d_scene(render_camera=False) vis3d.show(starting_camera_pose=CAMERA_POSE) # vis3d.save('/home/mjd3/Pictures/weird_pics/%d_%d_after.png' % (heap_id, bin_picking_env.timestep), starting_camera_pose=CAMERA_POSE) state.objs[0].T_obj_world = action['tmpR'] vis3d.figure() bin_picking_env.render_3d_scene(render_camera=False) vis3d.show(starting_camera_pose=CAMERA_POSE) state.objs[0].T_obj_world = action['final_state'] # increment the number of steps num_steps += 1 if num_steps >= steps_per_test_case: done = True except NoActionFoundException as e: logging.warning('The policy failed to plan an action!') done = True except Exception as e: # log an error logging.warning('Rollout failed!') logging.warning('%s' %(str(e))) logging.warning(traceback.print_exc()) # if debug: # raise # reset env del bin_picking_env gc.collect() bin_picking_env = BinPickingEnv(config, vis_config) # terminate current rollout failed = True done = True # update test case id n += 1 # dataset.flush() # logging.info("\n\nflushing") # logging.info("exiting") # sys.exit() # garbage collect del bin_picking_env gc.collect() # return the dataset # dataset.flush() # log time rollout_stop = time.time() logging.info('Rollouts took %.3f sec' %(rollout_stop-rollout_start)) return dataset
def save_samples(self, sample, grasps, T_obj_stp, collision_checker, grasp_metrics, stable_pose): self.tensor_datapoint['image_labels'] = self.cur_image_label T_stp_camera = sample.camera.object_to_camera_pose self.T_obj_camera = T_stp_camera * T_obj_stp.as_frames( 'obj', T_stp_camera.from_frame) if UNALIGNED or STP_ALIGNED: aligned_grasps = grasps else: aligned_grasps = self.align_grasps_with_camera(grasps) depth_im_table = sample.renders[RenderMode.DEPTH_SCENE].image self.tensor_datapoint['camera_poses'] = self.get_camera_pose(sample) final_camera_intr = sample.camera.camera_intr.crop( self.crop_height, self.crop_width, depth_im_table.center[0], depth_im_table.center[1]) self.tensor_datapoint['camera_intrs'] = np.r_[final_camera_intr.fx, final_camera_intr.fy, final_camera_intr.cx, final_camera_intr.cy] if not IM_TENSOR: self.save_image(depth_im_table) else: depth_im_tf = self._crop(depth_im_table, size=None) for cnt, aligned_grasp in enumerate(aligned_grasps): if UNALIGNED: aligned_grasp = aligned_grasp.grasp_y_axis_offset( np.random.uniform(0, 2 * np.pi)) T_stp_grasp = RigidTransform( stable_pose.r, from_frame='obj', to_frame='stp') * aligned_grasp.T_grasp_obj if T_stp_grasp.x_axis[-1] > 0: # Grasp from below table, rotate by 180 degrees aligned_grasp = aligned_grasp.grasp_y_axis_offset(np.pi) T_grasp_camera = aligned_grasp.gripper_pose(self.gripper).inverse() * \ self.T_obj_camera.inverse() # Calculate psi, the angle between the grasp approach axis and the camera principal ray z_axis = T_grasp_camera.z_axis psi = np.arccos( z_axis[2] / np.sqrt(z_axis[0]**2 + z_axis[1]**2 + z_axis[2]**2)) # if np.rad2deg(psi) > 20: # continue # Get gamma, the angle between the grasp approach axis and the table normal _, gamma, _ = aligned_grasp.grasp_angles_from_stp_z(stable_pose) if LIMIT_BETA: perpendicular_table = (np.abs(gamma) < np.deg2rad(5)) if not perpendicular_table: continue if VISUALISE_3D: vis.figure() T_obj_world = vis.mesh_stable_pose(self.obj.mesh.trimesh, stable_pose.T_obj_world, style='surface', dim=0.5, plot_table=True) T_camera_world = T_obj_world * self.T_obj_camera.inverse() vis.gripper(self.gripper, aligned_grasp, T_obj_world, color=(0.3, 0.3, 0.3), T_camera_world=T_camera_world) # vis.gripper(self.gripper, grasps[cnt], T_obj_world, color=(0.3, 0.3, 0.3)) # print(psi) vis.show() collision_free = self.is_grasp_collision_free( aligned_grasp, collision_checker) # Project grasp coordinates in image grasp_2d = aligned_grasp.project_camera(self.T_obj_camera, final_camera_intr) if IM_TENSOR: im, new_grasp = self.random_im_crop(depth_im_tf, (32, 32), grasp_2d.center.x, grasp_2d.center.y) self.tensor_datapoint['depth_ims_tf_table'] = im hand_pose = self.get_hand_pose(grasp_2d, T_grasp_camera.quaternion, psi, gamma, new_grasp=new_grasp) else: hand_pose = self.get_hand_pose(grasp_2d, T_grasp_camera.quaternion, psi, gamma) self.add_datapoint(hand_pose, collision_free, aligned_grasp, grasp_metrics) self.cur_image_label += 1
if config['debug']: random.seed(SEED) np.random.seed(SEED) env = GraspingEnv(config, config['vis']) env.reset() env.state.material_props._color = np.array([0.86274509803] * 3) obj_name = env.state.obj.key policy.set_environment(env.environment) if args.before: env.render_3d_scene() vis3d.show(starting_camera_pose=CAMERA_POSE) action = policy.action(env.state) vis3d.figure() env.render_3d_scene() num_vertices = len(action.metadata['vertices']) for i, vertex, q_increase in zip(np.arange(num_vertices), action.metadata['vertices'], action.metadata['quality_increases']): topples = action.metadata['final_pose_ind'][i] != 0 color = np.array( [min(1, 2 * (1 - q_increase)), min(2 * q_increase, 1), 0]) if topples else np.array([0, 0, 0]) # color = np.array([min(1, 2*(1-q_increase)), min(2*q_increase, 1), 0]) scale = .001 if i != action.metadata['best_ind'] else .003 vis3d.points(Point(vertex, 'world'), scale=scale, color=color) gripper = env.gripper(action) T_start_world = action.T_begin_world * gripper.T_mesh_grasp
def save_samples(self, sample, grasps, collisions, T_obj_stp, grasp_metrics, stable_pose): self.tensor_datapoint['image_labels'] = self.cur_image_label T_stp_camera = sample.camera.object_to_camera_pose self.T_obj_camera = T_stp_camera * T_obj_stp.as_frames( 'obj', T_stp_camera.from_frame) depth_im_table = sample.renders[RenderMode.DEPTH_SCENE].image camera_pose = self.get_camera_pose(sample) shifted_camera_intr = sample.camera.camera_intr cx = depth_im_table.center[1] cy = depth_im_table.center[0] self.tensor_datapoint['camera_intrs'] = np.r_[shifted_camera_intr.fx, shifted_camera_intr.fy, shifted_camera_intr.cx, shifted_camera_intr.cy] for aligned_grasp, collision_free in zip(grasps, collisions): # Project grasp coordinates in image grasp_point = aligned_grasp.close_fingers(self.obj, vis=False, check_approach=False) if not grasp_point[0]: raise Warning("Could not find contact points") continue grasp_width = aligned_grasp.width_from_endpoints( grasp_point[1][0].point, grasp_point[1][1].point) if VISUALISE_3D: c = np.array((1, 0, 0)) vis.figure() T_obj_world = vis.mesh_stable_pose(self.obj.mesh.trimesh, stable_pose.T_obj_world, style='surface', dim=0.5, plot_table=True) points = np.array( (np.append(grasp_point[1][0].point, 1), np.append(grasp_point[1][1].point, 1))) vis.gripper(self.gripper, aligned_grasp, T_obj_world, color=(0.3, 0.3, 0.3), point=points, point_color=c) vis.show() grasp_2d = aligned_grasp.project_camera(self.T_obj_camera, shifted_camera_intr) depth_im_tf = self.prepare_images(depth_im_table, grasp_2d, cx, cy) hand_pose = self.get_hand_pose(grasp_2d, shifted_camera_intr, grasp_width) self.add_datapoint(depth_im_tf, hand_pose, collision_free, camera_pose, aligned_grasp, grasp_metrics) self.cur_image_label += 1
def read_grasps(self, object_name, stable_pose_id, max_grasps=10, visualize=False): # load gripper gripper = RobotGripper.load( GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') # open Dex-Net API dexnet_handle = DexNet() dexnet_handle.open_database(self.database_path) dexnet_handle.open_dataset(self.dataset_name) # read the most robust grasp sorted_grasps, metrics = dexnet_handle.dataset.sorted_grasps( object_name, stable_pose_id=('pose_' + str(stable_pose_id)), metric='force_closure', gripper=gripper.name) if (len(sorted_grasps)) == 0: print('no grasps for this stable pose') if visualize: stable_pose = dexnet_handle.dataset.stable_pose( object_name, stable_pose_id=('pose_' + str(stable_pose_id))) obj = dexnet_handle.dataset.graspable(object_name) vis.figure() T_table_world = RigidTransform(from_frame='table', to_frame='world') T_obj_world = Visualizer3D.mesh_stable_pose( obj.mesh.trimesh, stable_pose.T_obj_world, T_table_world=T_table_world, color=(0.5, 0.5, 0.5), style='surface', plot_table=True, dim=0.15) vis.show(False) return None, None contact_points = map(get_contact_points, sorted_grasps) # ------------------------ VISUALIZATION CODE ------------------------ if visualize: low = np.min(metrics) high = np.max(metrics) if low == high: q_to_c = lambda quality: CONFIG['quality_scale'] else: q_to_c = lambda quality: CONFIG['quality_scale'] * ( quality - low) / (high - low) stable_pose = dexnet_handle.dataset.stable_pose( object_name, stable_pose_id=('pose_' + str(stable_pose_id))) obj = dexnet_handle.dataset.graspable(object_name) vis.figure() T_table_world = RigidTransform(from_frame='table', to_frame='world') T_obj_world = Visualizer3D.mesh_stable_pose( obj.mesh.trimesh, stable_pose.T_obj_world, T_table_world=T_table_world, color=(0.5, 0.5, 0.5), style='surface', plot_table=True, dim=0.15) for grasp, metric in zip(sorted_grasps, metrics): color = plt.get_cmap('hsv')((q_to_c(metric)))[:-1] vis.grasp(grasp, T_obj_world=stable_pose.T_obj_world, grasp_axis_color=color, endpoint_color=color) vis.show(False) # ------------------------ END VISUALIZATION CODE --------------------------- # ------------------------ START COLLISION CHECKING --------------------------- #stable_pose = dexnet_handle.dataset.stable_pose(object_name, stable_pose_id=('pose_'+str(stable_pose_id))) #graspable = dexnet_handle.dataset.graspable(object_name) #cc = GraspCollisionChecker(gripper).set_graspable_object(graspable, stable_pose.T_obj_world) stable_pose_matrix = self.get_stable_pose(object_name, stable_pose_id) # CLOSE DATABASE dexnet_handle.close_database() gripper_poses = [] for grasp in sorted_grasps[:max_grasps]: gripper_pose_matrix = np.eye(4, 4) center_world = np.matmul( stable_pose_matrix, [grasp.center[0], grasp.center[1], grasp.center[2], 1]) axis_world = np.matmul( stable_pose_matrix, [grasp.axis_[0], grasp.axis_[1], grasp.axis_[2], 1]) gripper_angle = math.atan2(axis_world[1], axis_world[0]) gripper_pose_matrix[:3, 3] = center_world[:3] gripper_pose_matrix[0, 0] = math.cos(gripper_angle) gripper_pose_matrix[0, 1] = -math.sin(gripper_angle) gripper_pose_matrix[1, 0] = math.sin(gripper_angle) gripper_pose_matrix[1, 1] = math.cos(gripper_angle) #if visualize: # vis.figure() # vis.gripper_on_object(gripper, grasp, obj, stable_pose=stable_pose.T_obj_world) # vis.show(False) gripper_poses.append(gripper_pose_matrix) return contact_points, gripper_poses
def antipodal_grasp_sampler(self): #of = ObjFile(OBJ_FILENAME) #sf = SdfFile(SDF_FILENAME) #mesh = of.read() #sdf = sf.read() #obj = GraspableObject3D(sdf, mesh) mass = 1.0 CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE mesh_processor = MeshProcessor(OBJ_FILENAME, CONFIG['cache_dir']) mesh_processor.generate_graspable(CONFIG) mesh = mesh_processor.mesh sdf = mesh_processor.sdf obj = GraspableObject3D(sdf, mesh) gripper = RobotGripper.load(GRIPPER_NAME) ags = AntipodalGraspSampler(gripper, CONFIG) grasps = ags.sample_grasps(obj, num_grasps=100) print(len(grasps)) quality_config = GraspQualityConfigFactory.create_config(CONFIG['metrics']['robust_ferrari_canny']) #quality_config = GraspQualityConfigFactory.create_config(CONFIG['metrics']['force_closure']) quality_fn = GraspQualityFunctionFactory.create_quality_function(obj, quality_config) vis.figure() #vis.points(PointCloud(nparraypoints), scale=0.001, color=np.array([0.5,0.5,0.5])) #vis.plot3d(nparraypoints) metrics = [] vis.mesh(obj.mesh.trimesh, style='surface') for grasp in grasps: success, c = grasp.close_fingers(obj) if success: c1, c2 = c #fn_fc = quality_fn(grasp).quality true_fc = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config) true_fc = true_fc metrics.append(true_fc) #color = quality_fn(grasp).quality #if (true_fc > 1.0): # true_fc = 1.0 #color = (1.0-true_fc, true_fc, 0.0) low = np.min(metrics) high = np.max(metrics) print(low) print(high) if low == high: q_to_c = lambda quality: CONFIG['quality_scale'] else: q_to_c = lambda quality: CONFIG['quality_scale'] * (quality - low) / (high - low) print(len(metrics)) for grasp, metric in zip(grasps, metrics): color = plt.get_cmap('hsv')(q_to_c(metric))[:-1] print(color) vis.grasp(grasp, grasp_axis_color=color,endpoint_color=color) vis.show(False)
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 save_samples(self, sample, grasps, collisions, T_obj_stp, grasp_metrics, stable_pose): self.tensor_datapoint['image_labels'] = self.cur_image_label T_stp_camera = sample.camera.object_to_camera_pose self.T_obj_camera = T_stp_camera * T_obj_stp.as_frames( 'obj', T_stp_camera.from_frame) depth_im_table = sample.renders[RenderMode.DEPTH_SCENE].image camera_pose = self.get_camera_pose(sample) final_camera_intr = sample.camera.camera_intr.crop( self.crop_height, self.crop_width, depth_im_table.center[0], depth_im_table.center[1]) self.tensor_datapoint['camera_intrs'] = np.r_[final_camera_intr.fx, final_camera_intr.fy, final_camera_intr.cx, final_camera_intr.cy] self.save_image(depth_im_table) for aligned_grasp, collision_free in zip(grasps, collisions): # Project grasp coordinates in image grasp_point = aligned_grasp.close_fingers(self.obj, vis=False, check_approach=False) if not grasp_point[0]: Warning("Could not find contact points") continue # Get grasp width in pixel from endpoints p_1 = np.dot(self.T_obj_camera.matrix, np.append(grasp_point[1][0].point, 1).T).T[:3] p_2 = np.dot(self.T_obj_camera.matrix, np.append(grasp_point[1][1].point, 1).T).T[:3] grasp_width_px = self.width_px(final_camera_intr, p_1, p_2) grasp_width = aligned_grasp.width_from_endpoints( grasp_point[1][0].point, grasp_point[1][1].point) if VISUALISE_3D: c = np.array((1, 0, 0)) vis.figure() T_obj_world = vis.mesh_stable_pose(self.obj.mesh.trimesh, stable_pose.T_obj_world, style='surface', dim=0.5, plot_table=True) points = np.array( (np.append(grasp_point[1][0].point, 1), np.append(grasp_point[1][1].point, 1))) vis.gripper(self.gripper, aligned_grasp, T_obj_world, color=(0.3, 0.3, 0.3), point=points, point_color=c) vis.show() grasp_2d = aligned_grasp.project_camera(self.T_obj_camera, final_camera_intr) grasp_center_x = self.crop_width // 2 - grasp_2d.center.x grasp_center_y = self.crop_height // 2 - grasp_2d.center.y hand_pose = self.get_hand_pose(grasp_center_x, grasp_center_y, grasp_2d, final_camera_intr, grasp_width, grasp_width_px) self.add_datapoint(hand_pose, collision_free, camera_pose, aligned_grasp, grasp_metrics) self.cur_image_label += 1
for phi_offset in phi_offsets: aligned_grasp.grasp_y_axis_offset(phi_offset) collides = collision_checker.collides_along_approach(aligned_grasp, approach_dist, delta_approach) if not collides: collision_free = True break # Load grasp metrics grasp_metrics = dataset.grasp_metrics(obj.key, aligned_grasps, gripper=gripper.name) # Project grasp coordinates in image grasp_2d = aligned_grasp.project_camera(T_obj_camera, shifted_camera_intr) if VISUALISE_3D: vis.figure() T_obj_world = vis.mesh_stable_pose(obj.mesh.trimesh, stable_pose.T_obj_world, style='surface', dim=0.5) T_camera_world = T_obj_world * T_obj_camera.inverse() vis.gripper(gripper, aligned_grasp, T_obj_world, color=(0.3, 0.3, 0.3), T_camera_world=T_camera_world) vis.show() # Get translation image distances to grasp dx = cx - grasp_2d.center.x dy = cy - grasp_2d.center.y translation = np.array([dy, dx]) # Transform, crop and resize image binary_im_tf = binary_im.transform(translation, grasp_2d.angle) depth_im_tf_table = depth_im_table.transform(translation, grasp_2d.angle)
def showgraspfrompicklewithcandidate(self, object_name,metric_name,onlynagative,onlypositive,openravechecker): object = self.dataset[object_name] labelgrasps,labelmetrics,candidate_labelgrasps,candidate_labelmetrics=self.readdatafrompicklewithcandidate(object_name) config=self._get_config(None) low =0.0 #np.min(metrics) high = np.max([labelmetrics[i][metric_name] for i in range(len(labelmetrics))]) # print 'high',high,config['quality_scale'] if low == high: q_to_c = lambda quality: config['quality_scale'] else: q_to_c = lambda quality: config['quality_scale'] * (quality - low) / (high - low) recalculate_grasp=[] vis.figure() vis.mesh(object.mesh.trimesh ,style='surface') for i in range(len(labelgrasps)): if labelmetrics[i][metric_name]==0 : if not onlypositive: vis.figure() vis.mesh(object.mesh.trimesh ,style='surface') color = plt.get_cmap('hsv')(q_to_c(labelmetrics[i][metric_name]))[:-1] vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=(1,0,0), grasp_axis_color=(1,0,0) ) for kk in range(len(candidate_labelgrasps[i] )): print 'candidate_labelgrasps[i][kk]',candidate_labelgrasps[i][kk] color = plt.get_cmap('hsv')(q_to_c(candidate_labelmetrics[i][candidate_labelgrasps[i][kk].id][metric_name]))[:-1] vis.graspwithapproachvectorusingcenter_point(candidate_labelgrasps[i][kk] ,approaching_color=color, grasp_axis_color=color ) recalculate_grasp.append(labelgrasps[i]) print 'maxtrics',i,labelmetrics[i][metric_name] # ,high vis.pose(RigidTransform(), alpha=0.1) vis.show(animate=False) elif labelmetrics[i][metric_name]==-1 : if not onlypositive: vis.shownormals(labelgrasps[i][1],labelgrasps[i][0],color=(0,0,1) ) else: if not onlynagative: vis.figure() vis.mesh(object.mesh.trimesh ,style='surface') color = plt.get_cmap('hsv')(q_to_c(labelmetrics[i][metric_name]))[:-1] if len(candidate_labelgrasps[i])>0: A=np.array(labelgrasps[i].rotated_full_axis[:,0] ) B=np.array(candidate_labelgrasps[i][0].rotated_full_axis[:,0] ) num = np.dot(A.T, B) print num if num<0.99: labelgrasps[i].approach_angle_= labelgrasps[i]._angle_aligned_with_table( -candidate_labelgrasps[i][0].rotated_full_axis[:,0]) vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=color, grasp_axis_color=(0,0,1) ) print 'length',len(candidate_labelgrasps[i] ) for kk in range(len(candidate_labelgrasps[i] )): color = plt.get_cmap('hsv')(q_to_c(candidate_labelmetrics[i][candidate_labelgrasps[i][kk].id][metric_name]))[:-1] vis.graspwithapproachvectorusingcenter_point(candidate_labelgrasps[i][kk] ,approaching_color=(0,1,1), grasp_axis_color=color ) recalculate_grasp.append(labelgrasps[i]) print 'maxtrics',i,labelmetrics[i][metric_name] # ,high vis.pose(RigidTransform(), alpha=0.1) vis.show(animate=False)
def save_samples(self, sample, grasps, T_obj_stp, collision_checker, grasp_metrics, stable_pose): self.tensor_datapoint['image_labels'] = self.cur_image_label T_stp_camera = sample.camera.object_to_camera_pose self.T_obj_camera = T_stp_camera * T_obj_stp.as_frames('obj', T_stp_camera.from_frame) aligned_grasps = self.align_grasps_with_camera(grasps) depth_im_table = sample.renders[RenderMode.DEPTH_SCENE].image cx = depth_im_table.center[1] cy = depth_im_table.center[0] camera_pose = self.get_camera_pose(sample) self.tensor_datapoint['camera_poses'] = camera_pose shifted_camera_intr = sample.camera.camera_intr self.tensor_datapoint['camera_intrs'] = np.r_[shifted_camera_intr.fx, shifted_camera_intr.fy, shifted_camera_intr.cx, shifted_camera_intr.cy] self.save_orig_image(depth_im_table, camera_pose, stable_pose, shifted_camera_intr) grid_space = 200 grids = 3 centre = np.empty((grids, grids, 2)) saved = np.ones((grids, grids)) * -1 depth_images = np.empty((grids, grids, 32, 32, 1)) # Crop images in a grid around the centre for x_grid in range(grids): x_pos = grid_space / 4 - x_grid * grid_space / (grids + 1) for y_grid in range(grids): y_pos = grid_space / 4 - y_grid * grid_space / (grids + 1) depth_im = self.prepare_images(depth_im_table, x_pos, y_pos) self._show_image(depth_im) depth_images[x_grid, y_grid, :, :, :] = depth_im centre[x_grid, y_grid, 0] = x_pos centre[x_grid, y_grid, 1] = y_pos for cnt, aligned_grasp in enumerate(aligned_grasps): collision_free = self.is_grasp_collision_free(aligned_grasp, collision_checker) # Project grasp coordinates in image grasp_2d = aligned_grasp.project_camera(self.T_obj_camera, shifted_camera_intr) grasp_point = aligned_grasp.close_fingers(self.obj, vis=False, check_approach=False) if not grasp_point[0]: Warning("Could not find contact points") continue # Get grasp width in pixel from endpoints p_1 = np.dot(self.T_obj_camera.matrix, np.append(grasp_point[1][0].point, 1).T).T[:3] p_2 = np.dot(self.T_obj_camera.matrix, np.append(grasp_point[1][1].point, 1).T).T[:3] grasp_width_px = self.width_px(shifted_camera_intr, p_1, p_2) grasp_width = aligned_grasp.width_from_endpoints(grasp_point[1][0].point, grasp_point[1][1].point) pos_x = cx - grasp_2d.center.x pos_y = cy - grasp_2d.center.y distance = np.abs((centre[:, :, 0] - pos_x)**2 + (centre[:, :, 1] - pos_y)**2) idx = np.where(distance == np.amin(distance)) idx_x = idx[0][0] idx_y = idx[1][0] grasp_center_x = (300 - grasp_2d.center.x - centre[idx_x, idx_y, 0])//3 grasp_center_y = (300 - grasp_2d.center.y - centre[idx_x, idx_y, 1])//3 if saved[idx_x, idx_y] == -1: np.save(self.image_dir + "/depth_im_{:07d}.npy".format(self.cur_image_file), depth_images[idx_x, idx_y, :, :, :]) saved[idx_x, idx_y] = self.cur_image_file self.tensor_datapoint['image_files'] = self.cur_image_file self.cur_image_file += 1 else: self.tensor_datapoint['image_files'] = saved[idx_x, idx_y] T_grasp_camera = aligned_grasp.gripper_pose(self.gripper).inverse() * self.T_obj_camera.inverse() if VISUALISE_3D: z_axis = T_grasp_camera.z_axis theta = np.rad2deg(np.arccos((z_axis[2]) / (np.sqrt((z_axis[0] ** 2 + z_axis[1] ** 2 + z_axis[2] ** 2))))) vis.figure() T_obj_world = vis.mesh_stable_pose(self.obj.mesh.trimesh, stable_pose.T_obj_world, style='surface', dim=0.5, plot_table=True) T_camera_world = T_obj_world * self.T_obj_camera.inverse() vis.gripper(self.gripper, aligned_grasp, T_obj_world, color=(0.3, 0.3, 0.3), T_camera_world=T_camera_world) print(theta) vis.show() hand_pose = self.get_hand_pose(grasp_center_x, grasp_center_y, grasp_2d, T_grasp_camera, grasp_width, grasp_width_px) self.add_datapoint(hand_pose, collision_free, aligned_grasp, grasp_metrics) self.cur_image_label += 1
def save_samples(self, sample, grasps, T_obj_stp, collision_checker, grasp_metrics, stable_pose): self.tensor_datapoint['image_labels'] = self.cur_image_label T_stp_camera = sample.camera.object_to_camera_pose self.T_obj_camera = T_stp_camera * T_obj_stp.as_frames( 'obj', T_stp_camera.from_frame) depth_im_table = sample.renders[RenderMode.DEPTH_SCENE].image camera_pose = self.get_camera_pose(sample) shifted_camera_intr = sample.camera.camera_intr aligned_grasps = self.align_grasps_with_camera(grasps) cx = depth_im_table.center[1] cy = depth_im_table.center[0] self.tensor_datapoint['camera_intrs'] = np.r_[shifted_camera_intr.fx, shifted_camera_intr.fy, shifted_camera_intr.cx, shifted_camera_intr.cy] for aligned_grasp in aligned_grasps: if not self.is_grasp_aligned(aligned_grasp): continue collision_free = self.is_grasp_collision_free( aligned_grasp, collision_checker) grasp_2d = aligned_grasp.project_camera(self.T_obj_camera, shifted_camera_intr) depth_im_tf = self.prepare_images(depth_im_table, grasp_2d, cx, cy) T_grasp_camera = aligned_grasp.gripper_pose( self.gripper).inverse() * self.T_obj_camera.inverse() if VISUALISE_3D: z_axis = T_grasp_camera.z_axis theta = np.rad2deg( np.arccos((z_axis[2]) / (np.sqrt( (z_axis[0]**2 + z_axis[1]**2 + z_axis[2]**2))))) vis.figure() T_obj_world = vis.mesh_stable_pose(self.obj.mesh.trimesh, stable_pose.T_obj_world, style='surface', dim=0.5, plot_table=True) T_camera_world = T_obj_world * self.T_obj_camera.inverse() vis.gripper(self.gripper, aligned_grasp, T_obj_world, color=(0.3, 0.3, 0.3), T_camera_world=T_camera_world) vis.show() theta = grasp_2d.angle R = np.array([[np.cos(theta), -np.sin(theta), 0], [np.sin(theta), np.cos(theta), 0], [0, 0, 1]]) rot = RigidTransform(rotation=R, to_frame='camera', from_frame='camera') new_quaternion = T_grasp_camera * rot hand_pose = self.get_hand_pose(grasp_2d, new_quaternion.quaternion) self.add_datapoint(depth_im_tf, hand_pose, collision_free, camera_pose, aligned_grasp, grasp_metrics) self.cur_image_label += 1
def display_grasps(self, object_name, gripper_name, metric_name, config=None): """ Display grasps for an object Parameters ---------- object_name : :obj:`str` Object to display gripper_name : :obj:`str` Gripper for which to display grasps metric_name : :obj:`str` Metric to color/rank grasps with config : :obj:`dict` Configuration dict for visualization. Parameters are in Other parameters. Values from self.default_config are used for keys not provided. Other Parameters ---------------- gripper_dir Directory where the grippers models and parameters are. quality_scale Range to scale quality metric values to show_gripper Whether or not to show the gripper in the visualization min_metric lowest value of metric to show grasps for max_plot_gripper Number of grasps to plot animate Whether or not to animate the displayed object """ self._check_opens() config = self._get_config(config) grippers = os.listdir(config['gripper_dir']) if gripper_name not in grippers: raise ValueError( "{} is not a valid gripper name".format(gripper_name)) gripper = gr.RobotGripper.load(gripper_name, gripper_dir=config['gripper_dir']) objects = self.dataset.object_keys if object_name not in objects: raise ValueError( "{} is not a valid object name".format(object_name)) metrics = self.dataset.available_metrics(object_name, gripper=gripper.name) if metric_name not in metrics: raise ValueError( "{} is not computed for gripper {}, object {}".format( metric_name, gripper.name, object_name)) logger.info('Displaying grasps for gripper %s on object %s' % (gripper.name, object_name)) object = self.dataset[object_name] grasps, metrics = self.dataset.sorted_grasps(object_name, metric_name, gripper=gripper.name) if len(grasps) == 0: raise RuntimeError('No grasps for gripper %s on object %s' % (gripper.name, object_name)) return low = np.min(metrics) high = np.max(metrics) if low == high: q_to_c = lambda quality: config['quality_scale'] else: q_to_c = lambda quality: config['quality_scale'] * ( quality - low) / (high - low) if config['show_gripper']: i = 0 stable_pose = self.dataset.stable_pose(object.key, 'pose_1') for grasp, metric in zip(grasps, metrics): if metric <= config['min_metric']: continue print 'Grasp %d %s=%.5f' % (grasp.id, metric_name, metric) T_obj_world = RigidTransform(from_frame='obj', to_frame='world') color = plt.get_cmap('hsv')(q_to_c(metric))[:-1] T_obj_gripper = grasp.gripper_pose(gripper) grasp = grasp.perpendicular_table(stable_pose) vis.figure() vis.gripper_on_object(gripper, grasp, object, gripper_color=(0.25, 0.25, 0.25), stable_pose=stable_pose, plot_table=False) vis.show(animate=config['animate']) i += 1 if i >= config['max_plot_gripper']: break else: i = 0 vis.figure() vis.mesh(object.mesh, style='surface') for grasp, metric in zip(grasps, metrics): if metric <= config['min_metric']: continue print 'Grasp %d %s=%.5f' % (grasp.id, metric_name, metric) T_obj_world = RigidTransform(from_frame='obj', to_frame='world') color = plt.get_cmap('hsv')(q_to_c(metric))[:-1] T_obj_gripper = grasp.gripper_pose(gripper) vis.grasp(grasp, grasp_axis_color=color, endpoint_color=color) i += 1 if i >= config['max_plot_gripper']: break vis.show(animate=config['animate'])
def antipodal_grasp_sampler(visual=False, debug=False): mass = 1.0 CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE mesh_processor = MeshProcessor(OBJ_FILENAME, CONFIG['cache_dir']) mesh_processor.generate_graspable(CONFIG) mesh = mesh_processor.mesh sdf = mesh_processor.sdf stable_poses = mesh_processor.stable_poses obj = GraspableObject3D(sdf, mesh) stable_pose = stable_poses[0] if visual: vis.figure() T_table_world = RigidTransform(from_frame='table', to_frame='world') T_obj_world = Visualizer3D.mesh_stable_pose(obj.mesh.trimesh, stable_pose.T_obj_world, T_table_world=T_table_world, color=(0.5, 0.5, 0.5), style='surface', plot_table=True, dim=0.15) if debug: print(len(stable_poses)) #for stable_pose in stable_poses: # print(stable_pose.p) # print(stable_pose.r) # print(stable_pose.x0) # print(stable_pose.face) # print(stable_pose.id) # glass = 22 is standing straight if debug: print(stable_pose.r) print(stable_pose.T_obj_world) gripper = RobotGripper.load( GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') ags = AntipodalGraspSampler(gripper, CONFIG) stable_pose.id = 0 grasps = ags.generate_grasps(obj, target_num_grasps=20, max_iter=5, stable_pose=stable_pose.r) quality_config = GraspQualityConfigFactory.create_config( CONFIG['metrics']['robust_ferrari_canny']) metrics = [] result = [] #grasps = map(lambda g : g.perpendicular_table(stable_pose), grasps) for grasp in grasps: c1, c2 = grasp.endpoints true_fc = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config) metrics.append(true_fc) result.append((c1, c2)) if debug: success, c = grasp.close_fingers(obj) if success: c1, c2 = c print("Grasp:") print(c1.point) print(c2.point) print(true_fc) low = np.min(metrics) high = np.max(metrics) if low == high: q_to_c = lambda quality: CONFIG['quality_scale'] else: q_to_c = lambda quality: CONFIG['quality_scale'] * (quality - low) / ( high - low) if visual: for grasp, metric in zip(grasps, metrics): #grasp2 = grasp.perpendicular_table(stable_pose) #c1, c2 = grasp.endpoints #axis = ParallelJawPtGrasp3D.axis_from_endpoints(c1, c2) #angle = np.dot(np.matmul(stable_pose.r, axis), [1,0,0]) #angle = math.tan(axis[1]/axis[0]) #angle = math.degrees(angle)%360 #print(angle) #print(angle/360.0) color = plt.get_cmap('hsv')(q_to_c(metric))[:-1] vis.grasp(grasp, T_obj_world=T_obj_world, grasp_axis_color=color, endpoint_color=color) #axis = np.array([[0,0,0], point]) #points = [(x[0], x[1], x[2]) for x in axis] #Visualizer3D.plot3d(points, color=(0,0,1), tube_radius=0.002) vis.show(False) pose_matrix = np.eye(4, 4) pose_matrix[:3, :3] = T_obj_world.rotation pose_matrix[:3, 3] = T_obj_world.translation return pose_matrix, result