• FaSTrack: Ensuring Safe Real-Time Navigation of Dynamic Systems
• WAFR 2016 Roundup
• V-REP Now Supports OMPL
• Finding Diverse Short Paths
• OMPL 1.1 Released!
• Report on the First MoveIt! Community Meeting
• or_ompl — OpenRAVE bindings for OMPL
• Extended support to optimal planners: CForest + tree pruning
• Bringing Belief Space Planning to OMPL
• ICRA 2014 Roundup
• Combining OMPL and MORSE
• Extending OMPL support for optimal path planning
• MoveIt! Release
• OMPL Google Summer of Code
• OMPL wins the 2012 OSS World Challenge Grand Prize
• Easier integration with the MoveIt wizard and advancements in OMPL
• ICRA 2012 / ROSCON update
• OMPL development activity
• Planning using the Vortex physics engine
• From differential equations to planning in one function
• Geometric planning for car-like vehicles
• Welcome!

FaSTrack: Ensuring Safe Real-Time Navigation of Dynamic Systems 05 Dec 2017

Sylvia Herbert, David Fridovich-Keil, and Claire Tomlin

[Cross-posted from the Berkeley Artificial Intellegince Research (BAIR) blog.]

The Problem: Fast and Safe Motion Planning

Real time autonomous motion planning and navigation is hard, especially when we care about safety. This becomes even more difficult when we have systems with complicated dynamics, external disturbances (like wind), and a priori unknown environments. Our goal in this work is to “robustify” existing real-time motion planners to guarantee safety during navigation of dynamic systems.

In control theory there are techniques like Hamilton-Jacobi Reachability Analysis that provide rigorous safety guarantees of system behavior, along with an optimal controller to reach a given goal (see Fig. 1). However, in general the computational methods used in HJ Reachability Analysis are only tractable in decomposable and/or low-dimensional systems; this is due to the “curse of dimensionality.” That means for real time planning we can’t process safe trajectories for systems of more than about two dimensions. Since most real-world system models like cars, planes, and quadrotors have more than two dimensions, these methods are usually intractable in real time.

On the other hand, geometric motion planners like rapidly-exploring random trees (RRT) and model-predictive control (MPC) can plan in real time by using simplified models of system dynamics and/or a short planning horizon. Although this allows us to perform real time motion planning, the resulting trajectories may be overly simplified, lead to unavoidable collisions, and may even be dynamically infeasible (see Fig. 1). For example, imagine riding a bike and following the path on the ground traced by a pedestrian. This path leads you straight towards a tree and then takes a 90 degree turn away at the last second. You can’t make such a sharp turn on your bike, and instead you end up crashing into the tree. Classically, roboticists have mitigated this issue by pretending obstacles are slightly larger than they really are during planning. This greatly improves the chances of not crashing, but still doesn’t provide guarantees and may lead to unanticipated collisions.

So how do we combine the speed of fast planning with the safety guarantee of slow planning?

Figure 1. On the left we have a high-dimensional vehicle moving through an obstacle course to a goal. Computing the optimal safe trajectory is a slow and sometimes intractable task, and replanning is nearly impossible. On the right we simplify our model of the vehicle (in this case assuming it can move in straight lines connected at points). This allows us to plan very quickly, but when we execute the planned trajectory we may find that we cannot actually follow the path exactly, and end up crashing.

The Solution: FaSTrack

FaSTrack: Fast and Safe Tracking, is a tool that essentially “robustifies” fast motion planners like RRT or MPC while maintaining real time performance. FaSTrack allows users to implement a fast motion planner with simplified dynamics while maintaining safety in the form of a precomputed bound on the maximum possible distance between the planner’s state and the actual autonomous system’s state at runtime. We call this distance the tracking error bound. This precomputation also results in an optimal control lookup table which provides the optimal error-feedback controller for the autonomous system to pursue the online planner in real time.

Figure 2. The idea behind FaSTrack is to plan using the simplified model (blue), but precompute a tracking error bound that captures all potential deviations of the trajectory due to model mismatch and environmental disturbances like wind, and an error-feedback controller to stay within this bound. We can then augment our obstacles by the tracking error bound, which guarantees that our dynamic system (red) remains safe. Augmenting obstacles is not a new idea in the robotics community, but by using our tracking error bound we can take into account system dynamics and disturbances.

Offline Precomputation

We precompute this tracking error bound by viewing the problem as a pursuit-evasion game between a planner and a tracker. The planner uses a simplified model of the true autonomous system that is necessary for real time planning; the tracker uses a more accurate model of the true autonomous system. We assume that the tracker — the true autonomous system — is always pursuing the planner. We want to know what the maximum relative distance (i.e. maximum tracking error) could be in the worst case scenario: when the planner is actively attempting to evade the tracker. If we have an upper limit on this bound then we know the maximum tracking error that can occur at run time.

Figure 3. Tracking system with complicated model of true system dynamics tracking a planning system that plans with a very simple model.

Because we care about maximum tracking error, we care about maximum relative distance. So to solve this pursuit-evasion game we must first determine the relative dynamics between the two systems by fixing the planner at the origin and determining the dynamics of the tracker relative to the planner. We then specify a cost function as the distance to this origin, i.e. relative distance of tracker to the planner, as seen in Fig. 4. The tracker will try to minimize this cost, and the planner will try to maximize it. While evolving these optimal trajectories over time, we capture the highest cost that occurs over the time period. If the tracker can always eventually catch up to the planner, this cost converges to a fixed cost for all time.

The smallest invariant level set of the converged value function provides determines the tracking error bound, as seen in Fig. 5. Moreover, the gradient of the converged value function can be used to create an optimal error-feedback control policy for the tracker to pursue the planner. We used Ian Mitchell’s Level Set Toolbox and Reachability Analysis to solve this differential game. For a more thorough explanation of the optimization, please see our recent paper from the 2017 IEEE Conference on Decision and Control.

Figures 4 & 5: On the left we show the value function initializing at the cost function (distance to origin) and evolving according to the differential game. On the right we should 3D and 2D slices of this value function. Each slice can be thought of as a “candidate tracking error bound.” Over time, some of these bounds become infeasible to stay within. The smallest invariant level set of the converged value function provides us with the tightest tracking error bound that is feasible.

Online real time Planning

In the online phase, we sense obstacles within a given sensing radius and imagine expanding these obstacles by the tracking error bound with a Minkowski sum. Using these padded obstacles, the motion planner decides its next desired state. Based on that relative state between the tracker and planner, the optimal control for the tracker (autonomous system) is determined from the lookup table. The autonomous system executes the optimal control, and the process repeats until the goal has been reached. This means that the motion planner can continue to plan quickly, and by simply augmenting obstacles and using a lookup table for control we can ensure safety!

Figure 6. MATLAB simulation of a 10D near-hover quadrotor model (blue line) “pursuing” a 3D planning model (green dot) that is using RRT to plan. As new obstacles are discovered (turning red), the RRT plans a new path towards the goal. Based on the relative state between the planner and the autonomous system, the optimal control can be found via look-up table. Even when the RRT planner makes sudden turns, we are guaranteed to stay within the tracking error bound (blue box).

