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 }