Add wrapper for OMPL path simplifier

include/aikido/planner/ompl/Planner.hpp

@@ -14,6 +14,9 @@
 #include <ompl/base/ProblemDefinition.h>
 #include <ompl/base/SpaceInformation.h>
 #include <ompl/base/goals/GoalRegion.h>
+#include <ompl/base/ScopedState.h>
+
+#include <ompl/geometric/PathSimplifier.h>
 
 namespace aikido {
 namespace planner {
@@ -241,10 +244,45 @@ trajectory::InterpolatedPtr planOMPL(const ::ompl::base::PlannerPtr &_planner,
                              statespace::InterpolatorPtr _interpolator,
                              double _maxPlanTime);
 
+
+/// Take in an aikido trajectory and simplify it using OMPL methods
+/// \param _statespace The StateSpace that the planner must plan within
+/// \param _interpolator An Interpolator defined on the StateSpace. This is used
+/// to interpolate between two points within the space.
+/// \param _dmetric A valid distance metric defined on the StateSpace
+/// \param _sampler A Sampleable that can sample states from the
+/// StateSpace. Warning: Many OMPL planners internally assume this sampler
+/// samples uniformly. Care should be taken when using a non-uniform sampler.
+/// \param _validityConstraint A constraint used to test validity during
+/// planning. This should include collision checking and any other constraints
+/// that must be satisfied for a state to be considered valid.
+/// \param _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
+/// satsified for a state to be considered valid.
+/// \param _boundsProjector A Projectable that projects a state back within
+/// valid bounds defined on the StateSpace
+/// \param _maxPlanTime The maximum time to allow the planner to search for a
+/// solution
+/// \param _maxDistanceBtwValidityChecks The maximum distance (under dmetric) between
+/// validity checking two successive points on a tree extension
+/// \param _originalTraj The untimed trajectory obtained from the planner, 
+/// needs simplifying.
+trajectory::InterpolatedPtr simplifyOMPL(statespace::StateSpacePtr _stateSpace,
+                                           statespace::InterpolatorPtr _interpolator,
+                                           distance::DistanceMetricPtr _dmetric,
+                                           constraint::SampleablePtr _sampler,
+                                           constraint::TestablePtr _validityConstraint,
+                                           constraint::TestablePtr _boundsConstraint,
+                                           constraint::ProjectablePtr _boundsProjector, 
+                                           double _maxDistanceBtwValidityChecks,
+                                           trajectory::InterpolatedPtr _originalTraj);
+
+
 } // namespace ompl
 } // namespace planner
 } // namespace aikido
 
 #include "detail/Planner-impl.hpp"
 
-#endif
+#endif

src/planner/ompl/Planner.cpp

@@ -317,6 +317,68 @@ trajectory::InterpolatedPtr planCRRTConnect(
                   std::move(_interpolator), _maxPlanTime);
 }
 
+//=============================================================================
+trajectory::InterpolatedPtr simplifyOMPL(statespace::StateSpacePtr _stateSpace,
+                                           statespace::InterpolatorPtr _interpolator,
+                                           distance::DistanceMetricPtr _dmetric,
+                                           constraint::SampleablePtr _sampler,
+                                           constraint::TestablePtr _validityConstraint,
+                                           constraint::TestablePtr _boundsConstraint,
+                                           constraint::ProjectablePtr _boundsProjector, 
+                                           double _maxDistanceBtwValidityChecks,
+                                           trajectory::InterpolatedPtr _originalTraj)
+{
+
+// Step 1: Generate the space information of OMPL type
+auto si = getSpaceInformation(
+      _stateSpace, _interpolator, std::move(_dmetric), std::move(_sampler),
+      std::move(_validityConstraint), std::move(_boundsConstraint),
+      std::move(_boundsProjector), _maxDistanceBtwValidityChecks);
+
+// Get the state space
+auto sspace = ompl_static_pointer_cast<GeometricStateSpace>(
+      si->getStateSpace());
+
+
+// Step 2: Convert the AIKIDO trajectory to Geometric Path 
+::ompl::geometric::PathGeometric *path = new ::ompl::geometric::PathGeometric(si);
+
+for (size_t idx = 0; idx < _originalTraj->getNumWaypoints(); ++idx) 
+{
+      auto ompl_state = sspace->allocState(_originalTraj->getWaypoint(idx));
+      path->append(ompl_state);
+      sspace->freeState(ompl_state);
+}
+
+// Step 3: Use the OMPL methods to simplify the path
+::ompl::geometric::PathSimplifierPtr simplifier;
+simplifier.reset(new ::ompl::geometric::PathSimplifier(si));
+bool const shortened = simplifier->shortcutPath(*path, 1, 1, 1.0, 0.005); 
+
+// Step 4: Convert the simplified geomteric path to AIKIDO untimed trajectory
+if(shortened)
+{
+  auto returnTraj = std::make_shared<trajectory::Interpolated>(
+        std::move(_stateSpace), std::move(_interpolator));
+
+  for (size_t idx = 0; idx < path->getStateCount(); ++idx) 
+  {
+        const auto *st =
+            static_cast<GeometricStateSpace::StateType *>(
+                path->getState(idx));
+        // Arbitrary timing
+        returnTraj->addWaypoint(idx, st->mState);
+  }
+  return returnTraj;  
 }
+else
+{
+  return _originalTraj;
 }
+
 }
+
+//=============================================================================
+} // ns aikido
+} // ns planner
+} // ns ompl