demos/Koules/KoulesSetup.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, 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 /* Author: Beck Chen, Mark Moll */ 00036 00037 #include "KoulesSetup.h" 00038 #include "KoulesGoal.h" 00039 #include "KoulesStateSpace.h" 00040 #include "KoulesControlSpace.h" 00041 #include "KoulesStatePropagator.h" 00042 #include "KoulesDirectedControlSampler.h" 00043 #include <ompl/base/spaces/RealVectorStateSpace.h> 00044 #include <ompl/control/planners/rrt/RRT.h> 00045 #include <ompl/control/planners/kpiece/KPIECE1.h> 00046 #include <ompl/control/planners/est/EST.h> 00047 #include <ompl/control/planners/pdst/PDST.h> 00048 00049 namespace ob = ompl::base; 00050 namespace oc = ompl::control; 00051 00053 class KoulesStateValidityChecker : public ompl::base::StateValidityChecker 00054 { 00055 public: 00056 KoulesStateValidityChecker(const ompl::base::SpaceInformationPtr &si) : ompl::base::StateValidityChecker(si) 00057 { 00058 } 00059 00060 virtual bool isValid(const ob::State *state) const 00061 { 00062 return si_->satisfiesBounds(state); 00063 } 00064 }; 00066 00067 ompl::control::DirectedControlSamplerPtr KoulesDirectedControlSamplerAllocator( 00068 const ompl::control::SpaceInformation *si, const ompl::base::GoalPtr &goal, bool propagateMax) 00069 { 00070 return ompl::control::DirectedControlSamplerPtr(new KoulesDirectedControlSampler(si, goal, propagateMax)); 00071 } 00072 00073 00074 KoulesSetup::KoulesSetup(unsigned int numKoules, const std::string& plannerName, 00075 const std::vector<double>& stateVec) 00076 : ompl::control::SimpleSetup(ompl::control::ControlSpacePtr(new KoulesControlSpace(numKoules))) 00077 { 00078 initialize(numKoules, plannerName, stateVec); 00079 } 00080 00081 KoulesSetup::KoulesSetup(unsigned int numKoules, const std::string& plannerName, double kouleVel) 00082 : ompl::control::SimpleSetup(ompl::control::ControlSpacePtr(new KoulesControlSpace(numKoules))) 00083 { 00084 initialize(numKoules, plannerName); 00085 double* state = getProblemDefinition()->getStartState(0)->as<KoulesStateSpace::StateType>()->values; 00086 double theta; 00087 ompl::RNG rng; 00088 for (unsigned int i = 0; i < numKoules; ++i) 00089 { 00090 theta = rng.uniformReal(0., 2. * boost::math::constants::pi<double>()); 00091 state[4 * i + 7] = kouleVel * cos(theta); 00092 state[4 * i + 8] = kouleVel * sin(theta); 00093 } 00094 } 00095 00096 void KoulesSetup::initialize(unsigned int numKoules, const std::string& plannerName, 00097 const std::vector<double>& stateVec) 00098 { 00099 const ob::StateSpacePtr& space = getStateSpace(); 00100 space->setup(); 00101 // setup start state 00102 ob::ScopedState<> start(space); 00103 if (stateVec.size() == space->getDimension()) 00104 space->copyFromReals(start.get(), stateVec); 00105 else 00106 { 00107 // Pick koule positions evenly radially distributed, but at a linearly 00108 // increasing distance from the center. The ship's initial position is 00109 // at the center. Initial velocities are 0. 00110 std::vector<double> startVec(space->getDimension(), 0.); 00111 double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules; 00112 startVec[0] = .5 * sideLength; 00113 startVec[1] = .5 * sideLength; 00114 startVec[4] = .5 * delta; 00115 for (unsigned int i = 0; i < numKoules; ++i, theta += delta) 00116 { 00117 r = .1 + i * .1 / numKoules; 00118 startVec[4 * i + 5] = .5 * sideLength + r * cos(theta); 00119 startVec[4 * i + 6] = .5 * sideLength + r * sin(theta); 00120 } 00121 space->copyFromReals(start.get(), startVec); 00122 } 00123 setStartState(start); 00124 // set goal 00125 setGoal(ob::GoalPtr(new KoulesGoal(si_))); 00126 // set propagation step size 00127 si_->setPropagationStepSize(propagationStepSize); 00128 // set min/max propagation steps 00129 si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps); 00130 // set directed control sampler; when using the PDST planner, propagate as long as possible 00131 si_->setDirectedControlSamplerAllocator( 00132 boost::bind(&KoulesDirectedControlSamplerAllocator, _1, getGoal(), plannerName == "pdst")); 00133 // set planner 00134 setPlanner(getConfiguredPlannerInstance(plannerName)); 00135 // set validity checker 00136 setStateValidityChecker(ob::StateValidityCheckerPtr(new KoulesStateValidityChecker(si_))); 00137 // set state propagator 00138 setStatePropagator(oc::StatePropagatorPtr(new KoulesStatePropagator(si_))); 00139 } 00140 00141 ob::PlannerPtr KoulesSetup::getConfiguredPlannerInstance(const std::string& plannerName) 00142 { 00143 if (plannerName == "rrt") 00144 { 00145 ob::PlannerPtr rrtplanner(new oc::RRT(si_)); 00146 rrtplanner->as<oc::RRT>()->setIntermediateStates(true); 00147 return rrtplanner; 00148 } 00149 else if (plannerName == "est") 00150 return ob::PlannerPtr(new oc::EST(si_)); 00151 else if (plannerName == "kpiece") 00152 return ob::PlannerPtr(new oc::KPIECE1(si_)); 00153 else 00154 { 00155 ob::PlannerPtr pdstplanner(new oc::PDST(si_)); 00156 pdstplanner->as<oc::PDST>()->setProjectionEvaluator( 00157 si_->getStateSpace()->getProjection("PDSTProjection")); 00158 return pdstplanner; 00159 } 00160 }