GeometricStateSpace.hpp

Go to the documentation of this file.

#ifndef AIKIDO_OMPL_AIKIDOGEOMETRICSTATESPACE_HPP_
#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 "aikido/statespace/GeodesicInterpolator.hpp"
#include "aikido/statespace/StateSpace.hpp"
 
namespace aikido {
namespace planner {
namespace ompl {
 
AIKIDO_DECLARE_POINTERS(GeometricStateSpace)
 
constexpr double EQUALITY_EPSILON = 1e-7;
 
class GeometricStateSpace : public ::ompl::base::StateSpace
{
public:
  class StateType : public ::ompl::base::State
  {
  public:
    explicit StateType(statespace::StateSpace::State* state);
 
    statespace::StateSpace::State* mState;
 
    bool mValid;
  };
 
  GeometricStateSpace(
      statespace::ConstStateSpacePtr sspace,
      statespace::ConstInterpolatorPtr interpolator,
      distance::DistanceMetricPtr dmetric,
      constraint::SampleablePtr sampler,
      constraint::ConstTestablePtr boundsConstraint,
      constraint::ProjectablePtr boundsProjection,
      double maxDistanceBetweenValidityChecks);
 
  unsigned int getDimension() const override;
 
  double getMaximumExtent() const override;
 
#if OMPL_VERSION_AT_LEAST(1, 0, 0)
  double getMeasure() const override;
#else
  double getMeasure() const;
#endif
 
  void enforceBounds(::ompl::base::State* state) const override;
 
  bool satisfiesBounds(const ::ompl::base::State* state) const override;
 
  void copyState(
      ::ompl::base::State* destination,
      const ::ompl::base::State* source) const override;
 
  double distance(
      const ::ompl::base::State* state1,
      const ::ompl::base::State* state2) const override;
 
  bool equalStates(
      const ::ompl::base::State* state1,
      const ::ompl::base::State* state2) const override;
 
  void interpolate(
      const ::ompl::base::State* from,
      const ::ompl::base::State* to,
      double t,
      ::ompl::base::State* state) const override;
 
  ::ompl::base::StateSamplerPtr allocDefaultStateSampler() const override;
 
  ::ompl::base::State* allocState() const override;
 
  ::ompl::base::State* allocState(
      const statespace::StateSpace::State* state) const;
 
  void freeState(::ompl::base::State* state) const override;
 
  statespace::ConstStateSpacePtr getAikidoStateSpace() const;
 
  aikido::statespace::ConstInterpolatorPtr getInterpolator() const;
 
  aikido::constraint::ConstTestablePtr getBoundsConstraint() const;
 
  double getMaxDistanceBetweenValidityChecks() const;
 
private:
  statespace::ConstStateSpacePtr mStateSpace;
 
  statespace::ConstInterpolatorPtr mInterpolator;
 
  distance::DistanceMetricPtr mDistance;
 
  constraint::SampleablePtr mSampler;
 
  constraint::ConstTestablePtr mBoundsConstraint;
 
  constraint::ProjectablePtr mBoundsProjection;
 
  double mMaxDistanceBetweenValidityChecks;
};
 
} // namespace ompl
} // namespace planner
} // namespace aikido
 
#endif