【OMPL】OMPL和Moveit的接口

---

作为入门者总是不禁去问，ompl作为一个独立于moveit的算法库究竟是如何调用的？本篇博文的目的就是梳理清楚它，其中主要关注的点：

• 碰撞检测
• 路径规划
• 适配器

---

参考

• official source code and api
• moveit官网-ompl-interface
• Plugin Interfaces
• Source Code
• Concepts
• MoveIt Related Projects
• ompl_moveit_api docs
• OMPL学习–第一篇之简介

---

也有比较好的翻译可以见

OMPL planner

开放运动规划库是最先进的基于采样的运动规划算法的强大集合，是 MoveIt 中的默认规划器。有关详细信息，请参阅项目网页。

OMPL Settings

在这里，我们回顾 OMPL 的重要配置设置。这些设置通常可以在机器人和ompl_planning中的 moveit_config.

最长有效段分数

该longest_valid_segment_fraction定义了用于碰撞检查的机器人运动的离散化，极大地影响了基于OMPL的解决方案的性能和可靠性。此上下文中的运动可视为图形中两个节点之间的边，其中节点是沿轨迹的航点。OMPL 中的默认运动碰撞检查器只是将边分到多个子状态以进行碰撞检查。OMPL/MoveIt 中当前没有连续碰撞检查，尽管这是当前讨论的一个领域。

具体来说，longest_valid_segment_fraction是机器人状态空间的一小部分，鉴于机器人目前未处于碰撞状态，我们假设机器人可以在保持无碰撞状态的同时移动。例如，如果 longest_valid_segment_fraction = 0.01，则假设如果两个节点之间的边小于状态空间的 1/100，则我们不需要显式检查沿该边缘的任何子状态，只需检查它连接的两个节点。

除了 longest_valid_segment_fraction.yaml 文件中的 ompl_planning 参数外，动态maximum_waypoint_distance文件中也存在该参数。maximum_waypoint_distance定义相同的机器人运动的离散化，用于碰撞检查，但它在绝对级别上这样做，而不是使用状态空间的分数。例如，如果 maximum_waypoint_distance = 0.1，则如果状态空间距离中的边小于 0.1，则我们不会显式检查沿该边缘的任何子状态。

如果longest_valid_segment_fraction maximum_waypoint_distance，则选择生成最保守的离散化的变量（将生成对给定边缘进行碰撞检查最多的状态的变量）。

设置longest_valid_segment_fraction（或maximum_waypoint_distance）过低，碰撞检查/运动计划将非常缓慢。设置过高，小对象或窄对象周围将错过碰撞。此外，高碰撞检查分辨率将导致路径平滑器输出难以理解的运动，因为它们能够"捕获"无效路径，然后尝试通过采样来修复它们，但并不完美。

此处记录了此参数对两个 MoveIt 教程示例的影响的快速分析。

投影评估器

系统projection_evaluator可以包含关节或链接的列表，以近似配置空间的覆盖范围。此设置由规划人员使用，如 KPIECE、BKPIECE、LBKPIECE 和 PDST。有关详细信息，请阅读相应的出版物。

在关节空间实施规划(Enforce Planning in Joint Space)

什么是拒绝采样

根据规划问题 MoveIt 在关节空间和笛卡尔空间之间选择问题表示。将组参数enforce_joint_model_state_space强制所有计划使用联合空间。
默认情况下，具有方向路径约束的规划请求在笛卡尔空间中采样，以便调用 IK 用作生成采样器。
通过强制实施联合空间，规划过程将使用拒绝采样来查找有效的请求。请不要说这可能会大大增加规划时间。

其他设置

根据您使用的规划器，其他设置可用于调优/参数扫描。这些设置的默认值在 MoveIt 设置助手中自动生成，并列在 ompl_planning.yaml 文件中 - 我们鼓励您调整它们。

Trade-offs in speed and optimality

OMPL 中的许多规划者（包括默认规划者）倾向于查找解决方案路径的速度，而不是路径质量。在后处理阶段，可平滑和缩短可行路径，以获得更接近最佳的路径。但是，不能保证找到全局最优或每次找到相同的解决方案，因为 OMPL 中的算法是概率的。其他库（如基于搜索的规划库 （SBPL））提供确定性结果，因为给定相同的环境、开始和目标，您始终会得到相同的路径。SBPL 基于 A，因此您将获得到所选搜索分辨率的最佳结果。但是，SBPL 也有缺点，例如难以以适当的分辨率定义状态空间格子（例如，如何定义关节角度或结束效应器姿势的离散化？*

