AppBase.h
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ioan Sucan */
12 
13 #ifndef OMPLAPP_APP_BASE_
14 #define OMPLAPP_APP_BASE_
15 
16 #include "omplapp/geometry/RigidBodyGeometry.h"
17 #include <ompl/geometric/SimpleSetup.h>
18 #include <ompl/control/SimpleSetup.h>
19 #include "omplapp/apps/detail/appUtil.h"
20 
21 namespace ompl
22 {
23  namespace app
24  {
25 
26  enum AppType
27  { GEOMETRIC, CONTROL };
28 
29  template<AppType T>
31  {
34  };
35 
36  template<>
37  struct AppTypeSelector<CONTROL>
38  {
41  };
42 
43  template<AppType T>
44  class AppBase : public AppTypeSelector<T>::SimpleSetup,
45  public RigidBodyGeometry
46  {
47  public:
48  AppBase(const typename AppTypeSelector<T>::SpaceType &space, MotionModel model) :
50  {
51  }
52 
53  ~AppBase() override = default;
54 
55  AppType getAppType()
56  {
57  return GEOMETRIC;
58  }
59  const std::string& getName()
60  {
61  return name_;
62  }
63 
64  virtual bool isSelfCollisionEnabled() const = 0;
65 
66  virtual base::ScopedState<> getDefaultStartState() const = 0;
67 
68  virtual base::ScopedState<> getFullStateFromGeometricComponent(const base::ScopedState<> &state) const = 0;
69 
70  virtual base::ScopedState<> getGeometricComponentState(const base::ScopedState<> &state, unsigned int index) const
71  {
72  return base::ScopedState<>(getGeometricComponentStateSpace(), getGeometricComponentStateInternal(state.get(), index));
73  }
74 
75  virtual const base::StateSpacePtr& getGeometricComponentStateSpace() const = 0;
76 
77  virtual unsigned int getRobotCount() const = 0;
78 
79  GeometricStateExtractor getGeometricStateExtractor() const
80  {
81  return [this](const base::State* state, unsigned int index)
82  {
83  return getGeometricComponentStateInternal(state, index);
84  };
85  }
86 
87  virtual void inferEnvironmentBounds()
88  {
89  InferEnvironmentBounds(getGeometricComponentStateSpace(), *static_cast<RigidBodyGeometry*>(this));
90  }
91 
92  virtual void inferProblemDefinitionBounds()
93  {
94  InferProblemDefinitionBounds(AppTypeSelector<T>::SimpleSetup::getProblemDefinition(), getGeometricStateExtractor(), factor_, add_,
95  getRobotCount(), getGeometricComponentStateSpace(), mtype_);
96  }
97 
98  void setup() override
99  {
100  inferEnvironmentBounds();
101 
102  if (AppTypeSelector<T>::SimpleSetup::getProblemDefinition()->getStartStateCount() == 0)
103  {
104  OMPL_INFORM("Adding default start state");
105  AppTypeSelector<T>::SimpleSetup::addStartState(getDefaultStartState());
106  }
107 
108  inferProblemDefinitionBounds();
109 
110  const base::StateValidityCheckerPtr &svc = allocStateValidityChecker(AppTypeSelector<T>::SimpleSetup::si_,
111  getGeometricStateExtractor(), isSelfCollisionEnabled());
112  if (AppTypeSelector<T>::SimpleSetup::si_->getStateValidityChecker() != svc)
113  AppTypeSelector<T>::SimpleSetup::si_->setStateValidityChecker(svc);
114 
116 
117  if (!AppTypeSelector<T>::SimpleSetup::getStateSpace()->hasDefaultProjection())
119  registerDefaultProjection(allocGeometricStateProjector(AppTypeSelector<T>::SimpleSetup::getStateSpace(),
120  mtype_, getGeometricComponentStateSpace(),
121  getGeometricStateExtractor()));
122 
124  }
125 
128  void setOptimizationObjectiveAndThreshold(const std::string &objective, double threshold)
129  {
131  getOptimizationObjective(this->si_, objective, threshold));
132  }
133 
135  {
137  mtype_, getGeometricComponentStateSpace());
138  }
139 
140  protected:
141 
142  virtual const base::State* getGeometricComponentStateInternal(const base::State* state, unsigned int index) const = 0;
143 
144  std::string name_;
145 
146  };
147 
148  template<>
149  inline AppType AppBase<CONTROL>::getAppType()
150  {
151  return CONTROL;
152  }
153 
154  }
155 }
156 
157 #endif
void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
Set the optimization objective to use.
Definition: SimpleSetup.h:167
void setOptimizationObjectiveAndThreshold(const std::string &objective, double threshold)
Convenience function for the omplapp GUI. The objective can be one of: "length", "max min clearance"...
Definition: AppBase.h:128
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:87
Definition of a scoped state.
Definition: ScopedState.h:56
A shared pointer wrapper for ompl::base::StateSpace.
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: AppBase.h:98
ompl::base::OptimizationObjectivePtr getOptimizationObjective(const base::SpaceInformationPtr &si, const std::string &objective, double threshold)
Create an optimization objective. The objective name can be: "length", "max min clearance", or "mechanical work".
Definition: appUtil.cpp:256
A shared pointer wrapper for ompl::base::StateValidityChecker.
A shared pointer wrapper for ompl::control::ControlSpace.
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:394
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
MotionModel
Specify whether bodies are moving in 2D or bodies moving in 3D.
Definition of an abstract state.
Definition: State.h:49
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:59
void addStartState(const base::ScopedState<> &state)
Add a starting state for planning. This call is not needed if setStartAndGoalStates() has been called...
Definition: SimpleSetup.h:178
control::DecompositionPtr allocDecomposition(const base::StateSpacePtr &space, MotionModel mtype, const base::StateSpacePtr &gspace)
Allocate a default 2D/3D grid decomposition (depending on the MotionModel) for use with the SyclopEST...
Definition: appUtil.cpp:245
A shared pointer wrapper for ompl::control::Decomposition.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68