Reducing Conservativeness through Meta-Planning

One consequence of formulating the safe tracking problem as a pursuit-evasion game between the planner and the tracker is that the resulting safe tracking bound is often rather conservative. That is, the tracker can’t guarantee that it will be close to the planner if the planner is always allowed to do the worst possible behavior. One solution is to use multiple planning models, each with its own tracking error bound, simultaneously at planning time. The resulting “meta-plan” is comprised of trajectory segments computed by each planner, each labelled with the appropriate optimal controller to track trajectories generated by that planner. This is illustrated in Fig. 7, where the large blue error bound corresponds to a planner which is allowed to move very quickly and the small red bound corresponds to a planner which moves more slowly.

Figure 7. By considering two different planners, each with a different tracking error bound, our algorithm is able to find a guaranteed safe “meta-plan” that prefers the less precise but faster-moving blue planner but reverts to the more precise but slower red planner in the vicinity of obstacles. This leads to natural, intuitive behavior that optimally trades off planner conservatism with vehicle maneuvering speed.

Safe Switching

The key to making this work is to ensure that all transitions between planners are safe. This can get a little complicated, but the main idea is that a transition between two planners — call them A and B — is safe if we can guarantee that the invariant set computed for A is contained within that for B. For many pairs of planners this is true, e.g. switching from the blue bound to the red bound in Fig. 7. But often it is not. In general, we need to solve a dynamic game very similar to the original one in FaSTrack, but where we want to know the set of states that we will never leave and from which we can guarantee we end up inside B’s invariant set. Usually, the resulting safe switching bound (SSB) is slightly larger than A’s tracking error bound (TEB), as shown below.

Figure 8. The safe switching bound for a transition between a planner with a large tracking error bound to one with a small tracking error bound is generally larger than the large tracking error bound, as shown.

Efficient Online Meta-Planning

To do this efficiently in real time, we use a modified version of the classical RRT algorithm. Usually, RRTs work by sampling points in state space and connecting them with line segments to form a tree rooted at the start point. In our case, we replace the line segments with the actual trajectories generated by individual planners. In order to find the shortest route to the goal, we favor planners that can move more quickly, trying them first and only resorting to slower-moving planners if the faster ones fail.

We do have to be careful to ensure safe switching bounds are satisfied, however. This is especially important in cases where the meta-planner decides to transition to a more precise, slower-moving planner, as in the example above. In such cases, we implement a one-step virtual backtracking algorithm in which we make sure the preceding trajectory segment is collision-free using the switching controller.

Implementation

We implemented both FaSTrack and Meta-Planning in C++ / ROS, using low-level motion planners from the Open Motion Planning Library (OMPL). Simulated results are shown below, with (right) and without (left) our optimal controller. As you can see, simply using a linear feedback (LQR) controller (left) provides no guarantees about staying inside the tracking error bound.

Figures 9 & 10. (Left) A standard LQR controller is unable to keep the quadrotor within the tracking error bound. (Right) The optimal tracking controller keeps the quadrotor within the tracking bound, even during radical changes in the planned trajectory.

It also works on hardware! We tested on the open-source Crazyflie 2.0 quadrotor platform. As you can see in Fig. 12, we manage to stay inside the tracking bound at all times, even when switching planners.

Figures 11 & 12. (Left) A Crazyflie 2.0 quadrotor being observed by an OptiTrack motion capture system. (Right) Position traces from a hardware test of the meta planning algorithm. As shown, the tracking system stays within the tracking error bound at all times, even during the planner switch that occurs approximately 4.5 seconds after the start.

This post is based on the following papers:

• FaSTrack: a Modular Framework for Fast and Guaranteed Safe Motion Planning
  Sylvia Herbert*, Mo Chen*, SooJean Han, Somil Bansal, Jaime F. Fisac, and Claire J. Tomlin
  Paper, Website

• Planning, Fast and Slow: A Framework for Adaptive Real-Time Safe Trajectory Planning
  David Fridovich-Keil*, Sylvia Herbert*, Jaime F. Fisac*, Sampada Deglurkar, and Claire J. Tomlin
  Paper, Github (code to appear soon)

We would like to thank our coauthors; developing FaSTrack has been a team effort and we are incredibly fortunate to have a fantastic set of colleagues on this project.

WAFR 2016 Roundup 09 Jan 2017

Mark Moll

A couple weeks ago the Workshop on the Algorithmic Foundations of Robotics (WAFR) took place in San Francisco, CA. As always, WAFR was excellent. It was good to see many friends and colleagues again. There were many excellent papers, but I will focus here on some of the papers that used OMPL one way or another.

1. Collision detection or nearest-neighbor search? On the computational bottleneck in sampling-based motion planning
   Michal Kleinbort, Oren Salzman, and Dan Halperin
   Blavatnik School of Computer Science, Tel-Aviv University, Israel
   Kleinbart et al. show that the computational bottleneck in asymptotically optimal planning algorithms is, in the limit, created by nearest-neighbor calculations. They also show that computing all near-neighbors within a ball of a given radius can provide speedups compared to computing k nearest neighbors.

2. Efficient Nearest-Neighbor Search for Dynamical Systems with Nonholonomic Constraints
   Valerio Varricchio, Brian Paden, Dmitry Yershov, and Emilio Frazzoli
   Massachusetts Institute of Technology
   Varricchio et al. also analyze nearest-neighbor search, but here the focus is on dynamical systems with nonholonomic constraints. The authors focus specifically on kd-trees and propose a new way to build and query such trees for nonholonomic systems. The proposed approach is evaluated using a Reeds-Shepp system and outperforms regular kd-trees, hierarchical clustering (as implemented in FLANN), and Geometric Near-neighbor Access Trees (as implemented in OMPL).

3. Matrix Completion as a Post-Processing Technique for Probabilistic Roadmaps
   Joel M. Esposito and John N. Wright
   United States Naval Academy, Annapolis, MD and Columbia University, New York, NY
   Esposito and Wright observe that the existence of many edges in a probabilistic roadmap can be predicted using only a small subset of the edges in the roadmap. This is done using matrix completion techniques. If valid and invalid edges can be predicted accurately, then many collision checks can be avoided. How to best leverage this insight into accelerating motion planning algorithms is the topic of further research.

4. Resolution-Exact Planner for Thick Non-Crossing 2-Link Robots
   Chee K. Yap, Zhongdi Luo, and Ching-Hsiang Hsu
   Department of Computer Science, Courant Institute, NYU
   Yap et al. present a new type of motion planning algorithm that is not based on sampling or heuristic search, but is based on subdivision instead. Initial results are presented for a 2-link robot in the plane. The algorithm is compared with a number of sampling-based planning algorithms in OMPL and performs really well in comparison. Generalizing the algorithm to higher-dimensional systems is non-trivial and is the subject of future work.

