ROS 运动规划 (Motion Planning): MoveIt! 与 OMPL_rviz motioplanning plan频率-程序员宅基地
技术标签： ROS
最近有不少人询问有关MoveIt!与OMPL相关的话题，但是大部分问题都集中于XXX功能怎么实现，XXX错误怎么解决。表面上看，解决这些问题的方法就是提供正确的代码，正确的编译方法，正确的运行步骤。
然而，这种解决方法只能解决这个特定的问题，而且解决之后我们也无法学到一些实际的东西。要想彻底明白，需要从源头入手，也就是说， 不要问“MoveIt! 怎么把机械手从空间一个点移到另一个点？“ ， 而是要问”MoveIt! 为什么能把机械手从空间一个点移到另一个点？“ 。 这一点明白之后，遇到类似的问题，才能从容应对。同理，这不仅适用于MoveIt，也同样适用于其他任何ROS功能。
所以，下文中我们会见到一些具体的例子，但整体上，更倾向于宏观的概念和一些基础的方法，希望对大家能有所帮助。这里的帮助指的是增强对运动规划和Moveit, OMPL的整体理解，而非局限于完成某一个功能，编译运行某一个文件。
我尝试用最简单，最通俗的表达方式来解释这些问题，其中不免会有一些学术上的错误用词和解释，请专业的朋友们见谅，也欢迎指出错误。
一. 基础概念
首先，我们要了解一些基础的概念，了解各个名词的意义和区别。
1.1. 运动规划 (Motion Planning)
我们这里讲的 运动规划 ，有别于 轨迹规划 (Path Planning)。一般来说，path planning用于无人车/无人机领域，而motion planning主要用于机械臂，类人机器人领域。当然了，这两者没有本质的区别，理论上说MoveIt!和OMPL同样可以用于无人车无人机的规划，但不免有些杀鸡用牛刀的感觉。两者规划的空间维度不同，导致他们的难易程度不同。举例说明，如果不考虑速度加速度，只考虑位置的话，无人车轨迹规划维度是3 (x,y,和角度), 无人机是6 (x,y,z,和另外3个量确定空间的旋转角度)。确定3D空间的一个姿势(pose)需要6个变量，而对于关节数大于6的机械臂结构，它的规划空间维度就大于6，成为冗余系统(redundant system)，从而使规划问题变得更为复杂。所谓冗余系统，就是说，存在多种关节角度配置能够使得终端达到相同的位姿，存在无数的解。这是达到的最终姿势有无数个解，那么如何到达这个最终姿势，整个运动的轨迹，更是存在无数个解。
既然存在无数的解，那么问题来了。很明显，存在两种不同的方向，一种是找到最好的那个解，另一种是快速的找到一个有效的解。前者，大部分算法使用最优规划 (Optimization-based Planning)，后者使用采样规划 (Sampling-based Planning)。具体的区别和算法，不在这里赘述。
1.2. 开源运动规划库 (OMPL).
接上文，而OMPL (Open Motion Planning Library)， 开源运动规划库，就是一个运动规划的C++库，其包含了很多运动规划领域的前沿算法。虽然OMPL里面提到了最优规划，但 总体来说OMPL还是一个采样规划算法库 。而采样规划算法中，最出名的莫过于 Rapidly-exploring Random Trees ( RRT ) 和 Probabilistic Roadmap ( PRM )了, 当然，这两个是比较老的，还有很多 其他 新算法。
1.3. 逆运动学 (Inverse Kinematics)
1.4. MoveIt!
问：我不想看也看不懂OMPL和各种算法，但是我想让机械臂动起来，怎么办？
答：那这正是MoveIt!的设计初衷。Move It！让它动起来！
OMPL是运动规划的“规划”部分，而MoveIt!是OMPL的ROS接口。当然这不完全准确，OMPL有单独的ROS接口，但依旧很繁杂，而MoveIt是OMPL ROS接口的接口。。。而且MoveIt!还结合了其他一些功能，总之MoveIt!就是个大接口。。
2.1 准备URDF package
首先我们要准备一个机械臂的urdf，如果你已有URDF，可以使用自己的urdf模型。若手头没有现成的URDF，可以从 此处 下载一个库卡LWR简化模型URDF，这是一个固定底座7自由度的机械臂。
从该连接处依次进入examples/sovlers/ik_solver_demo/resources，下载里面的lwr_simplified.urdf。
复制代码
将下载好的lwr_simplified.urdf放入urdf文件夹中，这样一个urdf package便创建好了。
2.2 MoveIt!配置助手 (MoveIt! Setup Assistant)
2.2.1 打开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里面，我们要创建所谓的虚拟关节。这个虚拟关节，可以理解为一个连接机器人和世界的关节。
2.2.5 创建机器人预设位姿 (Robot Poses)
2.2.6 配置终端控制器(End Effectors)
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，里面有
SRDF是moveit的配置文件，配合URDF使用。打开lwr.srdf，
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
如果你需要更改一些配置，那么可以直接运行
复制代码
这样就可以基于当前设置做更改，而不是重新设置。
四. 运行MoveIt!
4.1 Launch Demo
现在我们可以来尝试运行moveit了！
复制代码
等待几秒，当看到
All is well! Everyone is happy! You can start planning now!
的时候，就代表启动成功了。我们可以看到一个Rivz窗口，左下角有一个运动规划MotionPlanning模块。
4.3 运动规划 终于，到了运动规划的时候了。。在Planning子模块中单击 Plan ，一个运动轨迹就会出现与Rviz窗口中并循环播放。你可以在Display->MotionPlanning->Planned Path里面设置各种显示参数。
复制代码
我们可以看到，本次运动规划使用了LBKPIECE算法，并且使用了4线程并行规划，规划时间为
0.063719秒。你也可以在OMPL算法里选择其他规划算法比如RRT，RRT-Connect，PRM，EST等等等等。。看看他们之间的轨迹区别和速度区别。
复制代码
接下来可以尝试不同的OMPL算法，不同的目标位姿和不同的环境。来看看MoveIt的鲁棒性如何。
那么，MoveIt!和OMPL的运动规划就差不多讲完了，当然这是很浅显的，与实用性的东西都还有距离。 以上都是纯手打，现做的例子，希望有所帮助。接下来该讲什么，你们可以在下面留言，是更深入的讲MoveIt!，还是讲OMPL运动规划算法，还是讲如何模拟具体实例。
然而，这种解决方法只能解决这个特定的问题，而且解决之后我们也无法学到一些实际的东西。要想彻底明白，需要从源头入手，也就是说， 不要问“MoveIt! 怎么把机械手从空间一个点移到另一个点？“ ， 而是要问”MoveIt! 为什么能把机械手从空间一个点移到另一个点？“ 。 这一点明白之后，遇到类似的问题，才能从容应对。同理，这不仅适用于MoveIt，也同样适用于其他任何ROS功能。
所以，下文中我们会见到一些具体的例子，但整体上，更倾向于宏观的概念和一些基础的方法，希望对大家能有所帮助。这里的帮助指的是增强对运动规划和Moveit, OMPL的整体理解，而非局限于完成某一个功能，编译运行某一个文件。
我尝试用最简单，最通俗的表达方式来解释这些问题，其中不免会有一些学术上的错误用词和解释，请专业的朋友们见谅，也欢迎指出错误。
一. 基础概念
首先，我们要了解一些基础的概念，了解各个名词的意义和区别。
1.1. 运动规划 (Motion Planning)
我们这里讲的 运动规划 ，有别于 轨迹规划 (Path Planning)。一般来说，path planning用于无人车/无人机领域，而motion planning主要用于机械臂，类人机器人领域。当然了，这两者没有本质的区别，理论上说MoveIt!和OMPL同样可以用于无人车无人机的规划，但不免有些杀鸡用牛刀的感觉。两者规划的空间维度不同，导致他们的难易程度不同。举例说明，如果不考虑速度加速度，只考虑位置的话，无人车轨迹规划维度是3 (x,y,和角度), 无人机是6 (x,y,z,和另外3个量确定空间的旋转角度)。确定3D空间的一个姿势(pose)需要6个变量，而对于关节数大于6的机械臂结构，它的规划空间维度就大于6，成为冗余系统(redundant system)，从而使规划问题变得更为复杂。所谓冗余系统，就是说，存在多种关节角度配置能够使得终端达到相同的位姿，存在无数的解。这是达到的最终姿势有无数个解，那么如何到达这个最终姿势，整个运动的轨迹，更是存在无数个解。
既然存在无数的解，那么问题来了。很明显，存在两种不同的方向，一种是找到最好的那个解，另一种是快速的找到一个有效的解。前者，大部分算法使用最优规划 (Optimization-based Planning)，后者使用采样规划 (Sampling-based Planning)。具体的区别和算法，不在这里赘述。
1.2. 开源运动规划库 (OMPL).
接上文，而OMPL (Open Motion Planning Library)， 开源运动规划库，就是一个运动规划的C++库，其包含了很多运动规划领域的前沿算法。虽然OMPL里面提到了最优规划，但 总体来说OMPL还是一个采样规划算法库 。而采样规划算法中，最出名的莫过于 Rapidly-exploring Random Trees ( 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是关节角度。
- 为什么要用IK？OMPL是采样算法，也就是要在关节空间采样。 这与无人车的规划有一个最明显的区别，无人车的目标就是在采样空间， e.g. 目标是(x,y), 采样空间也是(x,y). 但是对于机械臂，目标是终端空间位置(xyz), 但采样空间却是关节空间(q0,q1,...qN)。有了IK之后，我们就可以把三维空间的目标p转化为关节空间的目标q。那么这样就会让采样算法能算的更快，具体方法不赘述，这样的算法有RRT-Connect，BKPIECE等等双向采样算法。
1.4. MoveIt!
问：我不想看也看不懂OMPL和各种算法，但是我想让机械臂动起来，怎么办？
答：那这正是MoveIt!的设计初衷。Move It！让它动起来！
OMPL是运动规划的“规划”部分，而MoveIt!是OMPL的ROS接口。当然这不完全准确，OMPL有单独的ROS接口，但依旧很繁杂，而MoveIt是OMPL ROS接口的接口。。。而且MoveIt!还结合了其他一些功能，总之MoveIt!就是个大接口。。
- MoveIt!能做什么？一句话，MoveIt!就是一个模块化的接口，让你在最短时间内，不用自己写太多代码，就能配置出一个ROS Package来为你的机械臂做运动规划。
2.1 准备URDF package
首先我们要准备一个机械臂的urdf，如果你已有URDF，可以使用自己的urdf模型。若手头没有现成的URDF，可以从 此处 下载一个库卡LWR简化模型URDF，这是一个固定底座7自由度的机械臂。
从该连接处依次进入examples/sovlers/ik_solver_demo/resources，下载里面的lwr_simplified.urdf。
复制代码
|
1
2
3
4
|
cd path_to_catkin_ws/src
catkin_create_pkg lwr_description
cd lwr_description
mkdir urdf
|
2.2 MoveIt!配置助手 (MoveIt! Setup Assistant)
2.2.1 打开MoveIt! Setup Assistant
复制代码
|
1
|
roslaunch moveit_setup_assistant setup_assistant.launch
|
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或者<span \"="" style="box-sizing: border-box;">IK_FAST，甚至你可以自己开发新的Solver然后插入进来，如果有空，我以后会发帖讲解如何创建新的运动学求解库并插入到MoveIt。
- Kin. Search Resolution: 关节空间的采样密度
- Kin. Search TImeout: 求解时间
- Kin. Solver Attempts: 求解失败尝试次数，一般来说这三项使用默认值就可以。你也可以根据具体需要做出适当调整。
在正中方我们可以看到这个机械臂的结构，一个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各种算法的各种参数。
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
如果你需要更改一些配置，那么可以直接运行
复制代码
|
1
|
roslaunch lwr_moveit_config setup_assistant.launch
|
四. 运行MoveIt!
4.1 Launch Demo
现在我们可以来尝试运行moveit了！
复制代码
|
1
|
roslaunch lwr_moveit_config demo.launch
|
第一个进入视野的就是Planning Library, OMPL。没错，这里告诉你当前用的是OMPL运动规划算法。在中间的下来菜单里面有很多的具体算法，之后你可以尝试不同的算法，看看他们的区别。
4.2 选择目标位姿
如果如上文第二章中设置，你会在rviz主窗口中看到一个互动标记位于机械臂终端位置。移动这个标记到另外一个地方，你可以看到一个橙色的目标位姿(
每一次移动标记，就运行了一次逆运动学IK求解过程)。
同样的，你也可以在MotionPlanning模块下的Planning子模块写的Query子模块里面设置随机的或者预设的目标位置。
4.3 运动规划 终于，到了运动规划的时候了。。在Planning子模块中单击 Plan ，一个运动轨迹就会出现与Rviz窗口中并循环播放。你可以在Display->MotionPlanning->Planned Path里面设置各种显示参数。
同时，在terminal里面我们可以看到一些输出
复制代码
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
|
[ 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
|
4.4 避障运动规划 Collision Free Motion Planning
上面这个简单的运动规划，环境中并没有障碍物。下面我们来为环境中增加几个物体。创建一个demo.scene文件，将以下代码复制进去（你可以自己设计不同的scene）
复制代码
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
|
(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是采样算法，由于其随机采样的特性，每次的路径是不同的，如下图。而且有可能失败。
那么，MoveIt!和OMPL的运动规划就差不多讲完了，当然这是很浅显的，与实用性的东西都还有距离。 以上都是纯手打，现做的例子，希望有所帮助。接下来该讲什么，你们可以在下面留言，是更深入的讲MoveIt!，还是讲OMPL运动规划算法，还是讲如何模拟具体实例。
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
智能推荐
分布式光纤传感器的全球与中国市场2022-2028年：技术、参与者、趋势、市场规模及占有率研究报告_预计2026年中国分布式传感器市场规模有多大-程序员宅基地
文章浏览阅读3.2k次。本文研究全球与中国市场分布式光纤传感器的发展现状及未来发展趋势，分别从生产和消费的角度分析分布式光纤传感器的主要生产地区、主要消费地区以及主要的生产商。重点分析全球与中国市场的主要厂商产品特点、产品规格、不同规格产品的价格、产量、产值及全球和中国市场主要生产商的市场份额。主要生产商包括：FISO TechnologiesBrugg KabelSensor HighwayOmnisensAFL GlobalQinetiQ GroupLockheed MartinOSENSA Innovati_预计2026年中国分布式传感器市场规模有多大
07_08 常用组合逻辑电路结构——为IC设计的延时估计铺垫_基4布斯算法代码-程序员宅基地
文章浏览阅读1.1k次，点赞2次，收藏12次。常用组合逻辑电路结构——为IC设计的延时估计铺垫学习目的：估计模块间的delay，确保写的代码的timing 综合能给到多少HZ，以满足需求！_基4布斯算法代码
OpenAI Manager助手（基于SpringBoot和Vue）_chatgpt网页版-程序员宅基地
文章浏览阅读3.3k次，点赞3次，收藏5次。OpenAI Manager助手（基于SpringBoot和Vue）_chatgpt网页版
关于美国计算机奥赛USACO，你想知道的都在这_usaco可以多次提交吗-程序员宅基地
文章浏览阅读2.2k次。USACO自1992年举办，到目前为止已经举办了27届，目的是为了帮助美国信息学国家队选拔IOI的队员，目前逐渐发展为全球热门的线上赛事，成为美国大学申请条件下，含金量相当高的官方竞赛。USACO的比赛成绩可以助力计算机专业留学，越来越多的学生进入了康奈尔，麻省理工，普林斯顿，哈佛和耶鲁等大学，这些同学的共同点是他们都参加了美国计算机科学竞赛（USACO)，并且取得过非常好的成绩。适合参赛人群USACO适合国内在读学生有意向申请美国大学的或者想锻炼自己编程能力的同学，高三学生也可以参加12月的第_usaco可以多次提交吗
MySQL存储过程和自定义函数_mysql自定义函数和存储过程-程序员宅基地
文章浏览阅读394次。1.1 存储程序1.2 创建存储过程1.3 创建自定义函数1.3.1 示例1.4 自定义函数和存储过程的区别1.5 变量的使用1.6 定义条件和处理程序1.6.1 定义条件1.6.1.1 示例1.6.2 定义处理程序1.6.2.1 示例1.7 光标的使用1.7.1 声明光标1.7.2 打开光标1.7.3 使用光标1.7.4 关闭光标1.8 流程控制的使用1.8.1 IF语句1.8.2 CASE语句1.8.3 LOOP语句1.8.4 LEAVE语句1.8.5 ITERATE语句1.8.6 REPEAT语句。_mysql自定义函数和存储过程
半导体基础知识与PN结_本征半导体电流为0-程序员宅基地
文章浏览阅读188次。半导体二极管——集成电路最小组成单元。_本征半导体电流为0
随便推点
【Unity3d Shader】水面和岩浆效果_unity 岩浆shader-程序员宅基地
文章浏览阅读2.8k次，点赞3次，收藏18次。游戏水面特效实现方式太多。咱们这边介绍的是一最简单的UV动画（无顶点位移），整个mesh由4个顶点构成。实现了水面效果（左图），不动代码稍微修改下参数和贴图可以实现岩浆效果（右图）。有要思路是1，uv按时间去做正弦波移动2，在1的基础上加个凹凸图混合uv3，在1、2的基础上加个水流方向4，加上对雾效的支持，如没必要请自行删除雾效代码(把包含fog的几行代码删除)S..._unity 岩浆shader
广义线性模型——Logistic回归模型（1）_广义线性回归模型-程序员宅基地
文章浏览阅读5k次。广义线性模型是线性模型的扩展，它通过连接函数建立响应变量的数学期望值与线性组合的预测变量之间的关系。广义线性模型拟合的形式为：其中g(μY)是条件均值的函数（称为连接函数）。另外，你可放松Y为正态分布的假设，改为Y 服从指数分布族中的一种分布即可。设定好连接函数和概率分布后，便可以通过最大似然估计的多次迭代推导出各参数值。在大部分情况下，线性模型就可以通过一系列连续型或类别型预测变量来预测正态分布的响应变量的工作。但是，有时候我们要进行非正态因变量的分析，例如：（1）类别型.._广义线性回归模型
HTML+CSS大作业 环境网页设计与实现(垃圾分类) web前端开发技术 web课程设计 网页规划与设计_垃圾分类网页设计目标怎么写-程序员宅基地
文章浏览阅读69次。环境保护、 保护地球、 校园环保、垃圾分类、绿色家园、等网站的设计与制作。 总结了一些学生网页制作的经验：一般的网页需要融入以下知识点：div+css布局、浮动、定位、高级css、表格、表单及验证、js轮播图、音频 视频 Flash的应用、ul li、下拉导航栏、鼠标划过效果等知识点，网页的风格主题也很全面：如爱好、风景、校园、美食、动漫、游戏、咖啡、音乐、家乡、电影、名人、商城以及个人主页等主题，学生、新手可参考下方页面的布局和设计和HTML源码（有用点赞△） 一套A+的网_垃圾分类网页设计目标怎么写
C# .Net 发布后,把dll全部放在一个文件夹中,让软件目录更整洁_.net dll 全局目录-程序员宅基地
文章浏览阅读614次，点赞7次，收藏11次。之前找到一个修改 exe 中 DLL地址 的方法, 不太好使,虽然能正确启动, 但无法改变 exe 的工作目录,这就影响了.Net 中很多获取 exe 执行目录来拼接的地址 ( 相对路径 ),比如 wwwroot 和 代码中相对目录还有一些复制到目录的普通文件 等等,它们的地址都会指向原来 exe 的目录, 而不是自定义的 “lib” 目录,根本原因就是没有修改 exe 的工作目录这次来搞一个启动程序,把 .net 的所有东西都放在一个文件夹,在文件夹同级的目录制作一个 exe._.net dll 全局目录
BRIEF特征点描述算法_breif description calculation 特征点-程序员宅基地
文章浏览阅读1.5k次。本文为转载，原博客地址：http://blog.csdn.net/hujingshuang/article/details/46910259简介 BRIEF是2010年的一篇名为《BRIEF:Binary Robust Independent Elementary Features》的文章中提出，BRIEF是对已检测到的特征点进行描述，它是一种二进制编码的描述子，摈弃了利用区域灰度..._breif description calculation 特征点
房屋租赁管理系统的设计和实现，SpringBoot计算机毕业设计论文_基于spring boot的房屋租赁系统论文-程序员宅基地
文章浏览阅读4.1k次，点赞21次，收藏79次。本文是《基于SpringBoot的房屋租赁管理系统》的配套原创说明文档，可以给应届毕业生提供格式撰写参考，也可以给开发类似系统的朋友们提供功能业务设计思路。_基于spring boot的房屋租赁系统论文