def callFakeGrasping(self, prob, container):
        print('------- DOING GRASPING ------- ')
        print(' grasp_point = ', self.grasp_point)
        grasp(objInput=self.grasp_point,
              listener=self.listener,
              br=self.br,
              isExecute=self.isExecute,
              binId=container,
              withPause=False,
              viz_pub=self.viz_array_pub)

        if self.is_control:
            #find new and improved grasp points
            back_img_list = self.controller.capture_images()
            best_grasp_dict = self.controller.control_policy(
                back_img_list, smirror=self.smirror)
            # self.controller.visualize_actions(with_CAM = False)
            # self.controller.visualize_best_action()
            print best_grasp_dict['delta_pos']

            #go for new grasp Point
            self.grasping_output = grasp_correction(
                self.grasp_point, best_grasp_dict['delta_pos'], self.listener,
                self.br)

        retrieve(listener=self.listener,
                 br=self.br,
                 isExecute=self.isExecute,
                 binId=container,
                 withPause=False,
                 viz_pub=self.viz_array_pub,
                 ws_object=self.weightSensor)

        f = random.choice(os.listdir(
            self.FAKE_GRASPING_DIR))  #Get fake output for the primitive
        with open(os.path.join(self.FAKE_GRASPING_DIR, f), 'r') as infile:
            self.grasping_output = json.load(infile)
        self.grasping_output = self.grasping_output['primitive_output']
        #Random decides if execution possible
        self.grasping_output['execution_possible'] = (np.random.rand() <= prob)
        self.execution_possible = self.grasping_output['execution_possible']
    def run_grasping(self, container=None):
        self.getBestGraspingPoint(container)

        #~Publish grasp proposal information
        grasp_proposal_msgs = Float32MultiArray()
        grasp_proposal_msgs.data = self.grasp_point
        if self.grasp_point is not None:
            self.grasp_proposal_pub.publish(grasp_proposal_msgs)
            comments_msgs = String()
            comments_msgs.data = self.experiment_description
            experiment_type_msgs = String()
            experiment_type_msgs.data = self.experiment_type
            self.experiment_comments_pub.publish(comments_msgs)
            self.experiment_type_pub.publish(experiment_type_msgs)
        else:
            print('There are no grasp proposal')
            self.execution_possible = False
            return

        #~Visualize grasp proposals
        markers_msg = MarkerArray()
        m0 = createDeleteAllMarker('pick_proposals')
        markers_msg.markers.append(m0)
        for i in range(10):
            self.proposal_viz_array_pub.publish(markers_msg)
        ik.visualize_helper.visualize_grasping_proposals(
            self.proposal_viz_array_pub, self.all_grasp_proposals,
            self.listener, self.br)
        ik.visualize_helper.visualize_grasping_proposals(
            self.proposal_viz_array_pub, np.asarray([self.grasp_point]),
            self.listener, self.br, True)

        #execute for grasp. Stop when the gripper is closed
        self.back_img_list = self.capture_images()

        #self.grasp_point=[0.92737001180648804, -0.391, -0.12441360205411911, 0.0, 0.0, -1.0, 0.067681394517421722, 0.059999998658895493, 0.0, 1.0, 0.0, 0.74888890981674194]

        self.grasping_output = grasp(objInput=self.grasp_point,
                                     listener=self.listener,
                                     br=self.br,
                                     isExecute=self.isExecute,
                                     binId=container,
                                     withPause=self.withPause,
                                     viz_pub=self.proposal_viz_array_pub,
                                     recorder=self.gdr)
        num_it = 0
        print('gripper open: ', gripper.getGripperopening())
        is_in_wrong_pose = (gripper.getGripperopening() < 0.04)
        while is_in_wrong_pose:
            '''
            self.retrieve_output = retrieve(listener=self.listener, br=self.br,
                                       isExecute=self.isExecute, binId=container,
                                       withPause=self.withPause, viz_pub=self.proposal_viz_array_pub,
                                       recorder=self.gdr, ws_object=self.weightSensor, isShake=False)
            self.grasping_output = grasp(objInput=self.grasp_point, listener=self.listener, br=self.br,
                                    isExecute=self.isExecute, binId=container, withPause=self.withPause,
                                    viz_pub=self.proposal_viz_array_pub, recorder=self.gdr, open_hand=False)
            '''
            # Motion heuristic
            initial_dz = 0.05
            dz = .022  #should be related to object length
            ik.helper.move_cart(dz=initial_dz)
            rospy.sleep(0.5)
            ik.helper.move_cart(dz=-initial_dz)
            rospy.sleep(0.5)
            gripper.move(90)
            ik.helper.move_cart(dz=dz)
            rospy.sleep(0.5)
            #should be done in the direction of the gripper plane
            dx = 0.038  #.02*num_it
            ik.helper.move_cart_hand(self.listener,
                                     dx=dx,
                                     dy=0,
                                     dz=0,
                                     speedName='fastest')
            rospy.sleep(0.5)
            ik.helper.move_cart(dz=-dz)
            rospy.sleep(0.5)
            ik.helper.move_cart_hand(self.listener,
                                     dx=-dx,
                                     dy=0,
                                     dz=0,
                                     speedName='fastest')
            rospy.sleep(0.5)
            gripper.close()
            rospy.sleep(0.5)
            print('gripper_open', gripper.getGripperopening())

            is_in_wrong_pose = (gripper.getGripperopening() < 0.04)
            if is_in_wrong_pose:
                gripper.move(90)
                rospy.sleep(0.5)
                ik.helper.move_cart(dz=dz)
                rospy.sleep(0.5)
                ik.helper.move_cart_hand(self.listener,
                                         dx=-dx,
                                         dy=0,
                                         dz=0,
                                         speedName='fastest')
                rospy.sleep(0.5)
                ik.helper.move_cart(dz=-dz)
                rospy.sleep(0.5)
                ik.helper.move_cart_hand(self.listener,
                                         dx=dx,
                                         dy=0,
                                         dz=0,
                                         speedName='fastest')
                rospy.sleep(0.5)

                gripper.close()
                rospy.sleep(0.5)
                self.gdr.save_data_recorded = False
                num_it += 1
                is_in_wrong_pose = (
                    gripper.getGripperopening() < 0.04
                )  #and (gripper.getGripperopening() > 0.015)

        self.retrieve_output = retrieve(listener=self.listener,
                                        br=self.br,
                                        isExecute=self.isExecute,
                                        binId=container,
                                        withPause=self.withPause,
                                        viz_pub=self.proposal_viz_array_pub,
                                        recorder=self.gdr,
                                        ws_object=self.weightSensor,
                                        isShake=False)
        self.execution_possible = self.retrieve_output['execution_possible']
    def getBestGraspingPoint(self, container):
        self.GetGraspPoints(num_points=100, container=container)

        self.execution_possible = False
        if self.num_pick_proposals == 0:  #If no grasp point available
            self.grasp_point = None
            self.grasp_score = 0
            return

        #Find the best point that it is not in collision:
        num_attempts = 0
        num_it = 0
        while not self.execution_possible and num_attempts < 20 and num_it < self.num_pick_proposals:  #Each time try at most 20 attempts
            grasp_point = copy.deepcopy(
                list(self.all_pick_proposals[num_it][:]))
            num_it += 1
            #If we already know it is a bad point, we do not try it again
            if grasp_point in self.bad_grasping_points:
                return
            #Check if in collision
            num_attempts += 1
            grasp_output = grasp(objInput=grasp_point,
                                 listener=self.listener,
                                 br=self.br,
                                 isExecute=False,
                                 binId=container,
                                 withPause=False,
                                 viz_pub=self.viz_array_pub)
            retrieve_output = retrieve(listener=self.listener,
                                       br=self.br,
                                       isExecute=False,
                                       binId=container,
                                       withPause=False,
                                       viz_pub=self.viz_array_pub,
                                       ws_object=self.weightSensor)

            if grasp_output['execution_possible'] and retrieve_output[
                    'execution_possible']:
                self.grasp_score = copy.deepcopy(self.all_pick_scores[num_it -
                                                                      1])
                self.grasp_point = copy.deepcopy(grasp_point)
                #Visualize grasp
                markers_msg = MarkerArray()
                m0 = createDeleteAllMarker('pick_proposals')
                markers_msg.markers.append(m0)
                for i in range(10):
                    self.proposal_viz_array_pub.publish(markers_msg)
                ik.visualize_helper.visualize_grasping_proposals(
                    self.proposal_viz_array_pub,
                    np.asarray([self.grasp_point]), self.listener, self.br,
                    True)
                print('Best grasp point:', grasp_point, ' with score: ',
                      self.grasp_score, 'in bin: ', container)
                return
            if grasp_point is not None:
                self.bad_grasping_points.append(grasp_point)
                self.bad_grasping_times.append(-1)
        print('NONE OF THE GRASPING POINTS WORKS')
        self.grasp_point = None
        self.grasp_score = 0
        return
    def run_grasping(self, container=None):
        self.getBestGraspingPoint(container)
        grasp_proposal_msgs = Float32MultiArray()
        grasp_proposal_msgs.data = self.grasp_point
        grasp_noise_msgs = Float32MultiArray()
        grasp_noise_msgs.data = self.grasp_noise

        if self.grasp_point is not None:
            self.grasp_proposal_pub.publish(grasp_proposal_msgs)
            self.grasp_noise_pub.publish(grasp_noise_msgs)
        comments_msgs = String()
        comments_msgs.data = self.experiment_description
        experiment_type_msgs = String()
        experiment_type_msgs.data = self.experiment_type
        self.experiment_comments_pub.publish(comments_msgs)
        self.experiment_type_pub.publish(experiment_type_msgs)

        if self.grasp_point is None:
            print(
                'It was suppose to do grasping, but there is no grasp proposal'
            )
            self.execution_possible = False
            return
        if self.visionType != 'real':
            self.callFakeGrasping(prob=0.8, container=container)
            return

        #~visualize grasp proposals
        markers_msg = MarkerArray()
        m0 = createDeleteAllMarker('pick_proposals')
        markers_msg.markers.append(m0)
        for i in range(10):
            self.proposal_viz_array_pub.publish(markers_msg)
        ik.visualize_helper.visualize_grasping_proposals(
            self.proposal_viz_array_pub, self.all_grasp_proposals,
            self.listener, self.br)
        ik.visualize_helper.visualize_grasping_proposals(
            self.proposal_viz_array_pub, np.asarray([self.grasp_point]),
            self.listener, self.br, True)

        #execute for grasp. Stop when the gripper is closed
        if self.is_control:
            back_img_list = self.controller.capture_images()

        self.grasping_output = grasp(objInput=self.grasp_point,
                                     listener=self.listener,
                                     br=self.br,
                                     isExecute=self.isExecute,
                                     binId=container,
                                     withPause=self.withPause,
                                     viz_pub=self.proposal_viz_array_pub,
                                     recorder=self.gdr)

        self.gdr.save_item(item_name='grasp_noise_std_dev',
                           data=self.grasp_std)
        self.gdr.save_item(item_name='grasp_noise', data=self.grasp_noise)

        if self.is_control:
            if gripper.getGripperopening() > 0.017:
                print('[Planner]: ', gripper.getGripperopening())
                # WE PAUSE THE RECOORDER TO SAVE DATA
                self.gdr.pause_recording()

                #find new and improved grasp points
                best_grasp_dict = self.controller.control_policy(
                    back_img_list, smirror=self.smirror)
                # self.controller.visualize_actions(with_CAM = False)
                # self.controller.visualize_best_action(with_CAM = False)
                #save network information action_dict and best_action_dict
                self.gdr.save_item(item_name='action_dict',
                                   data=self.controller.action_dict)
                self.gdr.save_item(item_name='best_action_dict',
                                   data=self.controller.best_action_dict)
                #WE UNPAUSE THE RECORDER
                self.gdr.replay_recording()
                #go for new grasp Point
                self.grasping_output = grasp_correction(
                    self.grasp_point, best_grasp_dict['delta_pos'],
                    self.listener, self.br)
                self.gdr.save_data_recorded = True
            else:
                self.gdr.save_data_recorded = False

        #frank hack for double grasping
        if self.experiment_type == 'is_data_collection':
            if gripper.getGripperopening() > 0.017:
                self.grasping_output = grasp_correction(
                    self.grasp_point, np.array([0, 0, 0]), self.listener,
                    self.br)
                self.gdr.save_data_recorded = True
            else:
                self.gdr.save_data_recorded = False

        self.retrieve_output = retrieve(listener=self.listener,
                                        br=self.br,
                                        isExecute=self.isExecute,
                                        binId=container,
                                        withPause=self.withPause,
                                        viz_pub=self.proposal_viz_array_pub,
                                        recorder=self.gdr,
                                        ws_object=self.weightSensor)
        self.execution_possible = self.retrieve_output['execution_possible']
