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; }