Add OMPL as an external (isolating the boost dependency)

[hongkai-dai]

I wish in this issue, we can discuss whether and how to bring in the sampling based motion planning into Drake.

Currently almost all motion planning in Drake is optimization based. On the other hand, there can be benefits to bring in sampling based motion planning, for at least two reasons

• Find a global solution.
• Handle non-differentiable constraint, such as collision avoidance constraint at some corner cases.

The immediate consumer for the sampling based planner could be

• Planning a collision free trajectory for the robot arm, or a collision free path for the HSR robot.
• Planning a good grasp

If we agree that we need a sample based planner in Drake, then we can either implement it ourselves, or use some third party library.

I list some existing third party libraries as follows.

1. OMPL, this is the plugin used in ROS MoveIt!. It supports PRM, RRT, and their variations.
2. OpenRave, this supports RRT, and a grasp gradient planner.

In the MATLAB implementation of Drake, we have a bi-directional RRT planner implemented, in https://github.com/RobotLocomotion/drake/blob/master/drake/matlab/solvers/MotionPlanningTree.m.

A quick implementation of RRT-connect in C++ should not be hard. @RussTedrake , do you think in the near future, we need to support a full spectrum of sample based planners as OMPL, including PRM, RRT, RRT*, LBTRRT, etc?

[jamiesnape]

OMPL is good, though it uses Boost.

[jamiesnape]

Implementation is easy enough, but then it has to be maintained. Using a pre-existing open-source library shares the maintenance cost and is good for the open-source community as a whole.

[hongkai-dai]

Copy from email so that we can keep track of the conversation, and get more input:
On Sep 14, 2016, at 10:15 AM, Russ Tedrake [email protected] wrote:

i wonder if it is possible to have a check in the add_mex() cmake script to make sure that boost does not get linked in (at least not directly) with anything that will share the memory space with matlab? that might be a sufficient guard.

On Sep 14, 2016, at 10:08 AM, Jeremy Nimmer [email protected] wrote:

Yes.

You can have Drake builds with Boost, or Matlab, but not both.
The superbuild could enforce the disjunction.

I have been considering adding Boost as a C++ dependency for other reasons.
If OMPL is highly desirable, I would be willing to help get Boost working in Drake.

Thanks,
Jeremy

On Wednesday, September 14, 2016 at 9:09:10 AM UTC-4, Grant Gould wrote:
My understanding is that matlab statically links a custom variant of boost, and that this means that if Drake uses boost then mexfile builds will get a mismatch between the non-custom boost headers used by Drake and the custom boost libraries linked by matlab.

If we would be linking in ompl, this would probable happen all over again.

I'm Cc:'ing sammy as he has a better idea than I do of the linker behaviour involved. However I suspect that we would have to find a way to make OMPL and mexfile builds mutually exclusive for this to work, or else find a way for them to live in separate process spaces.
--Grant

On Wed, Sep 14, 2016 at 12:57 AM Hongkai Dai [email protected] wrote:
I am looking at OMPL to figure out whether and how to integrate it into Drake. It seems a very good structure, which allows us to specify the robot and the environment using Drake. Unfortunately it uses boost library. Will this boost dependency prevent it from being integrated to Drake? My understanding is that we got rid of boost because MATLAB has its own boost. If we are only going to use OMPL in C++, not calling it from MATLAB, will this still be an issue?

Best,
Hongkai

[RussTedrake]

@RobotLocomotion/kitware -- what do you think about handling the boost dependency of OMPL?

[pangtao22]

@jamiesnape Hi Jamie, I am a new graduate student in Russ's group, and I will be working with Hongkai and you on integrating OMPL into Drake.

I tried to install OMPL using MacPort, as stated in the website. But I am getting the error message below:

dhcp-18-189-54-19:~ pangtao$ sudo port sync ; install ompl +app
Password:
---> Updating the ports tree
---> Computing dependencies for ompl
---> Dependencies to be installed: py27-pyqt5 py27-redis redis
---> Configuring py27-pyqt5
Error: org.macports.configure for port py27-pyqt5 returned: configure failure: command execution failed
Error: Failed to install py27-pyqt5
Please see the log file for port py27-pyqt5 for details:
/opt/local/var/macports/logs/_opt_local_var_macports_sources_rsync.macports.org_release_tarballs_ports_python_py-pyqt5/py27-pyqt5/main.log
Error: The following dependencies were not installed: py27-pyqt5 py27-redis redis
To report a bug, follow the instructions in the guide:
http://guide.macports.org/#project.tickets
Error: Processing of port ompl failed
dhcp-18-189-54-19:~ pangtao$ sudo port clean install ompl +app
Error: Port install not found