5. Motion Planning for Reconfigurable Mobile Robots Using Hierarchical Fast Marching Trees
   William Reid, Robert Fitch, Ali Haydar Göktogan and Salah Sukkarieh
   Australian Centre for Field Robotics, The University of Sydney and the Centre for Autonomous Systems, University of Technology Sydney, Australia
   Reid et al. present a hierarchical variant of FMT*, an asymptotically optimal planning algorithm. They use a path in a subspace of a high-dimensional state space to guide the exploration in the full state space while preserving optimality and completeness guarantees. The algorithm was developed for a reconfigurable mobile robot, specifically, a four-legged rover with wheels as feet. The idea may extend to other asymptotically optimal algorithms. Using this hierarchical approach the authors show a significantly improved convergence to energy-optimal paths for their rover.

V-REP Now Supports OMPL 10 Mar 2016

Mark Moll

I am very pleased to announce that V-REP has recently added support for OMPL. OMPL is now the recommended way to perform motion planning in V-REP.

The V-REP platform is a modular, generic and general purpose robot simulation framework that offers various tools related to robotics (4 physics engines, collision detection, minimum distance calculation, proximity sensor simulation, vision sensor simulation, full FK/IK kinematic solver, etc.), with various kinds of interfaces (ROS, remote API, plug-ins, add-ons) and language support: C/C++, Python, Java, Matlab, Octave, Lua. It is built on a distributed control architecture, allowing virtually any number of scripts running in parallel and controlling various aspects of a simulation. The OMPL interface for V-REP was implemented via a plug-in wrapping the OMPL functionality, and offering that functionality via scripting functions. This allows to quickly test various scenarios, without the need to recompile/load test code over and over again. In combination with V-REP’s kinematic functionality, complex movement sequences can easily be computed: e.g. V-REP can also quickly compute several valid robot configurations for a desired end-effector pose.

This is the latest robotics software package to add support for OMPL. Other packages include ROS/MoveIt!, OpenRAVE, MORSE, Kautham, VEROSIM, and others. A more detailed description of the integration between these packages and OMPL is described here.

Finding Diverse Short Paths 19 Jan 2016

Caleb Voss

[Cross-posted from Caleb Voss’ web site; see the original article titled “Randomizing Graph Searches for Robustness” here]

Finding the shortest path through a weighted graph is a well-understood task. With a cost heuristic, A* is the way to go. But what if the graph is not reliable? A broken edge or an incorrect weight can make the apparent shortest path a poor choice. The client wants options. So how can we find paths other than the shortest one? And what criteria can we use to evaluate the set of proposed alternatives?

It is still good for alternatives to be short. We could try the second shortest path, but in a moderately dense graph, it is likely to share almost all its edges with the original. The nth shortest, for large n, is probably better, but systematically checking all these options is expensive. Instead, consider an algorithm that encourages random deviations from known paths: it says, “What if this area of the graph, along the shortest path, is broken? Then what would the shortest path be? Now what about that area? Or both?” and so on, iteratively building a set of increasingly diverse alternatives. Intuitively, this heuristic simulates random breaking of the graph to build an arsenal that we hope will stand up against any specific issue with the actual graph.

This is the core of the contribution in my 2015 ICRA paper, together with applications for robotic motion planning and an analysis of the efficiency and quality of this approach. Also have a look at my code repository for this research.

OMPL 1.1 Released! 16 Nov 2015

Mark Moll

Recently, we released a new version of OMPL, version 1.1. We have added a ton of new planning algorithms and other features, while keeping the API more or less the same. In other words, there is no reason not upgrade right now! Below is detailed set of release notes:

• Added several new and updated planners:
  • Lightning: the Lightning Framework is a experienced-based motion planner that recalls from a database of previously generated paths the most similar one to the current planning problem and attempts to repair it, while at the same time planning from scratch in a different thread.
  • Thunder: the Thunder Framework is essentially an improved version of Lightning. It stores previously generated paths in a combined roadmap, thereby offering more opportunities for reuse of partial paths. The roadmap is sparse while still guaranteeing asymptotic near-optimality. This is done by borrowing ideas from the SPARS algorithm.
  • Informed RRT*: a variant of RRT* that uses heuristics to bound the search for optimal solutions.
  • Batch Informed Trees (BIT*): an anytime asymptotically optimal algorithm that uses heuristics to order and bound the search for optimal solutions.
  • An updated version of the Lower-Bound Tree Rapidly-expanding Random Tree (LBT-RRT) and a lazy version, LazyLBTRRT. These algorithms use Lifelong Planning A* and Dynamic Single-Source Shortest Path over graphs as subroutines, but these subroutines might be more generally useful in other algorithms as well.
  • An updated version of TRRT as well as a bidirectional version of TRRT (BiTRRT)
  • An updated version of FMT* that, among other things, caches collision checks.
• New web-based based version of OMPL.app! The web app has all the functionality of the standalone GUI. In addition, it allows you to interactively construct benchmark jobs that can be submitted to a benchmark server. We have a public version of the web app and benchmarking server running at http://omplapp.kavrakilab.org, but the web app and benchmark server can also be run locally.
• There are two new concepts, ompl::base::InformedSampler and ompl::base::InformedStateSampler, that capture the idea of using information about the state space and the current solution cost to limit future search to a planning subproblem that contains all possibly better solutions. The ompl::base::PathLengthDirectInfSampler is derived from InformedStateSampler and can be used to limit sampling to only those states that can lead to a shorter path than the best-found solution so far. This sampler is used in ompl::geometric:InformedRRTstar and ompl::geometric::BITstar.
• The ompl::geometric::PathSimplifier can now also optimize a path with respect to a (sampleable) goal. This means, for example, that a solution path is no longer “stuck” with an awkward inverse kinematics solution for a goal.
• Added a plannerarena script to simplify running Planner Arena locally.
• Added a new planner termination conditions that allow one to terminate after a fixed number of iterations.
• The GNAT data structure for nearest neighbor queries has been updated and should be faster in general. There is now also non-threadsafe version of the GNAT data structure that is automatically selected for single-threaded planners. This version should be even faster.
• Added an option to turn off the path simplification in benchmarking.
• Added support for parametrized benchmarks. Planner Arena can show performance across values for a given parameter.
• Made it easier to get repeatable runs of an algorithm by enabling the user to set the seed of the local random number generators (i.e., not just the global seed).
• The OMPL blog is now Jekyll-based and hosted as a repository on GitHub. If you have a project that uses OMPL, you can send us a pull request (please check with us first whether it would be appropriate for the blog before you write content).

Report on the First MoveIt! Community Meeting 14 Sep 2015

Sachin Chitta

(Editor’s note: Reposted from Sachin Chitta’s blog)

Thank you for coming to the MoveIt! Community Meeting and thanks to the presenters for making this a huge success. There were more than 240 people registered and more than 150 who attended from all over the world. I apologize to those running Ubuntu who had issues joining us (using Chrome was the suggested solution for the future).

MoveIt! now has a twitter account for updates: @moveitrobot

Meeting Video

The slides from the meeting are posted here.

