Merge 52b2399 into 65d3c23

CMakeLists.txt

@@ -87,7 +87,9 @@ function(format_add_sources)
 endfunction()
 
 # TODO: Temporarily split header list per subdirectories.
+file(GLOB_RECURSE headers_planner "${CMAKE_SOURCE_DIR}/include/aikido/planner/*.hpp")
 file(GLOB_RECURSE headers_util "${CMAKE_SOURCE_DIR}/include/aikido/util/*.hpp")
+format_add_sources(${headers_planner})
 format_add_sources(${headers_util})
 
 #==============================================================================

include/aikido/planner/PlanningResult.hpp

@@ -3,14 +3,16 @@
 
 #include <string>
 
-namespace aikido
-{
-namespace planner
+namespace aikido {
+namespace planner {
+
+struct PlanningResult
 {
-struct PlanningResult {
   // TODO fill out
   std::string message;
 };
-}
-}
-#endif  // AIKIDO_PLANNER_PLANNINGRESULT_HPP_
+
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_PLANNINGRESULT_HPP_

include/aikido/planner/SnapPlanner.hpp

@@ -1,8 +1,8 @@
 #ifndef AIKIDO_PLANNER_SNAP_PLANNER_HPP_
 #define AIKIDO_PLANNER_SNAP_PLANNER_HPP_
-#include "../statespace/StateSpace.hpp"
-#include "../statespace/Interpolator.hpp"
 #include "../constraint/Testable.hpp"
+#include "../statespace/Interpolator.hpp"
+#include "../statespace/StateSpace.hpp"
 #include "../trajectory/Interpolated.hpp"
 #include "PlanningResult.hpp"
 
@@ -23,14 +23,14 @@ namespace planner {
 /// \param[out] planningResult information about success or failure
 /// \return trajectory or \c nullptr if planning failed
 trajectory::InterpolatedPtr planSnap(
-  const std::shared_ptr<statespace::StateSpace>& stateSpace,
-  const statespace::StateSpace::State *startState,
-  const statespace::StateSpace::State *goalState,
-  const std::shared_ptr<statespace::Interpolator>& interpolator,
-  const std::shared_ptr<constraint::Testable>& constraint,
-  planner::PlanningResult& planningResult);
+    const std::shared_ptr<statespace::StateSpace>& stateSpace,
+    const statespace::StateSpace::State* startState,
+    const statespace::StateSpace::State* goalState,
+    const std::shared_ptr<statespace::Interpolator>& interpolator,
+    const std::shared_ptr<constraint::Testable>& constraint,
+    planner::PlanningResult& planningResult);
 
 } // namespace planner
 } // namespace aikido
 
-#endif  // AIKIDO_PLANNER_SNAP_PLANNER_HPP_
+#endif // AIKIDO_PLANNER_SNAP_PLANNER_HPP_

include/aikido/planner/ompl/BackwardCompatibility.hpp

@@ -7,6 +7,8 @@
 
 #include <ompl/config.h>
 
+// clang-format off
+
 #define OMPL_VERSION_AT_LEAST(x,y,z) \
   (OMPL_MAJOR_VERSION > x || (OMPL_MAJOR_VERSION >= x && \
   (OMPL_MINOR_VERSION > y || (OMPL_MINOR_VERSION >= y && \
@@ -25,7 +27,9 @@
   (OMPL_MAJOR_VERSION < x || (OMPL_MAJOR_VERSION <= x && \
   (OMPL_MINOR_VERSION < y || (OMPL_MINOR_VERSION <= y))))
 
-#if OMPL_VERSION_AT_LEAST(1,2,0)
+// clang-format on
+
+#if OMPL_VERSION_AT_LEAST(1, 2, 0)
 #include <memory>
 #else
 #include <boost/smart_ptr.hpp>
@@ -36,7 +40,7 @@ namespace aikido {
 namespace planner {
 namespace ompl {
 
-#if OMPL_VERSION_AT_LEAST(1,2,0)
+#if OMPL_VERSION_AT_LEAST(1, 2, 0)
 
 #define OMPL_PLACEHOLDER(ph) std::placeholders::ph
 
@@ -66,7 +70,7 @@ ompl_shared_ptr<T> ompl_static_pointer_cast(const ompl_shared_ptr<U>& r)
 
 template <class F, class... Args>
 auto ompl_bind(F&& f, Args&&... args)
--> decltype(std::bind(std::forward<F>(f), std::forward<Args>(args)...))
+    -> decltype(std::bind(std::forward<F>(f), std::forward<Args>(args)...))
 {
   return std::bind(std::forward<F>(f), std::forward<Args>(args)...);
 }
@@ -101,7 +105,7 @@ ompl_shared_ptr<T> ompl_static_pointer_cast(const ompl_shared_ptr<U>& r)
 
 template <class F, class... Args>
 auto ompl_bind(F&& f, Args&&... args)
--> decltype(boost::bind(std::forward<F>(f), std::forward<Args>(args)...))
+    -> decltype(boost::bind(std::forward<F>(f), std::forward<Args>(args)...))
 {
   return boost::bind(std::forward<F>(f), std::forward<Args>(args)...);
 }

include/aikido/planner/ompl/CRRT.hpp

@@ -2,37 +2,38 @@
 #define AIKIDO_PLANNER_OMPL_CRRT_HPP_
 
 #include <ompl/base/Planner.h>
-#include <ompl/geometric/planners/PlannerIncludes.h>
 #include <ompl/datastructures/NearestNeighbors.h>
+#include <ompl/geometric/planners/PlannerIncludes.h>
 
-#include "../../planner/ompl/BackwardCompatibility.hpp"
 #include "../../constraint/Projectable.hpp"
+#include "../../planner/ompl/BackwardCompatibility.hpp"
 
 namespace aikido {
 namespace planner {
 namespace ompl {
+
 /// Implements a constrained RRT planner
 class CRRT : public ::ompl::base::Planner
 {
 public:
   /// Constructor
   /// \param _si Information about the planning space
-  CRRT(const ::ompl::base::SpaceInformationPtr &_si);
+  CRRT(const ::ompl::base::SpaceInformationPtr& _si);
 
   /// Constructor
   /// \param _si Information about the planning space
   /// \param _name A name for this planner
-  CRRT(const ::ompl::base::SpaceInformationPtr &_si, const std::string &name);
+  CRRT(const ::ompl::base::SpaceInformationPtr& _si, const std::string& name);
 
   /// Destructor
-  virtual ~CRRT(void);
+  virtual ~CRRT();
 
   /// Get information about the current run of the motion planner. Repeated
   /// calls to this function will update data (only additions are made). This is
   /// useful to see what changed in the exploration datastructure, between calls
   /// to solve(), for example (without calling clear() in between).
   /// \param[out] data Data about the current run of the motion planner
-  void getPlannerData(::ompl::base::PlannerData &_data) const override;
+  void getPlannerData(::ompl::base::PlannerData& _data) const override;
 
   /// Function that can solve the motion planning problem. This function can be
   /// called multiple times on the same problem, without calling clear() in
@@ -45,15 +46,15 @@ class CRRT : public ::ompl::base::Planner
   /// true.
   /// \param ptc Conditions for terminating planning before a solution is found
   ::ompl::base::PlannerStatus solve(
-      const ::ompl::base::PlannerTerminationCondition &_ptc) override;
+      const ::ompl::base::PlannerTerminationCondition& _ptc) override;
 
   /// Solve the motion planning problem in the given time
   /// \param solveTime The maximum allowable time to solve the planning problem
   ::ompl::base::PlannerStatus solve(double _solveTime);
 
   /// Clear all internal datastructures. Planner settings are not affected.
   /// Subsequent calls to solve() will ignore all previous work.
-  void clear(void) override;
+  void clear() override;
 
   /// Set the goal bias. In the process of randomly selecting states in the
   /// state space to attempt to go towards, the algorithm may in fact choose the
@@ -64,7 +65,7 @@ class CRRT : public ::ompl::base::Planner
   void setGoalBias(double _goalBias);
 
   /// Get the goal bias the planner is using
-  double getGoalBias(void) const;
+  double getGoalBias() const;
 
   /// Set the range the planner is supposed to use. This parameter greatly
   /// influences the runtime of the algorithm. It represents the maximum length
@@ -74,14 +75,13 @@ class CRRT : public ::ompl::base::Planner
   void setRange(double _distance);
 
   /// Get the range the planner is using
-  double getRange(void) const;
+  double getRange() const;
 
   /// Set a projectable constraint to be applied throughout the trajectory.
   /// The projection is applied at the resolution set via
   /// setProjectionResolution
   /// \param _projectable The constraint
-  void setPathConstraint(
-      constraint::ProjectablePtr _projectable);
+  void setPathConstraint(constraint::ProjectablePtr _projectable);
 
   /// Set the resolution for the projection. During tree extension, a projection
   /// back to the constraint will be performed after any step larger than this
@@ -93,7 +93,7 @@ class CRRT : public ::ompl::base::Planner
   /// Get the resolution for the projection. During tree extension, a
   /// projection back to the constraint will be performed after any step
   /// larger than this distance.
-  double getProjectionResolution(void) const;
+  double getProjectionResolution() const;
 
   /// Set the minimum distance between two states for them to be considered
   /// "equivalent". This is used during extension to determine if a projection
@@ -105,16 +105,16 @@ class CRRT : public ::ompl::base::Planner
   /// "equivalent". This is used during extension to determine if a projection
   /// is near enough the previous projection to say progress is no longer being
   /// made and quit extending.
-  double getMinStateDifference(void) const;
+  double getMinStateDifference() const;
 
   /// Set a nearest neighbors data structure
   template <template <typename T> class NN>
-  void setNearestNeighbors(void);
+  void setNearestNeighbors();
 
   /// Perform extra configuration steps, if needed. This call will also issue a
   /// call to ompl::base::SpaceInformation::setup() if needed. This must be
   /// called before solving.
-  void setup(void) override;
+  void setup() override;
 
 protected:
   /// Representation of a node in the tree. Contains the state at the node and a
@@ -123,14 +123,23 @@ class CRRT : public ::ompl::base::Planner
   {
   public:
     /// Constructor. Sets state and parent to null ptr.
-    Motion(void) : state(nullptr), parent(nullptr) {}
+    Motion() : state(nullptr), parent(nullptr)
+    {
+      // Do nothing
+    }
 
     /// Constructor that allocates memory for the state
-    Motion(const ::ompl::base::SpaceInformationPtr &_si)
-        : state(_si->allocState()), parent(nullptr) {}
+    Motion(const ::ompl::base::SpaceInformationPtr& _si)
+      : state(_si->allocState()), parent(nullptr)
+    {
+      // Do nothing
+    }
 
     /// Destructor
-    ~Motion(void) {}
+    ~Motion()
+    {
+      // Do nothing
+    }
 
     /// The state contained in this node
     ::ompl::base::State* state;
@@ -140,15 +149,14 @@ class CRRT : public ::ompl::base::Planner
   };
 
   /// Free the memory allocated by this planner
-  virtual void freeMemory(void);
+  virtual void freeMemory();
 
   /// Compute distance between motions (actually distance between contained
   /// states
-  double distanceFunction(const Motion *a, const Motion *b) const;
-
+  double distanceFunction(const Motion* a, const Motion* b) const;
 
   /// A nearest-neighbor datastructure representing a tree of motions */
-  using TreeData = ompl_shared_ptr<::ompl::NearestNeighbors<Motion *>>;
+  using TreeData = ompl_shared_ptr<::ompl::NearestNeighbors<Motion*>>;
 
   /// A nearest-neighbors datastructure containing the tree of motions
   TreeData mStartTree;
@@ -167,10 +175,16 @@ class CRRT : public ::ompl::base::Planner
   /// \param[out] True if the extension reached the goal.
   /// \return fmotion If returnlast is true, the last node on the extension,
   /// otherwise the closest node along the extension to the goal
-  Motion *constrainedExtend(
-      const ::ompl::base::PlannerTerminationCondition &ptc, TreeData &tree,
-      Motion *nmotion, ::ompl::base::State *gstate, ::ompl::base::State *xstate,
-      ::ompl::base::Goal *goal, bool returnlast, double &dist, bool &foundgoal);
+  Motion* constrainedExtend(
+      const ::ompl::base::PlannerTerminationCondition& ptc,
+      TreeData& tree,
+      Motion* nmotion,
+      ::ompl::base::State* gstate,
+      ::ompl::base::State* xstate,
+      ::ompl::base::Goal* goal,
+      bool returnlast,
+      double& dist,
+      bool& foundgoal);
 
   /// State sampler
   ::ompl::base::StateSamplerPtr mSampler;
@@ -187,7 +201,7 @@ class CRRT : public ::ompl::base::Planner
   ::ompl::RNG mRng;
 
   /// The most recent goal motion.  Used for PlannerData computation
-  Motion *mLastGoalMotion;
+  Motion* mLastGoalMotion;
 
   /// The constraint that must be satisfied throughout the trajectory
   constraint::ProjectablePtr mCons;
@@ -199,6 +213,7 @@ class CRRT : public ::ompl::base::Planner
   /// when projection is no longer making progress during an extension.
   double mMinStepsize;
 };
+
 } // namespace ompl
 } // namespace planner
 } // namespace aikido