OMPL 中有几个规划者可以给出理论上的优化性保证，但通常只是无症状的：它们收敛到最优解决方案，但收敛速度可能很慢。这些规划者使用的优化目标通常是路径长度最小化，但也可以使用其他优化目标。

OMPL 优化目标

作为 OMPL 规划库一部分的多个规划人员能够针对指定的优化目标进行优化。本教程介绍配置这些目标所需的步骤。目前暴露于 MoveIt 的无症状（接近）最佳规划者是：

RRT*
PRM*
LazyPRM*
BFMT
FMT
Lower Bound Tree RRT (LBTRRT)
SPARS
SPARS2
Transition-based RRT (T-RRT)

OMPL 还提供一种名为"随时路径测量"的元优化算法，该算法可并行运行多个规划器，并采用路径快捷方式和路径杂交，这两种技术可局部优化解决方案路径。虽然尚未证明是最佳的，但在实践中，获得近乎最佳的解决方案路径通常是一种有效的策略。

OMPL 中的其他最佳规划者，但尚未在 MoveIt 中公开:

RRT#
RRTX
Informed RRT*
Batch Informed Trees (BIT*)
Sparse Stable RRT
CForest

以下优化目标可用： 这里翻译的是notic的，kinetic见here

PathLengthOptimizationObjective (Default) 最短路径规划
MechanicalWorkOptimizationObjective
MaximizeMinClearanceObjective
StateCostIntegralObjective
MinimaxObjective

这些优化目标的配置可以在 ompl_planning. yaml 中完成。名称为名称optimization_objective添加为配置参数。参数的值设置为所选优化目标的名称。例如，要将 RRTstar 配置为使用最大化明清除对象，ompl_planning.yaml 中的规划器条目将看起来像：

RRTstarkConfigDefault:
    type: geometric::RRTstar
    optimization_objective: MaximizeMinClearanceObjective
    range: 0.0
    goal_bias: 0.05
    delay_collision_checking: 1

其他优化目标可以以编程方式定义。有关 OMPL 最佳规划器的详细信息，请参阅 OMPL - 最佳规划文档。

OMPL 规划器终止条件

OMPL 中的规划器通常在超过给定时间限制时终止。但是，可以通过"ompl_planning.yaml"参数在 ompl_planning.yaml 中指定每个规划器termination_condition条件。可能的值包括：

迭代[num]：在 num 迭代后终止。在这里，num 应替换为正整数。
成本协议[解决方案窗口，epsilon]：在成本（优化目标指定）收敛后终止。参数解决方案窗口指定用于确定规划器是否收敛的最小解决方案数。参数 epsilon 是要考虑收敛的阈值。这应该是接近 0 的正数。如果发现新的更好的解决方案后，累积移动平均线没有相对 epsilon 的分数变化，则已达到收敛。此终止条件仅在 OMPL 1.5.0 和更新版本中可用。
精确解决方案：一旦找到确切的解决方案或发生超时，就终止。这修改了随时/优化规划者的行为，在发现第一个可行的解决方案后终止。

在所有情况下，当满足用户指定的终止条件或已达到超时指定的时间限制（以先发生者为准）时，计划程序将终止。
例如，要指定 RRTstar 应在收敛时终止，可以使用以下设置：

RRTstarkConfigDefault:
    type: geometric::RRTstar
    termination_condition: CostConvergence[10,.1]
    range: 0.0
    goal_bias: 0.05
    delay_collision_checking: 1

请注意，未指定优化目标，因此将使用默认的优化目标 PathLength 优化对象

后处理平滑

请注意，有多少平滑可以帮助减少间接路线是有限制的。还要注意，这里我们讨论仅基于几何（运动学）的平滑。速度/加速度/加速度率平滑在其他地方处理，请参见时间参数化。

您可以通过增加计划时间来调整MoveIt在平滑上花费的时间。找到初始计划之后但在allowed_planning_time用尽之前的所有剩余时间将用于平滑。 MoveIt还进行路径混合，将N个不同计划运行的最佳部分拼接在一起。因此，num_planning_attempts也影响质量。

尽管当前尚未在MoveIt（TODO）的最高级别公开，但可以通过在model_based_planning_context.cpp(在接口文件中)中将简化持续时间设置为0（无限制）来实现更平滑的处理。这将启用OMPL的**simplifyMax() **函数。

• 以上三个地方的修改方式

• 这个方式没有用源码安装不好修改因此暂时不用 simplifyMax()
  给定路径，尝试在保持路径有效的同时从其删除顶点。 然后，尝试使路径平滑。 此功能将相同的默认操作集应用于路径，但非度量空间除外，目的是简化路径。 在非度量空间中，某些操作被跳过，因为当三角不等式可能不成立时，它们无法正常工作。 如果简化的路径无效，则返回false。

• moveit::planning_interface::MoveGroupInterface 找到另外两个接口.
  使用后发现,实际效果随机且时间代价高,这个问题需要再上网搜一下!
  相似问题发生在MoveIt! default planner often goes wild and Optimization Objective has no effect
  除了内部OMPL平滑器之外，最近还做出了一些努力来进行STOMP / CHOMP的后处理。请参阅此博客文章。

Persistent Roadmaps(重用之前的规划路径)

默认情况下，每个运动规划请求的规划算法从头开始。但是，对于构建环境路线图的某些规划人员来说，如果规划场景或多或少是静态的，则重用以前运动规划请求中的路线图可能是有益的。请考虑以下计划配置：

PersistentLazyPRMstar: # use this with a representative environment to create a roadmap
    type: geometric::LazyPRMstar
    multi_query_planning_enabled: true
    store_planner_data: true
    load_planner_data: false
    planner_data_path: /tmp/roadmap.graph
PersistentLazyPRM: # use this to load a previously created roadmap
    type: geometric::LazyPRM
    multi_query_planning_enabled: true
    store_planner_data: false
    load_planner_data: true
    planner_data_path: /tmp/roadmap.graph
SemiPersistentLazyPRMstar: # reuses roadmap during lifetime of node but doesn't save/load roadmap to/from disk
    type: geometric::LazyPRMstar
    multi_query_planning_enabled: true
    store_planner_data: false
    load_planner_data: false
SemiPersistentLazyPRM: # reuses roadmap during lifetime of node but doesn't save/load roadmap to/from disk
    type: geometric::LazyPRM
    multi_query_planning_enabled: true
    store_planner_data: false
    load_planner_data: false

第一个规划器配置"持久拉齐PRMstar"将使用LazyPRM来继续构建每个运动规划请求的采样机器人配置之间无症状的最佳路径路线图。规划器实例销毁后，它会将路线图保存到磁盘。持久疯狂PRM配置类似，只是它将从磁盘加载路线图，但在销毁时不会保存它。半永久性规划器配置不会加载/保存路线图，但会随着每个运动规划请求（而不是在规划前清除路线图）不断扩展路线图。支持持久规划功能的四个规划者是：PRM、PRM、LazyPRM 和 LazyPRM*。它们之间的关键区别在于，在搜索路线图以寻找有效路径时，惰性变体将根据需要重新验证节点和边缘的有效性。非惰性变体将不检查路线图是否对当前环境仍然有效。换句话说，对静态环境使用非惰性变体，对具有小变化的环境使用惰性变体，如果环境可以显著更改，则使用非持久性规划器。

请注意，保存和加载路线图仅在 OMPL 1.5.0 和更新版本中可用。

---

调用

主要说明一下xml yaml是干嘛的。
首先github看moveit调用ompl的源码：moveit/moveit_planners/ompl/

tree

