Ejemplo n.º 1
0
    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 90 degrees in yaw direction
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(
            l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(
            l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(
            l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(
            block.name, places, scene=self.scene)
        return success
    def plan_place(self, desired_pose):
        places = list()
        ps = PoseStamped()
        # ps.pose = self.current_grasping_target.object.primitive_poses[0]
        ps.header.frame_id = self.current_grasping_target.object.header.frame_id

        grasp_to_pick_matrix = self.computeGraspToPickMatrix()
        place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose)
        grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix
        position_vector = grasp2_to_place_matrix[0:-1, 3]
        quaternion = quaternion_from_matrix(grasp2_to_place_matrix)
        position_array = position_vector.getA1()

        l = PlaceLocation()
        direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector
        l.place_pose.header.frame_id = ps.header.frame_id
        l.place_pose.pose.position.x = position_array[0]
        l.place_pose.pose.position.y = position_array[1]
        l.place_pose.pose.position.z = position_array[2]
        l.place_pose.pose.orientation.x = quaternion[0]
        l.place_pose.pose.orientation.y = quaternion[1]
        l.place_pose.pose.orientation.z = quaternion[2]
        l.place_pose.pose.orientation.w = quaternion[3]

        # copy the posture, approach and retreat from the grasp used
        approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach)
        approach.desired_distance /= 2.0

        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 90 degrees in roll direction
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
        places.append(copy.deepcopy(l))
        print "DEBUG: Places: ", places[0]
        success, place_result = self.pickplace.place_with_retry(
            self.current_grasping_target.object.name,
            places,
            scene=self.scene,
            allow_gripper_support_collision=False,
            planner_id="PRMkConfigDefault",
        )
        if success:
            rospy.loginfo("Planning succeeded.")
            trajectory_approved = get_user_approval()
            if trajectory_approved:
                return place_result.trajectory_stages
            else:
                rospy.loginfo("Plan rejected. Replanning...")
        else:
            rospy.logwarn("Planning failed. Replanning...")
        desired_pose[2] += 0.05
        return self.plan_place(desired_pose)
    def plan_place(self, desired_pose):
            places = list()
            ps = PoseStamped()
            #ps.pose = self.current_grasping_target.object.primitive_poses[0]
            ps.header.frame_id = self.current_grasping_target.object.header.frame_id

            grasp_to_pick_matrix = self.computeGraspToPickMatrix()
            place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose)
            grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix
            position_vector = grasp2_to_place_matrix[0:-1,3]
            quaternion = quaternion_from_matrix(grasp2_to_place_matrix)
            position_array = position_vector.getA1()
          
            l = PlaceLocation()
            direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector
            l.place_pose.header.frame_id = ps.header.frame_id
            l.place_pose.pose.position.x = position_array[0]
            l.place_pose.pose.position.y = position_array[1]
            l.place_pose.pose.position.z = position_array[2]
            l.place_pose.pose.orientation.x = quaternion[0]
            l.place_pose.pose.orientation.y = quaternion[1]
            l.place_pose.pose.orientation.z = quaternion[2]
            l.place_pose.pose.orientation.w = quaternion[3]

            # copy the posture, approach and retreat from the grasp used
            approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach)
            approach.desired_distance /= 2.0

            l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
            l.pre_place_approach = approach
            l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
            places.append(copy.deepcopy(l))
            # create another several places, rotate each by 90 degrees in roll direction
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
            places.append(copy.deepcopy(l))
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
            places.append(copy.deepcopy(l))
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
            places.append(copy.deepcopy(l))
            print "DEBUG: Places: ", places[0]
            success, place_result = self.pickplace.place_with_retry(self.current_grasping_target.object.name,
                                                                places,
                                                                scene=self.scene,
                                                                allow_gripper_support_collision=False,
                                                                planner_id='PRMkConfigDefault')
            if success:
               rospy.loginfo("Planning succeeded.")
               trajectory_approved = get_user_approval()
               if trajectory_approved:
                  return place_result.trajectory_stages
               else:
                  rospy.loginfo("Plan rejected. Replanning...")
            else:
               rospy.logwarn("Planning failed. Replanning...")
            desired_pose[2] += 0.05
            return self.plan_place(desired_pose)
Ejemplo n.º 4
0
    def place(self, block, pose_stamped, placePos):

        rospy.sleep(1)
        #creates a list of place positons for the block, with a specified x, y, and z.

        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = "base_link"

        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat

        #this x and y value are input as placePos through the function call.
        l.place_pose.pose.position.x = placePos[0]
        l.place_pose.pose.position.y = placePos[1]

        places.append(copy.deepcopy(l))

        #success = self.pickplace.place_with_retry(block.name, places, scene = self.scene)

        #return success

        ## create another several places, rotate each by 360/m degrees in yaw direction
        m = 16  # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m - 1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(
                l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(
            block.name, places, scene=self.scene)
        return success
Ejemplo n.º 5
0
    def place(self, block, pose_stamped, gripper=0):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = self.pick_result[
            gripper].grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result[
            gripper].grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result[
            gripper].grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 360/m degrees in yaw direction
        m = 16  # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m - 1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(
                l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace[gripper].place_with_retry(
            block.name, places, retries=1, scene=self.scene)
        self.place_result[gripper] = place_result
        self._last_gripper_placed = gripper
        return success
Ejemplo n.º 6
0
    def generate_place_poses(self, initial_place_pose):
        places = list()
        l = PlaceLocation()
        l.id = "dupadupa"
        l.place_pose.header.frame_id = "summit_xl_base_footprint"
        q = Quaternion(initial_place_pose.grasp_pose.pose.orientation.w,
                       initial_place_pose.grasp_pose.pose.orientation.x,
                       initial_place_pose.grasp_pose.pose.orientation.y,
                       initial_place_pose.grasp_pose.pose.orientation.z)
        # Load successful grasp pose
        l.place_pose.pose.position = initial_place_pose.grasp_pose.pose.position
        l.place_pose.pose.orientation.w = q.elements[0]
        l.place_pose.pose.orientation.x = q.elements[1]
        l.place_pose.pose.orientation.y = q.elements[2]
        l.place_pose.pose.orientation.z = q.elements[3]

        # Move 20cm to the right
        l.place_pose.pose.position.y += 0.2

        # Fill rest of the msg with some data
        l.post_place_posture = initial_place_pose.grasp_posture
        l.post_place_retreat = initial_place_pose.post_grasp_retreat
        l.pre_place_approach = initial_place_pose.pre_grasp_approach
        places.append(copy.deepcopy(l))

        # Rotate place pose to generate more possible configurations for the planner
        m = 16  # Number of possible place poses
        for i in range(0, m - 1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(
                l.place_pose.pose, 0, 0, 2 * math.pi / m)
            places.append(copy.deepcopy(l))
        return places
Ejemplo n.º 7
0
    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 90 degrees in yaw direction
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(block.name,
                                                                places,
                                                                scene=self.scene)
        return success
Ejemplo n.º 8
0
    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the postrection
        m = 16  # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m - 1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(
                l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(
            block.name, places, scene=self.scene)
        return success
Ejemplo n.º 9
0
    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 360/m degrees in yaw direction
        m = 16 # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m-1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(block.name,
                                                                places,
                                                                scene=self.scene)
        return success
def picknplace():
    # define the initial positions of baxter's joints in a dictionary
    initial_position = {'left_e0': -1.69483279891317, 'left_e1': 1.8669726956453, 'left_s0': 0.472137005716569,
                        'left_s1': -0.38852045702393034, 'left_w0': -1.9770933862776057, 'left_w1': -1.5701993084642143,
                        'left_w2': -0.6339059781326424, 'right_e0': 1.7238109084167481, 'right_e1': 1.7169079948791506,
                        'right_s0': 0.36930587426147465, 'right_s1': -0.33249033539428713, 'right_w0': -1.2160632682067871,
                        'right_w1': 1.668587600115967, 'right_w2': -1.810097327636719}

    # create two lists, one of initial locations and one of goal locations
    # for BAXTER to move them from one to another
    locs_x = [] # initial locations
    locs_y = []
    orien = []
    goal_move = []
    place_goal = [] # goal locations

    # using readlines() to read the plan.ipc file, the PDDL problem solution
    PDDL_solved = open('plan.ipc', 'r')
    Lines = PDDL_solved.readlines()
    # adding the goal positions from the PDDL solved file
    for line in Lines:
        orien.append(0);
        goal_move.append(line.split()[3][8:9])
        cord_dest = int(line.split()[2][8:9])
        # adding an offset to all location but the 5th
        if(cord_dest != 5):
        	locs_x.append(BLOCKS[cord_dest - 1][0] + 0.025)
        	locs_y.append(BLOCKS[cord_dest - 1][1] + 0.03)
	else:
		locs_x.append(BLOCKS[cord_dest - 1][0])
        	locs_y.append(BLOCKS[cord_dest - 1][1])

    for i in goal_move:
        place_goal_i = geometry_msgs.msg.Pose()
        if(int(i) != 5):
        	place_goal_i.position.x = BLOCKS[int(i) - 1][0] + 0.025
        	place_goal_i.position.y = BLOCKS[int(i) - 1][1] + 0.03
        else:
		place_goal_i.position.x = BLOCKS[int(i) - 1][0]
        	place_goal_i.position.y = BLOCKS[int(i) - 1][1]
        place_goal_i.position.z = BLOCKS[int(i) - 1][2]
        place_goal_i.orientation.x = 1.0
        place_goal_i.orientation.y = 0.0
        place_goal_i.orientation.z = 0.0
        place_goal_i.orientation.w = 0.0
        place_goal.append(place_goal_i)

    # the distance from the zero point in Moveit to the ground is 0.903 m (for the table we use)
    # the value is not always the same. (look in Readme)
    center_z_cube = -MOVEIT_Z_CONST + TABLE_SIZE[2] + BLOCK_SIZE[2] / 2

    # initialize a list for the objects and the stacked cubes.
    objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']

    both_arms.set_joint_value_target(initial_position)
    both_arms.plan()
    both_arms.go(wait=True)

    # remove models from the scene on shutdown
    #rospy.on_shutdown(delete_gazebo_models)
    # wait for All Clear message from emulator startup.
    rospy.wait_for_message("/robot/sim/started", Empty)

    planningScene.clear()
    """
    Attach a box into the planning scene.
    def moveit_python.planning_scene_interface.PlanningSceneInterface.attachBox	(self,
 	                                                                         name,               - Name of the object
 	                                                                         size_x,             - The x-dimensions size of the box
 	                                                                         size_y,             - The y-dimensions size of the box
 	                                                                         size_z,             - The z-dimensions size of the box
                                                                                 x,                  - The x position in link_name frame
                                                                                 y,                  - The y position in link_name frame
                                                                                 z,                  - The z position in link_name frame
                                                                                 link_name,          - Name of link to attach this object to
                                                                                 touch_links = None, - Names of robot links that can touch this object
                                                                                 detach_posture = None,
                                                                                 weight = 0.0,
                                                                                 wait = True         - When true, we wait for planning scene to actually
                                                                                                       update, this provides immunity against lost messages.
                                                                                 )		    
    add the table as attached object
    """
    planningScene.attachBox('table', TABLE_SIZE[0], TABLE_SIZE[1], TABLE_SIZE[2], CENTER[0], CENTER[1], CENTER[2],
                            'base', touch_links=['pedestal'])
    planningScene.waitForSync()
    rightgripper.open()

    # cProfile explanation - https://docs.python.org/2/library/profile.html#module-cProfile
    # cProfile to measure the performance (time) of the task.
    profile = cProfile.Profile()
    profile.enable()

    # loop performing "pick and place" till all objects are cleared from table.
    # locs_x,locs_y are the source fot he cubes.
    num_of_pick_n_place = 0
    while num_of_pick_n_place < len(goal_move):

        # do the task only if there are still objects on the table
        moved_objects = 0
        while moved_objects < len(locs_x):
            # clear planning scene
            planningScene.clear()
            # Add table as attached object.
            planningScene.attachBox('table', TABLE_SIZE[0], TABLE_SIZE[1], TABLE_SIZE[2], CENTER[0], CENTER[1],
                                    CENTER[2], 'base', touch_links=['pedestal'])

            # Initialize the data of the objects on the table.
            x_n = locs_x[moved_objects]
            y_n = locs_y[moved_objects]
            z_n = Z_N_INIT_VALUE
            th_n = orien[moved_objects]
            # if the angle is bigger than 45 degrees, I assume there will be problem with the pick,
            # therefore in that case we need to calibrate the angle to be in the range[-45,45] degrees.
            # I surmise the offset of the angle is always adding toward the positive axis for theta,
            # thus we need to calibrate theta toward the negative 45.
            if th_n > pi / 4:
                th_n = -1 * (th_n % (pi / 4))

            # Add the detected objects into the planning scene.
            for i in range(1, len(locs_x)):
                planningScene.addBox(objlist[i], BLOCK_SIZE[0], BLOCK_SIZE[1], BLOCK_SIZE[2], locs_x[i], locs_y[i],
                                     center_z_cube)
            planningScene.waitForSync()

            # defien a middle point from initial point to goal, as "approach_pick_goal".
            # initalize it as equal to the actual pick_goal in z and y axis, and different in z axis
            approach_pick_goal = geometry_msgs.msg.Pose()
            approach_pick_goal.position.x = x_n
            approach_pick_goal.position.y = y_n
            approach_pick_goal.position.z = z_n + MIDDLE_POINT_Z

            # PoseStamped is a Pose with reference coordinate frame and timestamp
            approach_pick_goal_dummy = PoseStamped()
            """"
            Header is  Standard metadata for higher-level stamped data types.
            This is generally used to communicate timestamped data in a particular coordinate frame.
            sequence ID: consecutively increasing ID  uint32 seq
            Two-integer timestamp that is expressed as:
            * stamp.sec: seconds (stamp_secs) since epoch (called 'secs')
            * stamp.nsec: nanoseconds since stamp_secs (called 'nsecs')
            """
            approach_pick_goal_dummy.header.frame_id = "base"
            approach_pick_goal_dummy.header.stamp = rospy.Time.now()
            approach_pick_goal_dummy.pose.position.x = x_n
            approach_pick_goal_dummy.pose.position.y = y_n
            approach_pick_goal_dummy.pose.position.z = z_n + MIDDLE_POINT_Z
            approach_pick_goal_dummy.pose.orientation.x = 1.0
            approach_pick_goal_dummy.pose.orientation.y = 0.0
            approach_pick_goal_dummy.pose.orientation.z = 0.0
            approach_pick_goal_dummy.pose.orientation.w = 0.0

            # orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
            approach_pick_goal_dummy.pose = rotate_pose_msg_by_euler_angles(approach_pick_goal_dummy.pose, 0.0, 0.0,
                                                                            th_n)
            approach_pick_goal.orientation.x = approach_pick_goal_dummy.pose.orientation.x
            approach_pick_goal.orientation.y = approach_pick_goal_dummy.pose.orientation.y
            approach_pick_goal.orientation.z = approach_pick_goal_dummy.pose.orientation.z
            approach_pick_goal.orientation.w = approach_pick_goal_dummy.pose.orientation.w

            # Move to the approach goal and the pick_goal.
            pick_goal = deepcopy(approach_pick_goal)  # create an exact copy of approach_pick_goal
            pick_goal.position.z = z_n
            move('right', approach_pick_goal)  # move is a function
            time.sleep(5)
            move('right', pick_goal)
            time.sleep(1)
            rightgripper.close()  # PICK!
            time.sleep(1)
            # move back to the approach_pick_goal.
            move('right', approach_pick_goal)

            # define the approach_place_goal
            approached_place_goal = deepcopy(place_goal[num_of_pick_n_place])
            approached_place_goal.position.z = approached_place_goal.position.z + MIDDLE_POINT_Z
            move('right', approached_place_goal)
            time.sleep(5)
            place_goal_val = place_goal[num_of_pick_n_place]
            place_goal_val.position.z = place_goal_val.position.z + BLOCK_SIZE[2]
            move('right', place_goal_val)
            time.sleep(1)
            rightgripper.open()  # PLACE!
            time.sleep(1)
            # move to the approach_place_goal.
            move('right', approached_place_goal)
            # increase iterators
            num_of_pick_n_place += 1
            moved_objects += 1
            # move the arms to initial position.
            both_arms.set_joint_value_target(initial_position)
            both_arms.plan()
            both_arms.go(wait=True)

    profile.disable()
    # pstats documentation - https://docs.python.org/3/library/profile.html#module-pstats
    pstats.Stats(profile).sort_stats('cumulative').print_stats(0.0)

    # exit MoveIt and shutting down the process
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)
    time.sleep(10)
Ejemplo n.º 11
0
def picknplace():
    # Define positions.
    rpos = geometry_msgs.msg.Pose()
    rpos.position.x = 0.555
    rpos.position.y = 0.0
    rpos.position.z = 0.206
    rpos.orientation.x = 1.0
    rpos.orientation.y = 0.0
    rpos.orientation.z = 0.0
    rpos.orientation.w = 0.0

    lpos = geometry_msgs.msg.Pose()
    lpos.position.x = 0.65
    lpos.position.y = 0.6
    lpos.position.z = 0.206
    lpos.orientation.x = 1.0
    lpos.orientation.y = 0.0
    lpos.orientation.z = 0.0
    lpos.orientation.w = 0.0

    placegoal = geometry_msgs.msg.Pose()
    placegoal.position.x = 0.55
    placegoal.position.y = 0.28
    placegoal.position.z = 0
    placegoal.orientation.x = 1.0
    placegoal.orientation.y = 0.0
    placegoal.orientation.z = 0.0
    placegoal.orientation.w = 0.0

    # Define variables.
    offset_zero_point = 0.903
    table_size_x = 0.714655654394
    table_size_y = 1.05043717328
    table_size_z = 0.729766045265
    center_x = 0.457327827197
    center_y = 0.145765166941
    center_z = -0.538116977368
    # The distance from the zero point in Moveit to the ground is 0.903 m.
    # The value is not allways the same. (look in Readme)
    center_z_cube = -offset_zero_point + table_size_z + 0.0275 / 2
    pressure_ok = 0
    j = 0
    k = 0
    start = 1
    locs_x = []
    # Initialize a list for the objects and the stacked cubes.
    objlist = [
        'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
        'obj09', 'obj10', 'obj11'
    ]
    boxlist = [
        'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08',
        'box09', 'box10', 'box11'
    ]
    # Clear planning scene.
    p.clear()
    # Add table as attached object.
    p.attachBox('table',
                table_size_x,
                table_size_y,
                table_size_z,
                center_x,
                center_y,
                center_z,
                'base',
                touch_links=['pedestal'])
    p.waitForSync()
    # Move both arms to start state.
    right_arm.set_pose_target(rpos)
    left_arm.set_pose_target(lpos)
    right_arm.plan()
    left_arm.plan()
    right_arm.go(wait=True)
    left_arm.go(wait=True)
    # cProfile to measure the performance (time) of the task.
    pr = cProfile.Profile()
    pr.enable()
    # Loop to continue pick and place until all objects are cleared from table.
    while locs_x or start:
        # Only for the start.
        if start:
            start = 0

        time.sleep(0.5)
        # Receive the data from all objects from the topic "detected_objects".
        temp = rospy.wait_for_message("detected_objects", PoseArray)
        locs = temp.poses

        locs_x = []
        locs_y = []
        orien = []
        size = []

        # Add the data from the objects.
        for i in range(len(locs)):
            locs_x.append(locs[i].position.x)
            locs_y.append(locs[i].position.y)
            orien.append(locs[i].position.z * pi / 180)
            size.append(locs[i].orientation.x)

    # Filter objects list to remove multiple detected locations for same objects.
        ind_rmv = []
        for i in range(0, len(locs)):
            if (locs_y[i] > 0.24 or locs_x[i] > 0.75):
                ind_rmv.append(i)
                continue
            for j in range(i, len(locs)):
                if not (i == j):
                    if sqrt((locs_x[i] - locs_x[j])**2 +
                            (locs_y[i] - locs_y[j])**2) < 0.018:
                        ind_rmv.append(i)

        locs_x = del_meth(locs_x, ind_rmv)
        locs_y = del_meth(locs_y, ind_rmv)
        orien = del_meth(orien, ind_rmv)
        size = del_meth(size, ind_rmv)

        # Do the task only if there are still objects on the table.
        if locs_x:
            # Clear planning scene.
            p.clear()
            # Add table as attached object.
            p.attachBox('table',
                        table_size_x,
                        table_size_y,
                        table_size_z,
                        center_x,
                        center_y,
                        center_z,
                        'base',
                        touch_links=['pedestal'])
            # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
            ig0 = itemgetter(0)
            sorted_lists = zip(*sorted(
                zip(size, locs_x, locs_y, orien), reverse=True, key=ig0))
            locs_x = list(sorted_lists[1])
            locs_y = list(sorted_lists[2])
            orien = list(sorted_lists[3])
            size = list(sorted_lists[0])
            # Initialize the data of the biggest object on the table.
            xn = locs_x[0]
            yn = locs_y[0]
            # -0.16 is the z position to grip the objects on the table.
            zn = -0.16
            thn = orien[0]
            sz = size[0]
            if thn > pi / 4:
                thn = -1 * (thn % (pi / 4))

            # Add the detected objects into the planning scene.
            #for i in range(1,len(locs_x)):
            #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube)
            # Add the stacked objects as collision objects into the planning scene to avoid moving against them.
            #for e in range(0, k):
            #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.position.x, placegoal.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes'])
            if k > 0:
                p.attachBox(boxlist[0],
                            0.07,
                            0.07,
                            0.0275 * k,
                            placegoal.position.x,
                            placegoal.position.y,
                            center_z_cube,
                            'base',
                            touch_links=['cubes'])
            p.waitForSync()
            # Initialize the approach pickgoal (5 cm to pickgoal).
            approach_pickgoal = geometry_msgs.msg.Pose()
            approach_pickgoal.position.x = xn
            approach_pickgoal.position.y = yn
            approach_pickgoal.position.z = zn + 0.05

            approach_pickgoal_dummy = PoseStamped()
            approach_pickgoal_dummy.header.frame_id = "base"
            approach_pickgoal_dummy.header.stamp = rospy.Time.now()
            approach_pickgoal_dummy.pose.position.x = xn
            approach_pickgoal_dummy.pose.position.y = yn
            approach_pickgoal_dummy.pose.position.z = zn + 0.05
            approach_pickgoal_dummy.pose.orientation.x = 1.0
            approach_pickgoal_dummy.pose.orientation.y = 0.0
            approach_pickgoal_dummy.pose.orientation.z = 0.0
            approach_pickgoal_dummy.pose.orientation.w = 0.0

            # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
            approach_pickgoal_dummy.pose = rotate_pose_msg_by_euler_angles(
                approach_pickgoal_dummy.pose, 0.0, 0.0, thn)
            approach_pickgoal.orientation.x = approach_pickgoal_dummy.pose.orientation.x
            approach_pickgoal.orientation.y = approach_pickgoal_dummy.pose.orientation.y
            approach_pickgoal.orientation.z = approach_pickgoal_dummy.pose.orientation.z
            approach_pickgoal.orientation.w = approach_pickgoal_dummy.pose.orientation.w
            # Move to the approach goal and the pickgoal.
            pickgoal = deepcopy(approach_pickgoal)
            pickgoal.position.z = zn
            move("right", approach_pickgoal, pickgoal)
            time.sleep(0.5)
            # Read the force in z direction.
            b = rightarm.endpoint_effort()
            z_ = b['force']
            z_force = z_.z
            # Search again for objects, if the gripper isn't at the right position and presses on an object.
            #print("----->force in z direction:", z_force)
            if z_force > -4:
                rightgripper.close()
                attempts = 0
                pressure_ok = 1
                # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
                while (rightgripper.force() < 25 and pressure_ok == 1):
                    time.sleep(0.04)
                    attempts += 1
                    if (attempts > 50):
                        rightgripper.open()
                        pressure_ok = 0
                        print("----->pressure is to low<-----")
            else:
                print("----->gripper presses on an object<-----")

    # Move back to the approach pickgoal.
            pickgoal.position.z = zn + 0.05
            move("right", pickgoal)

            if pressure_ok and z_force > -4:
                # Define the approach placegoal.
                # Increase the height of the tower every time by 2.75 cm.
                approached_placegoal = deepcopy(placegoal)
                approached_placegoal.position.z = -0.155 + (k * 0.0275) + 0.08
                # Define the placegoal
                placegoal.position.z = -0.155 + (k * 0.0275)
                # Move to the placegoal and open the gripper 4 mm above the tip of the tower.
                move("right", approached_placegoal, placegoal)
                rightgripper.open()
                while (rightgripper.force() > 10):
                    time.sleep(0.01)
        # Move to the approach placegoal.
                move("right", approached_placegoal)
                k += 1

            # Move right arm to start position.
            right_arm.set_pose_target(rpos)
            right_arm.plan()
            right_arm.go(wait=True)

    pr.disable()
    sortby = 'cumulative'
    ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
    p.clear()
    moveit_commander.roscpp_shutdown()
    # Exit MoveIt.
    moveit_commander.os._exit(0)
Ejemplo n.º 12
0
            exit(-1)

        # create a set of place locations for the cube
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0]
        l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id
        # invert the y of the pose
        l.place_pose.pose.position.y = -l.place_pose.pose.position.y
        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 90 degrees in yaw direction
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))

        # drop it like it's hot
        rospy.loginfo("Beginning to place.")
        while not rospy.is_shutdown():
            # can't fail here or moveit needs to be restarted
            success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene)
            if success:
                break

        # place arm back at side
Ejemplo n.º 13
0
def picknplace():
    # Define initial parameters
    p = PlanningSceneInterface("base")
    g = MoveGroupInterface("both_arms", "base")
    gr = MoveGroupInterface("right_arm", "base")
    gl = MoveGroupInterface("left_arm", "base")
    leftgripper = baxter_interface.Gripper('left')
    leftgripper.calibrate()
    leftgripper.open()
    jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
    pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
    lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]    
    rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    
    # Clear planning scene
    p.clear()
    # Add table as attached object
    p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])

    # Move both arms to start state              
    g.moveToJointPosition(jts_both, pos1, plan_only=False)
    
    # Get obj locations
    temp = rospy.wait_for_message("Dpos", PoseArray)
    locs = temp.poses 
    locs_x = []
    locs_y = []
    orien = []
    size = []

    for i in range(len(locs)):
        locs_x.append(0.57 + locs[i].position.x)
        locs_y.append(-0.011 + locs[i].position.y)
        orien.append(locs[i].position.z*pi/180)
        size.append(locs[i].orientation.x)

    # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
    ind_rmv = []
    for i in range(0,len(locs)):
        if (locs_y[i] > 0.42):
            ind_rmv.append(i)
            continue
        for j in range(i,len(locs)):
            if not (i == j):
                if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
                    ind_rmv.append(i)
    
    locs_x = del_meth(locs_x, ind_rmv)
    locs_y = del_meth(locs_y, ind_rmv)
    orien = del_meth(orien, ind_rmv) 
    size = del_meth(size, ind_rmv)

    # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
    if locs_x:  
        ig0 = itemgetter(0)
        sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))

        locs_x = list(sorted_lists[1])
        locs_y = list(sorted_lists[2])
        orien = list(sorted_lists[3])
        size = list(sorted_lists[0])

    # Loop to continue pick and place until all objects are cleared from table
    j=0
    k=0
    while locs_x:
        p.clear() # CLear planning scene of all collision objects

        # Attach table as attached collision object to base of baxter
        p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
        xn = locs_x[0]
        yn = locs_y[0]
        zn = -0.06
        thn = orien[0]
        sz = size[0]
        if thn > pi/4:
            thn = -1*(thn%(pi/4))

        # Add collision objects into planning scene
        objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
        for i in range(1,len(locs_x)):
            p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
        p.waitForSync()        

        # Move both arms to pos2 (Right arm away and left arm on table)
        g.moveToJointPosition(jts_both, pos2, plan_only=False)

        # Move left arm to pick object and pick object
        pickgoal = PoseStamped() 
        pickgoal.header.frame_id = "base"
        pickgoal.header.stamp = rospy.Time.now()
        pickgoal.pose.position.x = xn
        pickgoal.pose.position.y = yn
        pickgoal.pose.position.z = zn+0.1
        pickgoal.pose.orientation.x = 1.0
        pickgoal.pose.orientation.y = 0.0
        pickgoal.pose.orientation.z = 0.0
        pickgoal.pose.orientation.w = 0.0
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False)

        # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles 
        pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False) 

        pickgoal.pose.position.z = zn
        gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
        leftgripper.close()

        pickgoal.pose.position.z = zn+0.1
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
        
        # Move both arms to pos1
        g.moveToJointPosition(jts_both, pos1, plan_only=False)

        # Get obj locations
        temp = rospy.wait_for_message("Dpos", PoseArray)
        locs = temp.poses 
        locs_x = []
        locs_y = []
        orien = []
        size = []

        for i in range(len(locs)):
            locs_x.append(0.57 + locs[i].position.x)
            locs_y.append(-0.011 + locs[i].position.y)
            orien.append(locs[i].position.z*pi/180)
            size.append(locs[i].orientation.x)

        # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
        ind_rmv = []
        for i in range(0,len(locs)):
            if (locs_y[i] > 0.42):
                ind_rmv.append(i)
                continue
            for j in range(i,len(locs)):
                if not (i == j):
                    if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
                        ind_rmv.append(i)
        
        locs_x = del_meth(locs_x, ind_rmv)
        locs_y = del_meth(locs_y, ind_rmv)
        orien = del_meth(orien, ind_rmv) 
        size = del_meth(size, ind_rmv)

        # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
        if locs_x:  
            ig0 = itemgetter(0)
            sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))

            locs_x = list(sorted_lists[1])
            locs_y = list(sorted_lists[2])
            orien = list(sorted_lists[3])
            size = list(sorted_lists[0])

        # Place object
        stleft = PoseStamped() 
        stleft.header.frame_id = "base"
        stleft.header.stamp = rospy.Time.now()
        stleft.pose.position.x = 0.65
        stleft.pose.position.y = 0.55
        stleft.pose.position.z = 0.1
        stleft.pose.orientation.x = 1.0
        stleft.pose.orientation.y = 0.0
        stleft.pose.orientation.z = 0.0
        stleft.pose.orientation.w = 0.0

        # Stack objects (large cubes) if size if greater than some threshold
        if sz > 16.:            
            stleft.pose.position.x = 0.65
            stleft.pose.position.y = 0.7
            stleft.pose.position.z = -0.04+(k*0.05)
            k += 1
        
        gl.moveToPose(stleft, "left_gripper", plan_only=False)        
        leftgripper.open()

        stleft.pose.position.z = 0.3
        gl.moveToPose(stleft, "left_gripper", plan_only=False)
Ejemplo n.º 14
0
            exit(-1)

        # create a set of place locations for the cube
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0]
        l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id
        # invert the y of the pose
        l.place_pose.pose.position.y = -l.place_pose.pose.position.y
        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 90 degrees in yaw direction
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))

        # drop it like it's hot
        rospy.loginfo("Beginning to place.")
        while not rospy.is_shutdown():
            # can't fail here or moveit needs to be restarted
            success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene)
            if success:
                break

        # place arm back at side
Ejemplo n.º 15
0
def dyn_wedge():

	pub = rospy.Publisher("finished", String, queue_size=10)

	# Initialize MoveIt scene
	p = PlanningSceneInterface("base")
	arms_group = MoveGroupInterface("both_arms", "base")
	rightarm_group = MoveGroupInterface("right_arm", "base")
	leftarm_group = MoveGroupInterface("left_arm", "base")
	
	# Create right arm instance
	right_arm = baxter_interface.limb.Limb('right')
	
	# Create right gripper instance
	right_gripper = baxter_interface.Gripper('right')
	right_gripper.calibrate()
	right_gripper.open()
	
	right_gripper.close()

	offset_zero_point=0.903
	table_size_x = 0.714655654394
	table_size_y = 1.05043717328
	table_size_z = 0.729766045265
	center_x = 0.457327827197
	center_y = 0.145765166941
	center_z = -0.538116977368	 
	table_distance_from_gripper = -offset_zero_point+table_size_z+0.0275/2
	j=0
	k=0

	# Initialize object list
	objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
	p.clear()
	p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
	p.waitForSync()
	# Move both arms to start state.

	# Initial pose
	rpos = PoseStamped()
	rpos.header.frame_id = "base"
	rpos.header.stamp = rospy.Time.now()
	rpos.pose.position.x = 0.555
	rpos.pose.position.y = 0.0
	rpos.pose.position.z = 0.206 #0.18
	rpos.pose.orientation.x = 1.0
	rpos.pose.orientation.y = 0.0
	rpos.pose.orientation.z = 0.0
	rpos.pose.orientation.w = 0.0

   	lpos = PoseStamped()
	lpos.header.frame_id = "base"
	lpos.header.stamp = rospy.Time.now()
	lpos.pose.position.x = 0.555
	lpos.pose.position.y = 0.65
	lpos.pose.position.z = 0.206#0.35
	lpos.pose.orientation.x = 1.0
	lpos.pose.orientation.y = 0.0
	lpos.pose.orientation.z = 0.0
	lpos.pose.orientation.w = 0.0

	rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
	leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False)

	# Get the middle point between the two centroids
	locs = rospy.wait_for_message("clasificacion", PoseArray)

	if(len(locs.poses)!=0):

		punto_medio1 = PoseStamped()
		punto_medio1.header.frame_id = "base"
		punto_medio1.header.stamp = rospy.Time.now()
		punto_medio1.pose.position.x = locs.poses[0].position.x
		punto_medio1.pose.position.y = locs.poses[0].position.y
		punto_medio1.pose.position.z = -0.08
		punto_medio1.pose.orientation.x = locs.poses[0].orientation.x
		punto_medio1.pose.orientation.y = locs.poses[0].orientation.y
		punto_medio1.pose.orientation.z = locs.poses[0].orientation.z
		punto_medio1.pose.orientation.w = locs.poses[0].orientation.w

	else:
		sys.exit("No se encuentran los puntos")

	# Get the middle point again after a few seconds
	tiempo = 3
	time.sleep(tiempo)
	locs = rospy.wait_for_message("clasificacion", PoseArray)

	if(len(locs.poses)!=0):

		punto_medio2 = PoseStamped()
		punto_medio2.header.frame_id = "base"
		punto_medio2.header.stamp = rospy.Time.now()
		punto_medio2.pose.position.x = locs.poses[0].position.x
		punto_medio2.pose.position.y = locs.poses[0].position.y
		punto_medio2.pose.position.z = -0.08
		punto_medio2.pose.orientation.x = locs.poses[0].orientation.x
		punto_medio2.pose.orientation.y = locs.poses[0].orientation.y
		punto_medio2.pose.orientation.z = locs.poses[0].orientation.z
		punto_medio2.pose.orientation.w = locs.poses[0].orientation.w

	else:
		sys.exit("No se encuentran los puntos")

	# Calculate speed of objects
	vel = (punto_medio2.pose.position.y - punto_medio1.pose.position.y) / tiempo

	# Predict position within a certain time
	nuevo_tiempo = 2.1
	posicion = nuevo_tiempo * vel * 2

	if(posicion>=0):
		nueva_y = punto_medio2.pose.position.y - posicion
	else:
		nueva_y = punto_medio2.pose.position.y + posicion

	orientacion = (-1) * 1/punto_medio2.pose.orientation.x

	new_or = 0.26

	if orientacion > 0:
		new_or = -0.26

	# If there is time enough to sweep the objects
	if vel > -0.08:
		start = time.time()
		punto_medio2.pose.position.y = nueva_y - nueva_y/2
		punto_medio2.pose.position.x = punto_medio1.pose.position.x - 0.03
		punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion - new_or)
		rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
		end = time.time()

		tiempo = end - start
		while(nuevo_tiempo - tiempo >= 1):
			end = time.time()
			nuevo_tiempo = end - start
	else:
		punto_medio2.pose.position.y = nueva_y

	print(punto_medio2.pose.position.y)

	punto_medio2.pose.position.x = punto_medio1.pose.position.x
	punto_medio2.pose.position.y += 0.05 
	punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion)
	rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
	obj = punto_medio2.pose.position.y + 0.16
	neg = 1

	while punto_medio2.pose.position.y < obj:
		punto_medio2.pose.position.y += 0.03
		# Shake arm
		punto_medio2.pose.position.x += neg * 0.09
		rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
		neg = (-1)*neg

	time.sleep(2)

	rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)

	freemem = "free"
	pub.publish(freemem)
