All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
StateSampling.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 /* Author: Mark Moll */
00036 
00037 #include <ompl/base/SpaceInformation.h>
00038 #include <ompl/base/manifolds/SE3StateManifold.h>
00039 #include <ompl/base/samplers/ObstacleBasedValidStateSampler.h>
00040 #include <ompl/geometric/planners/rrt/RRTConnect.h>
00041 #include <ompl/geometric/SimpleSetup.h>
00042 
00043 #include <ompl/config.h>
00044 #include <boost/thread.hpp>
00045 #include <iostream>
00046 
00047 namespace ob = ompl::base;
00048 namespace og = ompl::geometric;
00049 
00050 // This is a problem-specific sampler that automatically generates valid
00051 // states; it doesn't need to call SpaceInformation::isValid. This is an
00052 // example of constrained sampling. If you can explicitly describe the set valid
00053 // states and can draw samples from it, then this is typically much more
00054 // efficient than generating random samples from the entire state space and
00055 // checking for validity.
00056 class MyValidStateSampler : public ob::ValidStateSampler
00057 {
00058 public:
00059     MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si)
00060     {
00061         name_ = "my sampler";
00062     }
00063     // Generate a sample in the valid part of the R^3 state space
00064     // Valid states satisfy the following constraints:
00065     // -1<= x,y,z <=1
00066     // if .25 <= z <= .5, then |x|>.8 and |y|>.8
00067     virtual bool sample(ob::State *state)
00068     {
00069         double* val = static_cast<ob::RealVectorStateManifold::StateType*>(state)->values;
00070         double z = rng_.uniformReal(-1,1);
00071 
00072         if (z>.25 && z<.5)
00073         {
00074             double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
00075             switch(rng_.uniformInt(0,3))
00076             {
00077                 case 0: val[0]=x-1;  val[1]=y-1;
00078                 case 1: val[0]=x-.8; val[1]=y+.8;
00079                 case 2: val[0]=y-1;  val[1]=x-1;
00080                 case 3: val[0]=y+.8; val[1]=x-.8;
00081             }
00082         }
00083         else
00084         {
00085             val[0] = rng_.uniformReal(-1,1);
00086             val[1] = rng_.uniformReal(-1,1);
00087         }
00088         val[2] = z;
00089         assert(si_->isValid(state));
00090         return true;
00091     }
00092     // We don't need this in the example below.
00093     virtual bool sampleNear(ob::State *state, const ob::State *near, const double distance)
00094     {
00095         throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented");
00096         return false;
00097     }
00098 protected:
00099     ompl::RNG rng_;
00100 };
00101 
00102 // this function is needed, even when we can write a sampler like the one
00103 // above, because we need to check path segments for validity
00104 bool isStateValid(const ob::State *state)
00105 {
00106     const ob::RealVectorStateManifold::StateType& pos = *state->as<ob::RealVectorStateManifold::StateType>();
00107     // Let's pretend that the validity check is computationally relatively
00108     // expensive to emphasize the benefit of explicitly generating valid
00109     // samples
00110     boost::this_thread::sleep(ompl::time::seconds(.001));
00111     // Valid states satisfy the following constraints:
00112     // -1<= x,y,z <=1
00113     // if .25 <= z <= .5, then |x|>.8 and |y|>.8
00114     return !(fabs(pos[0]<.8) && fabs(pos[1]<.8) && pos[2]>.25 && pos[2]<.5);
00115 }
00116 
00117 // return an obstacle-based sampler
00118 ob::ValidStateSamplerPtr allocOBValidStateSampler(const ob::SpaceInformation *si)
00119 {
00120     // we can perform any additional setup / configuration of a sampler here,
00121     // but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
00122     return ob::ValidStateSamplerPtr(new ob::ObstacleBasedValidStateSampler(si));
00123 }
00124 
00125 // return an instance of my sampler
00126 ob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si)
00127 {
00128     return ob::ValidStateSamplerPtr(new MyValidStateSampler(si));
00129 }
00130 
00131 
00132 void plan(int samplerIndex)
00133 {
00134     // construct the manifold we are planning in
00135     ob::StateManifoldPtr manifold(new ob::RealVectorStateManifold(3));
00136 
00137     // set the bounds
00138     ob::RealVectorBounds bounds(3);
00139     bounds.setLow(-1);
00140     bounds.setHigh(1);
00141     manifold->as<ob::RealVectorStateManifold>()->setBounds(bounds);
00142 
00143     // define a simple setup class
00144     og::SimpleSetup ss(manifold);
00145 
00146     // set state validity checking for this space
00147     ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
00148 
00149     // create a start state
00150     ob::ScopedState<> start(manifold);
00151     start[0] = start[1] = start[2] = 0;
00152 
00153     // create a goal state
00154     ob::ScopedState<> goal(manifold);
00155     goal[0] = goal[1] = 0.;
00156     goal[2] = 1;
00157 
00158     // set the start and goal states; this call allows SimpleSetup to infer the planning manifold, if needed
00159     ss.setStartAndGoalStates(start, goal);
00160 
00161     // set sampler (optional; the default is uniform sampling)
00162     if (samplerIndex==1)
00163         // use obstacle-based sampling
00164         ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);
00165     else if (samplerIndex==2)
00166         // use my sampler
00167         ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);
00168 
00169     // set the planner (optional)
00170     // create a planner for the defined space
00171     ob::PlannerPtr planner(new og::RRTConnect(ss.getSpaceInformation()));
00172     ss.setPlanner(planner);
00173 
00174     // attempt to solve the problem within ten seconds of planning time
00175     bool solved = ss.solve(10.0);
00176     if (solved)
00177     {
00178         std::cout << "Found solution:" << std::endl;
00179         // print the path to screen
00180         ss.simplifySolution();
00181         ss.getSolutionPath().print(std::cout);
00182     }
00183     else
00184         std::cout << "No solution found" << std::endl;
00185 }
00186 
00187 int main(int, char **)
00188 {
00189     std::cout << "Using default uniform sampler:" << std::endl;
00190     plan(0);
00191     std::cout << "\nUsing obstacle-based sampler:" << std::endl;
00192     plan(1);
00193     std::cout << "\nUsing my sampler:" << std::endl;
00194     plan(2);
00195 
00196     return 0;
00197 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator