Planning Graph Completely Refactored

[Jmeyer1292]

This PR is a work in progress; I intend to expand a little more on what I did and why it should replace what's already here.

Forward

This pull request represents a complete re-write of the planning_graph data structure that underlies Descartes' existing planners. It strips away much of the abstraction that was previously there and focuses on storing the vast amount of graph data in a compact fashion that mirrors the actual problem that we solve.

I tried a lot of small ways to fix various issues that really bugged me in Descartes' existing code. After a lot of proto-typing and internal debate, I decided that to achieve the speed, space efficiency, and correctness that I wanted would require me to drop the use of boost graph altogether.

The highlights are (for larger search problems with many edges):

• ~2x faster
• 1/10 of the memory usage
• Less code (planning_graph.cpp 1060 -> 260 LOC; even extras are only a few hundred lines)
• Easier to reason about graph mutation

These comparisons are made with respect to the current indigo-devel branch and DO NOT take into account the cost of collision checking. That dominates everything, and we'll need a different approach to make that more manageable.

Ladder Graph

The biggest change introduced by this PR is the replacement of the boost::adjacency_list graph with a custom graph type called a LadderGraph.

Data Structure

Previously, Descartes used several structures to represent data:

• A boost graph which contained vertices (with IDs), and edges (with start ID, stop ID, and cost).
• A map relating IDs to JointPoints (which contain joint values and auxiliary info)
• A map relating the IDs of user input points to information such as what joint IDs are valid for that point, the next and former point, etc
• A map relating IDs to boost vertex_descriptors

All of this indirection comes with a cost. The last item has to be recomputed every time a modification is done to the graph because all of the vertex descriptors are invalidated when the graph changes.

The new algorithm has one data structure (psuedo-code):

struct Edge {
  double cost;
  unsigned next; // the index of the next rung to which this edge connects
}

using EdgeList = vector<Edge>;
struct Rung
{
  vector<double> joint_data; // one contiguous storage for all vertices; vertex 0 is at position 0..DOF-1, vertex 1 is at DOF..2*DOF-1, etc...
  vector<EdgeList> out_edges; // out_edges.size() == joint_data.size() / DOF
  ID id;
  Timing time;
}

struct LadderGraph
{
  size_t DOF
  vector<Rung> rungs;
}

This directly stores the information we care about with just about no redundancy or extra overhead. A vertex can be accessed through an indirect read without having to do any extra lookups. Mutating the graph is also cheap, as rungs can be swapped without copying the underlying data using C++11 move semantics.

Assumptions

The new graph makes some assumptions:

• Every "vertex" will always have the same size (DOF)
• Vertices in a given rung can only connect to vertices in the next rung

Furthermore this structure emphasizes graph mutation that changes everything at a rung level. If we wanted to remove individual joint points, it would require some additional work and might be less efficient for that.

Dijkstra's Search

Dijkstra's search algorithm is currently used to search the graph. This PR does not change that, but provides a custom implementation of it to work directly with our existing graph. I attempted to keep the existing boost graph algorithm by adapting the new data structure, but the adapter code, particularly the iterators, ended up being longer than the actual implementation of the search.

The search is based on the existing algorithm and uses Boost's BinaryHeap to efficiently implement a priority queue.

Performance Numbers

The above chart shows the relative speed and memory improvements of solving:

• The roscon 2015 demo
• 1200 points
• time step = 0.1s between points

The graph was solved at different angular discretizations shown along the bottom. The y value represents the time/space consumption of this PR relative to indigo-devel.

Future Improvements

isValidMove()

After the changes in this PR, the test to see if an arc between two vertices is possible (i.e. if does not exceed joint vel limits) is actually a huge expense. This function looks like:

for (std::size_t i = 0; i < from_joint_pose.size(); ++i)
  {
    double dtheta = std::abs(from_joint_pose[i] - to_joint_pose[i]);
    double max_dtheta = dt * velocity_limits_[i];
    if (dtheta > max_dtheta)
      return false;
}

This does a lot of redundant work with the cost computation, also called in the hot loop:

double vector_diff = 0;
for (unsigned i = 0; i < start_vector.size(); i++)
{
  double joint_diff = std::abs(end_vector[i] - start_vector[i]);
  vector_diff += joint_diff;
}