Ejemplo n.º 16
0
def picknplace():
    # Initialize the planning scene interface.
    p = PlanningSceneInterface("base")
    # Connect the arms to the move group.
    g = MoveGroupInterface("both_arms", "base")
    gr = MoveGroupInterface("right_arm", "base")
    gl = MoveGroupInterface("left_arm", "base")
    # Create baxter_interface limb instance.
    leftarm = baxter_interface.limb.Limb('left')
    # Create baxter_interface gripper instance.
    leftgripper = baxter_interface.Gripper('left')
    leftgripper.calibrate()
    leftgripper.open()

    # Define the joints for the positions.
    jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
    pos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    lpos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024]
    lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]    
    rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    placegoal = PoseStamped() 
    placegoal.header.frame_id = "base"
    placegoal.header.stamp = rospy.Time.now()
    placegoal.pose.position.x = 0.55
    placegoal.pose.position.y = 0.28
    placegoal.pose.position.z = 0
    placegoal.pose.orientation.x = 1.0
    placegoal.pose.orientation.y = 0.0
    placegoal.pose.orientation.z = 0.0
    placegoal.pose.orientation.w = 0.0
    # Define variables.
    offset_zero_point = 0.903
    table_size_x = 0.714655654394
    table_size_y = 1.05043717328
    table_size_z = 0.729766045265
    center_x = 0.457327827197
    center_y = 0.145765166941
    center_z = -0.538116977368
    # The distance from the zero point in Moveit to the ground is 0.903 m.
    # The value is not allways the same. (look in Readme)
    center_z_cube= -offset_zero_point+table_size_z+0.0275/2
    pressure_ok=0
    j=0
    k=0
    start=1
    locs_x = []
    # Initialize a list for the objects and the stacked cubes.
    objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
    boxlist= ['box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11']
    # Clear planning scene.
    p.clear()
    # Add table as attached object.
    p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
    # Move both arms to start state where the right camera looks for objects.             
    g.moveToJointPosition(jts_both, pos1,  max_velocity_scaling_factor = 1, plan_only=False)
    time.sleep(0.5)
    # cProfile to measure the performance (time) of the task.
    pr = cProfile.Profile()
    pr.enable()
    # Loop to continue pick and place until all objects are cleared from table.
    while locs_x or start:
        # Only for the start.
	if start:
            start = 0		

        # Receive the data from all objects from the topic "detected_objects".
        temp = rospy.wait_for_message("detected_objects", PoseArray) 
        locs = temp.poses 
        locs_x = []
        locs_y = []
        orien = []
        size = []

        # Adds the data from the objects.
        for i in range(len(locs)):
            locs_x.append(locs[i].position.x) 
            locs_y.append(locs[i].position.y) 
            orien.append(locs[i].position.z*pi/180)
            size.append(locs[i].orientation.x)

        # Filter objects list to remove multiple detected locations for same objects.
        ind_rmv = []
        for i in range(0,len(locs) ):
            if (locs_y[i] > 0.24 or locs_x[i] > 0.75):
                ind_rmv.append(i)
                continue
            for j in range(i,len(locs)):
                if not (i == j):
                    if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
                        ind_rmv.append(i)
        
        locs_x = del_meth(locs_x, ind_rmv)
        locs_y = del_meth(locs_y, ind_rmv)
        orien = del_meth(orien, ind_rmv) 
        size = del_meth(size, ind_rmv)

        # Do the task only if there are still objects on the table.
        if locs_x: 
            # Clear planning scene.
	    p.clear() 
            # Add table as attached object.
            p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
            # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
            ig0 = itemgetter(0)
            sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
            locs_x = list(sorted_lists[1])
            locs_y = list(sorted_lists[2])
            orien = list(sorted_lists[3])
            size = list(sorted_lists[0])
	    # Initialize the data of the biggest object on the table.
	    xn = locs_x[0]
	    yn = locs_y[0]	
	    zn = -0.16
	    thn = orien[0]
	    sz = size[0]
	    if thn > pi/4:
	        thn = -1*(thn%(pi/4))

	    # Add the detected objects into the planning scene.
	    #for i in range(1,len(locs_x)):
	        #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube)
	    # Add the stacked objects as collision objects into the planning scene to avoid moving against them.
	    #for e in range(0, k):
	        #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes'])  
            if k>0:
	        p.attachBox(boxlist[0], 0.07, 0.07, 0.0275*k, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube, 'base', touch_links=['cubes'])   
	    p.waitForSync()
            # Move both arms to the second position.
            g.moveToJointPosition(jts_both, pos2, max_velocity_scaling_factor = 1, plan_only=False)
            # Initialize the pickgoal.
	    pickgoal = PoseStamped() 
	    pickgoal.header.frame_id = "base"
	    pickgoal.header.stamp = rospy.Time.now()
	    pickgoal.pose.position.x = xn
	    pickgoal.pose.position.y = yn
	    pickgoal.pose.position.z = zn		
	    pickgoal.pose.orientation.x = 1.0
	    pickgoal.pose.orientation.y = 0.0
	    pickgoal.pose.orientation.z = 0.0
	    pickgoal.pose.orientation.w = 0.0

            # Move to the approach pickgoal. (5 cm to pickgoal)
	    pickgoal.pose.position.z = zn+0.05
	    # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. 
	    pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
	    gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
	    pickgoal.pose.position.z = zn
            # Move to the pickgoal.
	    gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.3, plan_only=False)
            # Read the force in z direction.
            b=leftarm.endpoint_effort()
            z_= b['force']
	    z_force= z_.z
            # Search again for objects, if the gripper isn't at the right position and presses on an object.
	    #print("----->force in z direction:", z_force)
	    if z_force>-4:
                leftgripper.close()
                attempts=0
	        pressure_ok=1
                # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
	        while(leftgripper.force()<25 and pressure_ok==1):   
		    time.sleep(0.04)
		    attempts+=1
		    if(attempts>50):
                        leftgripper.open()
                        pressure_ok=0
	                print("----->pressure is to low<-----")
            else:
                print("----->gripper presses on an object<-----")

            # Move back to the approach pickgoal.
	    pickgoal.pose.position.z = zn+0.05
	    gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
            # Move both arms to position 1.
            g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False)

	    if pressure_ok and z_force>-4:
                # Define the approach placegoal.
                # Increase the height of the tower every time by 2.75 cm.
	        placegoal.pose.position.z =-0.145+(k*0.0275)+0.08
                # Move to the approach pickgoal.
	        gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
                # Define the placegoal.
	        placegoal.pose.position.z =-0.145+(k*0.0275)
	        gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
	        leftgripper.open()
                k += 1
		# Move to the approach placegoal.
		placegoal.pose.position.z = -0.145+(k*0.0275)+0.08
		gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)

	    # Move left arm to start position pos1.
            gl.moveToJointPosition(jts_left, lpos1, max_velocity_scaling_factor = 1, plan_only=False)  
    
    pr.disable()
    sortby = 'cumulative'
    ps=pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
    p.clear()
Ejemplo n.º 17
0
def picknplace():
    # Define positions.
    pos1 = {
        'left_e0': -1.441426162661994,
        'left_e1': 0.8389151064712133,
        'left_s0': 0.14240920034028015,
        'left_s1': -0.14501001475655606,
        'left_w0': -1.7630090377446503,
        'left_w1': -1.5706376573674472,
        'left_w2': 0.09225918246029519,
        'right_e0': 1.7238109084167481,
        'right_e1': 1.7169079948791506,
        'right_s0': 0.36930587426147465,
        'right_s1': -0.33249033539428713,
        'right_w0': -1.2160632682067871,
        'right_w1': 1.668587600115967,
        'right_w2': -1.810097327636719
    }
    rpos1 = {
        'right_e0': 1.7238109084167481,
        'right_e1': 1.7169079948791506,
        'right_s0': 0.36930587426147465,
        'right_s1': -0.33249033539428713,
        'right_w0': -1.2160632682067871,
        'right_w1': 1.668587600115967,
        'right_w2': -1.810097327636719
    }
    rpos2 = {
        'right_e0': 1.8166167480533013,
        'right_e1': 1.8768254939778037,
        'right_s0': -0.6435049405179311,
        'right_s1': -0.18867963690990588,
        'right_w0': -1.2919953185964896,
        'right_w1': 1.7215099392044055,
        'right_w2': -2.557145973404985
    }
    lpos1 = {
        'left_e0': -1.441426162661994,
        'left_e1': 0.8389151064712133,
        'left_s0': 0.14240920034028015,
        'left_s1': -0.14501001475655606,
        'left_w0': -1.7630090377446503,
        'left_w1': -1.5706376573674472,
        'left_w2': 0.09225918246029519
    }
    lpos2 = {
        'left_e0': -0.14956312681882783,
        'left_e1': 1.235238029444729,
        'left_s0': 0.3186845086831947,
        'left_s1': -0.0682621450609009,
        'left_w0': 2.1586944637517487,
        'left_w1': -1.5661943844310073,
        'left_w2': -0.4962427848809313
    }

    basket_start = geometry_msgs.msg.Pose()
    basket_start.position.x = 0.582905207564
    basket_start.position.y = 0.428866088097
    basket_start.position.z = -0.034942926096
    basket_start.orientation.x = 0.334360832507
    basket_start.orientation.y = 0.660214964821
    basket_start.orientation.z = -0.352475264948
    basket_start.orientation.w = 0.572782874667

    placegoal = geometry_msgs.msg.Pose()
    placegoal.position.x = 0.6
    placegoal.position.y = 0.6
    placegoal.position.z = 0
    placegoal.orientation.x = 1.0
    placegoal.orientation.y = 0.0
    placegoal.orientation.z = 0.0
    placegoal.orientation.w = 0.0
    # Define variables.
    offset_zero_point = 0.903
    table_size_x = 0.714655654394
    table_size_y = 1.05043717328
    table_size_z = 0.729766045265
    center_x = 0.457327827197
    center_y = 0.145765166941
    center_z = -0.538116977368
    # The distance from the zero point in Moveit to the ground is 0.903 m.
    # The value is not allways the same. (look in Readme)
    center_z_cube = -offset_zero_point + table_size_z + 0.0275 / 2
    center_z_basket = -offset_zero_point + table_size_z + 0.16 / 2
    pressure_ok = 0
    j = 0
    k = 0
    start = 1
    locs_x = []
    # Initialize a list for the objects and the stacked cubes.
    objlist = [
        'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
        'obj09', 'obj10', 'obj11'
    ]
    boxlist = [
        'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08',
        'box09', 'box10', 'box11'
    ]
    # Clear planning scene.
    p.clear()
    # Add table as attached object.
    p.attachBox('table',
                table_size_x,
                table_size_y,
                table_size_z,
                center_x,
                center_y,
                center_z,
                'base',
                touch_links=['pedestal'])
    p.waitForSync()
    # Try to find and grip the basket till it is between the left gripper.
    while (pressure_ok == 0):
        # Move both arms to start state.
        both_arms.set_joint_value_target(pos1)
        both_arms.plan()
        both_arms.go(wait=True)
        time.sleep(0.5)
        # Wait till it detect the basket on the table.
        while (True):
            temp = rospy.wait_for_message("detected_basket", PoseArray)
            locs = temp.poses
            locs_x = []
            locs_y = []
            orien = []
            size = []
            for i in range(len(locs)):
                locs_x.append(locs[i].position.x)
                locs_y.append(locs[i].position.y)
            if locs_x:
                break
            else:
                time.sleep(3)
            #sys.exit()
        # The first detected basket position isn't very precisly if the
        # basket isn't in an area under the right hand camera because
        # the function cvMinEnclosingCircle(c, &center, &radius) find
        # a circle around all the pink color. It moves to the position
        # and looks again with a better view.

        # Save the position where it normally looks from.
        rpos1_values = set_current_position("right")
        old_x_pos = rpos1_values.position.x
        old_y_pos = rpos1_values.position.y
        # Move to above the first detected position from the basket.
        rpos1_values.position.x = locs_x[0]
        rpos1_values.position.y = locs_y[0]
        right_arm.set_pose_target(rpos1_values)
        right_arm.plan()
        right_arm.go(wait=True)
        time.sleep(0.5)
        # Save the position where it looks the second time.
        second_view = set_current_position("right")
        new_x_pos = second_view.position.x
        new_y_pos = second_view.position.y
        # Calculate the offsets to the first position to get
        # the position in the baxter coordinate system.
        diff_x = new_x_pos - old_x_pos
        diff_y = new_y_pos - old_y_pos
        # Look the second time for the basket and add the offsets.
        temp = rospy.wait_for_message("detected_basket", PoseArray)
        locs = temp.poses
        locs_x = []
        locs_y = []
        orien = []
        size = []
        for i in range(len(locs)):
            locs_x.append(locs[i].position.x + diff_x)
            locs_y.append(locs[i].position.y + diff_y)
        # Clear planning scene.
        p.clear()
        # Add table as attached object.
        p.attachBox('table',
                    table_size_x,
                    table_size_y,
                    table_size_z,
                    center_x,
                    center_y,
                    center_z,
                    'base',
                    touch_links=['pedestal'])
        #p.attachBox('basket', 0.05, 0.05, 0.16,locs_x[0], locs_y[0], center_z_basket, 'base')
        p.waitForSync()
        # Move the right arm next to the table to make more space for the
        # left arm.
        right_arm.set_joint_value_target(rpos2)
        right_arm.plan()
        right_arm.go(wait=True)
        # The offsets are an approximation to the right basket position.
        approach_basket = geometry_msgs.msg.Pose()
        approach_basket.position.x = locs_x[0] - 0.07
        approach_basket.position.y = locs_y[0] + 0.14
        approach_basket.position.z = -0.0780530728262
        approach_basket.orientation.x = 0.355253135688
        approach_basket.orientation.y = 0.64271546033
        approach_basket.orientation.z = -0.311705465591
        approach_basket.orientation.w = 0.60295252662
        # The offsets cause the arm to move too far
        # that the basket can be better grip.
        basket = geometry_msgs.msg.Pose()
        basket.position.x = locs_x[0] + 0.02
        basket.position.y = locs_y[0] - 0.03
        basket.position.z = -0.0780530728262
        basket.orientation.x = 0.32068927
        basket.orientation.y = 0.6715956
        basket.orientation.z = -0.3556723
        basket.orientation.w = 0.565344935
        # Move the left arm to the approach position.
        left_arm.set_pose_target(approach_basket)
        left_arm.plan()
        left_arm.go(wait=True)
        # Clear planning scene again to remove the attached basket.
        p.clear()
        # Add table as attached object.
        p.attachBox('table',
                    table_size_x,
                    table_size_y,
                    table_size_z,
                    center_x,
                    center_y,
                    center_z,
                    'base',
                    touch_links=['pedestal'])
        p.waitForSync()
        # Move the left arm to the basket position.
        move("left", basket)
        leftgripper.close()
        attempts = 0
        pressure_ok = 1
        # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
        while (leftgripper.force() < 25 and pressure_ok == 1):
            time.sleep(0.04)
            attempts += 1
            if (attempts > 50):
                leftgripper.open()
                pressure_ok = 0
                print("----->pressure is to low<-----")

    # Lift the basket up.
    basket_up = deepcopy(basket)
    basket_up.position.z = 0.025
    move("left", basket_up)
    # Move to the start position with the basket.
    left_arm.set_joint_value_target(lpos2)
    left_arm.plan()
    left_arm.go(wait=True)
    # Move to the start position for the right arm.
    right_arm.set_joint_value_target(rpos1)
    right_arm.plan()
    right_arm.go(wait=True)
    # cProfile to measure the performance (time) of the task.
    pr = cProfile.Profile()
    pr.enable()
    # Loop to continue pick and place until all objects are cleared from table.
    while locs_x or start:
        # Only for the start.
        if start:
            start = 0
    # Receive the data from all objects from the topic "detected_objects".
        time.sleep(1)
        temp = rospy.wait_for_message("detected_objects", PoseArray)
        locs = temp.poses
        locs_x = []
        locs_y = []
        orien = []
        size = []
        # Adds the data from the objects.
        for i in range(len(locs)):
            locs_x.append(locs[i].position.x)
            locs_y.append(locs[i].position.y)
            orien.append(locs[i].position.z * pi / 180)
            size.append(locs[i].orientation.x)

    # Filter objects list to remove multiple detected locations
    # for same objects and objects which are too far away.
        ind_rmv = []
        for i in range(0, len(locs)):
            if (locs_y[i] > 0.3 or locs_x[i] > 0.75):
                ind_rmv.append(i)
                continue
            for j in range(i, len(locs)):
                if not (i == j):
                    if sqrt((locs_x[i] - locs_x[j])**2 +
                            (locs_y[i] - locs_y[j])**2) < 0.018:
                        ind_rmv.append(i)

        locs_x = del_meth(locs_x, ind_rmv)
        locs_y = del_meth(locs_y, ind_rmv)
        orien = del_meth(orien, ind_rmv)
        size = del_meth(size, ind_rmv)

        # Do the task only if there are still objects on the table.
        if locs_x:
            # Clear planning scene.
            p.clear()
            # Add table as attached object.
            p.attachBox('table',
                        table_size_x,
                        table_size_y,
                        table_size_z,
                        center_x,
                        center_y,
                        center_z,
                        'base',
                        touch_links=['pedestal'])
            # Initialize the data of the biggest object on the table.
            xn = locs_x[0]
            yn = locs_y[0]
            zn = -0.16
            thn = orien[0]
            sz = size[0]
            if thn > pi / 4:
                thn = -1 * (thn % (pi / 4))
            # Add the detected objects into the planning scene.
            #for i in range(1,len(locs_x)):
            #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube)
            p.waitForSync()

            # Initialize the approach pickgoal (5 cm to pickgoal).
            approach_pickgoal = geometry_msgs.msg.Pose()
            approach_pickgoal.position.x = xn
            approach_pickgoal.position.y = yn
            approach_pickgoal.position.z = zn + 0.05

            approach_pickgoal_dummy = PoseStamped()
            approach_pickgoal_dummy.header.frame_id = "base"
            approach_pickgoal_dummy.header.stamp = rospy.Time.now()
            approach_pickgoal_dummy.pose.position.x = xn
            approach_pickgoal_dummy.pose.position.y = yn
            approach_pickgoal_dummy.pose.position.z = zn + 0.05
            approach_pickgoal_dummy.pose.orientation.x = 1.0
            approach_pickgoal_dummy.pose.orientation.y = 0.0
            approach_pickgoal_dummy.pose.orientation.z = 0.0
            approach_pickgoal_dummy.pose.orientation.w = 0.0

            # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
            approach_pickgoal_dummy.pose = rotate_pose_msg_by_euler_angles(
                approach_pickgoal_dummy.pose, 0.0, 0.0, thn)
            approach_pickgoal.orientation.x = approach_pickgoal_dummy.pose.orientation.x
            approach_pickgoal.orientation.y = approach_pickgoal_dummy.pose.orientation.y
            approach_pickgoal.orientation.z = approach_pickgoal_dummy.pose.orientation.z
            approach_pickgoal.orientation.w = approach_pickgoal_dummy.pose.orientation.w
            # Move to the approach goal.
            right_arm.set_pose_target(approach_pickgoal)
            right_arm.plan()
            right_arm.go(wait=True)
            # Move to the pickgoal.
            pickgoal = deepcopy(approach_pickgoal)
            pickgoal.position.z = zn
            right_arm.set_pose_target(pickgoal)
            right_arm.plan()
            right_arm.go(wait=True)
            time.sleep(0.5)
            # Read the force in z direction.
            f_r = rightarm.endpoint_effort()
            z_r = f_r['force']
            z_force = z_r.z
            # Search again for objects, if the gripper isn't at the right position and presses on an object.
            #print("----->force in z direction:", z_force)
            if z_force > -4:
                rightgripper.close()
                attempts = 0
                pressure_ok = 1
                # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
                while (rightgripper.force() < 25 and pressure_ok == 1):
                    time.sleep(0.04)
                    attempts += 1
                    if (attempts > 50):
                        rightgripper.open()
                        pressure_ok = 0
                        print("----->pressure is to low<-----")
            else:
                print("----->gripper presses on an object<-----")

            if pressure_ok and z_force > -4:
                # Move with the object in the gripper above the pickgoal.
                picked_object = set_current_position("right")
                picked_object.position.z = 0.2
                right_arm.set_pose_target(picked_object)
                right_arm.plan()
                right_arm.go(wait=True)
                # Move with the basket under the gripper with the object.
                picked_object = set_current_position("right")
                picked_object.position.z = 0
                picked_object.orientation.x = 0.334360832507
                picked_object.orientation.y = 0.660214964821
                picked_object.orientation.z = -0.352475264948
                picked_object.orientation.w = 0.572782874667
                move("left", picked_object)
                rightgripper.open()
                while (rightgripper.force() > 10):
                    time.sleep(0.01)
                # Move the left arm back to the start position.
                move("left", basket_start)

    # Move the right arm back to the start position.
            right_arm.set_joint_value_target(rpos1)
            right_arm.plan()
            right_arm.go(wait=True)

    pr.disable()
    sortby = 'cumulative'
    ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
    p.clear()
    moveit_commander.roscpp_shutdown()
    # Exit MoveIt.
    moveit_commander.os._exit(0)