├── CHANGELOG.rst
├── CMakeLists.txt
├── ompl_interface
│ ├── cfg
│ │ ├── cpp
│ │ │ └── ompl_interface_ros
│ │ │ └── OMPLDynamicReconfigureConfig.h
│ │ ├── OMPLDynamicReconfigure.cfg
│ │ └── OMPLParamsDR.cfgc
│ ├── CMakeLists.txt(subdirectory)
│ ├── include
│ │ └── moveit
│ │ └── ompl_interface
│ │ ├── constraints_library.h
│ │ ├── detail
│ │ │ ├── constrained_goal_sampler.h
│ │ │ ├── constrained_sampler.h
│ │ │ ├── constrained_valid_state_sampler.h
│ │ │ ├── constraint_approximations.h
│ │ │ ├── goal_union.h
│ │ │ ├── projection_evaluators.h
│ │ │ ├── same_shared_ptr.hpp
│ │ │ ├── state_validity_checker.h
│ │ │ └── threadsafe_state_storage.h
│ │ ├── model_based_planning_context.h
│ │ ├── ompl_interface.h
│ │ ├── parameterization
│ │ │ ├── joint_space
│ │ │ │ ├── joint_model_state_space_factory.h
│ │ │ │ └── joint_model_state_space.h
│ │ │ ├── model_based_state_space_factory.h
│ │ │ ├── model_based_state_space.h
│ │ │ └── work_space
│ │ │ ├── pose_model_state_space_factory.h
│ │ │ └── pose_model_state_space.h
│ │ └── planning_context_manager.h
│ ├── src
│ │ ├── constraints_library.cpp
│ │ ├── detail
│ │ │ ├── constrained_goal_sampler.cpp
│ │ │ ├── constrained_sampler.cpp
│ │ │ ├── constrained_valid_state_sampler.cpp
│ │ │ ├── goal_union.cpp
│ │ │ ├── projection_evaluators.cpp
│ │ │ ├── state_validity_checker.cpp
│ │ │ └── threadsafe_state_storage.cpp
│ │ ├── generate_state_database.cpp
│ │ ├── model_based_planning_context.cpp
│ │ ├── ompl_interface.cpp
│ │ ├── ompl_planner.cpp (节点)
│ │ ├── ompl_planner_manager.cpp
│ │ ├── parameterization
│ │ │ ├── joint_space
│ │ │ │ ├── joint_model_state_space.cpp
│ │ │ │ └── joint_model_state_space_factory.cpp
│ │ │ ├── model_based_state_space.cpp
│ │ │ ├── model_based_state_space_factory.cpp
│ │ │ └── work_space
│ │ │ ├── pose_model_state_space.cpp
│ │ │ └── pose_model_state_space_factory.cpp
│ │ └── planning_context_manager.cpp
│ └── test
│ └── test_state_space.cpp
├── ompl_interface_plugin_description.xml
├── ompl.kdev4
└── package.xml

很清楚的可以看到

• CMakeLists.txt ：制定生成规则，应为moveit调用ompl的核心原理也不过与相当于调用插件，因此插件接口就需要进行相应的封装，这个包就相当于对这些借口代码进行封装，add_subdirectory(ompl_interface)。动态加载参数generate_dynamic_reconfigure_options
• ompl_interface_plugin_description.xml：对插件进行描述，代码很简单。分别是定义接口名称，子类名称和基类名称，其中基类为planning_interface::PlannerManager就相当于告诉moveit这是个接口类。

<library path="libmoveit_ompl_planner_plugin">
  <class name="ompl_interface/OMPLPlanner" type="ompl_interface::OMPLPlannerManager" base_class_type="planning_interface::PlannerManager">
    <description>
    </description>
  </class>
</library>

• package.xml：不用多说就是定义了包的一些依赖项，注意在最后export就相当于告诉moveit这里有一个插件。

  <export>
    <moveit_core plugin="${prefix}/ompl_interface_plugin_description.xml"/>
  </export>

ompl_interface文件夹

• official documents

从CMakeLists.txt入手看代码结构。

节点：ompl_planner.cpp

作为服务器通过回调的形式（服务）实现computePlan。req的接受和res的发送都是在这个节点(OMPLPlannerService)中完成的. srv的数据结构moveit_msgs::GetMotionPlan,其中由 MotionPlanRequest和MotionPlanResponse构成.

其中黄色部分为疑问:

1. 首先构建规划器的上下文,其中, getPlanningContext(OMPLInterface)
2. ->加上错误码error code(转到manager中)从response找到groupname和planner_id
3. ->确定规划空间是在关节还是在笛卡尔端在planning_context_manager.cpp中有

  // Check if sampling in JointModelStateSpace is enforced for this group by user.
  // This is done by setting 'enforce_joint_model_state_space' to 'true' for the desired group in ompl_planning.yaml.
  //
  // Some planning problems like orientation path constraints are represented in PoseModelStateSpace and sampled via IK.
  // However consecutive IK solutions are not checked for proximity at the moment and sometimes happen to be flipped,
  // leading to invalid trajectories. This workaround lets the user prevent this problem by forcing rejection sampling
  // in JointModelStateSpace.

可推断默认是发生在笛卡尔空间，
証拠：

• 笛卡尔空间规划发生的问题
• 官方解决

当增加约束后，笛卡尔端的规划可能出现“jerky points”，为了解决这个问题，可以将规划强制转化到“关节空间”。同时可以看看官方教学说明书。
这里我们进行了一些方针(可以理解为笛卡尔路径规划的确是随机的,特别是将目标姿态加上去后), 进一步可以理解为默认的规划算法并没有选择最优路径

PS: 默认算法-RRTconnect, 此算法不具备搜寻最优路径的能力->【路径规划】RRT(Rapidly-exploring Random Trees)算法

加入force_joint_model_state_space: true进行实验.
发布日志参见here
路径仍然是随机且不是最优的.

将默认算法更换为RRT-star, 理论上只要迭代次数够多就能规划出最优路径.
实际表现如下图:
规划时间很长且路径结果随机final cost不固定,这应该是由于迭代次数不够导致

[ INFO] [1610500891.933291859]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610500891.933674467]: Goal constraints are already satisfied. No need to plan or execute any motions
[ INFO] [1610500893.033187163]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1610500893.036148155]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610500893.058302345]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610500893.059985330]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610500893.060131980]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610500893.094175712]: RRTstar: Found an initial solution with a cost of 7.41 in 34 iterations (33 vertices in the graph)
[ INFO] [1610500898.058783480]: RRTstar: Created 9639 new states. Checked 274178 rewire options. 10 goal states in tree. Final solution cost 7.398
[ INFO] [1610500898.058825364]: Solution found in 4.999329 seconds
[ INFO] [1610500898.058913193]: SimpleSetup: Path simplification took 0.000011 seconds and changed from 2 to 2 states
[ INFO] [1610500898.061484730]: Execution request received
[ INFO] [1610500898.133151433]: Fake execution of trajectory
[ INFO] [1610500905.333374852]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610500905.333745605]: Execution completed: SUCCEEDED
[ INFO] [1610500906.432928218]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610500906.433133786]: Planning attempt 1 of at most 5
[ INFO] [1610500906.433890758]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610500906.434078787]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610500906.434271032]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610500906.434335443]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610500906.455740887]: RRTstar: Found an initial solution with a cost of 7.40 in 19 iterations (16 vertices in the graph)
[ INFO] [1610500911.434400238]: RRTstar: Created 9580 new states. Checked 272290 rewire options. 10 goal states in tree. Final solution cost 7.397
[ INFO] [1610500911.434444809]: Solution found in 5.000286 seconds
[ INFO] [1610500911.434516433]: SimpleSetup: Path simplification took 0.000037 seconds and changed from 2 to 2 states
[ INFO] [1610500911.533480740]: Fake execution of trajectory
[ INFO] [1610500918.733192850]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610500947.732635429]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610500947.732727726]: Goal constraints are already satisfied. No need to plan or execute any motions
[ INFO] [1610500948.833228988]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1610500948.839373526]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610500948.839603016]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610500948.839875419]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610500948.839938055]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610500948.901677872]: RRTstar: Found an initial solution with a cost of 11.23 in 97 iterations (81 vertices in the graph)
[ INFO] [1610500953.839736652]: RRTstar: Created 9662 new states. Checked 274914 rewire options. 10 goal states in tree. Final solution cost 10.460
[ INFO] [1610500953.839784114]: Solution found in 5.000079 seconds
[ INFO] [1610500953.844494008]: SimpleSetup: Path simplification took 0.004662 seconds and changed from 4 to 2 states
[ INFO] [1610500953.846393490]: Execution request received
[ INFO] [1610500953.933130022]: Fake execution of trajectory
[ INFO] [1610500959.733174626]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610500959.733421149]: Execution completed: SUCCEEDED
[ INFO] [1610500960.833356083]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610500960.833561735]: Planning attempt 1 of at most 5
[ INFO] [1610500960.840329863]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610500960.840598564]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610500960.840913780]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610500960.841022862]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610500960.866586877]: RRTstar: Found an initial solution with a cost of 9.04 in 18 iterations (18 vertices in the graph)
[ INFO] [1610500965.841134240]: RRTstar: Created 9507 new states. Checked 269954 rewire options. 10 goal states in tree. Final solution cost 9.037
[ INFO] [1610500965.841176115]: Solution found in 5.000474 seconds
[ INFO] [1610500965.841198747]: SimpleSetup: Path simplification took 0.000002 seconds and changed from 2 to 2 states
[ INFO] [1610500965.933169390]: Fake execution of trajectory
[ INFO] [1610500971.733252117]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610500979.333319004]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610500979.333492890]: Goal constraints are already satisfied. No need to plan or execute any motions
[ INFO] [1610500980.432470424]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1610500980.435731034]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610500980.435846211]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610500980.435972311]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610500980.436002329]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610500980.462497419]: RRTstar: Found an initial solution with a cost of 11.77 in 38 iterations (37 vertices in the graph)
[ INFO] [1610500985.436554186]: RRTstar: Created 9185 new states. Checked 259650 rewire options. 10 goal states in tree. Final solution cost 9.575
[ INFO] [1610500985.436596275]: Solution found in 5.000703 seconds
[ INFO] [1610500985.441352463]: SimpleSetup: Path simplification took 0.004683 seconds and changed from 4 to 2 states
[ INFO] [1610500985.443513027]: Execution request received
[ INFO] [1610500985.533127932]: Fake execution of trajectory
[ INFO] [1610500991.333110487]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610500991.333390317]: Execution completed: SUCCEEDED
[ INFO] [1610500992.432418988]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610500992.432498916]: Planning attempt 1 of at most 5
[ INFO] [1610500992.436370938]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610500992.436645308]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610500992.436954787]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610500992.436998251]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610500992.448389786]: RRTstar: Found an initial solution with a cost of 11.53 in 18 iterations (16 vertices in the graph)
[ INFO] [1610500997.436824845]: RRTstar: Created 9190 new states. Checked 259810 rewire options. 10 goal states in tree. Final solution cost 9.811
[ INFO] [1610500997.436862388]: Solution found in 5.000128 seconds
[ INFO] [1610500997.441502843]: SimpleSetup: Path simplification took 0.004597 seconds and changed from 4 to 2 states
[ INFO] [1610500997.532947190]: Fake execution of trajectory
[ INFO] [1610501003.332735159]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610501014.532502324]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610501014.532578249]: Goal constraints are already satisfied. No need to plan or execute any motions
[ INFO] [1610501015.632726650]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1610501015.637916653]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610501015.638127430]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610501015.638412235]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610501015.638463485]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610501015.655573197]: RRTstar: Found an initial solution with a cost of 10.56 in 18 iterations (17 vertices in the graph)
[ INFO] [1610501020.638862821]: RRTstar: Created 9408 new states. Checked 266786 rewire options. 10 goal states in tree. Final solution cost 10.549
[ INFO] [1610501020.638913749]: Solution found in 5.000684 seconds
[ INFO] [1610501020.638967084]: SimpleSetup: Path simplification took 0.000003 seconds and changed from 2 to 2 states
[ INFO] [1610501020.641032783]: Execution request received
[ INFO] [1610501020.732662145]: Fake execution of trajectory
[ INFO] [1610501028.032406186]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610501028.032520278]: Execution completed: SUCCEEDED
[ INFO] [1610501029.133077377]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610501029.133241062]: Planning attempt 1 of at most 5
[ INFO] [1610501029.142844519]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610501029.143258060]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610501029.143554114]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610501029.143641349]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610501029.163548070]: RRTstar: Found an initial solution with a cost of 12.35 in 21 iterations (19 vertices in the graph)
[ INFO] [1610501034.143671742]: RRTstar: Created 9289 new states. Checked 262978 rewire options. 10 goal states in tree. Final solution cost 10.850
[ INFO] [1610501034.143709995]: Solution found in 5.000338 seconds
[ INFO] [1610501034.145618695]: SimpleSetup: Path simplification took 0.001880 seconds and changed from 4 to 2 states
[ INFO] [1610501034.233104934]: Fake execution of trajectory
[ INFO] [1610501041.532612536]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610501046.032394266]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610501046.032469432]: Goal constraints are already satisfied. No need to plan or execute any motions
[ INFO] [1610501047.132600648]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1610501047.137850578]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610501047.138074392]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610501047.138289393]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610501047.138343295]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610501047.187468040]: RRTstar: Found an initial solution with a cost of 9.70 in 47 iterations (43 vertices in the graph)
[ INFO] [1610501052.139032890]: RRTstar: Created 9112 new states. Checked 257314 rewire options. 10 goal states in tree. Final solution cost 9.696
[ INFO] [1610501052.139077288]: Solution found in 5.000923 seconds
[ INFO] [1610501052.139107464]: SimpleSetup: Path simplification took 0.000001 seconds and changed from 2 to 2 states
[ INFO] [1610501052.141359203]: Execution request received
[ INFO] [1610501052.232581519]: Fake execution of trajectory
[ INFO] [1610501060.132463347]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610501060.132562667]: Execution completed: SUCCEEDED
[ INFO] [1610501061.232772916]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610501061.233080101]: Planning attempt 1 of at most 5
[ INFO] [1610501061.237297962]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610501061.237458406]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610501061.237633719]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610501061.237687225]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610501061.254496137]: RRTstar: Found an initial solution with a cost of 14.66 in 15 iterations (16 vertices in the graph)
[ INFO] [1610501066.237797728]: RRTstar: Created 8525 new states. Checked 238708 rewire options. 10 goal states in tree. Final solution cost 11.479
[ INFO] [1610501066.237837560]: Solution found in 5.000315 seconds
[ INFO] [1610501066.239986094]: SimpleSetup: Path simplification took 0.002097 seconds and changed from 3 to 2 states
[ INFO] [1610501066.332369110]: Fake execution of trajectory
[ INFO] [1610501074.333205512]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610502551.532339541]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610502551.532424989]: Goal constraints are already satisfied. No need to plan or execute any motions
[ INFO] [1610502552.632492758]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1610502552.635671573]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610502552.635776395]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610502552.635960078]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610502552.635988495]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610502552.676866628]: RRTstar: Found an initial solution with a cost of 6.56 in 77 iterations (66 vertices in the graph)
[ INFO] [1610502557.636129960]: RRTstar: Created 9313 new states. Checked 263746 rewire options. 10 goal states in tree. Final solution cost 6.533
[ INFO] [1610502557.636177710]: Solution found in 5.000330 seconds
[ INFO] [1610502557.636204388]: SimpleSetup: Path simplification took 0.000002 seconds and changed from 2 to 2 states
[ INFO] [1610502557.638023555]: Execution request received
[ INFO] [1610502557.732819201]: Fake execution of trajectory
[ INFO] [1610502562.832553436]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610502562.832668867]: Execution completed: SUCCEEDED
[ INFO] [1610502563.932407162]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610502563.932475436]: Planning attempt 1 of at most 5
[ INFO] [1610502563.934606186]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610502563.934695325]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610502563.934811289]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610502563.934845506]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610502563.947605115]: RRTstar: Found an initial solution with a cost of 6.53 in 20 iterations (20 vertices in the graph)
[ INFO] [1610502568.934813145]: RRTstar: Created 9585 new states. Checked 272450 rewire options. 10 goal states in tree. Final solution cost 6.534
[ INFO] [1610502568.934894719]: Solution found in 5.000148 seconds
[ INFO] [1610502568.934933968]: SimpleSetup: Path simplification took 0.000002 seconds and changed from 2 to 2 states
[ INFO] [1610502569.032541363]: Fake execution of trajectory
[ INFO] [1610502574.132580170]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610502578.432298987]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610502578.432406661]: Goal constraints are already satisfied. No need to plan or execute any motions
[ INFO] [1610502579.532832715]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1610502579.539411976]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610502579.539640883]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610502579.539898782]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610502579.539955021]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610502579.573173114]: RRTstar: Found an initial solution with a cost of 10.86 in 34 iterations (34 vertices in the graph)
[ INFO] [1610502584.540386861]: RRTstar: Created 9402 new states. Checked 266594 rewire options. 10 goal states in tree. Final solution cost 10.369
[ INFO] [1610502584.540461268]: Solution found in 5.000713 seconds
[ INFO] [1610502584.542285710]: SimpleSetup: Path simplification took 0.001778 seconds and changed from 3 to 2 states
[ INFO] [1610502584.544056309]: Execution request received
[ INFO] [1610502584.632504648]: Fake execution of trajectory
[ INFO] [1610502590.732537502]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1610502590.732658096]: Execution completed: SUCCEEDED
[ INFO] [1610502591.832310809]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1610502591.832384747]: Planning attempt 1 of at most 5
[ INFO] [1610502591.834626491]: Planner configuration 'panda_arm' will use planner 'geometric::RRTstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1610502591.834706684]: RRTstar: problem definition is not set, deferring setup completion...
[ INFO] [1610502591.834785559]: RRTstar: Starting planning with 1 states already in datastructure
[ INFO] [1610502591.834806672]: RRTstar: Initial k-nearest value of 3
[ INFO] [1610502591.854117312]: RRTstar: Found an initial solution with a cost of 12.24 in 40 iterations (37 vertices in the graph)
[ INFO] [1610502596.835015921]: RRTstar: Created 9360 new states. Checked 265250 rewire options. 10 goal states in tree. Final solution cost 11.493
[ INFO] [1610502596.835183695]: Solution found in 5.000430 seconds
[ INFO] [1610502596.840487668]: SimpleSetup: Path simplification took 0.005221 seconds and changed from 5 to 2 states
[ INFO] [1610502596.932580941]: Fake execution of trajectory
[ INFO] [1610502603.032457991]: Completed trajectory execution with status SUCCEEDED ...

