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