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 版权协议，转载请附上原文出处链接和本声明。
智能推荐
使用nginx解决浏览器跨域问题_nginx不停的xhr-程序员宅基地
文章浏览阅读1k次。通过使用ajax方法跨域请求是浏览器所不允许的，浏览器出于安全考虑是禁止的。警告信息如下：不过jQuery对跨域问题也有解决方案，使用jsonp的方式解决，方法如下：$.ajax({ async:false, url: 'http://www.mysite.com/demo.do', // 跨域URL ty..._nginx不停的xhr
在 Oracle 中配置 extproc 以访问 ST_Geometry-程序员宅基地
文章浏览阅读2k次。关于在 Oracle 中配置 extproc 以访问 ST_Geometry，也就是我们所说的 使用空间SQL 的方法，官方文档链接如下。http://desktop.arcgis.com/zh-cn/arcmap/latest/manage-data/gdbs-in-oracle/configure-oracle-extproc.htm其实简单总结一下，主要就分为以下几个步骤。..._extproc
Linux C++ gbk转为utf-8_linux c++ gbk->utf8-程序员宅基地
文章浏览阅读1.5w次。linux下没有上面的两个函数，需要使用函数 mbstowcs和wcstombsmbstowcs将多字节编码转换为宽字节编码wcstombs将宽字节编码转换为多字节编码这两个函数，转换过程中受到系统编码类型的影响，需要通过设置来设定转换前和转换后的编码类型。通过函数setlocale进行系统编码的设置。linux下输入命名locale -a查看系统支持的编码_linux c++ gbk->utf8
IMP-00009: 导出文件异常结束-程序员宅基地
文章浏览阅读750次。今天准备从生产库向测试库进行数据导入，结果在imp导入的时候遇到“ IMP-00009:导出文件异常结束” 错误，google一下，发现可能有如下原因导致imp的数据太大，没有写buffer和commit两个数据库字符集不同从低版本exp的dmp文件，向高版本imp导出的dmp文件出错传输dmp文件时，文件损坏解决办法：imp时指定..._imp-00009导出文件异常结束
python程序员需要深入掌握的技能_Python用数据说明程序员需要掌握的技能-程序员宅基地
文章浏览阅读143次。当下是一个大数据的时代，各个行业都离不开数据的支持。因此，网络爬虫就应运而生。网络爬虫当下最为火热的是Python，Python开发爬虫相对简单，而且功能库相当完善，力压众多开发语言。本次教程我们爬取前程无忧的招聘信息来分析Python程序员需要掌握那些编程技术。首先在谷歌浏览器打开前程无忧的首页，按F12打开浏览器的开发者工具。浏览器开发者工具是用于捕捉网站的请求信息，通过分析请求信息可以了解请..._初级python程序员能力要求
Spring @Service生成bean名称的规则（当类的名字是以两个或以上的大写字母开头的话，bean的名字会与类名保持一致）_@service beanname-程序员宅基地
文章浏览阅读7.6k次，点赞2次，收藏6次。@Service标注的bean，类名：ABDemoService查看源码后发现，原来是经过一个特殊处理：当类的名字是以两个或以上的大写字母开头的话，bean的名字会与类名保持一致public class AnnotationBeanNameGenerator implements BeanNameGenerator { private static final String C..._@service beanname
随便推点
二叉树的各种创建方法_二叉树的建立-程序员宅基地
文章浏览阅读6.9w次，点赞73次，收藏463次。1.前序创建#include<stdio.h>#include<string.h>#include<stdlib.h>#include<malloc.h>#include<iostream>#include<stack>#include<queue>using namespace std;typed_二叉树的建立
解决asp.net导出excel时中文文件名乱码_asp.net utf8 导出中文字符乱码-程序员宅基地
文章浏览阅读7.1k次。在Asp.net上使用Excel导出功能，如果文件名出现中文，便会以乱码视之。 解决方法： fileName = HttpUtility.UrlEncode(fileName, System.Text.Encoding.UTF8);_asp.net utf8 导出中文字符乱码
笔记-编译原理-实验一-词法分析器设计_对pl/0作以下修改扩充。增加单词-程序员宅基地
文章浏览阅读2.1k次，点赞4次，收藏23次。第一次实验 词法分析实验报告设计思想词法分析的主要任务是根据文法的词汇表以及对应约定的编码进行一定的识别，找出文件中所有的合法的单词，并给出一定的信息作为最后的结果，用于后续语法分析程序的使用；本实验针对 PL/0 语言 的文法、词汇表编写一个词法分析程序，对于每个单词根据词汇表输出： (单词种类, 单词的值) 二元对。词汇表：种别编码单词符号助记符0beginb..._对pl/0作以下修改扩充。增加单词
android adb shell 权限,android adb shell权限被拒绝-程序员宅基地
文章浏览阅读773次。我在使用adb.exe时遇到了麻烦.我想使用与bash相同的adb.exe shell提示符,所以我决定更改默认的bash二进制文件(当然二进制文件是交叉编译的,一切都很完美)更改bash二进制文件遵循以下顺序> adb remount> adb push bash / system / bin /> adb shell> cd / system / bin> chm..._adb shell mv 权限
投影仪-相机标定_相机-投影仪标定-程序员宅基地
文章浏览阅读6.8k次，点赞12次，收藏125次。1. 单目相机标定引言相机标定已经研究多年，标定的算法可以分为基于摄影测量的标定和自标定。其中，应用最为广泛的还是张正友标定法。这是一种简单灵活、高鲁棒性、低成本的相机标定算法。仅需要一台相机和一块平面标定板构建相机标定系统，在标定过程中，相机拍摄多个角度下（至少两个角度，推荐10~20个角度）的标定板图像（相机和标定板都可以移动），即可对相机的内外参数进行标定。下面介绍张氏标定法（以下也这么称呼）的原理。原理相机模型和单应矩阵相机标定，就是对相机的内外参数进行计算的过程，从而得到物体到图像的投影_相机-投影仪标定
Wayland架构、渲染、硬件支持-程序员宅基地
文章浏览阅读2.2k次。文章目录Wayland 架构Wayland 渲染Wayland的 硬件支持简 述： 翻译一篇关于和 wayland 有关的技术文章, 其英文标题为Wayland Architecture .Wayland 架构若是想要更好的理解 Wayland 架构及其与 X (X11 or X Window System) 结构；一种很好的方法是将事件从输入设备就开始跟踪, 查看期间所有的屏幕上出现的变化。这就是我们现在对 X 的理解。 内核是从一个输入设备中获取一个事件，并通过 evdev 输入_wayland