5. getPlannerSelector选择规划器(boost?)->但是在interface中完成configure->完成规划->

6. 生成response返回给client,为什么是planning_interface::MotionPlanResponse response,而不是moveit_msgs::GetMotionPlan::Response& res ??

7. 在哪里设置碰撞检测?

  bool computePlan(moveit_msgs::GetMotionPlan::Request& req, moveit_msgs::GetMotionPlan::Response& res)
  {
    ROS_INFO("Received new planning request...");
    if (debug_)
      pub_request_.publish(req.motion_plan_request);
    planning_interface::MotionPlanResponse response;

    ompl_interface::ModelBasedPlanningContextPtr context =
        ompl_interface_.getPlanningContext(psm_.getPlanningScene(), req.motion_plan_request);
    if (!context)
    {
      ROS_ERROR_STREAM_NAMED("computePlan", "No planning context found");
      return false;
    }
    context->clear();

    bool result = context->solve(response);

    if (debug_)
    {
      if (result)
        displaySolution(res.motion_plan_response);
      std::stringstream ss;
      ROS_INFO("%s", ss.str().c_str());
    }
    return result;
  }

节点：generate_state_database.cpp

本篇博客不细究,大概的如下:

Generates a database of states that follow the given constraints. An example of the constraint yaml that should be loaded to rosparam: "name: tool0_upright constraints:
type: orientation frame_id: world link_name: tool0 orientation: [0, 0, 0] tolerances: [0.01, 0.01, 3.15] weight: 1.0 "

