运动规划 (Motion Planning): MoveIt! 与 OMPL

Posted 周学伟

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了运动规划 (Motion Planning): MoveIt! 与 OMPL相关的知识,希望对你有一定的参考价值。

原创博文:转载请标明出处:http://www.cnblogs.com/zxouxuewei

最近有不少人询问有关MoveIt!与OMPL相关的话题,但是大部分问题都集中于XXX功能怎么实现,XXX错误怎么解决。表面上看,解决这些问题的方法就是提供正确的代码,正确的编译方法,正确的运行步骤。

然而,这种解决方法只能解决这个特定的问题,而且解决之后我们也无法学到一些实际的东西。要想彻底明白,需要从源头入手,也就是说,不要问“MoveIt! 怎么把机械手从空间一个点移到另一个点?“,而是要问”MoveIt! 为什么能把机械手从空间一个点移到另一个点?“。 这一点明白之后,遇到类似的问题,才能从容应对。同理,这不仅适用于MoveIt,也同样适用于其他任何ROS功能。

所以,下文中我们会见到一些具体的例子,但整体上,更倾向于宏观的概念和一些基础的方法,希望对大家能有所帮助。这里的帮助指的是增强对运动规划和Moveit, OMPL的整体理解,而非局限于完成某一个功能,编译运行某一个文件。


RRT) 和 Probabilistic Roadmap (PRM)了, 当然,这两个是比较老的,还有很多其他新算法。

  • OMPL能做什么? 简单说,就是提供一个运动轨迹。给定一个机器人结构(假设有N个关节),给定一个目标(比如终端移到xyz),给定一个环境,那么OMPL会提供给你一个 轨迹,包含M个数组,每一个数组长度是N,也就是一个完整的关节位置。沿着这个轨迹依次移动关节,就可以最终把终端移到xyz,当然,这个轨迹应当不与环 境中的任何障碍发生碰撞。
  • 为什么用OMPL? 运动规划的软件库和算法有很多,而OMPL由于其模块化的设计和稳定的更新,成为最流行的规划软件库之一。很多新算法都在OMPL开发。很多其他软件(包括ROS/MoveIt)都使用OMPL做运动规划。


1.3. 逆运动学 (Inverse Kinematics)

    • 什么是逆运动学(IK)?简单说,就是把终端位姿变成关节角度,q=IK(p)。p是终端位姿(xyz),q是关节角度。
      cd path_to_catkin_ws/src
      catkin_create_pkg lwr_description
      cd lwr_description
      mkdir urdf
      将下载好的lwr_simplified.urdf放入urdf文件夹中,这样一个urdf package便创建好了。

      2.2 MoveIt!配置助手 (MoveIt! Setup Assistant)
      2.2.1 打开MoveIt! Setup Assistant
      roslaunch moveit_setup_assistant setup_assistant.launch
         
      moveit_setup_assistant
      MoveIt! Setup Assistant 是一个图形界面,可以让我们不用写代码看代码,直接用鼠标点击就可以配置机器人的运动规划所需要的信息。点击Create New MoveIt Configuration Package来创建新的配置包,选择刚刚下载的urdf,然后点击Load Files 载入文件。

      2.2.2 创建碰撞免检矩阵(ACM)

      点击Setup Assisant的左边第二项\'Self-Collisions\',在这里我们将创建碰撞免检矩阵(Avoid Collision Matrix, ACM)。再次强调,怎么创建很简单,点击一下\'Regenerate Default Collision Matrix\'就可以了,问题是,为什么?ACM是做什么的?
      我们知道,碰撞检测是非常复杂的运算过程。对于多关节机械臂或者类人机器人来说,机械结构复杂,肢体多,碰撞检测 需要涉及很多的空间几何计算。但是对于刚体机器人来说,有些肢体之间是不可能发生碰撞的,比如原本就相邻的肢体,比如类人机器人的脚和头。这里生成的 ACM就是告诉我们,这个URDF所描述的机器人,哪些肢体之间是不会发生碰撞的。那么在之后的碰撞检测算法中,我们就可以略过对这些肢体之间的检测,以 提高检测效率。

      2.2.3 创建虚拟关节 (Virtual Joints)
      在Setup Assistant 第三项Virtual Joints里面,我们要创建所谓的虚拟关节。这个虚拟关节,可以理解为一个连接机器人和世界的关节。

      一般来说,Virtual Joint Name我们命名为‘world_joint’,而\'Child Link\'指的是我们要把‘世界’和机器人的那个部位连接,那么很显然我们选择基座\'base\'。‘Parent Frame Name’,是世界坐标的名字,在ROS中一般叫\'world_frame\'。关节类型 Joint Type, 很显然这里我们选择固定Fixed. 代表机器人相对于世界是固定的。而另外两种, Planar指的是平面移动底座(xy平面+角度),用于移动机器人比如PR2;
       
      还有一种Floating, 指的是浮动基座(xyz position+orientation),比如类人机器人。
       
      2.2.4 创建规划群 (Planning Groups)
      创建Planning Group是MoveIt的核心之一。首先,点击Add Group, 我们会看到一个界面,如下图,

      那么这些都是什么呢?
      • Group Name: 不用多说,名字。。。我们就叫Arm。
      • Kinematic Solver: 运动学求解工具,这个就是负责求解正向运动学(Forward Kinematics)和逆运动学(IK, 见1.3节)的。 一般我们选用KDL, The Kinematics and Dynamics Library。这是一个运动学与动力学的库,可以很好的解决6自由度以上的单链机械结构的正逆运动学问题。当然你也可以用其他IK Solver, 比如SRV或者IK_FAST,甚至你可以自己开发新的Solver然后插入进来,如果有空,我以后会发帖讲解如何创建新的运动学求解库并插入到MoveIt。
      • Kin. Search Resolution: 关节空间的采样密度
      • Kin. Search TImeout: 求解时间
      • Kin. Solver Attempts: 求解失败尝试次数,一般来说这三项使用默认值就可以。你也可以根据具体需要做出适当调整。
      接下来,我们要正式创建这个组群,有很多不同的方法,Add Joints, Add Links, Add Kin. Chain, Add Subgroups。我们这里选择\'Add Kin. Chain\',这样我们可以清楚的看到整个机器人的机械机构,

      在正中方我们可以看到这个机械臂的结构,一个link接着一个 link。下方我们可以看到有\'Base Link\'和\'Tip Link\',我们选择\'lwr_arm_0_link\'作为Base,选择\'lwr_arm_7_link\'作为Tip. 然后点击Save,这样一个规划组群就创建好了。同样的,我们可以再创建一个手的组群(Hand),这一次我们用Add Links,然后选择\'lwr_arm_7_link\'。


      2.2.5 创建机器人预设位姿 (Robot Poses)
      在Setup Assistant 第五项, ‘Robot Poses’,我们创建预设的机器人位姿。点击‘Add Pose’,我们为机械臂创建一个向上直立的位姿UpRight,选择Planning Group为Arm。可以看到很多滚动条,全设为0就是垂直向上的位姿。然后点击保存。当然,你可以根据需要设置其他不同位姿。

      2.2.6 配置终端控制器(End Effectors)
      终 端控制器,就是机械臂的手,以后用来在工作环境中直接控制的部位。我们添加一个叫做HandEff的终端控制器,End Effector Group选择之前创建好的Hand,Parent_Link选择机械臂的最后一个肢体lwr_arm_7_link。Parent Group选择Arm。

      2.2.7 配置被动关节(Passive Joints)

      所谓被动关节,就是指现实中不配置电机的关节,也就是不会出现在机器人的Joint State Msg里,以避免MoveIt与JointState出现匹配错误。这里我们的LWR机械臂并没有此类被动关节,所以可以直接跳过。

      2.2.8 生成配置文件(Configuration Files)
      最后一步,在Configuration Package Save Path里面选择一个保存地址,一般我们把他放在path_to_catkin_ws/src/lwr_moveit_config然后点击Generate Package,这样一个完整的MoveIt Configuration Package就创建好了!先不要急着运行,我们先来看看都生成了哪些东西,还有一些重要的配置参数都是在哪定义的。

      三. MoveIt 配置包详解
      打开刚刚创建好的lwr_moveit_config文件夹,我们发现有config和launch两个文件夹。3.1 MoveIt! 配置文件先看config,里面有
      • fake_controllers.yaml:这是虚拟控制器配置文件,方便我们在没有实体机器人,甚至没有任何模拟器(如gazebo)开启的情况下也能运行MoveIt。
      • joint_limits.yaml:这里记录了机器人各个关节的位置速度加速度的极限,这些都会被用于以后的规划中。
      • kinematics.yaml:这里就是上一章2.2.4里面设置的东西,用于初始化运动学求解库
      • lwr.srdf:这个是一个重要的MoveIt配置文件,我们将在下一节详解。
      • ompl_planning.yaml:这里是配置OMPL各种算法的各种参数。
      3.2 SRDF文件
      SRDF是moveit的配置文件,配合URDF使用。打开lwr.srdf,
      我 们可以看到这是一个xml格式的配置文件,根是robot,并有一个属性值name=\'lwr\'。下面各个项目应该很明显,就是我们刚刚在Setup Assistant里面所设置的东西,包含了组群,位姿,终端控制器,虚拟关节,以及碰撞免测矩阵ACM的定义。理论上,只要有了srdf和urdf,我 们就可以完全定义一个机器人moveit信息。

      3.3 Launch文件
      下面,我们看看launch文件夹,一打开发现有很多文件,瞬间不想看了。。不要急,我们来看看几个重要的文件。

      3.3.1 demo.launch
      demo是运行的总结点,打开我们可以看到他include了其他的launch文件。其中第14行说,如果有需 要,发布静态的tf。比如说,你的机器人基座不在世界坐标的原点,你可以发布一个静态tf来描述机器人在世界坐标中的位置。第17-21行,就是我们发布 虚拟机器人状态的地方了,当然,如果你有实体机器人或者有gazebo之类的模拟器,你需要去掉这一部分,有其他相应的节点来发布机器人状态。26-32 行运行了另一个moveit重要的节点,move group。

      3.3.2 move_group.launch
      顾名思义,move group的功能是让一个规划组群动起来。怎么动,那就要做运动规划了,在move_group.launch第24-26行定义了运动规划库的使用,我 们可以看到,默认的是使用ompl运动规划库。同样的,如果以后有时间,我会发帖详解如何创建新的运动规划库插件并让moveit使用其他的运动规划算 法。其他的都是设置一些基本参数,暂时可以略过。

      3.3.3 planning_context.launch
      这里我们可以看到,定义了所使用的urdf和srdf文件,以及运动学求解库。不建议手动更改这些,但是如果你需要使用不同的urdf,srdf,可以在这里更改。

      3.3.4 setup_assistant.launch
      如果你需要更改一些配置,那么可以直接运行

      复制代码
      roslaunch lwr_moveit_config setup_assistant.launch
      这样就可以基于当前设置做更改,而不是重新设置。

      四. 运行MoveIt!

      4.1 Launch Demo
      现在我们可以来尝试运行moveit了!

      复制代码
      roslaunch lwr_moveit_config demo.launch
       
      等待几秒,当看到 All is well! Everyone is happy! You can start planning now! 的时候,就代表启动成功了。我们可以看到一个Rivz窗口,左下角有一个运动规划MotionPlanning模块。

      第一个进入视野的就是Planning Library, OMPL。没错,这里告诉你当前用的是OMPL运动规划算法。在中间的下来菜单里面有很多的具体算法,之后你可以尝试不同的算法,看看他们的区别。
       
      4.2 选择目标位姿
      如果如上文第二章中设置,你会在rviz主窗口中看到一个互动标记位于机械臂终端位置。移动这个标记到另外一个地方,你可以看到一个橙色的目标位姿(每一次移动标记,就运行了一次逆运动学IK求解过程)。
      同样的,你也可以在MotionPlanning模块下的Planning子模块写的Query子模块里面设置随机的或者预设的目标位置。
      4.3 运动规划终于,到了运动规划的时候了。。在Planning子模块中单击

      [ INFO] [1453481861.884163555]: LBKPIECE1: Attempting to use default projection.
      [ INFO] [1453481861.884336258]: LBKPIECE1: Attempting to use default projection.
      [ INFO] [1453481861.884489778]: LBKPIECE1: Starting planning with 1 states already in datastructure
      [ INFO] [1453481861.884523826]: LBKPIECE1: Attempting to use default projection.
      [ INFO] [1453481861.884547702]: LBKPIECE1: Starting planning with 1 states already in datastructure
      [ INFO] [1453481861.884564358]: LBKPIECE1: Attempting to use default projection.
      [ INFO] [1453481861.884587404]: LBKPIECE1: Starting planning with 1 states already in datastructure
      [ INFO] [1453481861.884604829]: LBKPIECE1: Attempting to use default projection.
      [ INFO] [1453481861.884626253]: LBKPIECE1: Starting planning with 1 states already in datastructure
      [ INFO] [1453481861.905034917]: LBKPIECE1: Created 99 (46 start + 53 goal) states in 88 cells (45 start (45 on boundary) + 43 goal (43 on boundary))
      [ INFO] [1453481861.905633020]: LBKPIECE1: Created 87 (33 start + 54 goal) states in 76 cells (31 start (31 on boundary) + 45 goal (45 on boundary))
      [ INFO] [1453481861.913846457]: LBKPIECE1: Created 126 (76 start + 50 goal) states in 115 cells (75 start (75 on boundary) + 40 goal (40 on boundary))
      [ INFO] [1453481861.914639489]: LBKPIECE1: Created 220 (72 start + 148 goal) states in 201 cells (70 start (70 on boundary) + 131 goal (131 on boundary))
      [ INFO] [1453481861.948016518]: ParallelPlan::solve(): Solution found by one or more threads in 0.063719 seconds
      我们可以看到,本次运动规划使用了LBKPIECE算法,并且使用了4线程并行规划,规划时间为1
      (noname)+++++
      * obs1.dae
      1
      mesh
      24 12
      -0.0315993 0.126397 -0.0315993
      -0.0315993 -0.126397 -0.0315993
      -0.0315993 0.126397 0.0315993
      -0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      0.0315993 -0.126397 -0.0315993
      0.0315993 0.126397 -0.0315993
      0.0315993 -0.126397 0.0315993
      -0.0315993 -0.126397 0.0315993
      0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      -0.0315993 0.126397 0.0315993
      0.0315993 0.126397 -0.0315993
      0.0315993 -0.126397 -0.0315993
      -0.0315993 -0.126397 -0.0315993
      -0.0315993 0.126397 -0.0315993
      -0.0315993 -0.126397 0.0315993
      -0.0315993 -0.126397 -0.0315993
      0.0315993 -0.126397 -0.0315993
      0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 -0.0315993
      -0.0315993 0.126397 -0.0315993
      -0.0315993 0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      0 1 2
      1 3 2
      4 5 6
      4 7 5
      8 9 10
      11 8 10
      12 13 14
      12 14 15
      16 17 18
      19 16 18
      20 21 22
      20 22 23
      0.7 0 0.8
      0.706825 0 0 0.707388
      0.5 0 0 1
      * obs2.dae
      1
      mesh
      24 12
      -0.0315993 0.126397 -0.0315993
      -0.0315993 -0.126397 -0.0315993
      -0.0315993 0.126397 0.0315993
      -0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      0.0315993 -0.126397 -0.0315993
      0.0315993 0.126397 -0.0315993
      0.0315993 -0.126397 0.0315993
      -0.0315993 -0.126397 0.0315993
      0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      -0.0315993 0.126397 0.0315993
      0.0315993 0.126397 -0.0315993
      0.0315993 -0.126397 -0.0315993
      -0.0315993 -0.126397 -0.0315993
      -0.0315993 0.126397 -0.0315993
      -0.0315993 -0.126397 0.0315993
      -0.0315993 -0.126397 -0.0315993
      0.0315993 -0.126397 -0.0315993
      0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 -0.0315993
      -0.0315993 0.126397 -0.0315993
      -0.0315993 0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      0 1 2
      1 3 2
      4 5 6
      4 7 5
      8 9 10
      11 8 10
      12 13 14
      12 14 15
      16 17 18
      19 16 18
      20 21 22
      20 22 23
      0.6 0.5 0.8
      0.706825 0 0 0.707388
      0.5 0 0 1
      * obs3.dae
      1
      mesh
      24 12
      -0.0315993 0.126397 -0.0315993
      -0.0315993 -0.126397 -0.0315993
      -0.0315993 0.126397 0.0315993
      -0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      0.0315993 -0.126397 -0.0315993
      0.0315993 0.126397 -0.0315993
      0.0315993 -0.126397 0.0315993
      -0.0315993 -0.126397 0.0315993
      0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      -0.0315993 0.126397 0.0315993
      0.0315993 0.126397 -0.0315993
      0.0315993 -0.126397 -0.0315993
      -0.0315993 -0.126397 -0.0315993
      -0.0315993 0.126397 -0.0315993
      -0.0315993 -0.126397 0.0315993
      -0.0315993 -0.126397 -0.0315993
      0.0315993 -0.126397 -0.0315993
      0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 -0.0315993
      -0.0315993 0.126397 -0.0315993
      -0.0315993 0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      0 1 2
      1 3 2
      4 5 6
      4 7 5
      8 9 10
      11 8 10
      12 13 14
      12 14 15
      16 17 18
      19 16 18
      20 21 22
      20 22 23
      0.3 0 0.8
      0.999784 0 0 0.0207948
      0.5 0 0 1
      * table.dae
      1
      mesh
      24 12
      -0.420619 0.0420619 -0.841237
      -0.420619 -0.0420619 -0.841237
      -0.420619 0.0420619 0.841237
      -0.420619 -0.0420619 0.841237
      0.420619 0.0420619 0.841237
      0.420619 -0.0420619 -0.841237
      0.420619 0.0420619 -0.841237
      0.420619 -0.0420619 0.841237
      -0.420619 0.0420619 0.841237
      -0.420619 -0.0420619 0.841237
      0.420619 -0.0420619 0.841237
      0.420619 0.0420619 0.841237
      0.420619 -0.0420619 -0.841237
      -0.420619 -0.0420619 -0.841237
      -0.420619 0.0420619 -0.841237
      0.420619 0.0420619 -0.841237
      -0.420619 -0.0420619 0.841237
      -0.420619 -0.0420619 -0.841237
      0.420619 -0.0420619 -0.841237
      0.420619 -0.0420619 0.841237
      0.420619 0.0420619 -0.841237
      -0.420619 0.0420619 -0.841237
      -0.420619 0.0420619 0.841237
      0.420619 0.0420619 0.841237
      0 1 2
      1 3 2
      4 5 6
      4 7 5
      8 9 10
      8 10 11
      12 13 14
      15 12 14
      16 17 18
      19 16 18
      20 21 22
      20 22 23
      0.7 0 0.63
      0.706825 0 0 0.707388
      0 0.5 0.5 1
      * target
      1
      mesh
      24 12
      -0.0315993 0.126397 -0.0315993
      -0.0315993 -0.126397 -0.0315993
      -0.0315993 0.126397 0.0315993
      -0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      0.0315993 -0.126397 -0.0315993
      0.0315993 0.126397 -0.0315993
      0.0315993 -0.126397 0.0315993
      -0.0315993 -0.126397 0.0315993
      0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      -0.0315993 0.126397 0.0315993
      0.0315993 0.126397 -0.0315993
      0.0315993 -0.126397 -0.0315993
      -0.0315993 -0.126397 -0.0315993
      -0.0315993 0.126397 -0.0315993
      -0.0315993 -0.126397 0.0315993
      -0.0315993 -0.126397 -0.0315993
      0.0315993 -0.126397 -0.0315993
      0.0315993 -0.126397 0.0315993
      0.0315993 0.126397 -0.0315993
      -0.0315993 0.126397 -0.0315993
      -0.0315993 0.126397 0.0315993
      0.0315993 0.126397 0.0315993
      0 1 2
      1 3 2
      4 5 6
      4 7 5
      8 9 10
      11 8 10
      12 13 14
      12 14 15
      16 17 18
      19 16 18
      20 21 22
      20 22 23
      0.6 0.2 0.8
      0.706825 0 0 0.707388
      0 1 0 1
      .
      然后在SceneObjects模块中点击ImportFromText,选择刚刚创建的demo.scene文件,一个简单的桌面环境就被导入进了rviz。你可以通过选择各个物体,来调整他们的位置。
      回到Context子模块,点击Publish Current Scene,将当前的环境发布出去。
      然后再次点击Plan,你会看到一条不同的轨迹,这一轨迹应该绕过所有障碍物并且达到目标位姿。
       
      因为OMPL是采样算法,由于其随机采样的特性,每次的路径是不同的,如下图。而且有可能失败。