コード例 #1
0
    if not plan.joint_trajectory.points[0].velocities: 
        print('plan {} fail in picking!'.format(ii))
        continue
    
    group.execute(plan, wait =True)

    # print("Planning in Rviz")
    # for i in range(15):
    #     print(15-i)   
    #     rospy.sleep(1)

    ###########
    # Gripper #
    ###########
    open_gripper(left)
    close_gripper(left)

    for i in range(3):
        print(3-i)   
        rospy.sleep(1)


    waypoints_lifting = []
    waypoints_lifting.append(wpose1)

    wpose2 = geometry_msgs.msg.Pose()
    wpose2.orientation = Quaternion(q[0],q[1], q[2], q[3])
    wpose2.position.x = t_pre[0]
    wpose2.position.y = t_pre[1]
    wpose2.position.z = 0.2
コード例 #2
0
def main():
    """
    Main Script
    """
    #############
    offset = -0.3
    #############

    # Make sure that you've looked at and understand path_planner.py before starting

    planner = PathPlanner("left_arm")

    ##
    ## Init Gripper
    ##
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled
    left = baxter_interface.Gripper('left', CHECK_VERSION)

    # #Create a path constraint for the arm
    # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
    # orien_const = OrientationConstraint()
    # orien_const.link_name = "right_gripper";
    # orien_const.header.frame_id = "base";
    # orien_const.orientation.y = -1.0;
    # orien_const.absolute_x_axis_tolerance = 0.1;
    # orien_const.absolute_y_axis_tolerance = 0.1;
    # orien_const.absolute_z_axis_tolerance = 0.1;
    # orien_const.weight = 1.0;

    tf_listener = tf.TransformListener(rospy.Duration(1))
    id_num = raw_input("Input Grasp ID: ")
    tf_listener.waitForTransform('base', "sample_grasp_" + str(id_num),
                                 rospy.Time(0), rospy.Duration(2.0))
    t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(id_num),
                                       rospy.Time())

    matrix = quaternion_matrix(q)

    new_matrix = matrix
    # new_matrix = np.eye(4)
    # new_matrix[:, 0] = matrix[:, 2]
    # new_matrix[:, 1] = -matrix[:, 1]
    # new_matrix[:, 2] = matrix[:, 0]

    t_pre = t + new_matrix[0:3, 2] * offset
    quaternion_from_matrix
    q = quaternion_from_matrix(new_matrix)

    print("try to move to object...........Move it!")

    ##########
    id_num = 0
    ##########

    while not rospy.is_shutdown():
        i_pregrasp = 0
        while not rospy.is_shutdown():

            ###########################################
            # if i_pregrasp > 5:
            #     tf_listener.waitForTransform('base',  "sample_grasp_" + str(id_num), rospy.Time(0), rospy.Duration(2.0))
            #     t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(id_num), rospy.Time())
            #     if id_num > rospy.get_param('good_grasps'):
            #         print("Fail!")
            #         return
            ############################################

            i_pregrasp += 1
            try:
                goal_1 = PoseStamped()
                goal_1.header.frame_id = "base"

                #x, y, and z position
                goal_1.pose.position.x = t_pre[0]
                goal_1.pose.position.y = t_pre[1]
                goal_1.pose.position.z = t_pre[2]

                #Orientation as a quaternion
                goal_1.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

                plan = planner.plan_to_pose(goal_1, list())
                print(plan)
                if not plan:
                    raise Exception("Execution failed")
                ## in Rviz
                rospy.sleep(6)

                raw_input("Press <Enter> to PRE grasp pose: ")
                print("PRE grasp pose %d" % i_pregrasp)

                # if not planner.execute_plan(plan):
                #     raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break

        i_grasp = 0
        while not rospy.is_shutdown():
            i_grasp += 1
            try:
                goal_2 = PoseStamped()
                goal_2.header.frame_id = "base"

                #x, y, and z position
                goal_2.pose.position.x = t[0]
                goal_2.pose.position.y = t[1]
                goal_2.pose.position.z = t[2]

                #Orientation as a quaternion
                goal_2.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

                plan = planner.plan_to_pose(goal_2, list())

                if not plan:
                    raise Exception("Execution failed")

                ## in Rviz
                rospy.sleep(6)
                raw_input("Press <Enter> to GRASP pose : ")
                print("GRASP pose %d" % i_grasp)

                # if not planner.execute_plan(plan):
                #     raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break

        ##
        ## grasp
        ##
        # open_gripper(left)
        # close_gripper(left)

        i_leave = 1
        while not rospy.is_shutdown():
            i_leave += 1
            try:
                goal_3 = PoseStamped()
                goal_3.header.frame_id = "base"

                #x, y, and z position
                # [0.551, 0.690, -0.049]
                goal_3.pose.position.x = 0.551
                goal_3.pose.position.y = 0.690
                goal_3.pose.position.z = -0.049

                #Orientation as a quaternion
                goal_3.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

                plan = planner.plan_to_pose(goal_3, list())

                if not plan:
                    raise Exception("Execution failed")
                rospy.sleep(6)
                raw_input("Press <Enter> to LEAVE pose: ")
                print("LEAVE pose %d" % i_leave)

                # if not planner.execute_plan(plan):
                #     raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break

        ##
        ## grasp
        ##
        open_gripper(left)

        raw_input("Press <Enter> to continue")
