LTLSpaceInformation.cpp
1 #include "ompl/control/planners/ltl/LTLSpaceInformation.h"
2 #include "ompl/control/SpaceInformation.h"
3 #include "ompl/control/StatePropagator.h"
4 #include "ompl/control/planners/ltl/ProductGraph.h"
5 #include "ompl/base/StateValidityChecker.h"
6 #include "ompl/base/spaces/DiscreteStateSpace.h"
7 
8 namespace ob = ompl::base;
9 namespace oc = ompl::control;
10 
11 namespace
12 {
13  // Helper method to take a robot state space and product graph and return
14  // the hybrid state space representing their product.
15  static ob::StateSpacePtr extendStateSpace(const ob::StateSpacePtr& lowSpace,
16  const oc::ProductGraphPtr& prod);
17 }
18 
19 oc::LTLSpaceInformation::LTLSpaceInformation(const oc::SpaceInformationPtr& si,
20  const oc::ProductGraphPtr& prod)
21  : oc::SpaceInformation(extendStateSpace(si->getStateSpace(), prod),
22  si->getControlSpace()), prod_(prod), lowSpace_(si)
23 {
24  //TODO: Technically there's a bug here, as we've assigning LTLSpaceInformation's
25  // control space to be si->getControlSpace(), which internally holds a pointer
26  // to si->getStateSpace() instead of this->getStateSpace(). In practice, this
27  // is fine for now, since control space never actually uses its internal state
28  // space pointer.
29  extendPropagator(si);
30  extendValidityChecker(si);
31 }
32 
34 {
35  // Set up the low space, then match our parameters to it.
36  if (!lowSpace_->isSetup()) lowSpace_->setup();
37  // We never actually use the below parameters in LTLSpaceInformation while planning.
38  // All integrating is done in lowSpace. However, we will need these parameters when
39  // printing the path - PathControl::print() will convert path steps using these
40  // parameters.
41  setMinMaxControlDuration(lowSpace_->getMinControlDuration(),
42  lowSpace_->getMaxControlDuration());
43  setPropagationStepSize(lowSpace_->getPropagationStepSize());
44  setup_ = true;
45 }
46 
47 void oc::LTLSpaceInformation::getFullState(const ob::State* low, ob::State* full)
48 {
49  const ProductGraph::State* high = prod_->getState(low);
50  ob::CompoundState& cs = *full->as<ob::CompoundState>();
51  stateSpace_->as<ob::CompoundStateSpace>()->getSubspace(LOW_LEVEL)->
52  copyState(cs[LOW_LEVEL], low);
53  typedef ob::DiscreteStateSpace::StateType DiscreteState;
54  cs[REGION]->as<DiscreteState>()->value = high->getDecompRegion();
55  cs[COSAFE]->as<DiscreteState>()->value = high->getCosafeState();
56  cs[SAFE]->as<DiscreteState>()->value = high->getSafeState();
57 }
58 
59 ob::State* oc::LTLSpaceInformation::getLowLevelState(ob::State* s)
60 {
61  return const_cast<ob::State*>(getLowLevelState(const_cast<const ob::State*>(s)));
62 }
63 
64 const ob::State* oc::LTLSpaceInformation::getLowLevelState(const ob::State* s)
65 {
66  return s->as<ob::CompoundState>()->operator[](LOW_LEVEL);
67 }
68 
69 oc::ProductGraph::State* oc::LTLSpaceInformation::getProdGraphState(const ob::State* s) const
70 {
71  const ob::CompoundState& cs = *s->as<ob::CompoundState>();
72  typedef ob::DiscreteStateSpace::StateType DiscreteState;
73  return prod_->getState(cs[REGION]->as<DiscreteState>()->value,
74  cs[COSAFE]->as<DiscreteState>()->value,
75  cs[SAFE]->as<DiscreteState>()->value);
76 }
77 
78 void oc::LTLSpaceInformation::extendPropagator(const oc::SpaceInformationPtr& oldsi)
79 {
80  class LTLStatePropagator : public oc::StatePropagator
81  {
82  public:
83  LTLStatePropagator(oc::LTLSpaceInformation* ltlsi,
84  const oc::ProductGraphPtr& prod,
85  const oc::StatePropagatorPtr& lowProp)
86  : oc::StatePropagator(ltlsi),
87  prod_(prod), lowProp_(lowProp), ltlsi_(ltlsi) {}
88  virtual ~LTLStatePropagator() {}
89 
90  virtual void propagate(const ob::State* state, const oc::Control* control,
91  const double duration, ob::State* result) const
92  {
93  const ob::State* lowLevelPrev = ltlsi_->getLowLevelState(state);
94  ob::State* lowLevelResult = ltlsi_->getLowLevelState(result);
95  lowProp_->propagate(lowLevelPrev, control, duration, lowLevelResult);
96  const oc::ProductGraph::State* prevHigh = ltlsi_->getProdGraphState(state);
97  const oc::ProductGraph::State* nextHigh = prod_->getState(prevHigh, lowLevelResult);
98  result->as<ob::CompoundState>()->as
99  <ob::DiscreteStateSpace::StateType>(REGION)->value = nextHigh->getDecompRegion();
100  result->as<ob::CompoundState>()->as
101  <ob::DiscreteStateSpace::StateType>(COSAFE)->value = nextHigh->getCosafeState();
102  result->as<ob::CompoundState>()->as
103  <ob::DiscreteStateSpace::StateType>(SAFE)->value = nextHigh->getSafeState();
104  }
105 
106  virtual bool canPropagateBackward(void) const
107  {
108  return lowProp_->canPropagateBackward();
109  }
110  private:
111  const oc::ProductGraphPtr prod_;
112  const oc::StatePropagatorPtr lowProp_;
114  };
115 
116  // Some compilers have trouble with LTLStatePropagator being hidden in this function,
117  // and so we explicitly cast it to its base type.
118  setStatePropagator(oc::StatePropagatorPtr(static_cast<oc::StatePropagator*>(
119  new LTLStatePropagator(this, prod_, oldsi->getStatePropagator()))));
120 }
121 
122 void oc::LTLSpaceInformation::extendValidityChecker(const oc::SpaceInformationPtr& oldsi)
123 {
124  class LTLStateValidityChecker : public ob::StateValidityChecker
125  {
126  public:
127  LTLStateValidityChecker(oc::LTLSpaceInformation* ltlsi,
128  const oc::ProductGraphPtr& prod,
129  const ob::StateValidityCheckerPtr& lowChecker)
130  : ob::StateValidityChecker(ltlsi), prod_(prod), lowChecker_(lowChecker), ltlsi_(ltlsi)
131  {
132  }
133  virtual ~LTLStateValidityChecker() { }
134  virtual bool isValid(const ob::State* s) const
135  {
136  return ltlsi_->getProdGraphState(s)->isValid()
137  && lowChecker_->isValid(ltlsi_->getLowLevelState(s));
138  }
139  private:
140  const oc::ProductGraphPtr prod_;
141  const ob::StateValidityCheckerPtr lowChecker_;
143  };
144 
145  // Some compilers have trouble with LTLStateValidityChecker being hidden in this function,
146  // and so we explicitly cast it to its base type.
147  setStateValidityChecker(ob::StateValidityCheckerPtr(static_cast<ob::StateValidityChecker*>(
148  new LTLStateValidityChecker(this, prod_, oldsi->getStateValidityChecker()))));
149 }
150 
151 namespace
152 {
153  ob::StateSpacePtr extendStateSpace(const ob::StateSpacePtr& lowSpace,
154  const oc::ProductGraphPtr& prod)
155  {
156  const oc::AutomatonPtr cosafe (prod->getCosafetyAutom());
157  const oc::AutomatonPtr safe (prod->getSafetyAutom());
158  ob::StateSpacePtr regionSpace(new ob::DiscreteStateSpace(0, prod->getDecomp()->getNumRegions()-1));
159  ob::StateSpacePtr cosafeSpace (new ob::DiscreteStateSpace(0, cosafe->numStates()-1));
160  ob::StateSpacePtr safeSpace (new ob::DiscreteStateSpace(0, safe->numStates()-1));
161 
163  compound->addSubspace(lowSpace, 1.);
164  compound->addSubspace(regionSpace, 0.);
165  compound->addSubspace(cosafeSpace, 0.);
166  compound->addSubspace(safeSpace, 0.);
167  compound->lock();
168 
169  return ob::StateSpacePtr(compound);
170  }
171 }
Definition of a compound state.
Definition: State.h:95
const LTLSpaceInformation * ltlsi_
Handle to the control::SpaceInformation object.
Definition: LTLPlanner.h:179
Definition of an abstract control.
Definition: Control.h:48
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
A boost shared pointer wrapper for ompl::base::StateSpace.
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
virtual void setup(void)
Perform additional setup tasks (run once, before use)
int getCosafeState(void) const
Returns this State&#39;s co-safe Automaton state component.
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:109
int getDecompRegion(void) const
Returns this State&#39;s PropositionalDecomposition region component.
Model the effect of controls on system states.
State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:418
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
A space to allow the composition of state spaces.
Definition: StateSpace.h:544
bool isValid(void) const
Returns whether this State is valid. A State is valid if and only if none of its Automaton states are...
Abstract definition for a class checking the validity of states. The implementation of this class mus...
A State of a ProductGraph represents a vertex in the graph-based Cartesian product represented by the...
Definition: ProductGraph.h:74
Definition of an abstract state.
Definition: State.h:50
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
A boost shared pointer wrapper for ompl::control::SpaceInformation.
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
Definition: StateSpace.cpp:855
A boost shared pointer wrapper for ompl::control::ProductGraph.
A boost shared pointer wrapper for ompl::control::Automaton.
A space representing discrete states; i.e. there are a small number of discrete states the system can...
int getSafeState(void) const
Returns this State&#39;s safe Automaton state component.
T * as()
Cast this instance to a desired type.
Definition: Planner.h:247