这个节点其实也对应->Planning with Approximated Constraint Manifolds

OMPL 支持自定义约束，以启用遵循所需行为的规划轨迹。约束可以在关节空间和笛卡尔空间中定义，后者基于方向或位置。在规划轨迹时，每个关节状态都需要遵循所有设置的约束，默认情况下，这些约束由拒绝采样执行。但是，这可能会导致很长的规划时间，尤其是当约束非常严格，并且拒绝率相应高时。

Sucan 等人提出了一种方法，他们事先计算约束歧管的近似值，并在这方面执行轨迹规划。OMPL 插件包含用于执行给定约束集并将其保存在数据库中的功能。在以后的实例中，可以加载数据库，用于使用任何 OMPL 规划器进行受限规划，从而大大减少规划时间。

本教程包括有关如何构造约束近似数据库以及如何使用它进行约束轨迹规划的步骤。有关如何使用路径约束进行规划的信息，请看此处。
其他翻译略, 其实就是为了Running the database generator, 在产生后以后在启动moveit节点时再载入即可.

---

• 头文件
  ->moveit/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface

• 源文件
  ->moveit/moveit_planners/ompl/ompl_interface/src/

• planning_planner_manager.cpp
  可以简单的理解为对算法调用前的初始化配置上下文环境等作。
  包括初始化，规划器名称，算法名称，插件声明。

  /** \brief All the existing planning configurations. The name of the configuration is the key of the map. This name can be of the form “group_name[config_name]” if there are particular configurations specified for a group, or of the form “group_name” if default settings are to be used. */lerp_planning.yaml is loaded to set the lerp-specific parameters to ROS Parameter Server.