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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines