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 class AppType
27  { GEOMETRIC, CONTROL };
28 
29  template<AppType T>
30  struct AppTypeSelector
31  {
32  using SimpleSetup = geometric::SimpleSetup;
33  using SpaceType = base::StateSpacePtr;
34  };
35 
36  template<>
37  struct AppTypeSelector<AppType::CONTROL>
38  {
39  using SimpleSetup = control::SimpleSetup;
40  using SpaceType = control::ControlSpacePtr;
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 AppType::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());
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 
134  control::DecompositionPtr allocDecomposition()
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<>
150  {
151  return AppType::CONTROL;
152  }
153 
154  }
155 }
156 
157 #endif
const base::OptimizationObjectivePtr & getOptimizationObjective() const
Get the optimization objective to use.
Definition: SimpleSetup.h:198
RigidBodyGeometry(MotionModel mtype, CollisionChecker ctype)
Constructor expects a state space that can represent a rigid body.
MotionModel
Specify whether bodies are moving in 2D or bodies moving in 3D.
Definition of an abstract state.
Definition: State.h:113
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:126
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:144
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:489
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:156
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:54
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:126
void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
Set the optimization objective to use.
Definition: SimpleSetup.h:239
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
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:346
const base::StateValidityCheckerPtr & allocStateValidityChecker(const base::SpaceInformationPtr &si, const GeometricStateExtractor &se, bool selfCollision)
Allocate default state validity checker using FCL.
Definition of a scoped state.
Definition: ScopedState.h:120
const base::StateValidityCheckerPtr & getStateValidityChecker() const
Get the current instance of the state validity checker.
Definition: SimpleSetup.h:162
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:250
const base::ProblemDefinitionPtr & getProblemDefinition() const
Get the current instance of the problem definition.
Definition: SimpleSetup.h:144
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: AppBase.h:114
double factor_
The factor to multiply inferred environment bounds by (default 1)
double add_
The value to add to inferred environment bounds (default 0)
Main namespace. Contains everything in this library.
Definition: AppBase.h:21