Exemple #5
0
    def run_grasping(self, container=None):
        self.getBestGraspingPoint(container)
        grasp_proposal_msgs = Float32MultiArray()
        grasp_proposal_msgs.data = self.grasp_point
        grasp_noise_msgs = Float32MultiArray()
        grasp_noise_msgs.data = self.grasp_noise

        if self.grasp_point is not None:
            self.grasp_proposal_pub.publish(grasp_proposal_msgs)
            self.grasp_noise_pub.publish(grasp_noise_msgs)
        comments_msgs = String()
        comments_msgs.data = self.experiment_description
        experiment_type_msgs = String()
        experiment_type_msgs.data = self.experiment_type
        self.experiment_comments_pub.publish(comments_msgs)
        self.experiment_type_pub.publish(experiment_type_msgs)

        if self.grasp_point is None:
            print(
                'It was suppose to do grasping, but there is no grasp proposal'
            )
            self.execution_possible = False
            return
        if self.visionType != 'real':
            self.callFakeGrasping(prob=0.8, container=container)
            return

        #~visualize grasp proposals
        markers_msg = MarkerArray()
        m0 = createDeleteAllMarker('pick_proposals')
        markers_msg.markers.append(m0)
        for i in range(10):
            self.proposal_viz_array_pub.publish(markers_msg)
        ik.visualize_helper.visualize_grasping_proposals(
            self.proposal_viz_array_pub, self.all_grasp_proposals,
            self.listener, self.br)
        ik.visualize_helper.visualize_grasping_proposals(
            self.proposal_viz_array_pub, np.asarray([self.grasp_point]),
            self.listener, self.br, True)

        #execute for grasp. Stop when the gripper is closed
        if self.is_control:
            back_img_list = self.controller.capture_images()

        #self.grasp_point=[0.92737001180648804, -0.391, -0.12441360205411911, 0.0, 0.0, -1.0, 0.067681394517421722, 0.059999998658895493, 0.0, 1.0, 0.0, 0.74888890981674194]
        self.background_images = self.capture_images()

        self.grasping_output = grasp(objInput=self.grasp_point,
                                     listener=self.listener,
                                     br=self.br,
                                     isExecute=self.isExecute,
                                     binId=container,
                                     withPause=self.withPause,
                                     viz_pub=self.proposal_viz_array_pub,
                                     recorder=self.gdr)

        if self.is_record == True:
            self.gdr.save_item(item_name='grasp_noise_std_dev',
                               data=self.grasp_std)
            self.gdr.save_item(item_name='grasp_noise', data=self.grasp_noise)
        is_in_wrong_pose = (gripper.getGripperopening() < 0.03)  #raw_input()
        if is_in_wrong_pose:
            self.retrieve_output = retrieve(
                listener=self.listener,
                br=self.br,
                isExecute=self.isExecute,
                binId=container,
                withPause=self.withPause,
                viz_pub=self.proposal_viz_array_pub,
                recorder=self.gdr,
                ws_object=self.weightSensor)
            gripper.open()
            gripper.close()
            self.grasping_output = grasp(objInput=self.grasp_point,
                                         listener=self.listener,
                                         br=self.br,
                                         isExecute=self.isExecute,
                                         binId=container,
                                         withPause=self.withPause,
                                         viz_pub=self.proposal_viz_array_pub,
                                         recorder=self.gdr)

        if self.is_control:
            if gripper.getGripperopening() > 0.017:
                print('[Planner]: ', gripper.getGripperopening())
                # WE PAUSE THE RECOORDER TO SAVE DATA
                #self.gdr.pause_recording()
                #find new and improved grasp points
                best_grasp_dict, initial_score = self.controller.control_policy(
                    back_img_list,
                    smirror=self.smirror,
                    use_COM=self.use_COM,
                    use_raw=self.use_raw)

                # self.controller.visualize_actions(with_CAM = False)
                #self.controller.visualize_best_action(with_CAM = False)
                #pdb.set_trace()
                #save network information action_dict and best_action_dict
                #WE UNPAUSE THE RECORDER
                #self.gdr.replay_recording()
                self.gdr.save_item(item_name='initial_score',
                                   data=initial_score)
                self.gdr.save_item(item_name='best_grasp_dict',
                                   data=best_grasp_dict)
                self.gdr.save_item(item_name='action_dict',
                                   data=self.controller.action_dict)
                self.gdr.save_item(item_name='best_action_dict',
                                   data=self.controller.best_action_dict)
                #go for new grasp PointgraspPose
                self.grasping_output = grasp_correction(
                    self.grasp_point, best_grasp_dict['delta_pos'],
                    self.listener, self.br)
                second_best_grasp_dict, final_score = self.controller.control_policy(
                    back_img_list,
                    smirror=self.smirror,
                    use_COM=self.use_COM,
                    use_raw=self.use_raw)
                self.gdr.save_item(item_name='final_score', data=final_score)
                self.gdr.save_item(item_name='second_best_grasp_dict',
                                   data=second_best_grasp_dict)
                self.gdr.save_data_recorded = True

            else:
                self.gdr.save_data_recorded = False
        #frank hack for double grasping
        if self.experiment_type == 'is_data_collection':
            if gripper.getGripperopening() > 0.017:
                # self.grasping_output = grasp_correction(self.grasp_point, np.array([0,0,0]), self.listener, self.br)
                self.gdr.save_data_recorded = True
            else:
                self.gdr.save_data_recorded = False

        self.retrieve_output = retrieve(listener=self.listener,
                                        br=self.br,
                                        isExecute=self.isExecute,
                                        binId=container,
                                        withPause=self.withPause,
                                        viz_pub=self.proposal_viz_array_pub,
                                        recorder=self.gdr,
                                        ws_object=self.weightSensor)
        self.execution_possible = self.retrieve_output['execution_possible']