Loading...
Searching...
No Matches
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
10namespace ob = ompl::base;
11namespace oc = ompl::control;
12
13namespace
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
20oc::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
32void oc::LTLSpaceInformation::setup()
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
46void oc::LTLSpaceInformation::getFullState(const ob::State *low, ob::State *full)
47{
48 const ProductGraph::State *high = prod_->getState(low);
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
57ob::State *oc::LTLSpaceInformation::getLowLevelState(ob::State *s)
58{
59 return const_cast<ob::State *>(getLowLevelState(const_cast<const ob::State *>(s)));
60}
61
62const ob::State *oc::LTLSpaceInformation::getLowLevelState(const ob::State *s)
63{
64 return s->as<ob::CompoundState>()->operator[](LOW_LEVEL);
65}
66
67oc::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
75void 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
118void 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
145namespace
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.
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
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
STL namespace.