API to use OMPL Planners within the new planner framework

CHANGELOG.md

@@ -56,6 +56,7 @@
   * Planning calls with InverseKinematicsSampleable constraints explicitly set MetaSkeleton to solve IK with: [#379](https://github.com/personalrobotics/aikido/pull/379)
   * Added a kinodynamic timer that generates a time-optimal smooth trajectory without completely stopping at each waypoint: [#443](https://github.com/personalrobotics/aikido/pull/443)
   * Fixed segmentation fault on 32-bit machines in vector-field planner: [#459](https://github.com/personalrobotics/aikido/pull/459)
+  * Updated interface to OMPL planners to follow the style of the new refactored planning API: [#466](https://github.com/personalrobotics/aikido/pull/466)
 
 * Robot
 

include/aikido/planner/ompl/GeometricStateSpace.hpp

@@ -2,22 +2,21 @@
 #define AIKIDO_OMPL_AIKIDOGEOMETRICSTATESPACE_HPP_
 
 #include <ompl/base/StateSpace.h>
+#include "aikido/constraint/Projectable.hpp"
+#include "aikido/constraint/Sampleable.hpp"
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
 #include "aikido/planner/ompl/BackwardCompatibility.hpp"
-#include "../../constraint/Projectable.hpp"
-#include "../../constraint/Sampleable.hpp"
-#include "../../constraint/Testable.hpp"
-#include "../../distance/DistanceMetric.hpp"
-#include "../../statespace/GeodesicInterpolator.hpp"
-#include "../../statespace/StateSpace.hpp"
+#include "aikido/statespace/GeodesicInterpolator.hpp"
+#include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
 namespace planner {
 namespace ompl {
 
 AIKIDO_DECLARE_POINTERS(GeometricStateSpace)
 
-/// The maximum distance between two states for them to still be considered
-/// equal
+/// The maximum distance between two states for them to be considered equal
 constexpr double EQUALITY_EPSILON = 1e-7;
 
 /// Wraps an aikido StateSpace into a space recognized by OMPL
@@ -29,38 +28,43 @@ class GeometricStateSpace : public ::ompl::base::StateSpace
   {
   public:
     /// Constructor.
-    /// \param _st The state to wrap
-    explicit StateType(statespace::StateSpace::State* _st);
+    /// \param[in] state The state to wrap
+    explicit StateType(statespace::StateSpace::State* state);
 
     /// The wrapped aikido state
     statespace::StateSpace::State* mState;
 
     /// Indicates whether the state has been initialized to represent a valid
     /// state. This allows samplers to indicate failure by setting this flag to
-    /// false when sampling fails.  The StateValidityChecker should check this
-    /// flag when determining if the state is valide.
+    /// false when sampling fails. The StateValidityChecker should check this
+    /// flag when determining if the state is valid.
     /// The value defaults to true.
     bool mValid;
   };
 
   /// Construct a state space
-  /// \param _sspace The aikido::statespace::StateSpace to expose to OMPL
-  /// \param _interpolator An aikido interpolator used by the interpolate method
-  /// \param _dmetric The distance metric to use to compute distance between two
-  /// states in the StateSpace
-  /// \param _sampler A state sampler used to sample new states in the
+  /// \param[in] sspace The aikido::statespace::StateSpace to expose to OMPL
+  /// \param[in] interpolator An aikido interpolator used by the interpolate
+  /// method
+  /// \param[in] dmetric The distance metric to use to compute distance between
+  /// two states in the StateSpace
+  /// \param[in] sampler A state sampler used to sample new states in the
   /// StateSpace
-  /// \param _boundsConstraint A Testable used to determine whether
+  /// \param[in] boundsConstraint A Testable used to determine whether
   /// states fall with in bounds defined on the space.
-  /// \param _boundsProjection A Projectable that can be used to project a state
-  /// back within the valid boundary defined on the space.
+  /// \param[in] boundsProjection A Projectable that can be used to project a
+  /// state back within the valid boundary defined on the space.
+  /// \param[in] maxDistanceBetweenValidityChecks The maximum distance (under
+  /// dmetric) between validity checking two successive points on a tree
+  /// extension or an edge in a graph. Defines the "discreteness" of statespace.
   GeometricStateSpace(
-      statespace::ConstStateSpacePtr _sspace,
-      statespace::InterpolatorPtr _interpolator,
-      distance::DistanceMetricPtr _dmetric,
-      constraint::SampleablePtr _sampler,
-      constraint::TestablePtr _boundsConstraint,
-      constraint::ProjectablePtr _boundsProjection);
+      statespace::ConstStateSpacePtr sspace,
+      statespace::ConstInterpolatorPtr interpolator,
+      distance::DistanceMetricPtr dmetric,
+      constraint::SampleablePtr sampler,
+      constraint::ConstTestablePtr boundsConstraint,
+      constraint::ProjectablePtr boundsProjection,
+      double maxDistanceBetweenValidityChecks);
 
   /// Get the dimension of the space.
   unsigned int getDimension() const override;
@@ -76,50 +80,48 @@ class GeometricStateSpace : public ::ompl::base::StateSpace
   double getMeasure() const;
 #endif
 
-  /// Bring the state within the bounds of the state space using the
-  /// boundsProjection defined in the constructor.
-  /// \param _state The state to modify
-  void enforceBounds(::ompl::base::State* _state) const override;
+  /// Bring the state within the bounds of the statespace using
+  /// boundsProjection.
+  /// \param[in] state The state to modify.
+  void enforceBounds(::ompl::base::State* state) const override;
 
-  /// Check if a state satisfieds the boundsConstraint defined in the
-  /// constructor
-  /// \param _state The state to check
-  bool satisfiesBounds(const ::ompl::base::State* _state) const override;
+  /// Check if a state satisfies the boundsConstraint.
+  /// \param[in] state The state to check.
+  bool satisfiesBounds(const ::ompl::base::State* state) const override;
 
-  /// Copy the value of one state to another
-  /// \param[out] _destination The state to copy to
-  /// \param _source The state to copy from
+  /// Copy the value of one state to another.
+  /// \param[out] destination The state to copy to.
+  /// \param[in] source The state to copy from.
   void copyState(
-      ::ompl::base::State* _destination,
-      const ::ompl::base::State* _source) const override;
+      ::ompl::base::State* destination,
+      const ::ompl::base::State* source) const override;
 
-  /// Computes distance between two states using the _dmetric defined in the
-  /// constructor.
-  /// \param _state1 The first state
-  /// \param _state2 The second state
+  /// Computes distance between two states using the dmetric.
+  /// \param[in] state1 The first state.
+  /// \param[in] state2 The second state.
   double distance(
-      const ::ompl::base::State* _state1,
-      const ::ompl::base::State* _state2) const override;
+      const ::ompl::base::State* state1,
+      const ::ompl::base::State* state2) const override;
 
-  /// Check state equality. The returns true if the distance between the states
-  /// is 0.
-  /// \param _state1 The first state
-  /// \param _state2 The second state
+  /// Check state equality. Returns true if the distance between the states
+  /// less than EQUALITY_EPSILON.
+  /// \param[in] state1 The first state
+  /// \param[in] state2 The second state
   bool equalStates(
-      const ::ompl::base::State* _state1,
-      const ::ompl::base::State* _state2) const override;
+      const ::ompl::base::State* state1,
+      const ::ompl::base::State* state2) const override;
 
   /// Computes the state that lies at time t in [0, 1] on the segment
   /// that connects from state to to state.
-  /// \param _from The state that begins the segment
-  /// \param _to The state that ends the segment
-  /// \param _t The interpolation parameter (between 0 and 1)
-  /// \param[out] _state The result of the interpolation
+  /// \param[in] from The state that begins the segment.
+  /// \param[in] to The state that ends the segment.
+  /// \param[in] t The interpolation parameter (between 0 and 1).
+  /// \param[out] state The result of the interpolation.
   void interpolate(
-      const ::ompl::base::State* _from,
-      const ::ompl::base::State* _to,
-      double _t,
-      ::ompl::base::State* _state) const override;
+      const ::ompl::base::State* from,
+      const ::ompl::base::State* to,
+      double t,
+      ::ompl::base::State* state) const override;
 
   /// Allocate an instance of the state sampler for this space.
   ::ompl::base::StateSamplerPtr allocDefaultStateSampler() const override;
@@ -128,25 +130,49 @@ class GeometricStateSpace : public ::ompl::base::StateSpace
   ::ompl::base::State* allocState() const override;
 
   /// Allocate a state constaining a copy of the aikido state
-  /// \param _state The aikido state to copy and wrap in an OMPL state
+  /// \param[in] state The aikido state to copy and wrap in an OMPL state
   ::ompl::base::State* allocState(
-      const statespace::StateSpace::State* _state) const;
+      const statespace::StateSpace::State* state) const;
 
   /// Free the memory of the allocated state. This also frees the memory of the
   /// wrapped aikido state.
