ompl/control/planners/ltl/src/LTLSpaceInformation.cpp
00001 #include "ompl/control/planners/ltl/LTLSpaceInformation.h" 00002 #include "ompl/control/SpaceInformation.h" 00003 #include "ompl/control/StatePropagator.h" 00004 #include "ompl/control/planners/ltl/ProductGraph.h" 00005 #include "ompl/base/StateValidityChecker.h" 00006 #include "ompl/base/spaces/DiscreteStateSpace.h" 00007 00008 namespace ob = ompl::base; 00009 namespace oc = ompl::control; 00010 00011 namespace 00012 { 00013 // Helper method to take a robot state space and product graph and return 00014 // the hybrid state space representing their product. 00015 static ob::StateSpacePtr extendStateSpace(const ob::StateSpacePtr& lowSpace, 00016 const oc::ProductGraphPtr& prod); 00017 } 00018 00019 oc::LTLSpaceInformation::LTLSpaceInformation(const oc::SpaceInformationPtr& si, 00020 const oc::ProductGraphPtr& prod) 00021 : oc::SpaceInformation(extendStateSpace(si->getStateSpace(), prod), 00022 si->getControlSpace()), prod_(prod), lowSpace_(si) 00023 { 00024 //TODO: Technically there's a bug here, as we've assigning LTLSpaceInformation's 00025 // control space to be si->getControlSpace(), which internally holds a pointer 00026 // to si->getStateSpace() instead of this->getStateSpace(). In practice, this 00027 // is fine for now, since control space never actually uses its internal state 00028 // space pointer. 00029 extendPropagator(si); 00030 extendValidityChecker(si); 00031 } 00032 00033 void oc::LTLSpaceInformation::setup(void) 00034 { 00035 // Set up the low space, then match our parameters to it. 00036 if (!lowSpace_->isSetup()) lowSpace_->setup(); 00037 // We never actually use the below parameters in LTLSpaceInformation while planning. 00038 // All integrating is done in lowSpace. However, we will need these parameters when 00039 // printing the path - PathControl::print() will convert path steps using these 00040 // parameters. 00041 setMinMaxControlDuration(lowSpace_->getMinControlDuration(), 00042 lowSpace_->getMaxControlDuration()); 00043 setPropagationStepSize(lowSpace_->getPropagationStepSize()); 00044 setup_ = true; 00045 } 00046 00047 void oc::LTLSpaceInformation::getFullState(const ob::State* low, ob::State* full) 00048 { 00049 const ProductGraph::State* high = prod_->getState(low); 00050 ob::CompoundState& cs = *full->as<ob::CompoundState>(); 00051 stateSpace_->as<ob::CompoundStateSpace>()->getSubspace(LOW_LEVEL)-> 00052 copyState(cs[LOW_LEVEL], low); 00053 typedef ob::DiscreteStateSpace::StateType DiscreteState; 00054 cs[REGION]->as<DiscreteState>()->value = high->getDecompRegion(); 00055 cs[COSAFE]->as<DiscreteState>()->value = high->getCosafeState(); 00056 cs[SAFE]->as<DiscreteState>()->value = high->getSafeState(); 00057 } 00058 00059 ob::State* oc::LTLSpaceInformation::getLowLevelState(ob::State* s) 00060 { 00061 return const_cast<ob::State*>(getLowLevelState(const_cast<const ob::State*>(s))); 00062 } 00063 00064 const ob::State* oc::LTLSpaceInformation::getLowLevelState(const ob::State* s) 00065 { 00066 return s->as<ob::CompoundState>()->operator[](LOW_LEVEL); 00067 } 00068 00069 oc::ProductGraph::State* oc::LTLSpaceInformation::getProdGraphState(const ob::State* s) const 00070 { 00071 const ob::CompoundState& cs = *s->as<ob::CompoundState>(); 00072 typedef ob::DiscreteStateSpace::StateType DiscreteState; 00073 return prod_->getState(cs[REGION]->as<DiscreteState>()->value, 00074 cs[COSAFE]->as<DiscreteState>()->value, 00075 cs[SAFE]->as<DiscreteState>()->value); 00076 } 00077 00078 void oc::LTLSpaceInformation::extendPropagator(const oc::SpaceInformationPtr& oldsi) 00079 { 00080 class LTLStatePropagator : public oc::StatePropagator 00081 { 00082 public: 00083 LTLStatePropagator(oc::LTLSpaceInformation* ltlsi, 00084 const oc::ProductGraphPtr& prod, 00085 const oc::StatePropagatorPtr& lowProp) 00086 : oc::StatePropagator(ltlsi), 00087 prod_(prod), lowProp_(lowProp), ltlsi_(ltlsi) {} 00088 virtual ~LTLStatePropagator() {} 00089 00090 virtual void propagate(const ob::State* state, const oc::Control* control, 00091 const double duration, ob::State* result) const 00092 { 00093 const ob::State* lowLevelPrev = ltlsi_->getLowLevelState(state); 00094 ob::State* lowLevelResult = ltlsi_->getLowLevelState(result); 00095 lowProp_->propagate(lowLevelPrev, control, duration, lowLevelResult); 00096 const oc::ProductGraph::State* prevHigh = ltlsi_->getProdGraphState(state); 00097 const oc::ProductGraph::State* nextHigh = prod_->getState(prevHigh, lowLevelResult); 00098 result->as<ob::CompoundState>()->as 00099 <ob::DiscreteStateSpace::StateType>(REGION)->value = nextHigh->getDecompRegion(); 00100 result->as<ob::CompoundState>()->as 00101 <ob::DiscreteStateSpace::StateType>(COSAFE)->value = nextHigh->getCosafeState(); 00102 result->as<ob::CompoundState>()->as 00103 <ob::DiscreteStateSpace::StateType>(SAFE)->value = nextHigh->getSafeState(); 00104 } 00105 00106 virtual bool canPropagateBackward(void) const 00107 { 00108 return lowProp_->canPropagateBackward(); 00109 } 00110 private: 00111 const oc::ProductGraphPtr prod_; 00112 const oc::StatePropagatorPtr lowProp_; 00113 oc::LTLSpaceInformation* ltlsi_; 00114 }; 00115 00116 // Some compilers have trouble with LTLStatePropagator being hidden in this function, 00117 // and so we explicitly cast it to its base type. 00118 setStatePropagator(oc::StatePropagatorPtr(static_cast<oc::StatePropagator*>( 00119 new LTLStatePropagator(this, prod_, oldsi->getStatePropagator())))); 00120 } 00121 00122 void oc::LTLSpaceInformation::extendValidityChecker(const oc::SpaceInformationPtr& oldsi) 00123 { 00124 class LTLStateValidityChecker : public ob::StateValidityChecker 00125 { 00126 public: 00127 LTLStateValidityChecker(oc::LTLSpaceInformation* ltlsi, 00128 const oc::ProductGraphPtr& prod, 00129 const ob::StateValidityCheckerPtr& lowChecker) 00130 : ob::StateValidityChecker(ltlsi), prod_(prod), lowChecker_(lowChecker), ltlsi_(ltlsi) 00131 { 00132 } 00133 virtual ~LTLStateValidityChecker() { } 00134 virtual bool isValid(const ob::State* s) const 00135 { 00136 return ltlsi_->getProdGraphState(s)->isValid() 00137 && lowChecker_->isValid(ltlsi_->getLowLevelState(s)); 00138 } 00139 private: 00140 const oc::ProductGraphPtr prod_; 00141 const ob::StateValidityCheckerPtr lowChecker_; 00142 oc::LTLSpaceInformation* ltlsi_; 00143 }; 00144 00145 // Some compilers have trouble with LTLStateValidityChecker being hidden in this function, 00146 // and so we explicitly cast it to its base type. 00147 setStateValidityChecker(ob::StateValidityCheckerPtr(static_cast<ob::StateValidityChecker*>( 00148 new LTLStateValidityChecker(this, prod_, oldsi->getStateValidityChecker())))); 00149 } 00150 00151 namespace 00152 { 00153 ob::StateSpacePtr extendStateSpace(const ob::StateSpacePtr& lowSpace, 00154 const oc::ProductGraphPtr& prod) 00155 { 00156 const oc::AutomatonPtr cosafe (prod->getCosafetyAutom()); 00157 const oc::AutomatonPtr safe (prod->getSafetyAutom()); 00158 ob::StateSpacePtr regionSpace(new ob::DiscreteStateSpace(0, prod->getDecomp()->getNumRegions()-1)); 00159 ob::StateSpacePtr cosafeSpace (new ob::DiscreteStateSpace(0, cosafe->numStates()-1)); 00160 ob::StateSpacePtr safeSpace (new ob::DiscreteStateSpace(0, safe->numStates()-1)); 00161 00162 ob::CompoundStateSpace* compound = new ob::CompoundStateSpace(); 00163 compound->addSubspace(lowSpace, 1.); 00164 compound->addSubspace(regionSpace, 0.); 00165 compound->addSubspace(cosafeSpace, 0.); 00166 compound->addSubspace(safeSpace, 0.); 00167 compound->lock(); 00168 00169 return ob::StateSpacePtr(compound); 00170 } 00171 }