Make TRI motion planning tools available in drake

[calderpg-tri]

TRI uses a number of motion planning tools, both external (public) and internal that we would like to make available in drake. These tools include:

• Sampling-based motion planners (constrained and unconstrained RRT/BiRRT variants, PRM variants)
• Path and trajectory processing (shortcut and b-spline smoothing, TOTP [Time-optimal trajectory parametrization])
• Voxel-based geometry (dense voxelized occupancy and signed distance fields for collision checking/gradients)
• GPU-accelerated pointcloud->voxelized occupancy
• Collision checking interfaces and abstractions for parallelized collision checking, IK, planning, and optimization
• Additional constraints for collision avoidance
• Utilities used in planning problems (kNN, A*, some forms of clustering, etc)

Since the TRI-internal tools include interfaces with ROS for visualization, we cannot simply port these tools over until drake-ROS integration is more developed. Therefore, we propose to bring in the existing public tools into drake as externals and make the ROS-free subset of internal tools available in drake (i.e. in a new directory such as drake/multibody/planning) via a tool such as copybara. Using (for example) copybara, the TRI-internal version would remain the authoritative version while contributions would be accepted from both public drake and TRI-internal.

Roughly speaking, the steps to complete this:

1. Bring in common_robotics_utilities and voxelized_geometry_tools as externals. The limited ROS-dependent parts of these packages will not be built.
2. Add build support for OpenMP (directive-based CPU parallelization) and OpenCL (cross-platform GPU offload) to drake. [Note that while both OpenGL and OpenCL are officially deprecated by Apple, they have remained available for several releases]
3. Export the ROS-free subset of anzu/planning to drake/multibody/planning using a tool like copybara.
4. Provide additional public documentation and demos of the new planning tools.

Farther in the future, once drake-ROS integration has developed and sufficient visualization capabilities are exposed in drake itself, these TRI-internal planning tools will be moved completely to drake.

cc @ggould-tri @jwnimmer-tri @avalenzu @RussTedrake

[jamiesnape]

If our AWS instances do not do support OpenCL, is that going to be a blocker?

[calderpg-tri]

No. For now, we only need build support in CI - at runtime the relevant parts of voxelized_geometry_tools and anzu/planning detect if no GPU acceleration is available and fall back to a CPU-only implementation.

[jamiesnape]

Cool. If anything changes and you do think you will need GPU instances in the next TRI financial year, we probably would need to revise our AWS budget, so let us know if that is likely. FYI @ToffeeAlbina-TRI.

[calderpg-tri]

OpenCL support is likely going to require vendoring the cl.hpp C++ bindings (found on Ubuntu bionic at /usr/include/CL/cl.hpp) on macOS. Thoughts on where that should live are welcome - to avoid any include path weirdness it might be easiest to put it in voxelized_geometry_tools directly.

[huihuaTRI]

This could be really useful for drake users since that's what Drake is currently lacking. I would like to help if you @calderpg-tri need some manpower.

[RussTedrake]

After discussing with @calderpg-tri , I will offer to take a small slice of this by providing

PlanningResult PlanBiRRTPath(
    const Eigen::VectorXd& start, const Eigen::VectorXd& goal,
    const BiRRTPlannerParameters& parameters,
    const ScemeGraph& scene_graph,
    const std::unordered_set<drake::multibody::BodyIndex>& ignored_bodies,
    StateSamplingInterface<Eigen::VectorXd>* sampler);

as the first offering, in drake/planning/rrt_planners.h (with the minimal smoother being perhaps next on the list). It will do the simpler single-threaded collision checking against SceneGraph only; we can move the multi-threaded per-context CollisionCheckerBase workflow over later.

Note that this is consistent with the design constraint of avoiding any common-robotics-utilities header files in drake headers.

My goal is to provide something useful now, and to help keep things moving on the rest of this issue.