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