Ejemplo n.º 1
0
 def abort():
     print(
         '[Recorder] ***************************************************'
     )
     print('[Recorder] Sensor Issue: The sensor {} is empty. Abort!'.
           format(term))
     print(
         '[Recorder] ***************************************************'
     )
     print('info_dict', info_dict)
     gripper.open()
     spatula.open()
     sys.exit()
Ejemplo n.º 2
0
def callback(data):
    s = str(data)
    print(s)
    data = re.split('data: "| "| |"', s)
    print(len(data))
    print(data)
    if len(data) == 3:
        if data[1] == 'o':
            gripper.open()
        elif data[1] == 'c':
            gripper.close()
        else:
            gripper.stop()
    elif len(data) == 8:
        for i in range(1, 7):
            query[i] = float(data[i])
        dequeue()
Ejemplo n.º 3
0
    def run_experiments(self):
        #######################
        ## initialize system ##
        #######################
        #Get object list
        obj_list = self.get_objects()
        print(obj_list)
        obj_ans = raw_input('Are these the objects?(y/n)')
        while obj_ans != 'y':
            obj_list = self.get_objects()
            print(obj_list)
            obj_ans = raw_input('Are these the objects?(y/n)')

        objectList_msgs = String()
        objectList_msgs.data = '\n'.join(obj_list)
        self.objectList_pub.publish(objectList_msgs)  #HACK

        # Start execution
        goToHome.goToARC(slowDown=self.goHomeSlow)  # 1. Initialize robot state
        print("getPassiveVisionEstimate 'update hm sg', '', ", self.tote_ID)
        self.getPassiveVisionEstimate('update hm sg', '', self.tote_ID)
        self.save_passive_vision_images(self.tote_ID)

        ##################
        ## Manipulation loop ##
        ##################
        if (rospy.get_param('have_robot') == True and self.is_record == True):
            directory = '/media/mcube/data/Dropbox (MIT)/rgrasp_dataset'
            assert (directory)
            self.gdr = GraspDataRecorder(
                directory=directory)  #We instantiate the recorder
        else:
            self.gdr = None
        while True:
            print(
                '-----------------------------\n       RUNNING GRASPING      \n-----------------------------'
            )
            self.weightSensor.calibrateWeights(
                withSensor=self.withSensorWeight)
            self.all_grasp_proposals = None
            self.run_grasping(container=self.tote_ID)  # 4. Run grasping

            self.obj_ID = 'no_item'
            self.obj_weight = 0
            print(
                '-----------------------------\n Execution_possible according to primitive = {} \n-----------------------------'
                .format(self.execution_possible))
            if self.execution_possible != False:  # 5. Check the weight
                self.weight_info = self.weightSensor.readWeightSensor(
                    item_list=obj_list,
                    withSensor=self.withSensorWeight,
                    binNum=self.tote_ID,
                    givenWeights=-11)
                self.obj_weight = self.weight_info['weights']
                print('Detected weight:', self.obj_weight)
                if self.obj_weight > 10:  # Weight need to be higher than 10g
                    self.execution_possible = True
                #Identify object
                max_prob_index = (self.weight_info['probs']).tolist().index(
                    max(self.weight_info['probs']))
                self.obj_ID = obj_list[max_prob_index]
                if self.obj_ID == 'no_item' or self.execution_possible == None:
                    self.execution_possible = False
                print(
                    '-----------------------------\n Execution_possible according to weight = {} \n-----------------------------'
                    .format(self.execution_possible))
                #Manually change execution result
                self.execution_possible = raw_input('Did it succeeded?')

            #Publish experiment outcome
            grasp_status_msgs = sensor_msgs.msg.JointState()
            grasp_status_msgs.name = [
                'grasp_success', 'code_version', 'tote_ID', 'detected_weight'
            ]  #grasp proposals, grasp_point, scores, score,
            grasp_status_msgs.position = [
                self.execution_possible, self.version, self.tote_ID,
                self.obj_weight
            ]

            object_ID_msgs = String()
            object_ID_msgs.data = self.obj_ID

            if self.grasp_point is not None and rospy.get_param('have_robot'):
                self.grasp_status_pub.publish(grasp_status_msgs)
                self.objectType_pub.publish(object_ID_msgs)
                self.gdr.update_topic(key='grasp_status')
                self.gdr.update_topic(key='objectType')

            print(
                '-----------------------------\n Execution_possible = {} \n-----------------------------'
                .format(self.execution_possible))
            if self.execution_possible:  # 8. Placing
                self.fails_in_row = 0
                self.bbox_info = fake_bbox_info_1(
                    self.listener)  #Give bounding box to the object
                self.bbox_info[7:10] = [
                    self.max_dimension, self.max_dimension, self.max_dimension
                ]
                self.planned_place()  #TODO_M
            else:
                self.fails_in_row += 1
                gripper.open()
                spatula.open()
                if self.grasp_point is not None:  # 9. Add to bad grasp points
                    self.bad_grasping_points.append(self.grasp_point)
                    self.bad_grasping_times.append(time.time())

            print(
                '************************\n         End of grasping attempt         \n***********************************'
            )
            # 7. Update vision state of the tote
            self.getPassiveVisionEstimate('update hm sg', '', self.tote_ID)
            self.save_passive_vision_images(self.tote_ID)

            if self.fails_in_row > 4:  # 10. Failed too many times, take action
                print(
                    'The pick failed 4 times in a row, stopping if not infinite looping'
                )
                if not self.infinite_looping:
                    break
        # Finished stowing
        goToHome.goToARC(slowDown=self.goHomeSlow)
        print("Planner is done")
Ejemplo n.º 4
0
def go_up_arc():
    gripper.open()
    ik.helper.move_cart(dz=.3)
    goToHome.goToARC()
Ejemplo n.º 5
0
def go_up():
    gripper.open()
    ik.helper.move_cart(dz=.3)
Ejemplo n.º 6
0
def reset_gripper():
    gripper.open()
    spatula.open()
Ejemplo n.º 7
0
def openGripper():
    # call gripper node, open
    gripper.open()
    return 1
Ejemplo n.º 8
0
#!/usr/bin/env python
import gripper
from ik.helper import Timer

gripper.homing()

gripper.open(speed=50)
with Timer('close50'):
    gripper.close(speed=50)

with Timer('open50'):
    gripper.open(speed=50)

with Timer('close100'):
    gripper.close(speed=100)
with Timer('open100'):
    gripper.open(speed=100)

with Timer('close200'):
    gripper.close(speed=200)
with Timer('open200'):
    gripper.open(speed=200)

with Timer('close400'):
    gripper.close(speed=400)
with Timer('open400'):
    gripper.open(speed=400)
#gripper.grasp()
#gripper.grasp(move_pos=10, move_speed=50)
#gripper.release(speed=50)
#!/usr/bin/env python
import gripper
from ik.helper import Timer

gripper.homing()

gripper.open(speed=50)
with Timer('close50'):
    gripper.close(speed=50)
    
with Timer('open50'):
    gripper.open(speed=50)
    
with Timer('close100'):
    gripper.close(speed=100)
with Timer('open100'):
    gripper.open(speed=100)
    
with Timer('close200'):
    gripper.close(speed=200)
with Timer('open200'):
    gripper.open(speed=200)
    
with Timer('close400'):
    gripper.close(speed=400)
with Timer('open400'):
    gripper.open(speed=400)
