ompl/control/planners/ltl/src/LTLProblemDefinition.cpp
00001 #include "ompl/control/PathControl.h"
00002 #include "ompl/control/planners/ltl/LTLProblemDefinition.h"
00003 #include "ompl/control/planners/ltl/LTLSpaceInformation.h"
00004 #include "ompl/base/ProblemDefinition.h"
00005 
00006 namespace ob = ompl::base;
00007 namespace oc = ompl::control;
00008 
00009 oc::LTLProblemDefinition::LTLProblemDefinition(const LTLSpaceInformationPtr& ltlsi)
00010     : ob::ProblemDefinition(ltlsi), ltlsi_(ltlsi)
00011 {
00012     createGoal();
00013 }
00014 
00015 void oc::LTLProblemDefinition::addLowerStartState(const ob::State* s)
00016 {
00017     ob::ScopedState<ob::CompoundStateSpace> fullStart(si_);
00018     ltlsi_->getFullState(s, fullStart.get());
00019     addStartState(fullStart);
00020 }
00021 
00022 ob::PathPtr oc::LTLProblemDefinition::getLowerSolutionPath(void) const
00023 {
00024     PathControl* fullPath = static_cast<PathControl*>(getSolutionPath().get());
00025     ob::PathPtr lowPathPtr(new PathControl(ltlsi_->getLowSpace()));
00026     PathControl* lowPath = static_cast<PathControl*>(lowPathPtr.get());
00027 
00028     if (fullPath->getStateCount() > 0)
00029     {
00030         for(size_t i = 0; i < fullPath->getStateCount()-1; ++i)
00031             lowPath->append(ltlsi_->getLowLevelState(fullPath->getState(i)),
00032                             fullPath->getControl(i),
00033                             fullPath->getControlDuration(i));
00034 
00035         // The last state does not have a control
00036         lowPath->append(ltlsi_->getLowLevelState(fullPath->getState(fullPath->getStateCount()-1)));
00037     }
00038 
00039     return lowPathPtr;
00040 }
00041 
00042 void oc::LTLProblemDefinition::createGoal(void)
00043 {
00044     class LTLGoal : public base::Goal
00045     {
00046     public:
00047         LTLGoal(const LTLSpaceInformationPtr& ltlsi)
00048             : ob::Goal(ltlsi), ltlsi_(ltlsi), prod_(ltlsi->getProductGraph()) {}
00049         virtual ~LTLGoal(void) {}
00050         virtual bool isSatisfied(const ob::State* s) const
00051         {
00052             return prod_->isSolution(ltlsi_->getProdGraphState(s));
00053         }
00054     protected:
00055         const LTLSpaceInformationPtr ltlsi_;
00056         const ProductGraphPtr prod_;
00057     };
00058 
00059     // Some compilers have trouble with LTLGoal being hidden in this function,
00060     // and so we explicitly cast it to its base type.
00061     setGoal(ob::GoalPtr(static_cast<ob::Goal*>(new LTLGoal(ltlsi_))));
00062 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines