Ejemplo n.º 1
0
def go_up_arc():
    gripper.open()
    ik.helper.move_cart(dz=.3)
    goToHome.goToARC()
Ejemplo n.º 2
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.º 3
0
def goarc():
    goToHome.goToARC(slowDown=True)
Ejemplo n.º 4
0
def unit_test_centers():
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()
    #~ ~Build hand transform
    l1 = 0.0
    l2 = 0.0
    l3 = rospy.get_param("/gripper/spatula_tip_to_tcp_dist")
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    gripperOriHome = [0, 1, 0, 0]
    q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
    plans = []
    withPause = True
    #~generate plans
    #loop through sections (bins)
    plans = []
    goToHome.prepGripperPicking()
    goToHome.goToARC()

    for i in range(0, 9):
        binId = i
        collision_free_placing(binId, listener, isSuction=False)
        #~READ robot position
        rospy.sleep(.3)
        tcpPose = get_tcp_pose(listener, tcp_offset=l3)
        tcpPose[2] = rospy.get_param(
            '/bin' + str(i) +
            '_pose/z')  #~- rospy.get_param('/bin'+str(i)+'/height')
        rospy.sleep(.3)
        q_initial = get_joints()
        rospy.sleep(1.0)
        ##move to edge of structure
        map_bin_id_to_ws_id = {
            0: 0,
            1: 1,
            2: 2,
            3: 3,
            4: 0,
            5: 0,
            6: 4,
            7: 4,
            8: 5
        }
        plan, qf, plan_possible = generatePlan(q_initial, tcpPose[0:3],
                                               gripperOriHome,
                                               tip_hand_transform, 'fast')
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return '[IK] Error'
        ##move to edge of structure
        map_bin_id_to_ws_id = {
            0: 0,
            1: 1,
            2: 2,
            3: 3,
            4: 0,
            5: 0,
            6: 4,
            7: 4,
            8: 5
        }
        plan, qf, plan_possible = generatePlan(
            q_initial,
            tcpPose[0:3] - np.array([0, 0, 0.2]),
            gripperOriHome,
            tip_hand_transform,
            'fast',
            guard_on=WeightGuard(map_bin_id_to_ws_id[binId], threshold=100))
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return '[IK] Error'
        executePlanForward(plans, False)
        if binId > 5:
            collision_free_placing(binId, listener, isSuction=False)
        plans = []
Ejemplo n.º 5
0
def calibrate_storage():
    #################################################
    #~ Generate Plans
    gripperOri = []
    gripperOri.append([-1., 0., 0., 0.])
    gripperOri.append([-0.70710678118654757, -0.70710678118654757, 0., 0.])
    gripperOri.append([0., -1., 0., 0.])
    gripperOri.append([-0.70710678118654757, 0.70710678118654757, 0., 0.])
    #~build total list plan
    gripperOri_list = []
    ori_list = [
        0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0,
        0, 0, 1, 2, 3, 0, 0, 0
    ]
    for i in range(0, len(ori_list)):
        gripperOri_list.append(gripperOri[ori_list[i]])
    #~positions
    pos = []
    #~bin0
    bin_pose = get_params_yaml('bin0_pose')
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([1.00 + .12, -0.46 + 0.19647, 0.41])
    pos.append([0.75 + .12, bin_pose[1], 0.41])
    pos.append([1. + .12, -0.697281360626 + 0.19647, 0.41])
    pos.append([1.19 + .12, bin_pose[1], 0.41])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append(
        [1.00278019905 + .12, -0.763054132462 + 0.19647, 0.447774887085 + .05])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin1
    bin_pose = get_params_yaml('bin1_pose')
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([1.00 + .12, -0.0943171977997 + 0.19647, 0.41])
    pos.append([0.75 + .12, bin_pose[1], 0.41])
    pos.append([1. + .12, -0.330679833889 + 0.19647, 0.41])
    pos.append([1.19 + .12, bin_pose[1], 0.41])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([
        0.961158931255 + .12, -0.0221027284861 + 0.19647, 0.447779148817 + .05
    ])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin2
    bin_pose = get_params_yaml('bin2_pose')
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([1.00 + .12, 0.307756274939 + 0.19647, 0.41])
    pos.append([0.8 + .12, bin_pose[1], 0.41])
    pos.append([1 + .12, 0.0618322640657 + 0.19647, 0.41])
    pos.append([1.18 + .12, bin_pose[1], 0.41])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append(
        [0.940047621727 + .12, 0.353745698929 + 0.19647, 0.447779148817 + .05])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])

    ####################
    ## Generate Plans ##
    ####################
    q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
    tip_hand_transform = [0., 0., 0., 0., 0., 0.]
    dir_list = [
        None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None,
        0, 1, 2, 3, None
    ]
    point_counter = 0
    #~generate plans
    plans = []
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()
    rospy.sleep(0.5)
    goToHome.goToARC()
    #~ goToHome.prepGripperPicking()

    for i in range(0, len(pos)):
        plan, qf, plan_possible = generatePlan(q_initial, pos[i],
                                               gripperOri_list[i],
                                               tip_hand_transform, 'fastest')
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return '[IK] Error'
        #~execute plan
        executePlanForward(plans, True)
        plans = []
        if dir_list[i] is not None:
            gentle_move(dir_list[i], q_initial, pos[i], gripperOri_list[i],
                        tip_hand_transform, listener, point_counter)
            point_counter += 1
