unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control *control,
int steps, std::vector<base::State *> &result,
bool alloc) const
{
double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
steps = abs(steps);
if (alloc)
result.resize(steps);
else
{
if (result.empty())
return 0;
steps = std::min(steps, (int)result.size());
}
int st = 0;
if (st < steps)
{
if (alloc)
result[st] = allocState();
statePropagator_->propagate(state, control, signedStepSize, result[st]);
if (isValid(result[st]))
{
++st;
while (st < steps)
{
if (alloc)
result[st] = allocState();
statePropagator_->propagate(result[st - 1], control, signedStepSize, result[st]);
if (!isValid(result[st]))
{
if (alloc)
{
freeState(result[st]);
result.resize(st);
}
break;
}
else
++st;
}
}
else
{
if (alloc)
{
freeState(result[st]);
result.resize(st);
}
}
}
return st;
}
unsigned int ompl::base::SpaceInformation::randomBounceMotion(const StateSamplerPtr &sss, const State *start,
unsigned int steps, std::vector<State *> &states,
bool alloc) const
{
if (alloc)
{
states.resize(steps);
for (unsigned int i = 0; i < steps; ++i)
states[i] = allocState();
}
else if (states.size() < steps)
steps = states.size();
const State *prev = start;
std::pair<State *, double> lastValid;
unsigned int j = 0;
for (unsigned int i = 0; i < steps; ++i)
{
sss->sampleUniform(states[j]);
lastValid.first = states[j];
if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon())
prev = states[j++];
}
return j;
}
void ompl::control::SpaceInformation::propagate(const base::State *state, const Control *control, int steps,
std::vector<base::State *> &result, bool alloc) const
{
double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
steps = abs(steps);
if (alloc)
{
result.resize(steps);
for (auto &i : result)
i = allocState();
}
else
{
if (result.empty())
return;
steps = std::min(steps, (int)result.size());
}
int st = 0;
if (st < steps)
{
statePropagator_->propagate(state, control, signedStepSize, result[st]);
++st;
while (st < steps)
{
statePropagator_->propagate(result[st - 1], control, signedStepSize, result[st]);
++st;
}
}
}
trajectory::InterpolatedPtr planOMPL(
const statespace::StateSpace::State* _start,
const statespace::StateSpace::State* _goal,
statespace::ConstStateSpacePtr _stateSpace,
statespace::InterpolatorPtr _interpolator,
distance::DistanceMetricPtr _dmetric,
constraint::SampleablePtr _sampler,
constraint::TestablePtr _validityConstraint,
constraint::TestablePtr _boundsConstraint,
constraint::ProjectablePtr _boundsProjector,
double _maxPlanTime,
double _maxDistanceBtwValidityChecks)
{
// Create a SpaceInformation. This function will ensure state space matching
auto si = getSpaceInformation(
_stateSpace,
_interpolator,
std::move(_dmetric),
std::move(_sampler),
std::move(_validityConstraint),
std::move(_boundsConstraint),
std::move(_boundsProjector),
_maxDistanceBtwValidityChecks);
// Start and states
auto pdef = ompl_make_shared<::ompl::base::ProblemDefinition>(si);
auto sspace
= ompl_static_pointer_cast<GeometricStateSpace>(si->getStateSpace());
auto start = sspace->allocState(_start);
auto goal = sspace->allocState(_goal);
// ProblemDefinition clones states and keeps them internally
pdef->setStartAndGoalStates(start, goal);
sspace->freeState(start);
sspace->freeState(goal);
auto planner = ompl_make_shared<PlannerType>(si);
return planOMPL(
planner,
pdef,
std::move(_stateSpace),
std::move(_interpolator),
_maxPlanTime);
}
unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control *control,
int steps, base::State *result) const
{
if (steps == 0)
{
if (result != state)
copyState(result, state);
return 0;
}
double signedStepSize = steps > 0 ? stepSize_ : -stepSize_;
steps = abs(steps);
// perform the first step of propagation
statePropagator_->propagate(state, control, signedStepSize, result);
// if we found a valid state after one step, we can go on
if (isValid(result))
{
base::State *temp1 = result;
base::State *temp2 = allocState();
base::State *toDelete = temp2;
unsigned int r = steps;
// for the remaining number of steps
for (int i = 1; i < steps; ++i)
{
statePropagator_->propagate(temp1, control, signedStepSize, temp2);
if (isValid(temp2))
std::swap(temp1, temp2);
else
{
// the last valid state is temp1;
r = i;
break;
}
}
// if we finished the for-loop without finding an invalid state, the last valid state is temp1
// make sure result contains that information
if (result != temp1)
copyState(result, temp1);
// free the temporary memory
freeState(toDelete);
return r;
}
// if the first propagation step produced an invalid step, return 0 steps
// the last valid state is the starting one (assumed to be valid)
else
{
if (result != state)
copyState(result, state);
return 0;
}
}
double ompl::base::SpaceInformation::averageValidMotionLength(unsigned int attempts) const
{
// take the square root here because we in fact have a nested for loop
// where each loop executes #attempts steps (the sample() function of the UniformValidStateSampler if a for loop
// too)
attempts = std::max((unsigned int)floor(sqrt((double)attempts) + 0.5), 2u);
StateSamplerPtr ss = allocStateSampler();
auto uvss(std::make_shared<UniformValidStateSampler>(this));
uvss->setNrAttempts(attempts);
State *s1 = allocState();
State *s2 = allocState();
std::pair<State *, double> lastValid;
lastValid.first = nullptr;
double d = 0.0;
unsigned int count = 0;
for (unsigned int i = 0; i < attempts; ++i)
if (uvss->sample(s1))
{
++count;
ss->sampleUniform(s2);
if (checkMotion(s1, s2, lastValid))
d += distance(s1, s2);
else
d += distance(s1, s2) * lastValid.second;
}
freeState(s2);
freeState(s1);
if (count > 0)
return d / (double)count;
else
return 0.0;
}
double ompl::base::SpaceInformation::probabilityOfValidState(unsigned int attempts) const
{
if (attempts == 0)
return 0.0;
unsigned int valid = 0;
unsigned int invalid = 0;
StateSamplerPtr ss = allocStateSampler();
State *s = allocState();
for (unsigned int i = 0; i < attempts; ++i)
{
ss->sampleUniform(s);
if (isValid(s))
++valid;
else
++invalid;
}
freeState(s);
return (double)valid / (double)(valid + invalid);
}
trajectory::InterpolatedPtr planOMPL(
const statespace::StateSpace::State* _start,
constraint::TestablePtr _goalTestable,
constraint::SampleablePtr _goalSampler,
statespace::ConstStateSpacePtr _stateSpace,
statespace::InterpolatorPtr _interpolator,
distance::DistanceMetricPtr _dmetric,
constraint::SampleablePtr _sampler,
constraint::TestablePtr _validityConstraint,
constraint::TestablePtr _boundsConstraint,
constraint::ProjectablePtr _boundsProjector,
double _maxPlanTime,
double _maxDistanceBtwValidityChecks)
{
if (_goalTestable == nullptr)
{
throw std::invalid_argument("Testable goal is nullptr.");
}
if (_goalSampler == nullptr)
{
throw std::invalid_argument("Sampleable goal is nullptr.");
}
if (_goalTestable->getStateSpace() != _stateSpace)
{
throw std::invalid_argument("Testable goal does not match StateSpace");
}
if (_goalSampler->getStateSpace() != _stateSpace)
{
throw std::invalid_argument("Sampleable goal does not match StateSpace");
}
auto si = getSpaceInformation(
_stateSpace,
_interpolator,
std::move(_dmetric),
std::move(_sampler),
std::move(_validityConstraint),
std::move(_boundsConstraint),
std::move(_boundsProjector),
_maxDistanceBtwValidityChecks);
// Set the start and goal
auto pdef = ompl_make_shared<::ompl::base::ProblemDefinition>(si);
auto sspace
= ompl_static_pointer_cast<GeometricStateSpace>(si->getStateSpace());
auto start = sspace->allocState(_start);
pdef->addStartState(start); // copies
sspace->freeState(start);
auto goalRegion = ompl_make_shared<GoalRegion>(
si, std::move(_goalTestable), _goalSampler->createSampleGenerator());
pdef->setGoal(goalRegion);
auto planner = ompl_make_shared<PlannerType>(si);
return planOMPL(
planner,
pdef,
std::move(_stateSpace),
std::move(_interpolator),
_maxPlanTime);
}
unsigned int ompl::base::SpaceInformation::getMotionStates(const State *s1, const State *s2,
std::vector<State *> &states, unsigned int count,
bool endpoints, bool alloc) const
{
// add 1 to the number of states we want to add between s1 & s2. This gives us the number of segments to split the
// motion into
count++;
if (count < 2)
{
unsigned int added = 0;
// if they want endpoints, then at most endpoints are included
if (endpoints)
{
if (alloc)
{
states.resize(2);
states[0] = allocState();
states[1] = allocState();
}
if (states.size() > 0)
{
copyState(states[0], s1);
added++;
}
if (states.size() > 1)
{
copyState(states[1], s2);
added++;
}
}
else if (alloc)
states.resize(0);
return added;
}
if (alloc)
{
states.resize(count + (endpoints ? 1 : -1));
if (endpoints)
states[0] = allocState();
}
unsigned int added = 0;
if (endpoints && states.size() > 0)
{
copyState(states[0], s1);
added++;
}
/* find the states in between */
for (unsigned int j = 1; j < count && added < states.size(); ++j)
{
if (alloc)
states[added] = allocState();
stateSpace_->interpolate(s1, s2, (double)j / (double)count, states[added]);
added++;
}
if (added < states.size() && endpoints)
{
if (alloc)
states[added] = allocState();
copyState(states[added], s2);
added++;
}
return added;
}