-  /// \param _state The state to free.
-  void freeState(::ompl::base::State* _state) const override;
+  /// \param[in] state The state to free.
+  void freeState(::ompl::base::State* state) const override;
 
   /// Return the Aikido StateSpace that this OMPL StateSpace wraps
   statespace::ConstStateSpacePtr getAikidoStateSpace() const;
 
+  /// Return the interpolator used to interpolate between states in the space.
+  aikido::statespace::ConstInterpolatorPtr getInterpolator() const;
+
+  /// Return the bounds constraint for the statespace. Used to specify
+  /// constraints to the OMPL planner.
+  aikido::constraint::ConstTestablePtr getBoundsConstraint() const;
+
+  /// Returns the collision checking resolution.
+  double getMaxDistanceBetweenValidityChecks() const;
+
 private:
+  /// The AIKIDO statespace to be exposed to OMPL.
   statespace::ConstStateSpacePtr mStateSpace;
-  statespace::InterpolatorPtr mInterpolator;
+
+  /// An aikido interpolator to interpolate between states of the statespace.
+  statespace::ConstInterpolatorPtr mInterpolator;
+
+  /// Distance metric to compute distance between states in the statespace.
   distance::DistanceMetricPtr mDistance;
+
+  /// State sampler used to sample states in the statespace.
   constraint::SampleablePtr mSampler;
