Note for: “OMPL: A Primer”

This note is about a tutorial book for the ‘Open Motion Planning Library(OMPL)’. This book can be found in the OMPL webpage. Additionally, some other note about OMPL will recorded here.

Here’s the webpage of OMPL: OMPL_LINK

OMPL: A Primer

Chap 1 Introduction

OMPL支持sampling-based methods, 其库中已经包含了部分现有的sampling-based methods。整个doc的目的是：users should be able to use OMPL.app to solve motion planning queries in 2D and 3D workspaces, and utilize the OMPL framework to develop their own algorithms for state sampling, collision checking, nearest neighbor searching, and other components of sampling-based methods to build a new planner.

**NOTE: OMPL specializes in sampling-based motion planning! **

Chap 2 Introduction to Sampling-based Motion Planning

首先是回顾一下robotic motion planning的目标：seeks to find a solution to the problem of “Go from the start to the goal while respecting all of the robot’s constraints.”

接下来介绍motion planning的一种分类方式：

  1. Exact and Approximate Cell Decomposition

这类方法的目标就是通过精确计算or近似的方法构建出state space or joint space（主要构建的目标是free space即:无碰撞且满足约束的space）。也就是将不碰撞的workspace分成一系列discrete cell。可以通过graph or tree来构建这个space（vertices代表free cell，edge代表一个可行的trajectory可以让机器人从一个cell前往至另一个cell）。这样就可以在该space中搜索一个从start to goal的解。然后这种直接显式构建state space的方法基本上

这类方法also works well in “controllable” systems (e.g., omni-directional base). 但如果需要考虑robot的dynamics，或者robot的dynamics是复杂且non-linear的，就会导致not clear to find how to move the system from one cell to an adjacent cell.

  1. Control-based Methods（不太熟悉）

Control-based Methods是希望建立一个系统的运动方程（例如：y=Ax+b）,之后使用控制的方法，对这个系统进行控制，并希望达到特定的trajectory，例如使用optimal control的方法，来达到一个minimal error。其好处在于：基本上可以实现online。缺点在于：如果应用于complex dynamics或者cluttered environments时会导致往往无法计算出结果。

  1. Potential Fields

传统的Potential Fields方法就是对于workspace中的每一个点计算一个vector，该vector由：calculating the sum of an attractive force emanating from the goal, and a repulsive force from all of the obstacles. 之后机器人可以根据当前位置的梯度前往goal configuration。其缺点在于极其容易陷入local mininum。理想情况下在系统的state space中计算这个field，但这等效于解决原始问题。 一些方法考虑了navigation function，其中保证了Potential Fields具有单个最小值，但是仅在低维空间中才有可能计算这样的函数并且这是non-trivial的.

  1. Randomized Planning

在deterministic planners中，使用随机化已被证明是非常有效的。 例如，在Potential Fields中，在特定时间范围内应用随机动作的布朗运动已被证明在引导系统脱离局部极小值方面非常有效。 随机方法也已应用于非完整（non-holonomic）系统，尤其是应用于sampling-based planning。

Sampling-based methods were inspired by randomization, and the use of samples, random or deterministic, when planning are particularly effective for high degree of freedom systems or those with complex dynamics.

接下来介绍OMPL主要应用的核心：Sampling-based Motion Planning

Sampling-based Motion Planning

传统方法在往往会在high-dimensional spaces和负责constraints时出现问题，而sampling-based motion planning reasons over a finite set of configurations in the state space。大部分sampling-based methods provide probabilistic completeness. 即意味着：if a solution exists, the probability of finding a solution converges to one as the number of samples reasoned over increases to infinity.

NOTE: Sampling-based approaches cannot recognize a problem with no solution.

The goal of a sampling-based motion planning query: the task of finding a collision path in the state space of the robot from a distinct start state to a specific goal state, utilizing a path composed of configurations connected by collision free paths.

主要的sampling-based methods主要有以下两种：

  1. Probabilistic Roadmap(PRM)

本质是构建roadmap（graph）来表示一个free state space，之后在该roadmap上寻找一个最小路径。本质一个multi-query，即构建好的结果可以多次用于motion planning

NTOE: 对于任意的sampling-based methods，其free state space都是非显式可知的。**Each sample that is generated is checked for collision, and only collision free samples are retained. ** 其中如何sampling和如何进行碰撞检测都可以有user来自定义。

生成完samples之后，the roadmap itself can be constructed by connecting the random samples to form edges. 经典的PRM方法attempts to connect each sample to the k samples nearest to it by using a local planner that is tasked with finding short collision free paths. 其中这个local planner使用两个samples之间的差值来find path，并进行碰撞检测。

  1. Tree-based Planners

本质是构建树型结构来表示一个free state space，之后在该tree上寻找一个最小路径。不同于PRM，其是一个single-query，即每次query都要重新构建一次树。