Ejemplo n.º 18
0
def picknplace():
    # Define positions.
    pos1 = {
        'left_e0': -1.69483279891317,
        'left_e1': 1.8669726956453,
        'left_s0': 0.472137005716569,
        'left_s1': -0.38852045702393034,
        'left_w0': -1.9770933862776057,
        'left_w1': -1.5701993084642143,
        'left_w2': -0.6339059781326424,
        'right_e0': 1.7238109084167481,
        'right_e1': 1.7169079948791506,
        'right_s0': 0.36930587426147465,
        'right_s1': -0.33249033539428713,
        'right_w0': -1.2160632682067871,
        'right_w1': 1.668587600115967,
        'right_w2': -1.810097327636719
    }
    lpos1 = {
        'left_e0': -1.69483279891317,
        'left_e1': 1.8669726956453,
        'left_s0': 0.472137005716569,
        'left_s1': -0.38852045702393034,
        'left_w0': -1.9770933862776057,
        'left_w1': -1.5701993084642143,
        'left_w2': -0.6339059781326424
    }
    rpos1 = {
        'right_e0': 1.7238109084167481,
        'right_e1': 1.7169079948791506,
        'right_s0': 0.36930587426147465,
        'right_s1': -0.33249033539428713,
        'right_w0': -1.2160632682067871,
        'right_w1': 1.668587600115967,
        'right_w2': -1.810097327636719
    }

    rpos = geometry_msgs.msg.Pose()
    rpos.position.x = 0.555
    rpos.position.y = 0.0
    rpos.position.z = 0.206
    rpos.orientation.x = 1.0
    rpos.orientation.y = 0.0
    rpos.orientation.z = 0.0
    rpos.orientation.w = 0.0

    lpos = geometry_msgs.msg.Pose()
    lpos.position.x = 0.65
    lpos.position.y = 0.6
    lpos.position.z = 0.206
    lpos.orientation.x = 1.0
    lpos.orientation.y = 0.0
    lpos.orientation.z = 0.0
    lpos.orientation.w = 0.0

    placegoal = geometry_msgs.msg.Pose()
    placegoal.position.x = 0.55
    placegoal.position.y = 0.2
    # Open the gripper 4 mm above the tip of the tower.
    placegoal.position.z = 0
    placegoal.orientation.x = 1.0
    placegoal.orientation.y = 0.0
    placegoal.orientation.z = 0.0
    placegoal.orientation.w = 0.0

    # Define variables.
    table_size_x = 1.0
    table_size_y = 0.929999123509
    table_size_z = 0.75580154342
    center_x = 0.6
    center_y = -0.0441882472378
    center_z = -0.52509922829
    # The distance from the zero point in Moveit to the ground is 0.903 m.
    # The value is not allways the same. (look in Readme)
    center_z_cube = -0.903 + table_size_z + 0.0275 / 2
    j = 0
    k = 0
    u = 0
    locs_x = []
    # Initialize a list for the objects and the stacked cubes.
    objlist = [
        'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
        'obj09', 'obj10', 'obj11'
    ]
    boxlist = [
        'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08',
        'box09', 'box10', 'box11'
    ]

    #rightarm.move_to_joint_positions(rpos1)
    right_arm.set_pose_target(rpos)
    right_arm.plan()
    right_arm.go(wait=True)

    both_arms.set_joint_value_target(pos1)
    both_arms.plan()
    both_arms.go(wait=True)

    # Load Gazebo Models via Spawning Services.
    load_gazebo_models()
    # Remove models from the scene on shutdown.
    rospy.on_shutdown(delete_gazebo_models)
    # Wait for the All Clear from emulator startup.
    rospy.wait_for_message("/robot/sim/started", Empty)
    # Clear planning scene.
    p.clear()
    # Add table as attached object.
    p.attachBox('table',
                table_size_x,
                table_size_y,
                table_size_z,
                center_x,
                center_y,
                center_z,
                'base',
                touch_links=['pedestal'])
    p.waitForSync()
    rightgripper.open()
    # cProfile to measure the performance (time) of the task.
    pr = cProfile.Profile()
    pr.enable()
    # Loop to continue pick and place until all objects are cleared from table.
    while k < 4:
        locs_x = [0.6725 + 0.02, 0.55 + 0.02, 0.7 + 0.02, 0.58 + 0.02]
        locs_y = [0.0765 + 0.02, -0.2 + 0.02, -0.1 + 0.02, -0.03 + 0.02]
        orien = [0, 0, 0, 0]
        size = [5, 5, 5, 5]

        # Filter objects list to remove multiple detected locations for same objects.
        ind_rmv = []
        for i in range(0, len(locs_x)):
            if (locs_y[i] > 0.24):
                ind_rmv.append(i)
                continue
            for j in range(i, len(locs_x)):
                if not (i == j):
                    if sqrt((locs_x[i] - locs_x[j])**2 +
                            (locs_y[i] - locs_y[j])**2) < 0.018:
                        ind_rmv.append(i)

        locs_x = del_meth(locs_x, ind_rmv)
        locs_y = del_meth(locs_y, ind_rmv)
        orien = del_meth(orien, ind_rmv)
        size = del_meth(size, ind_rmv)

        # Do the task only if there are still objects on the table.
        while u < len(locs_x):
            # Clear planning scene.
            p.clear()
            # Add table as attached object.
            p.attachBox('table',
                        table_size_x,
                        table_size_y,
                        table_size_z,
                        center_x,
                        center_y,
                        center_z,
                        'base',
                        touch_links=['pedestal'])
            # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
            ig0 = itemgetter(0)
            sorted_lists = zip(*sorted(
                zip(size, locs_x, locs_y, orien), reverse=True, key=ig0))
            locs_x = list(sorted_lists[1])
            locs_y = list(sorted_lists[2])
            orien = list(sorted_lists[3])
            size = list(sorted_lists[0])
            # Initialize the data of the biggest object on the table.
            xn = locs_x[u]
            yn = locs_y[u]
            # -0.16 is the z position to grip the objects on the table.
            zn = -0.143
            thn = orien[u]
            sz = size[u]
            if thn > pi / 4:
                thn = -1 * (thn % (pi / 4))

            # Add the detected objects into the planning scene.
            for i in range(1, len(locs_x)):
                p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i],
                         center_z_cube)
            # Add the stacked objects as collision objects into the planning scene to avoid moving against them.
            for e in range(0, k):
                p.attachBox(boxlist[e],
                            0.05,
                            0.05,
                            0.0275,
                            placegoal.position.x,
                            placegoal.position.y,
                            center_z_cube + 0.0275 * (e - 1),
                            'base',
                            touch_links=['cubes'])
            p.waitForSync()
            # Initialize the approach pickgoal (5 cm to pickgoal).
            approach_pickgoal = geometry_msgs.msg.Pose()
            approach_pickgoal.position.x = xn
            approach_pickgoal.position.y = yn
            approach_pickgoal.position.z = zn + 0.05

            approach_pickgoal_dummy = PoseStamped()
            approach_pickgoal_dummy.header.frame_id = "base"
            approach_pickgoal_dummy.header.stamp = rospy.Time.now()
            approach_pickgoal_dummy.pose.position.x = xn
            approach_pickgoal_dummy.pose.position.y = yn
            approach_pickgoal_dummy.pose.position.z = zn + 0.05
            approach_pickgoal_dummy.pose.orientation.x = 1.0
            approach_pickgoal_dummy.pose.orientation.y = 0.0
            approach_pickgoal_dummy.pose.orientation.z = 0.0
            approach_pickgoal_dummy.pose.orientation.w = 0.0

            # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
            approach_pickgoal_dummy.pose = rotate_pose_msg_by_euler_angles(
                approach_pickgoal_dummy.pose, 0.0, 0.0, thn)
            approach_pickgoal.orientation.x = approach_pickgoal_dummy.pose.orientation.x
            approach_pickgoal.orientation.y = approach_pickgoal_dummy.pose.orientation.y
            approach_pickgoal.orientation.z = approach_pickgoal_dummy.pose.orientation.z
            approach_pickgoal.orientation.w = approach_pickgoal_dummy.pose.orientation.w
            # Move to the approach goal and the pickgoal.
            pickgoal = deepcopy(approach_pickgoal)
            pickgoal.position.z = zn
            move('right', approach_pickgoal, pickgoal)
            time.sleep(1)
            rightgripper.close()
            time.sleep(0.5)
            # Move back to the approach pickgoal.
            move('right', approach_pickgoal)

            # Define the approach placegoal.
            # Increase the height of the tower every time by 2.75 cm.
            approached_placegoal = deepcopy(placegoal)
            approached_placegoal.position.z = -0.143 + (k * 0.0275) + 0.08
            # Define the placegoal.
            placegoal.position.z = -0.143 + (k * 0.0275)
            move('right', approached_placegoal, placegoal)
            rightgripper.open()
            time.sleep(1)
            # Move to the approach placegoal.
            move('right', approached_placegoal)
            k += 1
            u += 1
            # Move right arm to start position.
            both_arms.set_joint_value_target(pos1)
            both_arms.plan()
            both_arms.go(wait=True)

    pr.disable()
    sortby = 'cumulative'
    ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)

    moveit_commander.roscpp_shutdown()
    # Exit MoveIt.
    moveit_commander.os._exit(0)
    time.sleep(10)
