Python MoveGroupCommander.get_planning_frame方法代码示例

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_planning_frame [as 别名]
    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cobra_move_position', anonymous=True)

        arm = MoveGroupCommander(ARM_GROUP)

        arm.allow_replanning(True)
        arm.set_planning_time(10)

        arm.set_named_target(START_POSITION)
        arm.go()
        rospy.sleep(2)

        print "======== create new goal position ========"

        start_pose = PoseStamped()
        start_pose.header.frame_id = arm.get_planning_frame()

        # Test Position
        start_pose.pose.position.x = 0.2  # 20 cm
        start_pose.pose.position.y = -0.11  # -11 cm
        start_pose.pose.position.z = 0.6  # 60 cm
        q = quaternion_from_euler(0.0, 0.0, 0.0)
        start_pose.pose.orientation = Quaternion(*q)

        print start_pose

        print "====== move to position ======="


        # KDL
        # arm.set_joint_value_target(start_pose, True)

        # IK Fast
        arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z],
                                arm.get_end_effector_link())

        arm.go()
        rospy.sleep(2)

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_planning_frame [as 别名]
    def __init__(self):
        group = MoveGroupCommander("arm")
        
        #group.set_orientation_tolerance([0.3,0.3,0,3])
        
        p = PoseStamped()
        p.header.frame_id = "/katana_base_link"
        p.pose.position.x = 0.4287
        #p.pose.position.y = -0.0847
        p.pose.position.y = 0.0
        p.pose.position.z = 0.4492
        
        
        q = quaternion_from_euler(2, 0, 0)
        
        p.pose.orientation.x = q[0]
        p.pose.orientation.y = q[1]
        p.pose.orientation.z = q[2]
        p.pose.orientation.w = q[3]
        


        
        print "Planning frame: " ,group.get_planning_frame()
        print "Pose reference frame: ",group.get_pose_reference_frame()
        
        #group.set_pose_reference_frame("katana_base_link")

        print "RPy target: 0,0,0"
        #group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
        #group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
        
        group.set_pose_target(p, "katana_gripper_tool_frame")
        
        group.go()
        print "Current rpy: " , group.get_current_rpy("katana_gripper_tool_frame")

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_planning_frame [as 别名]
    scene.add_sphere("ball", p, 0.1)

    p.pose.position.x = 0
    p.pose.position.y = 0
    p.pose.position.z = -0.1
    scene.add_box("table", p, (1.0, 2.6, 0.2))

    p.pose.position.x = 0
    p.pose.position.y = 0
    p.pose.position.z = -0.4
    scene.add_plane("ground_plane", p)

    rospy.sleep(20)

    ## We can get the name of the reference frame for this robot
    print ">>>>> Reference frame: %s" % group.get_planning_frame()
    print ">>>>> Printing robot state"
    print robot.get_current_state()

    print ">>>>> Printing robot pose"
    print group.get_current_pose()

    ## Planning to a Pose goal
    print ">>>>> Generating plan"
    pose_target = geometry_msgs.msg.Pose()
    #pose_target.orientation.w = 1.0
    pose_target.position.x = 0.5  #0.4
    pose_target.position.y = 0.2  #0.2
    pose_target.position.z = 0.2  #0.2
    group.set_pose_target(pose_target)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_planning_frame [as 别名]


if __name__=='__main__':
    rospy.init_node("cartesianpathtest")
    
    rospy.loginfo("Starting up move group commander for right arm")
    right_arm_mgc = MoveGroupCommander("right_arm_torso")

    #init_pose = right_arm_mgc.get_current_pose()
    init_pose = Pose(Point(0.4, -0.2, 1.2),Quaternion(0.0,0.0,0.0,1.0))
    goal_point = Point(0.45, -0.2, 1.4)
    goal_ori = Quaternion(0.0,0.0,0.0,1.0)
    goal_pose = Pose(goal_point, goal_ori)
    right_arm_mgc.set_pose_reference_frame('base_link')
    rospy.loginfo("Planning on frame:" + str(right_arm_mgc.get_planning_frame()))
    waypoints = []
    waypoints.append(init_pose)#.pose)
    waypoints.append(goal_pose)
    
    eef_step_max = 0.05
    jump_thresh_max = 10000
    while True:
        curr_eef_step = eef_step_max * random()
        curr_jump_thresh = jump_thresh_max * random()
        #path, fraction = right_arm_mgc.compute_cartesian_path(waypoints, curr_eef_step, curr_jump_thresh, False)
        path, fraction = right_arm_mgc.compute_cartesian_path(waypoints, 0.01, 1.2, False)

        rospy.loginfo("Fraction and path:")
        rospy.loginfo(str(fraction))
        if fraction <= 0: