Add wrapper for OMPL path simplifier (#164)

include/aikido/planner/ompl/Planner.hpp

@@ -1,20 +1,27 @@
 #ifndef AIKIDO_OMPL_OMPLPLANNER_HPP_
 #define AIKIDO_OMPL_OMPLPLANNER_HPP_
 
+#include <chrono>
+#include <utility> // std::pair
+
 #include "../../constraint/Projectable.hpp"
 #include "../../constraint/Sampleable.hpp"
 #include "../../constraint/Testable.hpp"
 #include "../../distance/DistanceMetric.hpp"
 #include "../../planner/ompl/BackwardCompatibility.hpp"
+#include "../../planner/ompl/GeometricStateSpace.hpp"
 #include "../../statespace/Interpolator.hpp"
 #include "../../statespace/StateSpace.hpp"
 #include "../../trajectory/Interpolated.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>
+
 namespace aikido {
 namespace planner {
 namespace ompl {
@@ -262,6 +269,66 @@ trajectory::InterpolatedPtr planOMPL(
     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 _maxDistanceBtwValidityChecks The maximum distance (under dmetric)
+/// between validity checking two successive points on a tree extension
+/// \param _timeout Timeout, in seconds, after which the simplifier terminates
+/// to return possibly shortened path
+/// \param _maxEmptySteps Maximum number of consecutive failed attempts at
+/// shortening before simplification terminates. Default 0, equal to number
+/// of states in the path
+/// \param _rangeRatio Maximum distance between states a connection is
+/// attempted, as a fraction relative to the total length of the path
+/// \param _snapToVertex Threshold distance for snapping a state on shortened
+/// path to a state on original path
+/// \param _originalTraj The untimed trajectory obtained from the planner,
+/// needs simplifying.
+std::pair<std::unique_ptr<trajectory::Interpolated>, bool> simplifyOMPL(
+    statespace::StateSpacePtr _stateSpace,
+    statespace::InterpolatorPtr _interpolator,
+    distance::DistanceMetricPtr _dmetric,
+    constraint::SampleablePtr _sampler,
+    constraint::TestablePtr _validityConstraint,
+    constraint::TestablePtr _boundsConstraint,
+    constraint::ProjectablePtr _boundsProjector,
+    double _maxDistanceBtwValidityChecks,
+    double _timeout,
+    size_t _maxEmptySteps,
+    trajectory::InterpolatedPtr _originalTraj);
+
+/// Take an interpolated trajectory and convert it into OMPL geometric path
+/// \param _interpolatedTraj the interpolated trajectory to be converted
+/// \param _sspace The space information pointer.
+/// returns the corresponding OMPL geometric path
+::ompl::geometric::PathGeometric toOMPLTrajectory(
+    const trajectory::InterpolatedPtr& _interpolatedTraj,
+    ::ompl::base::SpaceInformationPtr _si);
+
+/// Take an OMPL geometric path and convert it into an interpolated trajectory
+/// \param _path The OMPL geometric path
+/// \param _interpolator An Interpolator defined on the StateSpace. This is used
+/// to interpolate between two points within the space.
+/// returns the corresponding interpolated trajectory
+std::unique_ptr<trajectory::Interpolated> toInterpolatedTrajectory(
+    const ::ompl::geometric::PathGeometric& _path,
+    statespace::InterpolatorPtr _interpolator);
+
 } // namespace ompl
 } // namespace planner
 } // namespace aikido

src/planner/ompl/Planner.cpp

@@ -5,6 +5,8 @@
 #include <aikido/planner/ompl/MotionValidator.hpp>
 #include <aikido/planner/ompl/Planner.hpp>
 
+#include <dart/dart.hpp>
+
 namespace aikido {
 namespace planner {
 namespace ompl {
@@ -372,6 +374,126 @@ trajectory::InterpolatedPtr planCRRTConnect(
       std::move(_interpolator),
       _maxPlanTime);
 }
+
+//=============================================================================
+std::pair<std::unique_ptr<trajectory::Interpolated>, bool> simplifyOMPL(
+    statespace::StateSpacePtr _stateSpace,
+    statespace::InterpolatorPtr _interpolator,
+    distance::DistanceMetricPtr _dmetric,
+    constraint::SampleablePtr _sampler,
+    constraint::TestablePtr _validityConstraint,
+    constraint::TestablePtr _boundsConstraint,
+    constraint::ProjectablePtr _boundsProjector,
+    double _maxDistanceBtwValidityChecks,
+    double _timeout,
+    size_t _maxEmptySteps,
+    trajectory::InterpolatedPtr _originalTraj)
+{
+  if (_timeout < 0)
+  {
+    throw std::invalid_argument("Timeout must be >= 0");
+  }
+
+  // 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);
+
+  // Step 2: Convert AIKIDO Interpolated Trajectory to OMPL path
+  auto path = toOMPLTrajectory(_originalTraj, si);
+
+  // Step 3: Use the OMPL methods to simplify the path
+  ::ompl::geometric::PathSimplifier simplifier{si};
+
+  // Flag for user to know if shorten was successful
+  bool shorten_success = false;
+
+  // Set the parameters for termination of simplification process
+  std::chrono::system_clock::time_point const time_before
+      = std::chrono::system_clock::now();
+  std::chrono::system_clock::time_point time_current;
+  std::chrono::duration<double> const time_limit
+      = std::chrono::duration<double>(_timeout);
+  size_t empty_steps = 0;
+
+  do
+  {
+
+    bool const shortened
+        = simplifier.shortcutPath(path, 1, _maxEmptySteps, 1.0, 0.0);
+    if (shortened)
+      empty_steps = 0;
+    else
+      empty_steps += 1;
+
+    time_current = std::chrono::system_clock::now();
+    shorten_success = shorten_success || shortened;
+
+  } while (time_current - time_before <= time_limit
+           && empty_steps <= _maxEmptySteps);
+
+  // Step 4: Convert the simplified geomteric path to AIKIDO untimed trajectory
+  auto returnTraj = toInterpolatedTrajectory(path, _interpolator);
+
+  // Step 5: Return trajectory and notify user if shortening was successful
+  std::pair<std::unique_ptr<trajectory::Interpolated>, bool> returnPair;
+  return std::make_pair(std::move(returnTraj), shorten_success);
 }
+//=============================================================================
+
+// Following are helper functions.
+
+::ompl::geometric::PathGeometric toOMPLTrajectory(
+    const trajectory::InterpolatedPtr& _interpolatedTraj,
+    ::ompl::base::SpaceInformationPtr _si)
+{
+  auto sspace
+      = ompl_dynamic_pointer_cast<GeometricStateSpace>(_si->getStateSpace());
+
+  if (!sspace)
+  {
+    throw std::invalid_argument("GeometricStateSpace Required");
+  }
+
+  ::ompl::geometric::PathGeometric returnPath{std::move(_si)};
+
+  for (size_t idx = 0; idx < _interpolatedTraj->getNumWaypoints(); ++idx)
+  {
+    auto ompl_state = sspace->allocState(_interpolatedTraj->getWaypoint(idx));
+    returnPath.append(ompl_state);
+    sspace->freeState(ompl_state);
+  }
+  return returnPath;
 }
+
+std::unique_ptr<trajectory::Interpolated> toInterpolatedTrajectory(
+    const ::ompl::geometric::PathGeometric& _path,
+    statespace::InterpolatorPtr _interpolator)
+{
+  auto returnInterpolated = dart::common::make_unique<trajectory::Interpolated>(
+      std::move(_interpolator->getStateSpace()), std::move(_interpolator));
+
+  for (size_t idx = 0; idx < _path.getStateCount(); ++idx)
+  {
+    // Note that following static_cast is guaranteed to be safe because
+    // GeometricPath defines a path through a GeometricStateSpace, which
+    // always contains GeometricStateSpace::StateType states
+    const auto* st = static_cast<const GeometricStateSpace::StateType*>(
+        _path.getState(idx));
+
+    // Arbitrary timing
+    returnInterpolated->addWaypoint(idx, st->mState);
+  }
+  return returnInterpolated;
 }
+//=============================================================================
+
+} // ns ompl
+} // ns planner
+} // ns aikido

tests/planner/ompl/CMakeLists.txt

@@ -14,6 +14,12 @@ target_link_libraries(test_MotionValidator "${PROJECT_NAME}_planner_ompl")
 aikido_add_test(test_OMPLPlanner test_OMPLPlanner.cpp)
 target_link_libraries(test_OMPLPlanner "${PROJECT_NAME}_planner_ompl")
 
+aikido_add_test(test_OMPLSimplifier test_OMPLSimplifier.cpp)
+target_link_libraries(test_OMPLSimplifier "${PROJECT_NAME}_planner_ompl")
+
+aikido_add_test(test_TrajectoryConversions test_TrajectoryConversions.cpp)
+target_link_libraries(test_TrajectoryConversions "${PROJECT_NAME}_planner_ompl")
+
 aikido_add_test(test_StateSampler test_StateSampler.cpp)
 target_link_libraries(test_StateSampler "${PROJECT_NAME}_planner_ompl")
 

tests/planner/ompl/OMPLTestHelpers.hpp

@@ -296,4 +296,48 @@ class PlannerTest : public ::testing::Test
   aikido::constraint::TestablePtr boundsConstraint;
   aikido::constraint::TestablePtr collConstraint;
 };
+
+
+class SimplifierTest : public ::testing::Test
+{
+public:
+  virtual void SetUp()
+  {
+    using StateSpace = aikido::statespace::dart::MetaSkeletonStateSpace;
+
+    // Create robot
+    robot = createTranslationalRobot();
+
+    stateSpace = std::make_shared<StateSpace>(robot);
+    interpolator = std::make_shared<aikido::statespace::GeodesicInterpolator>(stateSpace);
+
+    // Collision constraint
+    collConstraint = std::make_shared<MockTranslationalRobotConstraint>(
+        stateSpace, Eigen::Vector3d(-0.1, -0.1, -0.1),
+        Eigen::Vector3d(0.1, 0.1, 0.1));
+
+    // Distance metric
+    dmetric = aikido::distance::createDistanceMetric(stateSpace);
+
+    // Sampler
+    sampler =
+        aikido::constraint::createSampleableBounds(stateSpace, make_rng());
+
+    // Projectable constraint
+    boundsProjection = aikido::constraint::createProjectableBounds(stateSpace);
+
+    // Joint limits
+    boundsConstraint = aikido::constraint::createTestableBounds(stateSpace);
+  }
+
+  dart::dynamics::SkeletonPtr robot;
+  aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace;
+  aikido::statespace::InterpolatorPtr interpolator;
+  aikido::distance::DistanceMetricPtr dmetric;
+  aikido::constraint::SampleablePtr sampler;
+  aikido::constraint::ProjectablePtr boundsProjection;
+  aikido::constraint::TestablePtr boundsConstraint;
+  aikido::constraint::TestablePtr collConstraint;
+};
+
 #endif