Ejemplo n.º 19
0
    def __init__(self):
        rospy.init_node('urb_pickup', anonymous=False)
        
        rospy.on_shutdown(self.shutdown)
        
        # Get Goal point where we pick up
        goal_x = rospy.get_param("~goal_x", 3.96) # meters
        goal_y = rospy.get_param("~goal_y", 4.01)
        goal_z = rospy.get_param("~goal_z", pi/2)

        parser = argparse.ArgumentParser(description="Simple demo of pick and place")
        parser.add_argument("--objects", help="Just do object perception", action="store_true")
        parser.add_argument("--all", help="Just do object perception, but insert all objects", action="store_true")
        parser.add_argument("--once", help="Run once.", action="store_true")
        parser.add_argument("--ready", help="Move the arm to the ready position.", action="store_true")
        parser.add_argument("--plan", help="Only do planning, no execution", action="store_true")
        parser.add_argument("--x", help="Recommended x offset, how far out an object should roughly be.", type=float, default=0.5)
        args, unknown = parser.parse_known_args()


        move_group = MoveGroupInterface("arm", "base_link", plan_only = args.plan)

        # Create a list to hold the target quaternions (orientations)         
        quaternions = list()  
        # Create a list to hold the waypoint poses
        waypoints = list()

        q_angle = quaternion_from_euler(0, 0, goal_z, axes='sxyz')  
        q = Quaternion(*q_angle)
        quaternions.append(q)           

        waypoints.append(Pose(Point(goal_x, goal_y, 0.0), quaternions[0]))         
           
        # Publisher to manually control the robot (e.g. to stop it)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        
        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        
        rospy.loginfo("Waiting for move_base action server...")
        
        # Wait 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))
        
        rospy.loginfo("Connected to move base server")
        rospy.loginfo("Starting navigation test")
 
        # Intialize the waypoint goal
        goal = MoveBaseGoal()
            
        # Use the map frame to define goal poses
        goal.target_pose.header.frame_id = 'map'
            
        # Set the time stamp to "now"
        goal.target_pose.header.stamp = rospy.Time.now()
          
        # Set the goal pose to the i-th waypoint
        goal.target_pose.pose = waypoints[0]
          
        # Start the robot moving toward the goal
        self.move(goal)

        # if all we want to do is prepare the arm, do it now
        if args.ready:
            self.move_to_ready(move_group)
            exit(0)

        scene = PlanningSceneInterface("base_link")
        pickplace = PickPlaceInterface("arm", "gripper", plan_only = args.plan, verbose = True)

        rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
        find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
        find_objects.wait_for_server()
        rospy.loginfo("...connected")

        while not rospy.is_shutdown():
            goal = FindGraspableObjectsGoal()
            goal.plan_grasps = True
            find_objects.send_goal(goal)
            find_objects.wait_for_result(rospy.Duration(5.0))
            find_result = find_objects.get_result()

            rospy.loginfo("Found %d objects" % len(find_result.objects))

            # remove all previous objects
            for name in scene.getKnownCollisionObjects():
                scene.removeCollisionObject(name, False)
            for name in scene.getKnownAttachedObjects():
                scene.removeAttachedObject(name, False)
            scene.waitForSync()
            # clear colors
            scene._colors = dict()

            # insert objects, find the one to grasp
            the_object = None
            the_object_dist = 0.35
            count = -1
            for obj in find_result.objects:
                count += 1
                scene.addSolidPrimitive("object%d"%count,
                                        obj.object.primitives[0],
                                        obj.object.primitive_poses[0],
                                        wait = False)
                # object must have usable grasps
                if len(obj.grasps) < 1:
                    continue
                # choose object in front of robot
                dx = obj.object.primitive_poses[0].position.x - args.x
                dy = obj.object.primitive_poses[0].position.y
                d = math.sqrt((dx * dx) + (dy * dy))
                if d < the_object_dist:
                    the_object_dist = d
                    the_object = count
    
            if the_object == None:
                rospy.logerr("Nothing to grasp! try again...")
                continue
    
            # insert table
            for obj in find_result.support_surfaces:
                # extend surface to floor
                height = obj.primitive_poses[0].position.z
                obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
                                                2.0, # make table wider
                                                obj.primitives[0].dimensions[2] + height]
                obj.primitive_poses[0].position.z += -height/2.0
    
                # add to scene
                scene.addSolidPrimitive(obj.name,
                                        obj.primitives[0],
                                        obj.primitive_poses[0],
                                        wait = False)
    
            obj_name = "object%d"%the_object
    
            # sync
            scene.waitForSync()
    
            # set color of object we are grabbing
            scene.setColor(obj_name, 223.0/256.0, 90.0/256.0, 12.0/256.0)  # orange
            scene.setColor(find_result.objects[the_object].object.support_surface, 0, 0, 0)  # black
            scene.sendColors()
    
            # exit now if we are just doing object update
            if args.objects:
                if args.once:
                    exit(0)
                else:
                    continue

            # get grasps (we checked that they exist above)
            grasps = find_result.objects[the_object].grasps
            support_surface = find_result.objects[the_object].object.support_surface    

            # call move_group to pick the object
            rospy.loginfo("Beginning to pick.")
            success, pick_result = pickplace.pick_with_retry(obj_name, grasps, support_name=support_surface, scene=scene)
            if not success:
                exit(-1)

            # create a set of place locations for the cube
            places = list()
            l = PlaceLocation()
            l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0]
            l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id
            # invert the y of the pose
            l.place_pose.pose.position.y = -l.place_pose.pose.position.y
            # copy the posture, approach and retreat from the grasp used
            l.post_place_posture = pick_result.grasp.pre_grasp_posture
            l.pre_place_approach = pick_result.grasp.pre_grasp_approach
            l.post_place_retreat = pick_result.grasp.post_grasp_retreat
            places.append(copy.deepcopy(l))
            # create another several places, rotate each by 90 degrees in yaw direction
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
            places.append(copy.deepcopy(l))
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
            places.append(copy.deepcopy(l))
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
            places.append(copy.deepcopy(l))

            # drop it like it's hot
            rospy.loginfo("Beginning to place.")
            while not rospy.is_shutdown():
                # can't fail here or moveit needs to be restarted
                success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene)
                if success:
                    break

            # place arm back at side
            self.move_to_ready(move_group)
            rospy.loginfo("Ready...")

            # rinse and repeat
            if args.once:
                exit(0)            
Ejemplo n.º 20
0
def picknplace():
    # Define positions.
    pos1 = {
        'left_e0': -1.69483279891317,
        'left_e1': 1.8669726956453,
        'left_s0': 0.472137005716569,
        'left_s1': -0.38852045702393034,
        'left_w0': -1.9770933862776057,
        'left_w1': -1.5701993084642143,
        'left_w2': -0.6339059781326424,
        'right_e0': 1.7238109084167481,
        'right_e1': 1.7169079948791506,
        'right_s0': 0.36930587426147465,
        'right_s1': -0.33249033539428713,
        'right_w0': -1.2160632682067871,
        'right_w1': 1.668587600115967,
        'right_w2': -1.810097327636719
    }
    lpos1 = {
        'left_e0': -1.69483279891317,
        'left_e1': 1.8669726956453,
        'left_s0': 0.472137005716569,
        'left_s1': -0.38852045702393034,
        'left_w0': -1.9770933862776057,
        'left_w1': -1.5701993084642143,
        'left_w2': -0.6339059781326424
    }
    rpos1 = {
        'right_e0': 1.7238109084167481,
        'right_e1': 1.7169079948791506,
        'right_s0': 0.36930587426147465,
        'right_s1': -0.33249033539428713,
        'right_w0': -1.2160632682067871,
        'right_w1': 1.668587600115967,
        'right_w2': -1.810097327636719
    }

    placegoal = geometry_msgs.msg.Pose()
    placegoal.position.x = 0.55
    placegoal.position.y = 0.22
    placegoal.position.z = 0
    placegoal.orientation.x = 1.0
    placegoal.orientation.y = 0.0
    placegoal.orientation.z = 0.0
    placegoal.orientation.w = 0.0

    # Define variables.
    offset_zero_point = 0.903
    table_size_x = 0.714655654394
    table_size_y = 1.05043717328
    table_size_z = 0.729766045265
    center_x = 0.457327827197
    center_y = 0.145765166941
    center_z = -0.538116977368
    # The distance from the zero point in Moveit to the ground is 0.903 m.
    # The value is not allways the same. (look in Readme)
    center_z_cube = -offset_zero_point + table_size_z + 0.0275 / 2
    pressure_l_ok = 0
    pressure_r_ok = 0
    left_ready = 0
    right_ready = 0
    j = 0
    k = 0
    start = 1
    locs_x_right = []
    locs_x_left = []
    # Initialize a list for the objects and the stacked cubes.
    objlist = [
        'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
        'obj09', 'obj10', 'obj11'
    ]
    boxlist = [
        'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08',
        'box09', 'box10', 'box11'
    ]
    # Clear planning scene.
    p.clear()
    # Add table as attached object.
    p.attachBox('table',
                table_size_x,
                table_size_y,
                table_size_z,
                center_x,
                center_y,
                center_z,
                'base',
                touch_links=['pedestal'])
    p.waitForSync()
    # Move both arms to start state.
    both_arms.set_joint_value_target(pos1)
    both_arms.plan()
    both_arms.go(wait=True)

    # cProfile to measure the performance (time) of the task.
    pr = cProfile.Profile()
    pr.enable()
    # Loop to continue pick and place until all objects are cleared from table.
    while locs_x_right or locs_x_left or start:
        # Only for the start.
        if start:
            start = 0

        time.sleep(0.5)
        # Receive the data from all objects from the topic "detected_objects_left".
        temp_left = rospy.wait_for_message("detected_objects_left", PoseArray)
        locs_left = temp_left.poses
        # Receive the data from all objects from the topic "detected_objects_right".
        temp_right = rospy.wait_for_message("detected_objects_right",
                                            PoseArray)
        locs_right = temp_right.poses

        locs_x_right = []
        locs_y_right = []
        orien_right = []
        size_right = []

        locs_x_left = []
        locs_y_left = []
        orien_left = []
        size_left = []

        # Adds the data from the objects from the left camera.
        for i in range(len(locs_left)):
            locs_x_left.append(locs_left[i].position.x)
            locs_y_left.append(locs_left[i].position.y)
            orien_left.append(locs_left[i].position.z * pi / 180)
            size_left.append(locs_left[i].orientation.x)
    # Adds the data from the objects from the right camera.
        for i in range(len(locs_right)):
            locs_x_right.append(locs_right[i].position.x)
            locs_y_right.append(locs_right[i].position.y)
            orien_right.append(locs_right[i].position.z * pi / 180)
            size_right.append(locs_right[i].orientation.x)

    # Filter objects list to remove multiple detected locations for same objects (left camera).
        ind_rmv = []
        for i in range(0, len(locs_left)):
            if (locs_y_left[i] < 0.28 or locs_y_left[i] > 0.62
                    or locs_x_left[i] > 0.75):
                ind_rmv.append(i)
                continue
            for j in range(i, len(locs_left)):
                if not (i == j):
                    if sqrt((locs_x_left[i] - locs_x_left[j])**2 +
                            (locs_y_left[i] - locs_y_left[j])**2) < 0.018:
                        ind_rmv.append(i)

        locs_x_left = del_meth(locs_x_left, ind_rmv)
        locs_y_left = del_meth(locs_y_left, ind_rmv)
        orien_left = del_meth(orien_left, ind_rmv)
        size_left = del_meth(size_left, ind_rmv)
        # Filter objects list to remove multiple detected locations for same objects (right camera).
        ind_rmv = []
        for i in range(0, len(locs_right)):
            if (locs_y_right[i] > 0.16 or locs_x_right[i] > 0.75):
                ind_rmv.append(i)
                continue
            for j in range(i, len(locs_right)):
                if not (i == j):
                    if sqrt((locs_x_right[i] - locs_x_right[j])**2 +
                            (locs_y_right[i] - locs_y_right[j])**2) < 0.018:
                        ind_rmv.append(i)

        locs_x_right = del_meth(locs_x_right, ind_rmv)
        locs_y_right = del_meth(locs_y_right, ind_rmv)
        orien_right = del_meth(orien_right, ind_rmv)
        size_right = del_meth(size_right, ind_rmv)

        # Do the task only if there are still objects on the table.
        if locs_x_right:
            # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
            ig0_right = itemgetter(0)
            sorted_lists_right = zip(*sorted(zip(size_right, locs_x_right,
                                                 locs_y_right, orien_right),
                                             reverse=True,
                                             key=ig0_right))
            locs_x_right = list(sorted_lists_right[1])
            locs_y_right = list(sorted_lists_right[2])
            orien_right = list(sorted_lists_right[3])
            size_right = list(sorted_lists_right[0])
            # Initialize the data of the biggest object on the table.
            xn_right = locs_x_right[0]
            yn_right = locs_y_right[0]
            zn_right = -0.16
            thn_right = orien_right[0]
            sz_right = size_right[0]
            if thn_right > pi / 4:
                thn_right = -1 * (thn_right % (pi / 4))

        if locs_x_left:
            ig0_left = itemgetter(0)
            sorted_lists_left = zip(
                *sorted(zip(size_left, locs_x_left, locs_y_left, orien_left),
                        reverse=True,
                        key=ig0_left))
            locs_x_left = list(sorted_lists_left[1])
            locs_y_left = list(sorted_lists_left[2])
            orien_left = list(sorted_lists_left[3])
            size_left = list(sorted_lists_left[0])
            # Initialize the data of the biggest object on the table.
            xn_left = locs_x_left[0]
            yn_left = locs_y_left[0]
            zn_left = -0.145
            thn_left = orien_left[0]
            sz_left = size_left[0]
            if thn_left > pi / 4:
                thn_left = -1 * (thn_left % (pi / 4))

        if locs_x_right or locs_x_left:
            # Clear planning scene.
            p.clear()
            # Add table as attached object.
            p.attachBox('table',
                        table_size_x,
                        table_size_y,
                        table_size_z,
                        center_x,
                        center_y,
                        center_z,
                        'base',
                        touch_links=['pedestal'])
            # Add the detected objects into the planning scene.
            #for i in range(1,len(locs_x_left)):
            #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x_left[i], locs_y_left[i], center_z_cube)
            #for i in range(1,len(locs_x_right)):
            #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x_right[i], locs_y_right[i], center_z_cube)
            # Add the stacked objects as collision objects into the planning scene to avoid moving against them.
            #for e in range(0, k):
            #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.position.x, placegoal.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes'])
            if k > 0:
                p.attachBox(boxlist[0],
                            0.07,
                            0.07,
                            0.0275 * k,
                            placegoal.position.x,
                            placegoal.position.y,
                            center_z_cube,
                            'base',
                            touch_links=['cubes'])
            p.waitForSync()
            if left_ready == 0 and locs_x_left:
                # Initialize the approach pickgoal left (5 cm to pickgoal).
                approach_pickgoal_l = geometry_msgs.msg.Pose()
                approach_pickgoal_l.position.x = xn_left
                approach_pickgoal_l.position.y = yn_left
                approach_pickgoal_l.position.z = zn_left + 0.05
                approach_pickgoal_l_dummy = PoseStamped()
                approach_pickgoal_l_dummy.header.frame_id = "base"
                approach_pickgoal_l_dummy.header.stamp = rospy.Time.now()
                approach_pickgoal_l_dummy.pose.position.x = xn_left
                approach_pickgoal_l_dummy.pose.position.y = yn_left
                approach_pickgoal_l_dummy.pose.position.z = zn_left + 0.05
                approach_pickgoal_l_dummy.pose.orientation.x = 1.0
                approach_pickgoal_l_dummy.pose.orientation.y = 0.0
                approach_pickgoal_l_dummy.pose.orientation.z = 0.0
                approach_pickgoal_l_dummy.pose.orientation.w = 0.0

                # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
                approach_pickgoal_l_dummy.pose = rotate_pose_msg_by_euler_angles(
                    approach_pickgoal_l_dummy.pose, 0.0, 0.0, thn_left)
                approach_pickgoal_l.orientation.x = approach_pickgoal_l_dummy.pose.orientation.x
                approach_pickgoal_l.orientation.y = approach_pickgoal_l_dummy.pose.orientation.y
                approach_pickgoal_l.orientation.z = approach_pickgoal_l_dummy.pose.orientation.z
                approach_pickgoal_l.orientation.w = approach_pickgoal_l_dummy.pose.orientation.w
                # Move to the approach goal.
                left_arm.set_pose_target(approach_pickgoal_l)
                left_arm.plan()
                left_arm.go(wait=True)
                # Move to the pickgoal.
                pickgoal_l = deepcopy(approach_pickgoal_l)
                pickgoal_l.position.z = zn_left
                left_arm.set_pose_target(pickgoal_l)
                left_arm.plan()
                left_arm.go(wait=True)
                time.sleep(0.5)
                # Read the force in z direction.
                f_l = leftarm.endpoint_effort()
                z_l = f_l['force']
                z_force_l = z_l.z
                # Search again for objects, if the gripper isn't at the right position and presses on an object.
                #print("----->force in z direction:", z_force_l)
                if z_force_l > -4:
                    leftgripper.close()
                    attempts = 0
                    pressure_l_ok = 1
                    # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
                    while (leftgripper.force() < 25 and pressure_l_ok == 1):
                        time.sleep(0.04)
                        attempts += 1
                        if (attempts > 50):
                            leftgripper.open()
                            pressure_l_ok = 0
                            print("----->pressure is to low<-----")
                else:
                    print("----->gripper presses on an object<-----")

                # Move back to the approach pickgoal.
                left_arm.set_pose_target(approach_pickgoal_l)
                left_arm.plan()
                left_arm.go(wait=True)

                if pressure_l_ok and z_force_l > -4:
                    left_ready = 1
                else:
                    # Move back to lpos1.
                    left_arm.set_joint_value_target(lpos1)
                    left_arm.plan()
                    left_arm.go(wait=True)

            if (left_ready == 1 or not locs_x_left) and locs_x_right:
                # Initialize the approach pickgoal right (5 cm to pickgoal).
                approach_pickgoal_r = geometry_msgs.msg.Pose()
                approach_pickgoal_r.position.x = xn_right
                approach_pickgoal_r.position.y = yn_right
                approach_pickgoal_r.position.z = zn_right + 0.05

                approach_pickgoal_r_dummy = PoseStamped()
                approach_pickgoal_r_dummy.header.frame_id = "base"
                approach_pickgoal_r_dummy.header.stamp = rospy.Time.now()
                approach_pickgoal_r_dummy.pose.position.x = xn_right
                approach_pickgoal_r_dummy.pose.position.y = yn_right
                approach_pickgoal_r_dummy.pose.position.z = zn_right + 0.05
                approach_pickgoal_r_dummy.pose.orientation.x = 1.0
                approach_pickgoal_r_dummy.pose.orientation.y = 0.0
                approach_pickgoal_r_dummy.pose.orientation.z = 0.0
                approach_pickgoal_r_dummy.pose.orientation.w = 0.0

                # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
                approach_pickgoal_r_dummy.pose = rotate_pose_msg_by_euler_angles(
                    approach_pickgoal_r_dummy.pose, 0.0, 0.0, thn_right)
                approach_pickgoal_r.orientation.x = approach_pickgoal_r_dummy.pose.orientation.x
                approach_pickgoal_r.orientation.y = approach_pickgoal_r_dummy.pose.orientation.y
                approach_pickgoal_r.orientation.z = approach_pickgoal_r_dummy.pose.orientation.z
                approach_pickgoal_r.orientation.w = approach_pickgoal_r_dummy.pose.orientation.w
                # Move to the approach goal.
                right_arm.set_pose_target(approach_pickgoal_r)
                right_arm.plan()
                right_arm.go(wait=True)
                # Move to the pickgoal.
                pickgoal_r = deepcopy(approach_pickgoal_r)
                pickgoal_r.position.z = zn_right
                right_arm.set_pose_target(pickgoal_r)
                right_arm.plan()
                right_arm.go(wait=True)
                time.sleep(0.5)
                # Read the force in z direction.
                f_r = rightarm.endpoint_effort()
                z_r = f_r['force']
                z_force_r = z_r.z
                # Search again for objects, if the gripper isn't at the right position and presses on an object.
                #print("----->force in z direction:", z_force_l)
                if z_force_r > -4:
                    rightgripper.close()
                    attempts = 0
                    pressure_r_ok = 1
                    # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
                    while (rightgripper.force() < 25 and pressure_r_ok == 1):
                        time.sleep(0.04)
                        attempts += 1
                        if (attempts > 50):
                            rightgripper.open()
                            pressure_r_ok = 0
                            print("----->pressure is to low<-----")
                else:
                    print("----->gripper presses on an object<-----")

                # Move back to the approach pickgoal.
                right_arm.set_pose_target(approach_pickgoal_r)
                right_arm.plan()
                right_arm.go(wait=True)

                if pressure_r_ok and z_force_r > -4:
                    right_ready = 1
                else:
                    # Move back to rpos1.
                    right_arm.set_joint_value_target(rpos1)
                    right_arm.plan()
                    right_arm.go(wait=True)

            if (left_ready == 1 or not locs_x_left) and (right_ready == 1
                                                         or not locs_x_right):
                # Move both arms to start state.
                #both_arms.set_joint_value_target(pos1)
                #both_arms.plan()
                #both_arms.go(wait=True)
                if (left_ready == 1):
                    # Define the approach placegoal for the left arm.
                    # Increase the height of the tower every time by 2.75 cm.
                    approached_placegoal = deepcopy(placegoal)
                    approached_placegoal.position.z = -0.14 + (k *
                                                               0.0275) + 0.08
                    # Move to the approach placegoal.
                    left_arm.set_pose_target(approached_placegoal)
                    left_arm.plan()
                    left_arm.go(wait=True)
                    # Define the placegoal.
                    placegoal.position.z = -0.14 + (k * 0.0275)
                    placegoal.position.x = 0.54
                    left_arm.set_pose_target(placegoal)
                    left_arm.plan()
                    left_arm.go(wait=True)
                    leftgripper.open()
                    while (leftgripper.force() > 10):
                        time.sleep(0.01)
                    k += 1
                    # Move back to the approach placegoal.
                    left_arm.set_pose_target(approached_placegoal)
                    left_arm.plan()
                    left_arm.go(wait=True)
                    # Move back to lpos1.
                    left_arm.set_joint_value_target(lpos1)
                    left_arm.plan()
                    left_arm.go(wait=True)
                    left_ready = 0
                if (right_ready == 1):
                    # Define the approach placegoal for the right arm.
                    # Increase the height of the tower every time by 2.75 cm.
                    approached_placegoal = deepcopy(placegoal)
                    approached_placegoal.position.z = -0.155 + (k *
                                                                0.0275) + 0.08
                    # Move to the approach placegoal.
                    right_arm.set_pose_target(approached_placegoal)
                    right_arm.plan()
                    right_arm.go(wait=True)
                    # Define the placegoal.
                    placegoal.position.z = -0.155 + (k * 0.0275)
                    placegoal.position.x = 0.53
                    right_arm.set_pose_target(placegoal)
                    right_arm.plan()
                    right_arm.go(wait=True)
                    rightgripper.open()
                    while (rightgripper.force() > 10):
                        time.sleep(0.01)
                    k += 1
                    # Move back to the approach placegoal.
                    right_arm.set_pose_target(approached_placegoal)
                    right_arm.plan()
                    right_arm.go(wait=True)
                    # Move back to rpos1.
                    right_arm.set_joint_value_target(rpos1)
                    right_arm.plan()
                    right_arm.go(wait=True)
                    right_ready = 0

    pr.disable()
    sortby = 'cumulative'
    ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
    p.clear()
    moveit_commander.roscpp_shutdown()
    # Exit MoveIt.
    moveit_commander.os._exit(0)