or_ompl — OpenRAVE bindings for OMPL 29 Oct 2014

Michael Koval

A guest post from Michael Koval at CMU:

The Personal Robotics Lab at CMU is excited to announce or_ompl, an OpenRAVE planner plugin that provides bindings for OMPL. This short video outlines several key features of the plugin:

In summary, or_ompl enables you to:

• Plan in OpenRAVE using the full suite of OMPL geometric motion planners
• Call OMPL’s optimal motion planners, like BIT*, PRM*, and RRT*, in an anytime fashion
• Shortcut trajectories using OMPL’s geometric path simplifier
• Easily expose new OMPL motion planners to OpenRAVE

The source code is BSD licensed and available on Github:

https://github.com/personalrobotics/or_ompl

See the README for installation and usage instructions. Please do not hesitate to open an issue or pull request on Github if you have any issues.

Extended support to optimal planners: CForest + tree pruning 21 Oct 2014

Javier V Gomez

The OMPL has extended its capabilities regarding optimal planning by implementing the CForest parallelization framework and the RRTstar (RRT*) tree pruning option. Both of them work under the same assumptions:

• The configuration space obeys the triangle inequality.
• There exists an admissible heuristic (for the optimization objective!).

We are looking forward you to use these new features and hear your feedback!

RRTstar tree prunning

Pruning was implemented as a tool for the CForest parallelization framework. However, as it is a useful tool (that other RRT* versions also use) we decided to implement it as an independent feature.

Every time RRT* finds a new, better solution, it is possible to discard those nodes of the tree that surely will not lead to a better solution. Moreover, it is also possible to immediately discard those samples that will not be useful, thus saving tons of time.

Therefore, the search tree will focus on those zones in which the solution can be only improved. This implies a speedup in the convergence rate since the probability of sampling states close to the optimal path is increased.

How to use it?

To use this feature just use the RRTstar planner as always and add a call to its setPrune() method with a true argument.

Results

These two images correspond to benchmarks carried out in the alpha 1.5 puzzle and with the kinematic chain problem using 10 links, respectively.

RRT* vs pruned RRT* in alpha 1.5 puzzle

RRT* vs pruned RRT* in a 10-links kinematic chain

We highly recommend to benchmark whether the prune utility is interesting for the problem you aim to solve. We never found standard RRT* to outperform pruned RRT*, however for some specific (usually easy to solve) problems the gain is too little. For the general case, the convergence towards the optimal path is greatly improved.

CForest parallelization framework

Now, let’s move to the next level. What if instead of one single random tree we grow many of them in parallel?  Probability of finding a better solution increases. But now, going a bit further, what if we allow the trees to communicate each other, so they share the best paths found so far? Then, we would be thinking in the CForest parallelization framework.

This very simple, yet powerful idea is already implemented in OMPL! CForest expands many trees in different threads (running underlying planner instances of regular planning algorithms, such RRT*). Every time a new, better solution is found by one of the trees, it is shared to all other trees. Also, trees are pruned according to the new shared path.

How to use it?

CForest has been included as a new geometric planner, in order to make it easier for the user. Create it as any other planner. By default, it will expand as many trees as available cores in your machine. However, you can configure it to any number of threads and also you can decide if you want to prune the trees.

New demos have been included into OMPL that show how to use and configure it.

Currently, RRT* is the only underlying planner for the CForest available in OMPL. However, any incremental, optimal planning algorithm could be intergrated into CForest. Check the code documentation to learn how.

Results

CForest implies a huge performance improvement in most of the problems. The convergence towards a path with optimal cost is much faster, as the following images show, running CForest with 4, 8 and 16 trees (in independent CPUs):

CForest vs RRT* in alpha 1.5 puzzle

CForest vs RRT* in the circles grid benchmark

CForest vs RRT* in a 10-links kinematic chain

Only 1 cpu available?

Do not worry! Take a look at the following picture. There, CForest expanding 2, 4 and 8 threads in a single core is being compared to standard RRT* and pruned RRT* pruned in the alpha 1.5 puzzle. Even this way, we are getting an important speedup! This is what CForest authors called sequential CForest. In this case, we built it implicitly as each tree is running in a separate thread, but all threads are running in the same CPU.

Be careful, if you increment the number of threads too much, it could happen that CForest introduces too much overhead and it can result in a performance worsening.

Single core CForest vs RRT* in a alpha 1.5 puzzle

Important Notes

CForest is designed for single-query, shortest path planning problems. This does not restrict it to path length optimization. Optimizing path execution time or energy consumed can be treated as shortest path length.

If you want to push CForest to its limits, please read carefully the API documentation and the optimal planning section. There are many differences with the paper version that you could take into account. We aimed to keep the code as simple as possible.

Bringing Belief Space Planning to OMPL 21 Oct 2014

Saurav Agarwal

Editor’s note: this is a guest post by Saurav Agarwal from Texas A&M.

Sampling based deterministic motion planning has shown great success in the past. However, as we progress towards more realistic modeling and planning for robotic systems, we need to account for uncertainties in our systems. Uncertainties mainly arise from:

1. Sensing or measurement noise (also called observation noise) i.e. sensors do not give perfect measurements, instead the measurements are corrupted by some noises
2. Motion uncertainty (also called process noise) i.e. the robot’s actuators make some errors in following the control commands
3. Modelling uncertainties i.e. our physical models of the system and environment usually have approximations/errors.

Thus, all these uncertainties call for a new class of motion planners, planners that can reason about the uncertainty in the system and then make smart (optimal) decisions.

In the absence of the exact state of the system due to sensing uncertainty, the robot forms a probability distribution over all possible states (referred to as a belief) and the planning and decision making has to happen based on the available belief of the system. However, motion planning in the belief space is a challenging problem due to the computational intractability of its exact solution (it can be classified as a Partially Observable Markov Decision Process). This problem becomes even more challenging in changing environments as the robot needs to reason not only about its own state but about external disturbances such as moving people and unforeseen obstacles (furniture, doors etc.).

Research conducted at Texas A&M University and recently joint work with MIT has resulted in an innovative solution called FIRM (Feedback-based Information Roadmaps) that is able to transform the intractable POMDP problem to a graph based structure in belief space. FIRM essentially constructs a roadmap graph in belief space and is able to generate feedback policies and give guarantees on the minimum success probability for a planning task. FIRM generates a policy that is able to guide the robot through paths that minimize the localization uncertainty and collision probabilities. Thus, FIRM provides motion plans that are able to deal with uncertainties in our systems. Further, FIRM is also capable of dynamic replanning in real-time to deal with unforeseen changes in the environment and robot’s state (kidnapping).

We have successfully applied this method in simulation and on real physical robots and demonstrated its robustness in realistic scenarios. Here is a video that shows FIRM running on an IRobot Create that is tasked with visiting multiple goal locations sequentially in an everyday office environment. The goal points are not predefined and submitted by user online. Notice that there are people walking, doors are opened and closed randomly and the robot is kidnapped to unknown locations. Our robot is able to handle all these challenges and accomplish its mission!

We presented our work at ICRA 2014, Hong Kong and you can find all related papers here: http://www.mit.edu/~aliagha/Web/publications.htm

Further, we are now linking FIRM with OMPL. You can currently download a working simulation of the FIRM graph construction and plan execution from https://github.com/sauravag/FIRM-OMPL

The code is provided as a Code::Blocks project, feel free to play with it and give us your feedback!

Saurav Agarwal
Ph.D. Candidate
Aerospace Engineering
Texas A&M University
Research Web: http://edplab.org
Personal: http://sauravag.com

ICRA 2014 Roundup 26 Jun 2014

Mark Moll

A couple weeks ago the IEEE International Conference on Robotics and Automation took place in Hong Kong. There were many papers that used OMPL one way or another. Here is an overview of some of them:

1. Asymptotically near-optimal RRT for fast, high-quality, motion planning
   Oren Salzman and Dan Halperin
   Blavatnik School of Computer Science, Tel-Aviv University, Israel
   This paper describes Lower Bound Tree-RRT (LBT-RRT), a single-query sampling-based algorithm that is asymptotically near-optimal. By varying the approximation factor one can get behavior that ranges from regular RRT to RRT*. An implementation of this algorithm has been contributed by the authors.

2. Fast Stochastic Motion Planning with Optimality Guarantees using Local Policy Reconfiguration
   Ryan Luna, Morteza Lahijanian, Mark Moll, and Lydia E. Kavraki
   Department of Computer Science, Rice University
   This paper presents a framework for fast reconfiguration of local control policies for a stochastic system to satisfy a high-level task specification. It uses Bounded Markov Decision Processes at the high-level. Some follow-up work will be presented at AAAI 2014 and at WAFR 2014. See http://www.kavrakilab.org/biblio.

3. Improving Efficiency of Intricate Manipulation Planning through Mapping of Grasp Feasibility Zones
   Mihai Pomârlan [1] and Ioan A. Sucan [2]
   1 Universitatea Politehnica Timisoara, facultatea ETC
   2 Willow Garage (now at Google[X])
   This paper is on pre-computing (off-line) a ranking of grasping poses for each configuration in a roadmap constructed for the object to be manipulated. This allows choosing more likely grasping poses when manipulating known objects and leads to faster computation of manipulation plans. The paper uses the LazyPRM and RRTConnect implementations from OMPL.

4. AUV Mission Control via Temporal Planning
   Michael Cashmore [1], Maria Fox [1], Tom Larkworthy [2], Derek Long [1], and Daniele Magazzeni [1]
   1 King’s College London, UK
   2 Heriot-Watt University, Edinburgh, UK
   This paper combines high-level temporal planning with low-level motion planning. As the title suggests, they apply this to AUVs and have some nice experimental results.

5. SMT-Based Synthesis of Integrated Task and Motion Plans from Plan Outlines
   Srinivas Nedunuri, Sailesh Prabhu, Mark Moll, Swarat Chaudhuri and Lydia E. Kavraki
   Department of Computer Science, Rice University
   In this paper the authors propose an integrated task and motion planning framework called ROBOSYNTH that translates high-level task specifications in the form of plan outlines to detailed low-level plans. In a plan outline there are “holes” that ROBOSYNTH will fill in. It can, e.g., determine a low-level path for picking up an object subject to user constraints or a feasible order in which dishes could be loaded in the dishwasher. This is done by combining an SMT solver with OMPL.

6. A Sampling-Based Strategy Planner for Nondeterministic Hybrid Systems
   Morteza Lahijanian, Lydia E. Kavraki, and Moshe Y. Vardi
   Department of Computer Science, Rice University
   This paper presents a strategy planner for hybrid systems with nondeterministic discrete transitions. Examples of such systems include a car-like robot with faulty transmission (gearbox) causing nondeterministic switching between gears.

7. Motion Planning for Robotic Manipulators with Independent Wrist Joints
   Kalin Gochev [1], Venkatraman Narayanan [2], Benjamin Cohen [1], Alla Safonova [1] Maxim Likhachev [2]
   1 Department of Computer and Information Science, University of Pennsylvania
   2 Robotics Institute, Carnegie Mellon University
   This paper proposes to decompose high-dimensional planning problems into lower-dimensional subproblems and solve them with deterministic heuristic search methods. The approach is compared to OMPL and is shown to have some advantages to some planner in OMPL on some metrics. It discusses trade-offs in path quality, speed, and completeness.

8. Gaussian Process Kernels for Rotations and 6D Rigid Body Motions
   Muriel Lang, Oliver Dunkley and Sandra Hirche
   Technische Universität München
   This paper extends Gaussian Processes to quaternions and dual quaternions, which is useful for learning the dynamics of rotational or rigid body motion, respectively. The approach is shown to be superior to using Euler angles. OMPL is used to generate trajectories on which the approach is tested.

9. Interactive-rate Motion Planning for Concentric Tube Robots
   Luis G. Torres, Cenk Baykal, and Ron Alterovitz
   Department of Computer Science, University of North Carolina
   The authors are interested in planning minimally invasive surgery using concentric tube robots. The approach combines a precomputed roadmap with an iterative IK solver to obtain interactive-rate planning times.

10. Poisson-RRT
    Chonhyon Park and Jia Pan and Dinesh Manocha
    Department of Computer Science, University of North Carolina
    [project page]
    The authors present an RRT-based motion planning algorithm that uses the maximal Poisson-disk sampling scheme. The approach exploits the free-disk property of the maximal Poisson-disk samples to generate nodes and perform tree expansion. The authors demonstrate that the approach can be parallelized on CPUs as well as GPUs.

If you use OMPL in your work, please let us know! We are always interested to how and where it is used.

Combining OMPL and MORSE 11 Oct 2013

Caleb Voss

The Modular OpenRobots Simulation Engine (MORSE) is an environment that allows the user or other code to direct a robot in a physics simulation. The program is built on the 3D modeling software, Blender, which can be used to design and animate simulation scenarios. We saw an opportunity to enhance both the functionality of MORSE and the user accessibility of OMPL by combining the two of them, thus enabling the modeling, solving, and animating of a planning problem from the same interface.

A user of OMPL may wish to work with a problem where the motion of one or more robots and its environment are governed by the physics of the system and input controls to the robot(s). For example, suppose the user has a car-like robot, for which a desired velocity and turning speed can be specified, in an environment where the robot and other objects can collide and be pushed around. Previously, to plan a path in a problem like this, one could build a model of the environment through the use of primitive geometric objects and joints in the Open Dynamics Engine (ODE), and then invoke the OMPL interface for ODE to find a solution. Encoding such a model is tedious work and error-prone, as the user must visualize what all the vertex coordinates and joint relationships are describing. After OMPL provides a path, the user may wish to animate it, which requires supplying a rendering engine to draw the objects.