Also the max_dtheta is the same for all joints associated with a given user input point (because they all share a single timing constraint).

Long story, short: If codify the idea we will test for joint velocity limits directly into the planner we can get another 1.3 to 2x speed improvement depending on the graph size. This change involves adding a getJointVelocityLimits() to the robot model.

OMP

With these changes, OMP can be used to completely parallelize the graph building using three extra lines of code (plus some config file changes). I saw 3-4x speed improvements from this.

Known Issues

• There may be some regressions in warning messages generated by the graph
• The remove vertex function (not exercised by sparse planner) has not really been tested
• There is a bug with sparse planner and timing constraints (again/still). It is present in both indigo-devel and this PR. To fix other issues and do testing, this PR does include changes in #156.

[Jmeyer1292]

I recognize that is PR will need more documentation, but I really want to get some eyes on it: @shaun-edwards @jrgnicho

[shaun-edwards]

I almost want to approve this PR based on the exquisite write-up, but that would be frowned upon. I guess I have to review the code. :/

[Jmeyer1292]

I will need to expose the LadderGraph accessor in DensePlanner and SparsePlanner for users like @davetcoleman that need direct access. This change will break code like that, so I understand waiting or pushing it into a new release.

I think you'll find that accessing the underlying structure of the graph is far easier now.

[Jmeyer1292]

@shaun-edwards Thank you for looking at this.

[shaun-edwards]

@Jmeyer1292, overall this is a thing of beauty. I'm hesitant to accept this without some test comparing the new approach to the old approach. Also, as a path to release, I would recommend creating this as an alternate planner in indigo and then replacing the planner in jade.

[Jmeyer1292]

I will strike out changes related to SparsePlanner (for the moment), and create an alternative DensePlanner that uses this graph.

I will move toward adding unit tests that show the equivalence of these two methods. In my own testing, I have used my custom benchmarking branch to run tests (printing final cost and joint solutions along the way) which is based on the UR5. That has matured quite a bit, so maybe it's time to add that as a stepping stone to the final result.

[Jmeyer1292]

@shaun-edwards What do you think an appropriate name or namespacing for a new planner would be?

Is it appropriate to put things in an "experimental" namespace, ala descartes_planner::experimental::DensePlanner?

[BrettHemes]

Any progress updates on this? I am pretty interested in having this available in Indigo. I can maybe find some time to help test if that would speed up the merging.

[BrettHemes]

Dood, I merged #182 into this and ran some tests. I am seeing amazing results in my trials!

Here is one of the runs: Before refactoring (with #182): Calculating Joint Solutions: 4s Calculating Edge Weights: 3s Populating Graph Vertices: 2s Populating Graph Edges: 9s Total Run Time: 22s

After refactoring (this PR with #182): Calculating Joint Solutions: 4s Populating Graph Vertices: 0.2s Calculating/Populating Graph Edges: 2s Total Run Time: 7s

Awesome job :+1: :+1: :+1:

[BrettHemes]

Travis CI still has some problems with my cloning functions for reasons I haven't figured out, but here are the omp changes applied to your refactoring #1

Edit: Travis CI now passes for #182 and #1

[davetcoleman]

Impressive!

[Jmeyer1292]

@BrettHemes Your numbers are appreciated. Helps me feel more accomplished.

I'm open to other suggestions, but I'd really like to get these changes merged with some breaking changes to the base classes and without labeling things as experimental.

With that in mind, I'll aim to release a jade-devel version of this library (that you can still download and compile on Indigo).

[geoffreychiou]

+1

[Jmeyer1292]

Alright, I've left this repository alone for long enough. I'm rebasing on kinetic-devel and I'm going to start pushing these changes.

My plan forward:

1. Merge this pull request and the modifications made by @BrettHemes to parallelize the graph construction.
2. Add documentation and videos to the front page of the repo.
3. Fix / expand the unit tests. In particular I want known good paths and speed tests to be incorporated. A benchmarking suite will be super useful.
4. I will add the ability to serialize/deserialize motion plans and joint space graphs for testing and tooling purposes.
5. Implement iterative solving to help smooth out motion plans.
6. Implement other numeric techniques to optimize on rough solutions created by Descartes, especially for high DOF problems.
7. Refactor the basic user interface to give better feedback about error conditions. This might mean refactoring the high level interface.

[BrettHemes]

🙌