Ejemplo n.º 21
0
def clasificar():

    adaptative = True
    pr_b = False
    precision = 0.7

    # Can free memory?
    pub = rospy.Publisher("finished", String, queue_size=10)

    # Initialize MoveIt! scene
    p = PlanningSceneInterface("base")

    arms_group = MoveGroupInterface("both_arms", "base")
    rightarm_group = MoveGroupInterface("right_arm", "base")
    leftarm_group = MoveGroupInterface("left_arm", "base")

    # Create right arm instance
    right_arm = baxter_interface.limb.Limb('right')

    # Create right gripper instance
    right_gripper = baxter_interface.Gripper('right')
    right_gripper.calibrate()
    right_gripper.open()

    right_gripper.close()

    offset_zero_point = 0.903
    table_size_x = 0.714655654394
    table_size_y = 1.05043717328
    table_size_z = 0.729766045265
    center_x = 0.457327827197
    center_y = 0.145765166941
    center_z = -0.538116977368
    table_distance_from_gripper = -offset_zero_point + table_size_z + 0.0275 / 2
    j = 0
    k = 0

    # Initialize object list
    objlist = [
        'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
        'obj09', 'obj10', 'obj11'
    ]
    p.clear()
    p.attachBox('table',
                table_size_x,
                table_size_y,
                table_size_z,
                center_x,
                center_y,
                center_z,
                'base',
                touch_links=['pedestal'])
    p.waitForSync()
    # Move both arms to start state.

    # Initial pos
    rpos = PoseStamped()
    rpos.header.frame_id = "base"
    rpos.header.stamp = rospy.Time.now()
    rpos.pose.position.x = 0.555
    rpos.pose.position.y = 0.0
    rpos.pose.position.z = 0.206
    rpos.pose.orientation.x = 1.0
    rpos.pose.orientation.y = 0.0
    rpos.pose.orientation.z = 0.0
    rpos.pose.orientation.w = 0.0

    lpos = PoseStamped()
    lpos.header.frame_id = "base"
    lpos.header.stamp = rospy.Time.now()
    lpos.pose.position.x = 0.65
    lpos.pose.position.y = 0.6
    lpos.pose.position.z = 0.206
    lpos.pose.orientation.x = 1.0
    lpos.pose.orientation.y = 0.0
    lpos.pose.orientation.z = 0.0
    lpos.pose.orientation.w = 0.0

    while True:

        # Move to initial position
        rightarm_group.moveToPose(rpos,
                                  "right_gripper",
                                  max_velocity_scaling_factor=1,
                                  plan_only=False)
        leftarm_group.moveToPose(lpos,
                                 "left_gripper",
                                 max_velocity_scaling_factor=1,
                                 plan_only=False)

        # Get the middle point
        locs = PoseArray()
        locs = rospy.wait_for_message("clasificacion", PoseArray)

        if (len(locs.poses) != 0):

            punto_medio = PoseStamped()
            punto_medio.header.frame_id = "base"
            punto_medio.header.stamp = rospy.Time.now()
            punto_medio.pose.position.x = locs.poses[0].position.x
            punto_medio.pose.position.y = locs.poses[0].position.y
            punto_medio.pose.position.z = locs.poses[0].position.z
            punto_medio.pose.orientation.x = locs.poses[0].orientation.x
            punto_medio.pose.orientation.y = locs.poses[0].orientation.y
            punto_medio.pose.orientation.z = locs.poses[0].orientation.z
            punto_medio.pose.orientation.w = locs.poses[0].orientation.w

            alfa = 0.1

            # Two parameters:
            # When pr_b == 1, the program will try to adapt the trajectory for getting the precision established in the parameter of the same name
            # When adaptative == 1, the program will try to get the best precision based on the precision of the last execution.

            if (pr_b and not adaptative):
                if precision >= 1:
                    punto_medio.pose.position.x += 0.01

                else:
                    punto_medio.pose.position.x -= alfa * (1 - precision)

            else:

                print("If it is the first time executing, write -1")
                precision_value = input()

                if precision_value != -1.0:
                    punto_medio.pose.position.x += alfa * (1 - precision_value)
                elif precision_value == 1.0:
                    adaptative = False

        # Get the normal orientation for separating the objects
            orient = (-1) * (1 / punto_medio.pose.orientation.x)

            punto_medio.pose = rotate_pose_msg_by_euler_angles(
                punto_medio.pose, 0.0, 0.0, orient)
            rightarm_group.moveToPose(punto_medio,
                                      "right_gripper",
                                      max_velocity_scaling_factor=1,
                                      plan_only=False)

            orient = 1.57 - orient
            punto_medio.pose = rotate_pose_msg_by_euler_angles(
                punto_medio.pose, 0.0, 0.0, orient)
            rightarm_group.moveToPose(punto_medio,
                                      "right_gripper",
                                      max_velocity_scaling_factor=1,
                                      plan_only=False)

            punto_medio.pose.position.x += 0.15
            rightarm_group.moveToPose(punto_medio,
                                      "right_gripper",
                                      max_velocity_scaling_factor=1,
                                      plan_only=False)
            time.sleep(1)
            punto_medio.pose.position.x -= 0.3
            rightarm_group.moveToPose(punto_medio,
                                      "right_gripper",
                                      max_velocity_scaling_factor=1,
                                      plan_only=False)
            time.sleep(1)

            rightarm_group.moveToPose(rpos,
                                      "right_gripper",
                                      max_velocity_scaling_factor=1,
                                      plan_only=False)

            # Free the memory when finished
            freemem = "free"
            if not adaptative:
                pub.publish(freemem)

        else:
            sys.exit("Cannot identify any object")
Ejemplo n.º 22
0
def picknplace():
    # Define initial parameters
    p = PlanningSceneInterface("base")
    g = MoveGroupInterface("both_arms", "base")
    gr = MoveGroupInterface("right_arm", "base")
    gl = MoveGroupInterface("left_arm", "base")
    leftgripper = baxter_interface.Gripper('left')
    leftgripper.calibrate()
    leftgripper.open()
    jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
    pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
    lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]    
    rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
    rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
    
    # Clear planning scene
    p.clear()
    # Add table as attached object
    p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])

    # Move both arms to start state              
    g.moveToJointPosition(jts_both, pos1, plan_only=False)
    
    # Get obj locations
    temp = rospy.wait_for_message("Dpos", PoseArray)
    locs = temp.poses 
    locs_x = []
    locs_y = []
    orien = []
    size = []

    for i in range(len(locs)):
        locs_x.append(0.57 + locs[i].position.x)
        locs_y.append(-0.011 + locs[i].position.y)
        orien.append(locs[i].position.z*pi/180)
        size.append(locs[i].orientation.x)

    # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
    ind_rmv = []
    for i in range(0,len(locs)):
        if (locs_y[i] > 0.42):
            ind_rmv.append(i)
            continue
        for j in range(i,len(locs)):
            if not (i == j):
                if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
                    ind_rmv.append(i)
    
    locs_x = del_meth(locs_x, ind_rmv)
    locs_y = del_meth(locs_y, ind_rmv)
    orien = del_meth(orien, ind_rmv) 
    size = del_meth(size, ind_rmv)

    # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
    if locs_x:  
        ig0 = itemgetter(0)
        sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))

        locs_x = list(sorted_lists[1])
        locs_y = list(sorted_lists[2])
        orien = list(sorted_lists[3])
        size = list(sorted_lists[0])

    # Loop to continue pick and place until all objects are cleared from table
    j=0
    k=0
    while locs_x:
        p.clear() # CLear planning scene of all collision objects

        # Attach table as attached collision object to base of baxter
        p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
        xn = locs_x[0]
        yn = locs_y[0]
        zn = -0.06
        thn = orien[0]
        sz = size[0]
        if thn > pi/4:
            thn = -1*(thn%(pi/4))

        # Add collision objects into planning scene
        objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
        for i in range(1,len(locs_x)):
            p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
        p.waitForSync()        

        # Move both arms to pos2 (Right arm away and left arm on table)
        g.moveToJointPosition(jts_both, pos2, plan_only=False)

        # Move left arm to pick object and pick object
        pickgoal = PoseStamped() 
        pickgoal.header.frame_id = "base"
        pickgoal.header.stamp = rospy.Time.now()
        pickgoal.pose.position.x = xn
        pickgoal.pose.position.y = yn
        pickgoal.pose.position.z = zn+0.1
        pickgoal.pose.orientation.x = 1.0
        pickgoal.pose.orientation.y = 0.0
        pickgoal.pose.orientation.z = 0.0
        pickgoal.pose.orientation.w = 0.0
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False)

        # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles 
        pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False) 

        pickgoal.pose.position.z = zn
        gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
        leftgripper.close()

        pickgoal.pose.position.z = zn+0.1
        gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
        
        # Move both arms to pos1
        g.moveToJointPosition(jts_both, pos1, plan_only=False)

        # Get obj locations
        temp = rospy.wait_for_message("Dpos", PoseArray)
        locs = temp.poses 
        locs_x = []
        locs_y = []
        orien = []
        size = []

        for i in range(len(locs)):
            locs_x.append(0.57 + locs[i].position.x)
            locs_y.append(-0.011 + locs[i].position.y)
            orien.append(locs[i].position.z*pi/180)
            size.append(locs[i].orientation.x)

        # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
        ind_rmv = []
        for i in range(0,len(locs)):
            if (locs_y[i] > 0.42):
                ind_rmv.append(i)
                continue
            for j in range(i,len(locs)):
                if not (i == j):
                    if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
                        ind_rmv.append(i)
        
        locs_x = del_meth(locs_x, ind_rmv)
        locs_y = del_meth(locs_y, ind_rmv)
        orien = del_meth(orien, ind_rmv) 
        size = del_meth(size, ind_rmv)

        # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
        if locs_x:  
            ig0 = itemgetter(0)
            sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))

            locs_x = list(sorted_lists[1])
            locs_y = list(sorted_lists[2])
            orien = list(sorted_lists[3])
            size = list(sorted_lists[0])

        # Place object
        stleft = PoseStamped() 
        stleft.header.frame_id = "base"
        stleft.header.stamp = rospy.Time.now()
        stleft.pose.position.x = 0.65
        stleft.pose.position.y = 0.55
        stleft.pose.position.z = 0.1
        stleft.pose.orientation.x = 1.0
        stleft.pose.orientation.y = 0.0
        stleft.pose.orientation.z = 0.0
        stleft.pose.orientation.w = 0.0

        # Stack objects (large cubes) if size if greater than some threshold
        if sz > 16.:            
            stleft.pose.position.x = 0.65
            stleft.pose.position.y = 0.7
            stleft.pose.position.z = -0.04+(k*0.05)
            k += 1
        
        gl.moveToPose(stleft, "left_gripper", plan_only=False)        
        leftgripper.open()

        stleft.pose.position.z = 0.3
        gl.moveToPose(stleft, "left_gripper", plan_only=False)