Ejemplo n.º 6
0
def calibrate_boxes():
    #################################################
    #~ Generate Plans
    gripperOri = []
    gripperOri.append([-1., 0., 0., 0.])
    gripperOri.append([-0.715217411518, -0.715217411518, 0., 0.])
    gripperOri.append([0., -1., 0., 0.])
    gripperOri.append([-0.715217411518, 0.715217411518, 0., 0.])
    #~build total list plan
    gripperOri_list = []
    ori_list = [
        0, 0, 1, 2, 3, 3, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0,
        0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0
    ]
    for i in range(0, len(ori_list)):
        gripperOri_list.append(gripperOri[ori_list[i]])
    #~positions
    pos = []
    #~bin4
    bin_pose = get_params_yaml('bin4_pose')
    pos.append([
        bin_pose[0], bin_pose[1],
        0.15 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([1.1, -0.498443692923, 0.5512911677361 - .15])
    pos.append([1.05, -0.599031984806, 0.5512911677361 - .15])
    pos.append([1.14, -0.70, 0.5512911677361 - .15])
    pos.append([1.2, -0.618239402771, 0.5512911677361 - .15])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.30 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([1.08824729919, -0.808329701424, 0.5512911677361 - 0.05])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin5
    bin_pose = get_params_yaml('bin5_pose')
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([0.79457449913, -0.498443692923, 0.5512911677361 - .15])
    pos.append([0.757445454597, -0.599031984806, 0.5512911677361 - .15])
    pos.append([0.79457449913, -0.70, 0.5512911677361 - .15])
    pos.append([0.889182715416, -0.618239402771, 0.5512911677361 - .15])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([0.79457449913, -0.788900852203, 0.5512911677361 - .05])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin6
    bin_pose = get_params_yaml('bin6_pose')
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([0.09962657094, -0.55, 0.539945185184 + 0.0])
    pos.append([0.0830265730619, -0.6, 0.539945185184 + 0.0])
    pos.append([0.099, -0.68, 0.539945185184 + 0.0])
    pos.append([0.15, -0.6, 0.539945185184 + 0.0])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([0.145160362124, -0.846666872501, 0.65])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin7
    bin_pose = get_params_yaml('bin7_pose')
    pos.append([
        bin_pose[0], bin_pose[1],
        0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([-0.199928298593, -0.5, 0.539945185184 + 0.0])
    pos.append([-0.272910028696, -0.6, 0.539945185184 + 0.0])
    pos.append([-0.195086687803, -0.75, 0.539945185184 + 0.0])
    pos.append([-0.100125610828, -0.6, 0.539945185184 + 0.0])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([-0.203513264656, -0.86, 0.65])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin8
    bin_pose = get_params_yaml('bin8_pose')
    pos.append([
        bin_pose[0], bin_pose[1],
        0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([-0.09, 0.68, 0.716840863228])
    pos.append([-0.22, 0.62, 0.716840863228])
    pos.append([-0.09, 0.5, 0.716840863228])
    pos.append([0.1, 0.62, 0.716840863228])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([0.106256209314, 0.78, 0.8])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])

    ####################
    ## Generate Plans ##
    ####################
    q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
    tip_hand_transform = [0., 0., 0., 0., 0., 0.]
    dir_list = [
        None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None,
        0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1,
        2, 3, None, 4, None
    ]
    point_counter = 0
    #~generate plans
    plans = []
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()
    rospy.sleep(0.5)
    goToHome.goToARC()
    goToHome.prepGripperPicking()

    ###############################
    ## Unwrap angles example use ##
    ###############################
    #~1) function to unwarp an angle to the range [-pi,pi]
    def unwrap(angle):
        if angle > np.pi:
            angle = np.mod(angle, 2 * np.pi)
            if angle > np.pi:
                angle = angle - 2 * np.pi
        elif angle < -np.pi:
            angle = np.mod(angle, -2 * np.pi)
            if angle < -np.pi:
                angle = angle + 2 * np.pi
        return angle

    for i in range(0, 16):
        plan, qf, plan_possible = generatePlan(q_initial, pos[i],
                                               gripperOri_list[i],
                                               tip_hand_transform, 'fastest')
        if plan_possible:
            #~2) unwrap angle of joint 6 only
            plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
            plans.append(plan)
            #~3) set q_initial (for next plan) with the unwrapped angle joint state
            q_initial = plan.q_traj[-1]
        else:
            return '[IK] Error'
        #~execute plan
        executePlanForward(plans, True)
        plans = []
        if dir_list[i] is not None:
            gentle_move_boxes(dir_list[i], q_initial, pos[i],
                              gripperOri_list[i], tip_hand_transform, listener,
                              point_counter)
            point_counter += 1
    ##################################

    plans_col_free = []
    plans_col_free, q_initial, plan_possible, motion = collision_free_plan(
        q_initial, plans_col_free, [0, 1, 0, 0], False, 6)
    executePlanForward(plans_col_free, True)

    for i in range(16, 32):
        #~ for i in range(0,16):
        plan, qf, plan_possible = generatePlan(q_initial, pos[i],
                                               gripperOri_list[i],
                                               tip_hand_transform, 'fastest')
        if plan_possible:
            plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
            plans.append(plan)
            q_initial = plan.q_traj[-1]
        else:
            return '[IK] Error'
        #~execute plan
        executePlanForward(plans, True)
        plans = []
        if dir_list[i] is not None:
            gentle_move_boxes(dir_list[i], q_initial, pos[i],
                              gripperOri_list[i], tip_hand_transform, listener,
                              point_counter)
            point_counter += 1

    plans_col_free = []
    plans_col_free, q_initial, plan_possible, motion = collision_free_plan(
        q_initial, plans_col_free, [0, 1, 0, 0], False, 8)
    executePlanForward(plans_col_free, True)

    for i in range(32, 40):
        #~ for i in range(16,24):
        plan, qf, plan_possible = generatePlan(q_initial, pos[i],
                                               gripperOri_list[i],
                                               tip_hand_transform, 'fastest')
        if plan_possible:
            plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
            plans.append(plan)
            q_initial = plan.q_traj[-1]
        else:
            return '[IK] Error'
        #~execute plan
        executePlanForward(plans, True)
        plans = []
        if dir_list[i] is not None:
            gentle_move_boxes(dir_list[i], q_initial, pos[i],
                              gripperOri_list[i], tip_hand_transform, listener,
                              point_counter)
            point_counter += 1
Ejemplo n.º 7
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")