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_;
110  oc::LTLSpaceInformation *ltlsi_;
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_;
137  oc::LTLSpaceInformation *ltlsi_;
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 }
A space to allow the composition of state spaces.
Definition: StateSpace.h:574
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
Definition of a compound state.
Definition: State.h:87
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:95
bool setup_
Flag indicating whether setup() has been called on this instance.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
Definition of an abstract control.
Definition: Control.h:48
void setup() override
Perform additional setup tasks (run once, before use). If state validity checking resolution has not ...
A State of a ProductGraph represents a vertex in the graph-based Cartesian product represented by the...
Definition: ProductGraph.h:84
int getDecompRegion() const
Returns this State's PropositionalDecomposition region component.
int getCosafeState() const
Returns this State's co-safe Automaton state component.
int getSafeState() const
Returns this State's safe Automaton state component.
void setPropagationStepSize(double stepSize)
When controls are applied to states, they are applied for a time duration that is an integer multiple...
void setMinMaxControlDuration(unsigned int minSteps, unsigned int maxSteps)
Set the minimum and maximum number of steps a control is propagated for.
Model the effect of controls on system states.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:45