投稿
  1. 首页
  2. 随笔

运动规划

code • 2022-4-12 • 随笔 • 阅读 116

运动规划 ,第1张

运动规划 (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 是一个图形界面，可以让我们不用写代码看代码，直接用鼠标点击就可以配置机器人的运动规划所需要的信息。


      点击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  states already in datastructure
      
[ INFO] [1453481861.884523826]: LBKPIECE1: Attempting to use default projection.
      
[ INFO] [1453481861.884547702]: LBKPIECE1: Starting planning with  states already in datastructure
      
[ INFO] [1453481861.884564358]: LBKPIECE1: Attempting to use default projection.
      
[ INFO] [1453481861.884587404]: LBKPIECE1: Starting planning with  states already in datastructure
      
[ INFO] [1453481861.884604829]: LBKPIECE1: Attempting to use default projection.
      
[ INFO] [1453481861.884626253]: LBKPIECE1: Starting planning with  states already in datastructure
      
[ INFO] [1453481861.905034917]: LBKPIECE1: Created  ( start +  goal) states in  cells ( start ( on boundary) +  goal ( on boundary))
      
[ INFO] [1453481861.905633020]: LBKPIECE1: Created  ( start +  goal) states in  cells ( start ( on boundary) +  goal ( on boundary))
      
[ INFO] [1453481861.913846457]: LBKPIECE1: Created  ( start +  goal) states in  cells ( start ( on boundary) +  goal ( on boundary))
      
[ INFO] [1453481861.914639489]: LBKPIECE1: Created  ( start +  goal) states in  cells ( start ( on boundary) +  goal ( on boundary))
      
[ INFO] [1453481861.948016518]: ParallelPlan::solve(): Solution found by one or more threads in 0.063719 seconds

      我们可以看到，本次运动规划使用了LBKPIECE算法，并且使用了4线程并行规划，规划时间为1

      (noname)+++++
      
* obs1.dae

mesh

-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.7  0.8
      
0.706825   0.707388
      
0.5
      
* obs2.dae

mesh

-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.6 0.5 0.8
      
0.706825   0.707388
      
0.5
      
* obs3.dae

mesh

-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.3  0.8
      
0.999784   0.0207948
      
0.5
      
* table.dae

mesh

-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.7  0.63
      
0.706825   0.707388
      
 0.5 0.5
      
* target

mesh

-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.6 0.2 0.8
      
0.706825   0.707388

.
      然后在SceneObjects模块中点击ImportFromText，选择刚刚创建的demo.scene文件，一个简单的桌面环境就被导入进了rviz。


      你可以通过选择各个物体，来调整他们的位置。


      回到Context子模块，点击Publish Current Scene，将当前的环境发布出去。


      然后再次点击Plan，你会看到一条不同的轨迹，这一轨迹应该绕过所有障碍物并且达到目标位姿。


       

      因为OMPL是采样算法，由于其随机采样的特性，每次的路径是不同的，如下图。


      而且有可能失败。


       

      接下来可以尝试不同的OMPL算法，不同的目标位姿和不同的环境。


      来看看MoveIt的鲁棒性如何。


      那么，MoveIt!和OMPL的运动规划就差不多讲完了，当然这是很浅显的，与实用性的东西都还有距离。


      以上都是纯手打，现做的例子，希望有所帮助。


      接下来该讲什么，你们可以在下面留言，是更深入的讲MoveIt!，还是讲OMPL运动规划算法，还是讲如何模拟具体实例。


欢迎分享，转载请注明来源：内存溢出

原文地址: https://outofmemory.cn/zaji/587408.html

赞 (0)
打赏 微信扫一扫 微信扫一扫 支付宝扫一扫 支付宝扫一扫
code code 管理员组
0 0
上一篇 2022-04-12
下一篇 2022-04-12

发表评论

登录后才能评论

评论列表（0条）

保存