37 #include <ompl/base/SpaceInformation.h>
38 #include <ompl/base/spaces/SE3StateSpace.h>
39 #include <ompl/base/samplers/ObstacleBasedValidStateSampler.h>
40 #include <ompl/geometric/planners/prm/PRM.h>
41 #include <ompl/geometric/SimpleSetup.h>
43 #include <ompl/config.h>
44 #include <boost/thread.hpp>
47 namespace ob = ompl::base;
48 namespace og = ompl::geometric;
73 double z = rng_.uniformReal(-1,1);
77 double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
78 switch(rng_.uniformInt(0,3))
80 case 0: val[0]=x-1; val[1]=y-1;
81 case 1: val[0]=x-.8; val[1]=y+.8;
82 case 2: val[0]=y-1; val[1]=x-1;
83 case 3: val[0]=y+.8; val[1]=x-.8;
88 val[0] = rng_.uniformReal(-1,1);
89 val[1] = rng_.uniformReal(-1,1);
92 assert(si_->isValid(state));
98 throw ompl::Exception(
"MyValidStateSampler::sampleNear",
"not implemented");
109 bool isStateValid(
const ob::State *state)
119 return !(fabs(pos[0])<.8 && fabs(pos[1])<.8 && pos[2]>.25 && pos[2]<.5);
137 void plan(
int samplerIndex)
152 ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
156 start[0] = start[1] = start[2] = 0;
160 goal[0] = goal[1] = 0.;
164 ss.setStartAndGoalStates(start, goal);
169 ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);
170 else if (samplerIndex==2)
172 ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);
176 ss.setPlanner(planner);
182 std::cout <<
"Found solution:" << std::endl;
184 ss.getSolutionPath().print(std::cout);
187 std::cout <<
"No solution found" << std::endl;
190 int main(
int,
char **)
192 std::cout <<
"Using default uniform sampler:" << std::endl;
194 std::cout <<
"\nUsing obstacle-based sampler:" << std::endl;
196 std::cout <<
"\nUsing my sampler:" << std::endl;