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;
A boost shared pointer wrapper for ompl::base::ValidStateSampler.
Definition of a scoped state.
A boost shared pointer wrapper for ompl::base::StateSpace.
State StateType
Define the type of state allocated by this space.
Generate valid samples using obstacle based sampling. First sample an invalid state, then sample a valid state. Then, interpolate from the invalid state to the valid state, returning the first valid state encountered.
const T * as(void) const
Cast this instance to a desired type.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Create the set of classes typically needed to solve a geometric problem.
A boost shared pointer wrapper for ompl::base::Planner.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Abstract definition of a state sampler.
A class to store the exit status of Planner::solve()
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
The exception type for ompl.
virtual bool sample(State *state)=0
Sample a state. Return false in case of failure.
The lower and upper bounds for an Rn space.
Probabilistic RoadMap planner.
virtual bool sampleNear(State *state, const State *near, const double distance)=0
Sample a state near another, within specified distance. Return false, in case of failure.