//The direct ellipsoid sampling class for path-length:
PathLengthDirectInfSampler::PathLengthDirectInfSampler(const StateSpace* space, const ProblemDefinitionPtr probDefn, const Cost* bestCost)
: InformedStateSampler(space, probDefn, bestCost),
informedIdx_(0u),
uninformedIdx_(0u)
{
//Variables
//The foci of the ellipse as State* s
State* startFocusState;
State* goalFocusState;
//The foci of the ellipse as std::vectors
std::vector<double> startFocusVector;
std::vector<double> goalFocusVector;
//Sanity check the problem.
if (probDefn_->getStartStateCount() != 1u)
{
throw Exception("The direct path-length informed sampler currently only supports 1 start state.");
}
if (probDefn_->getGoal()->hasType(GOAL_STATE) == false)
{
throw Exception("The direct path-length informed sampler currently only supports goals that can be cast to goal states.");
}
//Check that the provided statespace is compatible and extract the necessary indices.
//The statespace must either be R^n or SE(2) or SE(3)
if (StateSampler::space_->isCompound() == false)
{
if ( std::strncmp(StateSampler::space_->getName().c_str(), "RealVector", std::strlen("RealVector")) == 0 )
{
informedIdx_ = 0u;
uninformedIdx_ = 0u;
}
else
{
throw Exception("PathLengthDirectInfSampler only supports RealVector, SE2 and SE3 StateSpaces.");
}
}
else if (StateSampler::space_->isCompound() == true)
{
//Check that it is SE2 or SE3
if ( std::strncmp(StateSampler::space_->getName().c_str(), "SE2", std::strlen("SE2")) == 0 || std::strncmp(StateSampler::space_->getName().c_str(), "SE3", std::strlen("SE3")) == 0 )
{
//Variable:
//An ease of use upcasted pointer to the space as a compound space
const CompoundStateSpace* compoundSpace;
//Get the space as a compound space
compoundSpace = StateSampler::space_->as<CompoundStateSpace>();
//Sanity check
if (compoundSpace->getSubspaceCount() != 2u)
{
//Pout
throw Exception("The provided compound StateSpace is SE(2) or SE(3) but does not have exactly 2 subspaces.");
}
//Iterate over the state spaces, finding the real vector and so components.
for (unsigned int idx = 0u; idx < StateSampler::space_->as<CompoundStateSpace>()->getSubspaceCount(); ++idx)
{
//Check if the space is realvectored, so2 or so3
if ( std::strncmp(compoundSpace->getSubspace(idx)->getName().c_str(), "RealVector", std::strlen("RealVector")) == 0 )
{
informedIdx_ = idx;
}
else if ( std::strncmp(compoundSpace->getSubspace(idx)->getName().c_str(), "SO2", std::strlen("SO2")) == 0 )
{
uninformedIdx_ = idx;
}
else if ( std::strncmp(compoundSpace->getSubspace(idx)->getName().c_str(), "SO3", std::strlen("SO3")) == 0 )
{
uninformedIdx_ = idx;
}
else
{
//Pout
throw Exception("The provided compound StateSpace is SE(2) or SE(3) but contains a subspace that is not R^2, R^3, SO(2), or SO(3).");
}
}
}
else
{
throw Exception("PathLengthDirectInfSampler only supports RealVector, SE2 and SE3 statespaces.");
}
}
//Create a sampler for the whole space that we can use if we have no information
baseSampler_ = StateSampler::space_->allocDefaultStateSampler();
//Set it's seed to the same as mine
baseSampler_->setLocalSeed( this->getLocalSeed() );
//Check if the space is compound
if (StateSampler::space_->isCompound() == false)
{
//It is not.
//Store the foci
startFocusState = probDefn_->getStartState(0u);
goalFocusState = probDefn_->getGoal()->as<GoalState>()->getState();
//The informed subspace is the full space
informedSubSpace_ = StateSampler::space_;
//And the uniformed subspace and its associated sampler are null
uninformedSubSpace_ = NULL;
uninformedSubSampler_ = StateSamplerPtr();
}
else
{
//Store the foci
startFocusState = probDefn_->getStartState(0u)->as<CompoundState>()->components[informedIdx_];
goalFocusState = probDefn_->getGoal()->as<GoalState>()->getState()->as<CompoundState>()->components[informedIdx_];
//The informed subset is the real vector space. StateSampler::space_ is a raw pointer, so for this variable to be able to hold all of space_, we need to store the raw pointer to the subspace...
informedSubSpace_ = StateSampler::space_->as<CompoundStateSpace>()->getSubspace(informedIdx_).get();
//And the uninformed subspace is the remainder. Raw pointer for consistency with the above.
uninformedSubSpace_ = StateSampler::space_->as<CompoundStateSpace>()->getSubspace(uninformedIdx_).get();
//Create a sampler for the uniformed subset:
uninformedSubSampler_ = uninformedSubSpace_->allocDefaultStateSampler();
//Set it's seed to the same as mine
uninformedSubSampler_->setLocalSeed( this->getLocalSeed() );
}
//Now extract the foci of the ellipse
informedSubSpace_->copyToReals(startFocusVector, startFocusState);
informedSubSpace_->copyToReals(goalFocusVector, goalFocusState);
//Create the definition of the PHS
phsPtr_ = boost::make_shared<ProlateHyperspheroid>(informedSubSpace_->getDimension(), &startFocusVector[0], &goalFocusVector[0]);
}
ompl::base::StateSamplerPtr ompl::base::TimeStateSpace::allocDefaultStateSampler(void) const
{
return StateSamplerPtr(new TimeStateSampler(this));
}
ompl::base::StateSamplerPtr ompl::base::DiscreteStateSpace::allocDefaultStateSampler() const
{
return StateSamplerPtr(new DiscreteStateSampler(this));
}
