CRRT.hpp

Go to the documentation of this file.

#ifndef AIKIDO_PLANNER_OMPL_CRRT_HPP_
#define AIKIDO_PLANNER_OMPL_CRRT_HPP_
 
#include <ompl/base/Planner.h>
#include <ompl/datastructures/NearestNeighbors.h>
#include <ompl/geometric/planners/PlannerIncludes.h>
 
#include "aikido/constraint/Projectable.hpp"
#include "aikido/planner/ompl/BackwardCompatibility.hpp"
 
namespace aikido {
namespace planner {
namespace ompl {
 
class CRRT : public ::ompl::base::Planner
{
public:
  explicit CRRT(const ::ompl::base::SpaceInformationPtr& _si);
 
  CRRT(const ::ompl::base::SpaceInformationPtr& _si, const std::string& _name);
 
  virtual ~CRRT();
 
  void getPlannerData(::ompl::base::PlannerData& _data) const override;
 
  ::ompl::base::PlannerStatus solve(
      const ::ompl::base::PlannerTerminationCondition& _ptc) override;
 
  ::ompl::base::PlannerStatus solve(double _solveTime);
 
  void clear() override;
 
  void setGoalBias(double _goalBias);
 
  double getGoalBias() const;
 
  void setRange(double _distance);
 
  double getRange() const;
 
  void setPathConstraint(constraint::ProjectablePtr _projectable);
 
  void setProjectionResolution(double _resolution);
 
  double getProjectionResolution() const;
 
  void setMinStateDifference(double _mindist);
 
  double getMinStateDifference() const;
 
  template <template <typename T> class NN>
  void setNearestNeighbors();
 
  void setup() override;
 
protected:
  class Motion
  {
  public:
    Motion() : state(nullptr), parent(nullptr)
    {
      // Do nothing
    }
 
    explicit Motion(const ::ompl::base::SpaceInformationPtr& _si)
      : state(_si->allocState()), parent(nullptr)
    {
      // Do nothing
    }
 
    ~Motion()
    {
      // Do nothing
    }
 
    ::ompl::base::State* state;
 
    Motion* parent;
  };
 
  virtual void freeMemory();
 
  double distanceFunction(const Motion* a, const Motion* b) const;
 
  using TreeData = ompl_shared_ptr<::ompl::NearestNeighbors<Motion*>>;
 
  TreeData mStartTree;
 
  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);
 
  ::ompl::base::StateSamplerPtr mSampler;
 
  double mGoalBias;
 
  double mMaxDistance;
 
  ::ompl::RNG mRng;
 
  Motion* mLastGoalMotion;
 
  constraint::ProjectablePtr mCons;
 
  double mMaxStepsize;
 
  double mMinStepsize;
};
 
} // namespace ompl
} // namespace planner
} // namespace aikido
 
#include "detail/CRRT-impl.hpp"
 
#endif // AIKIDO_PLANNER_OMPL_CRRT_HPP_