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 }