ompl/extensions/morse/src/MorseSimpleSetup.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Authors: Ioan Sucan, Caleb Voss */
00036 
00037 #include "ompl/extensions/morse/MorseSimpleSetup.h"
00038 #include "ompl/extensions/morse/MorseControlSpace.h"
00039 #include "ompl/extensions/morse/MorseProjection.h"
00040 #include "ompl/extensions/morse/MorseStatePropagator.h"
00041 #include "ompl/extensions/morse/MorseStateValidityChecker.h"
00042 #include "ompl/extensions/morse/MorseTerminationCondition.h"
00043 #include "ompl/util/Console.h"
00044 
00045 ompl::control::MorseSimpleSetup::MorseSimpleSetup(const base::MorseEnvironmentPtr &env) :
00046     SimpleSetup(ControlSpacePtr(new MorseControlSpace(base::StateSpacePtr(new base::MorseStateSpace(env))))),
00047     env_(env)
00048 {
00049     si_->setPropagationStepSize(env_->stepSize_);
00050     si_->setMinMaxControlDuration(env_->minControlSteps_, env_->maxControlSteps_);
00051     si_->setStatePropagator(StatePropagatorPtr(new MorseStatePropagator(si_)));
00052 }
00053 
00054 ompl::base::ScopedState<ompl::base::MorseStateSpace> ompl::control::MorseSimpleSetup::getCurrentState() const
00055 {
00056     base::ScopedState<base::MorseStateSpace> current(getStateSpace());
00057     getStateSpace()->as<base::MorseStateSpace>()->readState(current.get());
00058     return current;
00059 }
00060 
00061 void ompl::control::MorseSimpleSetup::setCurrentState(const base::State *state)
00062 {
00063     getStateSpace()->as<base::MorseStateSpace>()->writeState(state);
00064 }
00065 
00066 void ompl::control::MorseSimpleSetup::setCurrentState(const base::ScopedState<> &state)
00067 {
00068     getStateSpace()->as<base::MorseStateSpace>()->writeState(state.get());
00069 }
00070 
00071 void ompl::control::MorseSimpleSetup::setup()
00072 {
00073     if (!si_->getStateValidityChecker())
00074     {
00075         OMPL_INFORM("Using default state validity checker for MORSE");
00076         si_->setStateValidityChecker(base::StateValidityCheckerPtr(new base::MorseStateValidityChecker(si_)));
00077     }
00078     base::StateSpacePtr space = si_->getStateSpace();
00079     if (!space->hasDefaultProjection())
00080     {
00081         OMPL_INFORM("Registering MorseProjection as default projection evaluator for MORSE");
00082         space->registerDefaultProjection(base::ProjectionEvaluatorPtr(new base::MorseProjection(space)));
00083     }
00084     if (pdef_->getStartStateCount() == 0)
00085     {
00086         OMPL_INFORM("Using the initial state of MORSE as the starting state for the planner");
00087         pdef_->addStartState(getCurrentState());
00088     }
00089     SimpleSetup::setup();
00090 }
00091 
00092 ompl::base::PlannerStatus ompl::control::MorseSimpleSetup::solve()
00093 {
00094     setup();
00095     // terminate if user exits MORSE
00096     return SimpleSetup::solve(base::MorseTerminationCondition(env_));
00097 }
00098 
00099 void ompl::control::MorseSimpleSetup::playSolutionPath() const
00100 {
00101     if (haveSolutionPath())
00102         playPath(pdef_->getSolutionPath());
00103 }
00104 
00105 void ompl::control::MorseSimpleSetup::playPath(const base::PathPtr &path) const
00106 {
00107     PathControl *pc = dynamic_cast<PathControl*>(path.get());
00108     if (pc)
00109     {
00110         unsigned int i;
00111         base::State *result = si_->allocState();
00112         for (i = 0; i < pc->getControlCount(); i++)
00113         {
00114             si_->getStatePropagator()->propagate(pc->getState(i), pc->getControl(i), pc->getControlDuration(i), result);
00115         }
00116         getStateSpace()->as<base::MorseStateSpace>()->writeState(pc->getState(i));
00117     }
00118     else
00119     {
00120         geometric::PathGeometric *pg = dynamic_cast<geometric::PathGeometric*>(path.get());
00121         if (!pg)
00122             throw Exception("Unknown type of path");
00123         if (pg->getStateCount() > 0)
00124         {
00125             double d = si_->getPropagationStepSize();
00126             getStateSpace()->as<base::MorseStateSpace>()->writeState(pg->getState(0));
00127             for (unsigned int i = 1 ; i < pg->getStateCount() ; ++i)
00128             {
00129                 getEnvironment()->worldStep(d);
00130                 getStateSpace()->as<base::MorseStateSpace>()->writeState(pg->getState(i));
00131             }
00132         }
00133     }
00134 }
00135 
00136 ompl::base::PathPtr ompl::control::MorseSimpleSetup::simulateControl(const double *control, unsigned int steps) const
00137 {
00138     Control *c = si_->allocControl();
00139     memcpy(c->as<MorseControlSpace::ControlType>()->values, control, sizeof(double) * getControlSpace()->getDimension());
00140     base::PathPtr path = simulateControl(c, steps);
00141     si_->freeControl(c);
00142     return path;
00143 }
00144 
00145 ompl::base::PathPtr ompl::control::MorseSimpleSetup::simulateControl(const Control *control, unsigned int steps) const
00146 {
00147     PathControl *p = new PathControl(si_);
00148 
00149     base::State *s0 = si_->allocState();
00150     getStateSpace()->as<base::MorseStateSpace>()->readState(s0);
00151     p->getStates().push_back(s0);
00152 
00153     base::State *s1 = si_->allocState();
00154     si_->propagate(s0, control, steps, s1);
00155     p->getStates().push_back(s1);
00156 
00157     p->getControls().push_back(si_->cloneControl(control));
00158     p->getControlDurations().push_back(steps);
00159     return base::PathPtr(p);
00160 }
00161 
00162 ompl::base::PathPtr ompl::control::MorseSimpleSetup::simulate(unsigned int steps) const
00163 {
00164     Control *c = si_->allocControl();
00165     si_->nullControl(c);
00166     base::PathPtr path = simulateControl(c, steps);
00167     si_->freeControl(c);
00168     return path;
00169 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines