virtual void sampleFullState(const ompl::base::StateSamplerPtr &sampler,
const std::vector<double>& coord, ompl::base::State *s) const
{
double* pos = space_->getValueAddressAtLocation(s, position_);
sampler->sampleUniform(s);
pos[0] = coord[0];
pos[1] = coord[1];
}
virtual void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const
{
sampler->sampleUniform(s);
ob::SE2StateSpace::StateType* ws = s->as<ob::SE2StateSpace::StateType>();
ws->setXY(coord[0], coord[1]);
}
void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const override
{
sampler->sampleUniform(s);
s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
}