这类方法主要包含以下步骤：These methods begin by rooting a tree at the starting configuration of the robot. With the first node of the tree intact, random sampling of the free space then occurs. The planner employs an expansion heuristic, which typically gives the method its name, from which the sample is connected to the tree along a collision free path.

  1. 二者的不同之处
    • multi-query vs single-query
    • These trees do not normally cover the free space in the same manner that a roadmap would. However, when planning with differential constraints, it is not easy to encode control information into an undirected edge.
    • Controls are usually directed commands, and require a specific pre-condition in order for a particular control to be valid. Tree-based methods, on the other hand, excel at planning with complex dynamics because of the directed, acyclic nature of the underlying data structure. Control information can be encoded for each edge of the tree, with the vertices of the tree satisfying the prerequisites for the valid controls.

NOTE: sampling based methods并不会对state space做一个explicit representation。

Primitives of Sampling-based Planning

  • 存在许多Sampling-based Planner，且它们具有许多共性，但所有这些方法的关键在于：如何对状态空间进行采样。

  • Collision checking。It is used not only in the local planner when attempting to find collision free paths between samples, but also during the
    sampling process itself. Collision checking的任务往往只是：accept a configuration of the robot and quickly determine whether or not this state is in collision.

  • Nearest neighbor searching。It is from the ability of determining whether two states of the robot are close that many of the common approaches
    are able to effectively find paths through a high-dimensional space.

Chap 3 Getting Started with OMPL.app

OMPL.app provides a graphical front-end to the core OMPL library, and allows a user to see many of the ideas from sampling-based motion planning in action。 主要简单的介绍了如何只用OMPL.app，并不是我们的重点。

此章节值得注意的点：

  • 3.1.3节 Bounding the Environment

By default, the robot is constrained to move inside a tight bounding box around the environment, the start pose, and the goal pose. The bounding box is visualized in OMPL.app as the white frame around the environment and robot. These bounds apply to a reference point for the robot; the origin of the coordinate frame that defines its pose. This means that parts of the robot can stick outside the bounding box. It also means that if the reference point for your robot is far away from the robot itself, you can get rather unintuitive results. The reference point is whatever the origin is in the mesh; OMPL.app is not using the geometric center of the mesh as the reference point.

  • 3.2节 OMPL.app vs OMPL

OMPL.app是基于OMPL的功能开发的，并且specifying a geometric representation for the robot and its environment, and provides a collision checking mechanism for the representation to the existing planners in the library. OMPL.app 使用了【Refer 1,2】作为其碰撞检测器。

而OMPL并没有明确表示机器人or环境，而是留给user根据其application来自定义的空间。

Chap 4 Planning with OMPL

Design Considerations

the library does not explicitly represent the geometry of the workspace or the robot operating in it. 而且也没有提供一个默认的explicit state validity/collision detection method. 这需要由用户根据使用的数据类型来自己定义。

OMPL也尽量的减少第三方的依赖，主要的依赖为：Boost：which allows OMPL to function in most major operating systems.

OMPL Foundations

再次回顾Sampling-based Motion Planning的三个关键组件：

  • a sampler to compute valid configurations of the robot
  • a state validity checker to quickly evaluate a specific robot configuration
  • a local planner to connect two samples along a collision free path

而OMPL将这些组件定义为了一些class，主要的class的功能如下：

  1. StateSampler

provides methods for uniform and Gaussian sampling in the most common state space configurations

  1. NearestNeighbors

This is an abstract class that is utilized to provide a common interface to the planners for the purpose of performing a nearest neighbor search among samples in the state space.

  1. StateValidityChecker

evaluating a single state to determine if the configuration collides with an environment obstacle and respects the constraints of the robot. A default checker is not provided by OMPL

  1. MotionValidator(analogous to the local planner)

checks to see if the motion of the robot between two states is valid. At a high level, the MotionValidator must be able to evaluate whether the motion between two states is collision free and respects all the motion constraints of the robot.

  1. OptimizationObjective
  2. ProblemDefinition

A motion planning query is specified by the ProblemDefinition object. Instances of this class define a start state and goal configuration for the robot, and the optimization objective to meet, if any.

Solving a Query

整个OMPL的框架。

image-20210405230510848

When solving a particular query, the user is not required to instantiate all of the objects detailed in top figure. In fact, most of these objects have default implementations that are sufficient for general planning.

Benchmarking

OMPL更是一个极好的用于compare two or more planners的平台。

Chap 5 Advanced Topics in OMPL

实际上是要向user提供有关库的一些常用可选功能以及如何将它们添加至代码中的。

State Space Construction

例如：n维欧氏空间，SO(3)，SE(3)等。

Planner Customization

可以比较方便的定制化自己的planner？

Python Bindings

完全支持使用python来调用。

Reference

  1. Eric Larsen, Stefan Gottschalk, Ming C. Lin, and Dinesh Manocha. Fast proximity queries with swept sphere volumes. In IEEE International Conference on Robotics and Automation, pages 3719–3726, 2000.
  2. Jia Pan, Sachin Chitta, and Dinesh Manocha. FCL: A general purpose library for collision and proximity queries. In IEEE International Conference on Robotics and Automation, May 2012.