There are two different kinds of samplers that sound similar, but have different roles: state space samplers (ompl::base::StateSampler) and valid state samplers (ompl::base::ValidStateSampler). For each type of state space there needs to exist a corresponding derived ompl::base::StateSampler class that allows one to generate uniform samples from that state space, generate states near another state from that state space and generate states using a Gaussian distribution. The valid state samplers use the state space samplers as a low level primitive. Typically, they generate a number of state samples using the appropriat state space sampler until a valid state is found or a maximum number of iterations is exceeded. The validity of a state is determined through the ompl::base::SpaceInformation::isValid method. There are three pre-defined derived ompl::base::ValidStateSampler classes:
Below we will describe how you can specify a planner to use one of these samplers and how to write your own valid state sampler. The code examples are taken from the StateSampling.cpp demo program (note that there is also a Python version of his demo).
We cannot set the type of sampler directly in the SimpleSetup or SpaceInformation classes, because each thread needs it own copy of a sampler. Instead, we need to define a ompl::base::ValidStateSamplerAllocator, a function that, given a pointer to an ompl::base::SpaceInformation, returns ompl::base::ValidStateSamplerPtr. This function can also configure the valid state sampler according to the specific space information before returning it. The following simple example shows how to use the ObstacleBasedValidStateSampler:
namespace ob = ompl::base; namespace og = ompl::geometric; ob::ValidStateSamplerPtr allocOBValidStateSampler(const ob::SpaceInformation *si) { // we can perform any additional setup / configuration of a sampler here, // but there is nothing to tweak in case of the ObstacleBasedValidStateSampler. return ob::ValidStateSamplerPtr(new ob::ObstacleBasedValidStateSampler(si)); } void plan(int samplerIndex) { // construct the state space we are planning in ob::StateSpacePtr space(new ob::RealVectorStateSpace(3)); og::SimpleSetup ss(space); // set sampler (optional; the default is uniform sampling) if (samplerIndex==1) // use obstacle-based sampling ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);
A wide variety of heuristics have been proposed to improve the sampling of states. The quality of a sample can be characterized, e.g., by its distance to the nearest obstacle or by the “visibility” from a state. There are also two common cases where problem-specific information can be exploited:
In the code below we are planning for a 3D point moving around inside a cube centered at the origin. There is one rectangular obstacle. Since the valid region is easy to describe, we can sample directly from the free space.
namespace ob = ompl::base; namespace og = ompl::geometric; // This is a problem-specific sampler that automatically generates valid // states; it doesn't need to call SpaceInformation::isValid. This is an // example of constrained sampling. If you can explicitly describe the set valid // states and can draw samples from it, then this is typically much more // efficient than generating random samples from the entire state space and // checking for validity. class MyValidStateSampler : public ob::ValidStateSampler { public: MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si) { name_ = "my sampler"; } // Generate a sample in the valid part of the R^3 state space // Valid states satisfy the following constraints: // -1<= x,y,z <=1 // if .25 <= z <= .5, then |x|>.8 and |y|>.8 virtual bool sample(ob::State *state) { double* val = static_cast<ob::RealVectorStateSpace::StateType*>(state)->values; double z = rng_.uniformReal(-1,1); if (z>.25 && z<.5) { double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2); switch(rng_.uniformInt(0,3)) { case 0: val[0]=x-1; val[1]=y-1; case 1: val[0]=x-.8; val[1]=y+.8; case 2: val[0]=y-1; val[1]=x-1; case 3: val[0]=y+.8; val[1]=x-.8; } } else { val[0] = rng_.uniformReal(-1,1); val[1] = rng_.uniformReal(-1,1); } val[2] = z; assert(si_->isValid(state)); return true; } // We don't need this in the example below. virtual bool sampleNear(ob::State *state, const ob::State *near, const double distance) { throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented"); return false; } protected: ompl::RNG rng_; };
ob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si) { return ob::ValidStateSamplerPtr(new MyValidStateSampler(si)); }
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);