コード例 #3
0
ファイル: grasp_lab7_v2.py プロジェクト: woooks/grasping
def main():
    """
    Main Script
    """
    #############
    offset_p = -0.07
    offset_g = 0.04
    #############

    # Make sure that you've looked at and understand path_planner.py before starting

    planner = PathPlanner("left_arm")

    ##
    ## Init Gripper
    ##
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled
    left = baxter_interface.Gripper('left', CHECK_VERSION)

    tf_listener = tf.TransformListener(rospy.Duration(1))

    raw_input("Press <Enter> to start!")
    try_another_grasp = 0
    good_grasps = rospy.get_param('good_grasps')
    for g in range(good_grasps):
        i_pregrasp = 0
        ## get grasp pose
        tf_listener.waitForTransform('base', "sample_grasp_" + str(g),
                                     rospy.Time(0), rospy.Duration(2.0))
        t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(g),
                                           rospy.Time())
        matrix = quaternion_matrix(q)
        new_matrix = np.eye(4)
        new_matrix[:, 0] = matrix[:, 2]
        new_matrix[:, 1] = -matrix[:, 1]
        new_matrix[:, 2] = matrix[:, 0]

        t_pre = t + new_matrix[0:3, 2] * offset_p
        t_g = t + new_matrix[0:3, 2] * offset_g

        quaternion_from_matrix
        q = quaternion_from_matrix(new_matrix)

        print("try grasp {0} to move to object...........Move it!".format(g))

        while not rospy.is_shutdown():

            i_pregrasp += 1
            try:
                goal_1 = PoseStamped()
                goal_1.header.frame_id = "base"

                #x, y, and z position
                goal_1.pose.position.x = t_pre[0]
                goal_1.pose.position.y = t_pre[1]
                goal_1.pose.position.z = t_pre[2]

                #Orientation as a quaternion
                goal_1.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

                plan = planner.plan_to_pose(goal_1, list())

                ## In Rviz
                # rospy.sleep(6)

                # raw_input("Press <Enter> to PRE grasp pose: ")
                print("PRE grasp pose {0}, try {1} times".format(
                    g, i_pregrasp))
                if not planner.execute_plan(plan):
                    raise Exception("Execution failed")
            except Exception as e:
                print e
                ## jump to next grasp pose (1)
                if i_pregrasp > 2:
                    try_another_grasp = 1
                    break
            else:
                break

        ## jump to next grasp pose (2)
        if try_another_grasp == 1:
            print('Try another !')
            try_another_grasp = 0
            planner.clear_goal()
            continue

        i_grasp = 0
        while not rospy.is_shutdown():
            i_grasp += 1
            try:
                goal_2 = PoseStamped()
                goal_2.header.frame_id = "base"

                #x, y, and z position
                goal_2.pose.position.x = t_g[0]
                goal_2.pose.position.y = t_g[1]
                goal_2.pose.position.z = t_g[2]

                #Orientation as a quaternion
                goal_2.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

                plan = planner.plan_to_pose(goal_2, list())

                ## in Rviz
                # rospy.sleep(6)

                # raw_input("Press <Enter> to GRASP pose : ")
                print("GRASP pose %d" % i_grasp)

                if not planner.execute_plan(plan):
                    raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break

        ##
        ## grasp
        ##
        open_gripper(left)
        close_gripper(left)

        i_leave = 1
        while not rospy.is_shutdown():
            i_leave += 1
            try:
                goal_3 = PoseStamped()
                goal_3.header.frame_id = "base"

                #x, y, and z position
                # [0.551, 0.690, -0.049]
                goal_3.pose.position.x = 0.551
                goal_3.pose.position.y = 0.690
                goal_3.pose.position.z = -0.049

                #Orientation as a quaternion
                goal_3.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

                plan = planner.plan_to_pose(goal_3, list())

                # raw_input("Press <Enter> to LEAVE pose: ")
                print("LEAVE pose %d" % i_leave)

                if not planner.execute_plan(plan):
                    raise Exception("Execution failed")
            except Exception as e:
                print e
            else:
                break

        ##
        ## grasp
        ##
        open_gripper(left)

        raw_input("Press <Enter> to continue")