-  constraint::TestablePtr mBoundsConstraint;
+
+  /// Constraint to determine if a state is within statespace bounds.
+  constraint::ConstTestablePtr mBoundsConstraint;
+
+  /// A Projectable that projects a state within valid bounds of statespace.
   constraint::ProjectablePtr mBoundsProjection;
+
+  /// Collision checking resolution.
+  double mMaxDistanceBetweenValidityChecks;
 };
 
 } // namespace ompl

include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp

@@ -0,0 +1,91 @@
+#ifndef AIKIDO_PLANNER_OMPL_OMPLCONFIGURATIONTOCONFIGURATIONPLANNER_HPP_
+#define AIKIDO_PLANNER_OMPL_OMPLCONFIGURATIONTOCONFIGURATIONPLANNER_HPP_
+
+#include <ompl/base/Planner.h>
+#include <ompl/base/ProblemDefinition.h>
+#include <ompl/base/ScopedState.h>
+#include <ompl/base/SpaceInformation.h>
+#include <ompl/base/goals/GoalRegion.h>
+#include <ompl/geometric/PathSimplifier.h>
+#include "aikido/common/RNG.hpp"
+#include "aikido/constraint/Projectable.hpp"
+#include "aikido/constraint/Sampleable.hpp"
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/planner/ConfigurationToConfiguration.hpp"
+#include "aikido/planner/ConfigurationToConfigurationPlanner.hpp"
+#include "aikido/planner/ompl/GeometricStateSpace.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+
+namespace aikido {
+namespace planner {
+namespace ompl {
+
+/// Creates an OMPL Planner.
+///
+/// \tparam PlannerType The OMPL Planner to use.
+template <class PlannerType>
+class OMPLConfigurationToConfigurationPlanner
+    : public aikido::planner::ConfigurationToConfigurationPlanner
+{
+public:
+  /// Constructor.
+  ///
+  /// \param[in] stateSpace State space that this planner associated with.
+  /// \param[in] rng Random number generator to create the state sampler.
+  /// \param[in] interpolator Interpolator used to interpolate between two
+  /// states. GeodesicInterpolator is used by default.
+  /// \param[in] dmetric A valid distance metric defined on the StateSpace.
+  /// Distance metric relevant to the statespace is used by default.
+  /// \param[in] sampler A Sampleable to sample states from StateSpace.
+  /// \note OMPL planners assume this sampler samples from the statespace
+  /// uniformly. Care must be taken when using a non-uniform sampler.
+  /// \param[in] boundsConstraint A constraint used to determine whether states
+  /// encountered during planning fall within any bounds specified on the
+  /// StateSpace. In addition to the validityConstraint, this must also be
+  /// satisfied for a state to be considered valid.
+  /// \param[in] boundsProjector A Projectable that projects a state back within
+  /// valid bounds defined on the StateSpace.
+  /// \param[in] maxDistanceBtwValidityChecks The maximum distance (under
+  /// dmetric) between validity checking two successive points on a tree
+  /// extension or an edge in a graph.
+  OMPLConfigurationToConfigurationPlanner(
+      statespace::ConstStateSpacePtr stateSpace,
+      common::RNG* rng = nullptr,
+      statespace::ConstInterpolatorPtr interpolator = nullptr,
+      distance::DistanceMetricPtr dmetric = nullptr,
+      constraint::SampleablePtr sampler = nullptr,
+      constraint::ConstTestablePtr boundsConstraint = nullptr,
+      constraint::ProjectablePtr boundsProjector = nullptr,
+      double maxDistanceBtwValidityChecks = 0.1);
+
+  /// Plans a trajectory from start state to goal state by using an interpolator
+  /// to interpolate between them.
+  ///
+  /// If successful, the planner returns a trajectory that satisfies the
+  /// constraint. If not, it returns a \c nullptr.
+  /// The corresponding message is stored in result.
+  ///
+  /// \param[in] problem Planning problem.
+  /// \param[out] result Information about success or failure.
+  /// \return Trajectory or \c nullptr if planning failed.
+  /// \throw If \c problem is not ConfigurationToConfiguration.
+  /// \throw If \c result is not ConfigurationToConfiguration::Result.
+  trajectory::TrajectoryPtr plan(
+      const SolvableProblem& problem, Result* result = nullptr) override;
+
+  /// Returns the underlying OMPL planner used.
+  ::ompl::base::PlannerPtr getOMPLPlanner();
+
+protected:
+  /// Pointer to the underlying OMPL Planner.
+  ::ompl::base::PlannerPtr mPlanner;
+};
+
+} // namespace ompl
+} // namespace planner
+} // namespace aikido
+
+#include "aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp"
+
+#endif // AIKIDO_PLANNER_OMPL_OMPLCONFIGURATIONTOCONFIGURATIONPLANNER_HPP_