There is a more natural method of modeling a path planning problem that is less time-consuming and easily scalable to larger, more complicated systems. By introducing MORSE and the Blender modeling environment to OMPL, we have made utilizing the library more accessible and desirable to those who would rather not spend so much time and effort upfront on the modeling of their problems. MORSE is designed to facilitate the simulation of robot motion in dynamic environments using Blender’s physics engine and renderer. Entirely encapsulated in the MORSE/Blender workflow is the ability to construct environments and introduce robots from the MORSE library, complete with physical attributes like mass, friction, and joints with other objects, and then to simulate the system based on control inputs. We have designed an interface for OMPL to supply these inputs, thereby directing the simulation in order to solve a path planning problem.

This interface is in the form of a Blender add-on that supplies a number of useful functions, allowing the entire process to take place in the same workspace, without any tedious coding. First, in defining the problem, there are functions to link MORSE robots into the environment, set up bounds on the control inputs, and define goal positions for objects in the environment. The user then presses a button to start planning, after which the solution path is saved to disk in a concise format. Saved paths may be played back for review; at this time, frame-by-frame animation data is generated in the Blender file format. Finally, the user can dress up the scene in this file, tweak the results, and use the full extent of Blender’s rendering capabilities to create a high-quality animation of the solution. Refer to the usage tutorial for a detailed description of how to utilize our Blender add-on.

Our method allows the planning for these systems to take place on a prototype of the scene with simple models, since we can substitute and add fancier models for the rendering after the solution is saved. Here are some demonstration videos of the end result of this process:

This first video illustrates a problem in which the goal is defined by the location of an object other than the robot. The green box must be pushed onto the red ‘X’. In order to reach the box, OMPL must discover how to drive the car off the ramp quickly enough to make it to the other side.

The second video shows a simple puzzle involving two robots that must coordinate movements in a specific order so they can swap places, since the red ball is impeding any attempts to merely drive straight for the goal. The blue robot must first push the ball past the intersection to get it out of the way before the two robots may exchange places.

We hope you enjoy!

Extending OMPL support for optimal path planning 03 Oct 2013

Luis Torres

In some motion planning problems, you might not want just any valid path between your start and goal. You might be interested in the shortest path, or perhaps the path that steers farthest away from obstacles. In these cases you’re looking for an optimal path: a path that satisfies your constraints (connects start and goal states without collisions) and also optimizes some path quality metric. Path length and path clearance are two examples of path quality metrics, or optimization objectives.

As part of the 2013 Google Summer of Code, I have worked with Ioan Şucan and Mark Moll to extend OMPL’s support for optimal motion planning. Over the summer we made the following extensions to OMPL:

Generalized Optimization Objectives

We refined and extended the interface for defining optimization objectives for planning in OMPL. Users can define their own objectives by implementing the ompl::base::OptimizationObjective interface. OMPL also includes predefined implementations of the following optimization objectives:

• path length
• minimum path clearance
• general state cost integrals
• mechanical work

We also include functionality to easily combine optimization objectives using the ompl::base::MultiOptimizationObjective class.

Extending Optimizing Planner Support for Generalized Optimization Objectives

Motion planners that attempt to optimize path quality objectives are known as optimizing planners. OMPL comes with some optimizing planners, but before this summer they only supported the optimization of path length. We extended the following optimizing planners in OMPL to support generalized optimization objectives:

• PRM*
• RRT*
• TRRT

Check out the results of running RRT* on a motion planning problem using different optimization objectives:

Minimizing path length:

Maximizing minimum path clearance from obstacle:

Balancing clearance and path length:

More Detailed Planner Benchmarks

OMPL offers a powerful framework for benchmarking motion planning algorithms. Before the summer, the framework could only collect individual properties of a planner at the end of each planning run, such as execution time, number of configuration samples taken, or solution path length. Now, the benchmarking framework can collect data about how properties of a planner change over time in a planning run. This is very useful for benchmarking optimizing planners so we can analyze how the solution path quality improves over the planner’s execution. Planners can be implemented to specify any number of properties to “report” during execution, and during benchmarking we collect the reported data in a separate thread in such a way that doesn’t interfere with planner timing.

We then extended OMPL’s benchmark analysis script to output plots that chart the average values of these “planner progress properties” over time. Here’s a plot (automatically generated by our script) charting path length versus time for the RRT* planner on an example planning problem:

Want to learn more?

Check out our optimal planning overview for a deeper introduction to the optimal planning framework, as well as the optimal planning tutorials in the tutorials page.

MoveIt! Release 27 May 2013

Mark Moll

(Reposted from the Willow Garage blog)

UPDATE: the slides and video of the MoveIt! presentation at ROSCON 2013 have now been posted

Willow Garage is proud to announce the initial release of MoveIt! : new software targeted at allowing you to build advanced applications integrating motion planning, kinematics, collision checking with grasping, manipulation, navigation, perception, and control. MoveIt! is robot agnostic software that can be quickly set up with your robot if a URDF representation of the robot is available. The MoveIt! Setup Assistant lets you configure MoveIt! for any robot, allowing you to visualize and interact with the robot model.

MoveIt! can incorporate both actual sensor data and simulated models to build an environment representation. Sensor information (3D) can be automatically integrated realtime in the representation of the world that MoveIt! maintains. CAD models can also be imported in the same world representation if desired. Collision-free motion planning, execution and monitoring are core capabilities that MoveIt! provides for any robot. MoveIt! updates its representation of the environment on the fly, enabling reactive motion planning and execution, which is essential for applications in human-robot collaborative environments.

MoveIt! interfaces with controllers through a standard ROS interface, allowing for ease of inter-operability, i.e. the ability to use the same higher-level software with a variety of robots without needing to change code. MoveIt! is architected to be flexible, using a plugin architecture to allow users to integrate their own custom components while still providing out-of-the-box functionality using default implementations. Furthermore, the ROS communication and configuration layer of MoveIt! is separated from core computational components such as motion planning or collision checking, the latter components being provided separately as C++ libraries.

Workspace analysis tools allow robot designers to test out the capabilities of their robot designs before building the hardware, using environment and object specific task specifications to quantify the workspace characteristics of different designs. This reduces costly mistakes and iterations in the design stage. We are actively working on completing the pick and place capabilities in MoveIt!, integrating with object recognition, perception, and grasping to allow manipulators to execute generalized pick and place actions.

Get MoveIt!

More Information about MoveIt!, including instructions on how to get and use it, can be found on the MoveIt! website. MoveIt! is currently an alpha release.

Catch the MoveIt! team at ICRA 2013 and ROSCON:

• ICRA Booth Demo: The Willow Garage Booth will have a MoveIt! demo as part of the exhibit. The booth is open on Tuesday, Wednesday and Thursday (May 7-9, 2013).
• ICRA Workshop Talk: Sachin Chitta is giving a talk on “MoveIt!: Software for Rapid Development of New Robotics Applications” at the ICRA Industrial Mobile Assistance Robots Workshop on Monday, May 6, 2013.
• ICRA Tutorial: MoveIt! will be presented at a tutorial on Friday May 10, 2013: Motion Planning for Mobile Manipulation: State-of-the-art Methods and Tools, organized by Sachin Chitta, Ioan Sucan, Mark Moll, Lydia Kavraki and Maxim Likhachev.
• ROSCON Keynote Talk: Sachin Chitta, Ioan Sucan and Acorn Pooley will be at ROSCON presenting MoveIt! at 9:30 AM on Saturday May 10, 2013.

Acknowledgements

Willow Garage gratefully acknowledges the contributions of the following people to MoveIt! and associated packages that MoveIt! uses and depends on:

• Lydia Kavraki, Mark Moll, and associated members of the Kavraki Lab (Rice University) for developing OMPL - a suite of randomized planners that MoveIt! uses extensively.
• Dinesh Manocha and Jia Pan of UNC Chapel Hill for developing FCL - a package of collision checking algorithm used extensively by MoveIt!
• Maxim Likhachev (CMU), Ben Cohen (Penn) and Mike Phillips (CMU) for developing SBPL, a search-based planning library integrated with MoveIt!
• Armin Hornung, Kai Wurm, Maren Bennewitz, Cyril Stachniss, and Wolfram Burgard for developing Octomap - software for 3D occupancy mapping used by MoveIt!
• Mrinal Kalakrishnan, Peter Pastor and Stefan Schaal at USC for developing STOMP, the distance field components in MoveIt! and the implementation of the CHOMP algorithm in Arm Navigation
• Dave Coleman from the University of Colorado, Boulder for developing the MoveIt! Setup Assistant and adding documentation to the MoveIt! website.

MoveIt! evolved from the Arm Navigation and Grasping Pipeline components of ROS and we gratefully acknowledge the seminal contributions of all developers and researchers to those packages, especially Edward Gil Jones, Matei Ciocarlie, Kaijen Hsiao, Adam Leeper, and Ken Anderson.

We also acknowledge the contributions of the Willow Garage interns who have worked on MoveIt!, Arm Navigation and associated components, members of the ROS and PR2 communities who have used, provided feedback and provided contributions to MoveIt! and Arm Navigation and members of the ROS community for developing the infrastructure that MoveIt! builds on.

We also acknowledge the contributions of the ROS-Industrial consortium led by the Southwest Research Institute for supporting and building up infrastructure for applying MoveIt! and Arm Navigation to industrial robots and environments. Similarly, we acknowledge the contributions of Fraunhofer IPA to MoveIt! and support for the ROS-Industrial effort in Europe.

For more information visit moveit.ros.org

OMPL Google Summer of Code 09 Apr 2013

Mark Moll

We have been accepted as a mentoring organization for the Google Summer of Code 2013.

• Check the Google SoC website for general information about the Google Summer of Code.
• You will find additional information on their page on Advice for Students.
• Please consult the official timeline when you prepare your proposal. .

We have put together a list of ideas, but we are open to other suggestions. Even if you have no interest in participating in the Google Summer of Code, let us know what you would like to see added to OMPL.

Please email Mark Moll, mmoll (AT) rice.edu, if interested.

OMPL wins the 2012 OSS World Challenge Grand Prize 05 Dec 2012

Mark Moll

Recently, the Open Motion Planning Library (OMPL) won the Grand Prize in the Open Source Software World Challenge. This is a yearly competition organized by the South Korean government and is intended to promote open source software and expand various exchanges among open source software developers all over the world. Last year’s winner was the Point Cloud Library, so we are in good company! This year, 55 teams from 23 countries participated in the international division. The second and third prize winners were:

• Stan, a package for obtaining Bayesian inference using the No-U-Turn sampler, a variant of Hamiltonian Monte Carlo, from Columbia University.
• DDT, a system for testing closed-source binary device drivers from the Dependable Systems Lab at EPFL, Switzerland. .

Besides the international category, there were also several categories at the national level, ranging from elementary school level (!), to college level. Web development and mobile apps seemed to be of particular interest. Several high-ranking government officials were in attendance, gave speeches, and handed out awards. Mark Moll attended the award ceremony and gave a talk about OMPL.

Easier integration with the MoveIt wizard and advancements in OMPL 07 Sep 2012

Mark Moll

Watch what Dave Coleman has been doing with MoveIt! and OMPL during his internship at Willow Garage this summer:

Click here for more information.

We expect that his implementation of T-RRT will be integrated with the OMPL distribution soon.

ICRA 2012 / ROSCON update 13 Jun 2012

Mark Moll

It was good meeting many of the OMPL users at ICRA 2012 and ROSCON a couple weeks ago. There were a number of ICRA talks that featured OMPL, directly or indirectly, that I’d like to point out:

Towards Small Asymptotically Near-Optimal Roadmaps, by James D. Marble and Kostas E. Bekris

This work shows how to construct provably near-optimal roadmaps that are a fraction of the size of roadmaps constructed with kPRM* (an algorithm in OMPL that converges to optimal roadmaps). A small roadmap makes retrieval of paths much faster. With a bit of smoothing added, near-optimal paths get even closer to optimal. We expect that the algorithm described in this paper will become part of OMPL in the near future.

Real-time footstep planning for humanoid robots among 3D obstacles using a hybrid bounding box, by Nicolas Perrin, Olivier Stasse, Florent Lamiraux , Young J. Kim and Dinesh Manocha

This paper describes a footstep planning algorithm that does not simply reduce the problem to a 2D problem (as is often done), but allows the robot to step over obstacles if necessary. Although we didn’t design OMPL specifically for this application, the authors mention that OMPL provides exactly the interface they needed. Neat!

A Robot Path Planning Framework that Learns from Experience, by Dmitry Berenson, Pieter Abbeel, and Ken Goldberg

This paper describes an algorithm that does the following two things in parallel when given a motion planning query as input: (1) plan from scratch, and (2) deform/extend a path for a similar motion planning query that was solved previously to satisfy the current query. Whichever finishes first terminates the other process. New paths are added to a path library, so that over time very little planning from scratch will have to be done. The initial implementation might have been done in OpenRAVE (I think), but has since been implemented using OMPL and ROS. See the Lightning Planning Framework for ROS.

FCL: A General Purpose Library for Proximity and Collision Queries, by Jia Pan, Sachin Chitta, and Dinesh Manocha

This is only indirectly related to OMPL. OMPL.app can use FCL for collision checking, and FCL is already used in ROS together with OMPL. There is a lot more that can be done with FCL in the context of motion planning. The proximity to obstacles could be exploited to create a more intelligent configuration sampler.

If you use OMPL in your work, please let me know.

At ROSCON, Sachin Chitta and Ioan Sucan gave a talk about MoveIt!, the new motion planning stack in ROS. It provides a common interface to motion planning libraries in ROS (including OMPL). It will eventually replace the arm navigation stack. Watch the whole talk:

OMPL development activity 20 May 2012

Mark Moll

(Movie generated with gource.)

Planning using the Vortex physics engine 27 Apr 2012

Mark Moll

OMPL can be used to plan for complex systems that can be simulated by a physics engine. From a planning perspective, the physics engine is a black box that acts like a state propagator. OMPL includes support for the Open Dynamics Engine (OpenDE), but support for other physics engines can be added using the OpenDE bindings as a starting point. To show that this can indeed be done, I have developed bindings for Vortex. Vortex is a commercial physics simulator. It contains incredibly detailed models for vehicles (transmission, gears, suspension, etc.) and surface interactions. It also has snazzy OpenSceneGraphics built in. The bindings mirror the structure of the OpenDE bindings. To use these bindings, the user has to implement some derived classes. The derived VortexEnvironment class needs to define the simulated environment and how controls map forces, torques or velocities in the simulation. The derived state space is responsible for defining a distance between states. Interestingly, the VortexStateSpace represents states completely opaquely: the dimensionality is unknown and the internal structure is unknown. There is just a distance function and a user-defined projection to a low-dimensional embedding. This is sufficient for planners like KPIECE, SBL, and EST (but not RRT, which needs to be able to sample uniformly random states). I have attached a patch for OMPL 0.10.2 that adds vortex support. You can also download and extract the tar file which adds the Vortex-related files and overwrites a few files to add Vortex support.

Below is a simple demo program (included in the patch and tar file) that illustrates very basic usage of the Vortex support. In this demo the objective is to push a box from a starting position to a desired goal region. Once a solution is found, it uses the Vortex graphics capabilities to play back the path.

The inputs do not need to correspond to forces or torques. They can also be inputs to a controller. For example, if you want to plan for a VxVehicle, you could have function that maps OMPL controls to the simulator like so:

virtual void applyControl(const double *control) const
{
    double throttle = control[0], steering=control[1];
    if (throttle>=0)
        vehicle->getEngine()->setInputThrottle(throttle);
    else
        vehicle->setBrakeInput(throttle,throttle);
    vehicle->setSteeringInput(steering);
}

The attached code has only been lightly tested, but if you find it useful or end up extending it, please let us know.

Download:

[patch-vortex.diff] [vortex.tar]

From differential equations to planning in one function 28 Mar 2012

Ryan Luna

When planning for a physical robot, the motion of the system is constrained due to those pesky “laws of physics.” If you drive your car to the grocery store, you cannot directly control the position of the car or even how fast it moves down the street. Terrifying! You can only control the rate at which the velocity changes (acceleration) and the direction the car is steering toward. Furthermore, the acceleration of our cars is bounded; we cannot stop immediately or achieve a particular speed instantly. This makes the car a second-order system. We can directly control the acceleration, but the velocity and position are the result of the changes in acceleration and steering angle over time. The same scenario applies to physical robots: a typical system cannot be explicitly controlled. Instead, we apply a series of inputs (like pressing the gas or the brake in a car) to control motors and other actuators to execute a motion plan. Mathematically we can describe the motion of a robot as a set of ordinary differential equations which depend on an input control and time. The exact state of a robot can then be computed by integrating the equations given the control input and duration to apply that input.

OMPL’s StatePropagator concept can be used to incorporate integration of the differential equations for the system, but the user is tasked with implementing the ODE, integrating the equations and updating the state for the system, as in the rigid body planning with integration and controls demo. This task is simplified in OMPL v0.10 with the addition of the ODESolver class, which takes care of numerical integration and state updates. All the user has to do is provide the ODE definition in a single function:

void ODE(const ODESolver::StateType &q, const Control *c, ODESolver::StateType &qdot)
    /* Insert code here */
}

This method takes the input state q and a control c, and computes the change in the value of q, storing the differential into qdot. The ODESolver wraps around the open-source ODEInt library, which provides methods for high-order numerical integration. Gone are the days of worrying about the numerical instability and potential large error of quick and dirty Euler integration. A StatePropagator object can be extracted from the ODESolver class which encapsulates the entire integration and update process.

A tutorial has been created to show how to use OMPL’s ODESolver class in further detail. A new demo is also bundled in OMPL v0.10, rigid body planning with ODESolver and controls, which shows a comparison with the “old” way of doing things and how the ODESolver simplifies this process. The ODEInt library is bundled with OMPL, so there is no extra dependency to install, and by using the ODESolver class you’ll be planning for your system in no time! Below are plots of actual paths computed using the ODESolver for an inverted pendulum and a second-order car:

Geometric planning for car-like vehicles 18 Mar 2012

Mark Moll

For several vehicle models it is possible to compute the optimal (i.e., shortest) path between a start and end state. The two most commonly studied vehicle models are the Dubins car and the Reeds-Shepp car. Both are first-order models. The Dubins car’s controls are: go straight, turn left, and turn right. The Reeds-Shepp car has the same controls, but can also execute them in reverse. The optimal paths for these vehicles can be computed analytically and consist of 3 to 5 circle and straight-line segments. In OMPL we could model cars with a system of differential equations and use a control-based planner. For the Dubins and Reeds-Shepp cars, however, we simply use a geometric planner and plan in SE(2). Rather than straight-line interpolation between states, we’d like to use the appropriate optimal path in our local planner. This is done by creating two new state spaces, ompl::base::DubinsStateSpace and ompl::base::ReedsSheppStateSpace, that override the distance and interpolate member functions. The distance is redefined to be the length of the optimal path connecting two states.

There is a demo program (demo_GeometricCarPlanning) that shows how to solve planning problems in this state space. Imagine a long corridor with the start and goal states of a Reeds-Shepp car at each end point, facing the wall. Normally, for a control based planner this could be a very challenging problem. With the Reeds-Shepp state space this is very easy and we get paths that looks like this:

(Note the extra zig-zag at the end; the paths are only optimal between sampled states, not globally optimal.)

The distance function is not a metric for Dubins cars, since it is not symmetric. With an optional flag you can create a DubinsStateSpace where distance is symmetrized (which essentially allows the car to instantaneously change direction when it reaches a sampled state). This is still not a metric, since it doesn’t satisfy the triangle inequality.

The computation of optimal paths is not optimized at all. For both Dubins and Reeds-Shepp cars all candidate solutions are computed and the shortest one is returned. There exist classification schemes that depending on the relative distance and orientation between two states can eliminate many of the candidate solutions. It may also be possible to use some form of memoization. The implementation of Dubins and Reeds-Shepp vehicles may be usable for other vehicle models. If someone wants to contribute code for the generalized solutions from Furtuna and Balkcom’s paper on Generalizing Dubins Curves: Minimum-time Sequences of Body-fixed Rotations and Translations in the Plane, that’d be much appreciated.

Finally, just because we can, we made some videos of the distances between a car at (0,0) and heading along the X-axis and all other possible poses. Each frame shows the distances for a particular heading at the endpoint. Black correspond to small distances and white corresponds to large distances.

Welcome! 15 Mar 2012

Mark Moll

Welcome to the OMPL Blog. Here, we’ll occasionally post messages about new and upcoming features in the Open Motion Planning Library. Not all these features will be prominently featured on the front page, so they might be easy to miss. In the near future, you can expect posts about: motion planning for Dubins and Reeds-Shepp cars, using the built-in numerical integration to plan for systems described by ordinary differential equations, and path optimization.