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