Moreover, I believe we need to build OMPL from source, like what I did with Drake, so that we can view and modify the source files, right? But currently build from source instruction is only given for Ubuntu. Is there a way to build OMPL from source on OS X?

Thank you!

[jamiesnape]

True, we will definitely not be using MacPorts. Try this for now:

1. Install Homebrew.
2. brew update
3. brew install cmake boost
4. git clone https://github.com/ompl/ompl.git
5. cd ompl
6. mkdir build
7. cd build
8. cmake ..
9. make

[pangtao22]

@jamiesnape Thanks! I am able to build OMPL following your instructions. I have a few more questions to ask.

1. Is there a way to setup the OMPL project in an IDE such as Clion? Or is it better to work in command line, using Vim as editor and run make to compile and build?
2. When I ran this tutorial program (http://ompl.kavrakilab.org/geometricPlanningSE3.html), the output I am getting is in the command line, like this:

OMPL version: 1.2.1
Info: RRTConnect: Space information setup was not yet called. Calling now.
Debug: RRTConnect: Planner range detected to be 1.006980
Settings for the state space 'SE3CompoundSpace0'

• state validity check resolution: 1%
• valid segment count factor: 1
• state space:
  Compound state space 'SE3CompoundSpace0' of dimension 6 (locked) [
  Real vector state space 'RealVectorSpace1' of dimension 3 with bounds:
• min: -1 -1 -1
• max: 1 1 1
  of weight 1
  SO(3) state space 'SO3Space2' (represented using quaternions)
  of weight 1
  ]
  Registered projections:
• Projection of dimension 3 Cell sizes (computed defaults): [0.1 0.1 0.1]

Declared parameters:
longest_valid_segment_fraction = 0.01
projection.cellsize.0 = 0.10000000000000001
projection.cellsize.1 = 0.10000000000000001
projection.cellsize.2 = 0.10000000000000001
projection.cellsize_factor =
valid_segment_count_factor = 1
Valid state sampler named uniform with parameters:
nr_attempts = 100
Start states:
Compound state [
RealVectorState [0.993948 -0.12907 -0.784821]
SO3State [0.134671 0.704035 -0.663013 -0.215901]
]
Goal state, threshold = 2.22045e-16, memory address = 0x7f8602e00c88, state =
Compound state [
RealVectorState [-0.802957 -0.169367 0.477989]
SO3State [0.23774 0.943757 -0.151153 0.173073]
]
OptimizationObjective = nullptr
There are 0 solutions
Info: RRTConnect: Starting planning with 1 states already in datastructure
Info: RRTConnect: Created 6 states (2 start + 4 goal)
Found solution:
Geometric path with 5 states
Compound state [
RealVectorState [0.993948 -0.12907 -0.784821]
SO3State [0.134671 0.704035 -0.663013 -0.215901]
]
Compound state [
RealVectorState [0.498571 0.141127 -0.364582]
SO3State [0.261813 0.559948 -0.645537 -0.448546]
]
Compound state [
RealVectorState [0.247566 0.0812466 -0.202089]
SO3State [0.274998 0.684529 -0.581723 -0.342629]
]
Compound state [
RealVectorState [-0.277696 -0.0440602 0.13795]
SO3State [0.274853 0.872844 -0.392859 -0.0908904]
]
Compound state [
RealVectorState [-0.802957 -0.169367 0.477989]
SO3State [0.23774 0.943757 -0.151153 0.173073]
]

Info: No planner specified. Using default.
Info: LBKPIECE1: Attempting to use default projection.
Debug: LBKPIECE1: Planner range detected to be 1.006980
Properties of the state space 'SE3CompoundSpace3'

• signature: 6 5 6 1 3 3 3
• dimension: 6
• extent: 5.0349
• sanity checks for state space passed
• probability of valid states: 1
• average length of a valid motion: 2.4723
• average number of samples drawn per second: sampleUniform()=7.69231e+06 sampleUniformNear()=2.91545e+06 sampleGaussian()=3.59712e+06
  Settings for the state space 'SE3CompoundSpace3'
• state validity check resolution: 1%
• valid segment count factor: 1
• state space:
  Compound state space 'SE3CompoundSpace3' of dimension 6 (locked) [
  Real vector state space 'RealVectorSpace4' of dimension 3 with bounds:
• min: -1 -1 -1
• max: 1 1 1
  of weight 1
  SO(3) state space 'SO3Space5' (represented using quaternions)
  of weight 1
  ]
  Registered projections:
• Projection of dimension 3 Cell sizes (computed defaults): [0.1 0.1 0.1]

Declared parameters:
longest_valid_segment_fraction = 0.01
projection.cellsize.0 = 0.10000000000000001
projection.cellsize.1 = 0.10000000000000001
projection.cellsize.2 = 0.10000000000000001
projection.cellsize_factor =
valid_segment_count_factor = 1
Valid state sampler named uniform with parameters:
nr_attempts = 100
Planner LBKPIECE1 specs:
Multithreaded: No
Reports approximate solutions: No
Can optimize solutions: No
Aware of the following parameters: border_fraction min_valid_path_fraction range
Declared parameters for planner LBKPIECE1:
border_fraction = 0.90000000000000002
min_valid_path_fraction = 0.5
range = 1.0069795883865302
Start states:
Compound state [
RealVectorState [0.668716 -0.800568 0.0844035]
SO3State [-0.52952 0.163809 0.356749 0.752001]
]
Goal state, threshold = 2.22045e-16, memory address = 0x7f8602f014e8, state =
Compound state [
RealVectorState [-0.161148 -0.691942 0.666629]
SO3State [-0.328573 0.782839 0.526584 -0.0437245]
]
OptimizationObjective = nullptr
There are 0 solutions
Info: LBKPIECE1: Starting planning with 1 states already in datastructure
Info: LBKPIECE1: Created 236 (119 start + 117 goal) states in 235 cells (118 start (118 on boundary) + 117 goal (117 on boundary))
Info: Solution found in 0.001039 seconds
Found solution:
Info: SimpleSetup: Path simplification took 0.000121 seconds and changed from 164 to 2 states
Geometric path with 2 states
Compound state [
RealVectorState [0.668716 -0.800568 0.0844035]
SO3State [-0.52952 0.163809 0.356749 0.752001]
]
Compound state [
RealVectorState [-0.161148 -0.691942 0.666629]
SO3State [-0.328573 0.782839 0.526584 -0.0437245]
]

logout
Saving session...
...copying shared history...
...saving history...truncating history files...
...completed.

[Process completed]

How can I visualise the output? Should I use their GUI, which seems to need to be built separately, or store the output in a file and then plot them in MATLAB?

[jamiesnape]

cmake -GXcode .. would allow you to use Xcode or AppCode. Not sure about CLion. I usually just use BBEdit.

For visualization, you could probably try either. I have not used their GUI, but it would seem a sensible place to start.

[pangtao22]

@jamiesnape Thanks! Do you happen to know how to build their GUI? This is what happens when I am trying to run the GUI script now:

dhcp-18-189-9-111:gui pangtao$ pwd
/Users/dideng/omplapp/gui

dhcp-18-189-9-111:gui pangtao$ ./ompl_app.py
Traceback (most recent call last):
File "./ompl_app.py", line 36, in
import OpenGL
ImportError: No module named OpenGL

[jamiesnape]

pip install PyOpenGL

[pangtao22]

I am getting a new error after installing PyOpenGL:

dhcp-18-189-9-111:gui pangtao$ ./ompl_app.py
Traceback (most recent call last):
File "./ompl_app.py", line 48, in
from ompl.util import OutputHandler, useOutputHandler, LogLevel, OMPL_DEBUG, OMPL_INFORM, OMPL_WARN, OMPL_ERROR
File "/Users/dideng/omplapp/ompl/py-bindings/ompl/util/init.py", line 4, in
from ompl.util._util import *
ImportError: No module named _util

[hongkai-dai]

Thank @jamiesnape for helping setting up OMPL.

How should we build it as an external of Drake? Do we want to push OMPL to get rid of Boost dependency? (Since FCL does not depend on Boost any more).

[jamiesnape]

It would be beneficial if OMPL could remove Boost, but at first glance it would be a lot of work. For now, we could make OMPL and MATLAB mutually exclusive.

First step is to make a list of the dependencies that we need to install as additional externals. There are a lot of optional ones that may or may not add functionality that we need.

[billhoffman]

The only other option would be to use BCP http://www.boost.org/doc/libs/1_62_0/tools/bcp/doc/html/index.html to namespace and limit the amount of boost that is built for OMPL. Then you can mix it with matlab.

[RussTedrake]

FYI @mamoll

[RussTedrake]

Note that the boost dependency might not be as crippling for drake anymore, now that we have a clear separation between matlab and the c++ code.

[mamoll]

When OMPL switched to C++11, we tried to remove much of the Boost usage. Boost.Graph and a few other Boost things are still needed.

Are OMPL's python bindings useful for Drake? By default they don't get generated, because it's painfully slow (it takes hours). This is because we don't explicitly mark parts of the API for Python bindings (using something like SWIG), but instead programmatically parse all code with CastXML and have general rules on how to generate Boost.Python code. This minimizes the effort required to update the binding code as the API evolves, but makes for a slow build process.

On OS X, you should be able to get the latest release by doing brew install ompl (using HomeBrew) or port install ompl (using MacPorts). The MacPorts version includes Python bindings.

[RussTedrake]

I would expect that we would connect directly via C++ (not python).

[jwnimmer-tri]

The license is good, and at first glance the ompl.BUILD file would be straightforward and similar to other externals (a simple config.h file and globbed list of sources).

The Boost dependency will be the question, because non-header-only Boost and MATLAB cannot co-exist. The OMPL build system declares hard dependencies on non-header-only portions of Boost (chrono, date-time, filesystem, thread, serialization, system), though with a quick code search perhaps some of those are vestigial and/or easily worked around (e.g., test-only). To the extent that the Boost use is hidden within *.cpp files, BCP might also help. The details will require further study.

@hongkai-dai For a first goal, do we want to use this only in some examples and tests, or do we also imagine our installed toolbox libdrake.so linking to OMPL? I guess in general, could you better explain the intended shape of the uses here?

[jwnimmer-tri]

@hongkai-dai On Ubuntu 16.04 there is a package of OMPL already at version 1.0.0. Do you know if that version is satisfactory for your needs?

[jwnimmer-tri]

FYI Proof of concept at https://github.com/jwnimmer-tri/drake/tree/ompl-spike. This uses pkg_config to find the system OMPL.

[jamiesnape]

Assuming the Protobuf interface to MATLAB is the future, If we modularized libdrake.so even just to isolate core, the Boost is not a problem (or we avoid even linking to core and try to only rely on headers).

[jwnimmer-tri]

As I understand it, though the call_matlab RPC is a portion of the future (for easy visualization from C++), in order to use the full toolbox, the major plan is to import pydrake from within MATLAB in order to call most anything that is wrapped in Python, e.g., #5981.

I do agree, though, that modularizing libdrake.so is another way to help limit the MATLAB vs Boost pollution.

[hongkai-dai]

@jwnimmer-tri thanks for looking into this. Currently I think OMPL will be mainly used for the manipulation task in Drake. There are two candidate manipulation scenarios that will consume OMPL

1. The pick-and-place task. To guarantee that the robot runs for more than 100 hours per week, doing manipulation task, we need some safety guarantee on a collision-free motion plan. OMPL will be used to help generate that plan, as a supplement to our optimization-based planner.
2. In the box rotation task that Rick and I are working on, we designed a planner as a mixture of sampling based approach, and optimization based approach. The sampling based approach will use OMPL.

In the first scenario, the OMPL library is called in just ta example code. In the second scenario, I will write some functions in drake/solver, that will call OMPL code.

[jwnimmer-tri]

@hongkai-dai Is the ompl-spike link that I posted above enough to let you sanity check whether the Ubuntu 16.04 packaged version of OMPL is satisfactory for your needs? We'll need feedback on that question before we go much further.

[hongkai-dai]

@jwnimmer-tri thanks for creating the ompl-spike branch. OMPL 1.0.0 looks good for us. We only use some basic functionalities in OMPL, like an RRT or RRT*.

[jwnimmer-tri]

Sounds good.

Here were the patches, for the record:

Add to WORKSPACE:

pkg_config_package(
    name = "ompl",
    modname = "ompl",
)

Use it from our code:

cc_test(
    name = "foo",
    srcs = ["foo.cc"],
    deps = [
        "@ompl",
    ],
)

(I'll probably kill off my branch in a week or so.)

I think you can take that as a starting point, and then work on a branch to develop the features you want. I think the OMPL dependency can wait to be added to master until such a time as your new features are ready; I don't think Jamie or I need to PR it ahead of time.

[EricCousineau-TRI]

Just to check, is this still high-priority? Came across this from our docs:

Should this be captured in a higher-level issue, reflected by what we have in our docs?
\cc @calderpg-tri

EDIT: Relates #459

[mamoll]

I am one of the OMPL developers. If there's anything I can do to make progress here, let me know. I released version 1.4.1 today, a patch-level release. The next version will likely be 1.5.0 and we can change the API a bit if needed to facilitate integration with Drake.

[calderpg-tri]

I think upstreaming all the planners and interfaces in anzu/planning is a higher priority, since we'll need those to fully open-source clutter and other parts of our stack. Those planners serve the role OMPL integration was initially going to serve,

On a longer-term basis, having it be easy to use Drake with OMPL, both as a state/motion verifier wrapping the collision checker(s) and in the kinodynamic planners, would be ideal. Much of the work for this on Drake-side APIs will be done when we move anzu/planning's interfaces upstream.

For now I think it's most useful to identify build and dependency issues so that these can be worked on for OMPL 1.5.0 and leave specific Drake<->OMPL API questions for later.

[hongkai-dai]

Close it for now. Will open it when needed.

EDIT (by Eric Cousineau): Just to expand, this isn't on any near-term roadmap at the moment. However, we'll reopen it once there is a valuable use case or high user demand for this interface. Thanks!