#gripper.grasp()
#gripper.grasp(move_pos=10, move_speed=50)
#gripper.release(speed=50)
Ejemplo n.º 10
0
    def run_experiments(self):
        #######################
        ## initialize system ##
        #######################
        #Get object list
        obj_list = self.get_objects()
        print(obj_list)
        #assert(False)
        obj_ans = raw_input('Are these the objects?(y/n)')
        while obj_ans != 'y':
            obj_list = self.get_objects()
            print(obj_list)
            obj_ans = raw_input('Are these the objects?(y/n)')

        objectList_msgs = String()
        objectList_msgs.data = '\n'.join(obj_list)
        #HACK
        self.objectList_pub.publish(objectList_msgs)
        goToHome.goToARC(slowDown=self.goHomeSlow)  # 1. Initialize robot state
        # calibrate_gelsight()
        num_grasps = 0
        if self.visionType == 'real':  # 2. Passive vision update bins
            number_bins = 2
            im_passive_vision = []
            for bin_id in range(number_bins):
                print("getPassiveVisionEstimate 'update hm sg', '', ", bin_id)
                self.getPassiveVisionEstimate('update hm sg', '', bin_id)
            self.save_passive_vision_images(self.tote_ID)
        ##################
        ## stowing loop ##
        ##################
        if (rospy.get_param('have_robot') == True and self.is_record == True):
            directory = '/media/mcube/data/Dropbox (MIT)/rgrasp_dataset'
            assert (directory)
            self.gdr = GraspDataRecorder(
                directory=directory)  #We instantiate the recorder
        else:
            self.gdr = None
        self.num_attempts = 0
        self.num_attempts_failed = 0
        while True:
            if num_grasps == 100:
                calibrate_gelsight()
                num_grasps = 0
            self.num_attempts += 1
            print(
                '-----------------------------\n       RUNNING GRASPING      \n-----------------------------'
            )
            self.weightSensor.calibrateWeights(
                withSensor=self.withSensorWeight)
            self.all_grasp_proposals = None
            self.run_grasping(container=self.tote_ID)  # 4. Run grasping

            self.obj_ID = 'no_item'
            self.obj_weight = 0
            if self.execution_possible != False:  # 5. Check the weight
                self.weight_info[
                    self.tote_ID] = self.weightSensor.readWeightSensor(
                        item_list=obj_list,
                        withSensor=self.withSensorWeight,
                        binNum=self.tote_ID,
                        givenWeights=-11)
                print(
                    '-----------------------------\n Execution_possible according to primitive = {} \n-----------------------------'
                    .format(self.execution_possible))
                print('Detected weight:',
                      self.weight_info[self.tote_ID]['weights'])
                self.obj_weight = self.weight_info[self.tote_ID]['weights']
                if self.obj_weight > 10:
                    self.execution_possible = True

                #Identify object
                max_prob_index = (
                    self.weight_info[self.tote_ID]['probs']).tolist().index(
                        max(self.weight_info[self.tote_ID]['probs']))
                self.obj_ID = obj_list[max_prob_index]
                if self.obj_ID == 'no_item':
                    self.execution_possible = False
                if self.execution_possible == None:
                    self.execution_possible = False


#            if self.visionType == 'real': # 7. Update vision state of the tote
#                self.getPassiveVisionEstimate('update hm sg', '', self.tote_ID)
#                self.save_passive_vision_images(self.tote_ID)

#Publish experiment outcome
            grasp_status_msgs = sensor_msgs.msg.JointState()
            grasp_status_msgs.name = [
                'grasp_success', 'code_version', 'tote_ID', 'detected_weight'
            ]  #grasp proposals, grasp_point, scores, score,
            grasp_status_msgs.position = [
                self.execution_possible, self.version, self.tote_ID,
                self.obj_weight
            ]

            object_ID_msgs = String()
            object_ID_msgs.data = self.obj_ID  #, self.obj_weight]

            if self.grasp_point is not None and rospy.get_param('have_robot'):
                self.grasp_status_pub.publish(grasp_status_msgs)
                self.objectType_pub.publish(object_ID_msgs)
                self.gdr.update_topic(key='grasp_status')
                self.gdr.update_topic(key='objectType')

            print(
                '-----------------------------\n Execution_possible = {} \n-----------------------------'
                .format(self.execution_possible))
            if self.execution_possible:  # 8. Placing
                num_grasps += 1
                self.fails_in_row = 0
                self.bbox_info = fake_bbox_info_1(
                    self.listener)  #Give bounding box to the object
                self.bbox_info[7:10] = [
                    self.max_dimension, self.max_dimension, self.max_dimension
                ]
                self.planned_place()  #TODO_M
            else:
                self.num_attempts_failed += 1
                self.fails_in_row += 1
                gripper.open()
                spatula.open()
                if self.grasp_point is not None:  # 9. Add to bad grasp points
                    self.bad_grasping_points.append(self.grasp_point)
                    self.bad_grasping_times.append(time.time())

            print(
                '***********************************************************')
            if self.visionType == 'real':  # 7. Update vision state of the tote
                self.getPassiveVisionEstimate('update hm sg', '', self.tote_ID)
                self.save_passive_vision_images(self.tote_ID)

            if self.fails_in_row > 3:  # 10. Failed too many times, take action
                if self.infinite_looping and len(obj_list) > 1:
                    #self.switch_tote()
                    print(
                        'The pick failed 4 times in a row, switching totes, the tote id is {}'
                        .format(self.tote_ID))
                else:
                    print('The pick failed 4 times in a row, stopping')
                    break
        # Finished stowing
        goToHome.goToARC(slowDown=self.goHomeSlow)
        print("Planner is done")
Ejemplo n.º 11
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']
Ejemplo n.º 12
0
def openGripper():
    # call gripper node, open
    gripper.open()
    return 1