美文网首页
MoveIt教程8 - Move Group (Python接口

MoveIt教程8 - Move Group (Python接口

作者: Danny_a44d | 来源:发表于2019-07-12 14:23 被阅读0次

    类比于C++的MoveGroupInterface,MoveIt也提供了相应接口的Python封装。
    可以编写.py程序控制机器人的常用基本操作:

    • 设置关节角度或机器人姿态目标
    • 运动规划
    • 移动机器人
    • 添加物体到环境里/从环境移除
    • 将物体绑定到机器人上/从机器人上解绑

    这些接口也是基于ROS的框架。


    这里通过Panda机械臂的案例看一下怎么玩。

    运行程序

    启动panda机械臂MoveIt主程序

    roslaunch panda_moveit_config demo.launch
    

    在另一个终端rosrun运行python脚本

    rosrun moveit_tutorials move_group_python_interface_tutorial.py
    

    结果

    教程中,对机械臂进行了以下几种控制:

      1. 规划并运动到指定关节目标 (joint goal)
      1. 规划一个到指定姿态目标(pose goal)的路径
      1. 规划一个坐标系路径 (Cartesian path)
      1. 显示坐标系路径(Cartesian path)
      1. 执行坐标系路径规划
      1. 在末端执行器除添加一个立方体
      1. 绑定立方体到机器人上
      1. 在立方体绑定的情况下规划和执行一个坐标系路径(Cartesian path)
      1. 解绑立方体
      1. 移除立方体

    代码分析

    教程的完整代码https://github.com/ros-planning/moveit_tutorials/blob/kinetic-devel/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py

    下面我们分析一下代码块:

    Setup

    使用python接口时,我们通常需要加载moveit_commander, 这是一个namespace package,里面会加载MoveGroupCommander, PlanningSceneInterface
    以及RobotCommander

    另外加载一些用到的ROS messages

    import sys
    import copy
    import rospy
    import moveit_commander
    import moveit_msgs.msg
    import geometry_msgs.msg
    from math import pi
    from std_msgs.msg import String
    from moveit_commander.conversions import pose_to_list
    

    初始化moveit_commander,创建一个node

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

    创建一个RobotCommander的对象。RobotCommander是针对整个机器人的控制。

    robot = moveit_commander.RobotCommander()
    

    创建一个PlanningSceneInterface的对象。PlanningSceneInterface是用于和机器人环境的交互(如添加物体是会用到)

    scene = moveit_commander.PlanningSceneInterface()
    

    创建MoveGroupCommander的对象。MoveGroupCommander是针对一个规划组进行控制。这里通过设置group_name = panda_arm控制Panda机器人的手臂。

    group_name = "panda_arm"
    group = moveit_commander.MoveGroupCommander(group_name)
    

    创建一个Publisher,发布的类型是DisplayTrajectory,用于rivz显示

    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                   moveit_msgs.msg.DisplayTrajectory,
                                                   queue_size=20)
    

    Getting Basic Information

    打印一些基本的信息

    # We can get the name of the reference frame for this robot:
    planning_frame = group.get_planning_frame()
    print "============ Reference frame: %s" % planning_frame
    
    # We can also print the name of the end-effector link for this group:
    eef_link = group.get_end_effector_link()
    print "============ End effector: %s" % eef_link
    
    # We can get a list of all the groups in the robot:
    group_names = robot.get_group_names()
    print "============ Robot Groups:", robot.get_group_names()
    
    # Sometimes for debugging it is useful to print the entire state of the
    # robot:
    print "============ Printing robot state"
    print robot.get_current_state()
    print ""
    

    Planning to a Joint Goal

    规划到一个关节目标

    # We can get the joint values from the group and adjust some of the values:
    joint_goal = group.get_current_joint_values()
    joint_goal[0] = 0
    joint_goal[1] = -pi/4
    joint_goal[2] = 0
    joint_goal[3] = -pi/2
    joint_goal[4] = 0
    joint_goal[5] = pi/3
    joint_goal[6] = 0
    
    # The go command can be called with joint values, poses, or without any
    # parameters if you have already set the pose or joint target for the group
    group.go(joint_goal, wait=True)
    
    # Calling ``stop()`` ensures that there is no residual movement
    group.stop()
    

    Planning to a Pose Goal

    定义一个姿态目标

    pose_goal = geometry_msgs.msg.Pose()
    pose_goal.orientation.w = 1.0
    pose_goal.position.x = 0.4
    pose_goal.position.y = 0.1
    pose_goal.position.z = 0.4
    group.set_pose_target(pose_goal)
    

    规划并执行

    plan = group.go(wait=True)
    # Calling `stop()` ensures that there is no residual movement
    group.stop()
    # It is always good to clear your targets after planning with poses.
    # Note: there is no equivalent function for clear_joint_value_targets()
    group.clear_pose_targets()
    

    Cartesian Paths

    通过给一个指定的途经点waypoints的list来规划坐标系路径cartesian path

    waypoints = []
    
    wpose = group.get_current_pose().pose
    wpose.position.z -= scale * 0.1  # First move up (z)
    wpose.position.y += scale * 0.2  # and sideways (y)
    waypoints.append(copy.deepcopy(wpose))
    
    wpose.position.x += scale * 0.1  # Second move forward/backwards in (x)
    waypoints.append(copy.deepcopy(wpose))
    
    wpose.position.y -= scale * 0.1  # Third move sideways (y)
    waypoints.append(copy.deepcopy(wpose))
    
    # We want the Cartesian path to be interpolated at a resolution of 1 cm
    # which is why we will specify 0.01 as the eef_step in Cartesian
    # translation.  We will disable the jump threshold by setting it to 0.0 disabling:
    (plan, fraction) = group.compute_cartesian_path(
                                       waypoints,   # waypoints to follow
                                       0.01,        # eef_step
                                       0.0)         # jump_threshold
    
    # Note: We are just planning, not asking move_group to actually move the robot yet:
    return plan, fraction
    

    Displaying a Trajectory

    在rviz中显示路径。调用group.plan()规划路径的时候回自动在rviz中显示。

    也可以手动进行显示,创建一个 DisplayTrajectory
    的消息对象,然后发布到'/move_group/display_planned_path'话题。

    DisplayTrajectory消息主要有两个属性,起始点trajectory_start 和 路径trajectory。

    display_trajectory = moveit_msgs.msg.DisplayTrajectory()
    display_trajectory.trajectory_start = robot.get_current_state()
    display_trajectory.trajectory.append(plan)
    # Publish
    display_trajectory_publisher.publish(display_trajectory);
    

    Executing a Plan

    执行计算出来的路径

    group.execute(plan, wait=True)
    

    Adding Objects to the Planning Scene

    在场景中添加一个立方体, 设置位置在panda_leftfinger

    box_pose = geometry_msgs.msg.PoseStamped()
    box_pose.header.frame_id = "panda_leftfinger"
    box_pose.pose.orientation.w = 1.0
    box_name = "box"
    scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))
    

    Ensuring Collision Updates Are Receieved

    在添加/移除物体后,会发布一个更新碰撞物体的消息,这个消息在发布出去的时候可能会丢失,为了确保物体成功添加/移除,可以通过get_known_object_names()获取当前已知的物体来检查是否成功。

    start = rospy.get_time()
    seconds = rospy.get_time()
    while (seconds - start < timeout) and not rospy.is_shutdown():
      # Test if the box is in attached objects
      attached_objects = scene.get_attached_objects([box_name])
      is_attached = len(attached_objects.keys()) > 0
    
      # Test if the box is in the scene.
      # Note that attaching the box will remove it from known_objects
      is_known = box_name in scene.get_known_object_names()
    
      # Test if we are in the expected state
      if (box_is_attached == is_attached) and (box_is_known == is_known):
        return True
    
      # Sleep so that we give other threads time on the processor
      rospy.sleep(0.1)
      seconds = rospy.get_time()
    
    # If we exited the while loop without returning then we timed out
    return False
    

    Attaching Objects to the Robot

    绑定物体到机械臂。在用机械臂操纵物体的时候,为了防止MoveIt把某些link和物体的正常接触当做是碰撞而报,可以把这些link加入到touch_links,这样就会忽略这些link和物体的碰撞。

    grasping_group = 'hand'
    touch_links = robot.get_link_names(group=grasping_group)
    scene.attach_box(eef_link, box_name, touch_links=touch_links)
    

    Detaching Objects from the Robot

    解绑

    scene.remove_attached_object(eef_link, name=box_name)
    

    Removing Objects from the Planning Scene

    移除

    scene.remove_world_object(box_name)
    

    注意: 物体必须被解绑后才可以移除

    相关文章

      网友评论

          本文标题:MoveIt教程8 - Move Group (Python接口

          本文链接:https://www.haomeiwen.com/subject/vjiqkctx.html