37 #include "ompl/control/SimpleDirectedControlSampler.h"
38 #include "ompl/control/SpaceInformation.h"
45 ompl::control::SimpleDirectedControlSampler::~SimpleDirectedControlSampler(
void)
51 return getBestControl(control, source, dest, NULL);
56 return getBestControl(control, source, dest, previous);
63 cs_->sampleNext(control, previous, source);
65 cs_->sample(control, source);
67 const unsigned int minDuration = si_->getMinControlDuration();
68 const unsigned int maxDuration = si_->getMaxControlDuration();
70 unsigned int steps = cs_->sampleStepCount(minDuration, maxDuration);
73 steps = si_->propagateWhileValid(source, control, steps, bestState);
75 if (numControlSamples_ > 1)
77 Control *tempControl = si_->allocControl();
79 double bestDistance = si_->distance(bestState, dest);
82 for (
unsigned int i = 1; i < numControlSamples_; ++i)
84 unsigned int sampleSteps = cs_->sampleStepCount(minDuration, maxDuration);
86 cs_->sampleNext(tempControl, previous, source);
88 cs_->sample(tempControl, source);
90 sampleSteps = si_->propagateWhileValid(source, tempControl, sampleSteps, tempState);
91 double tempDistance = si_->distance(tempState, dest);
92 if (tempDistance < bestDistance)
94 si_->copyState(bestState, tempState);
95 si_->copyControl(control, tempControl);
96 bestDistance = tempDistance;
101 si_->freeState(tempState);
102 si_->freeControl(tempControl);
105 si_->copyState(dest, bestState);
106 si_->freeState(bestState);
Definition of an abstract control.
virtual unsigned int sampleTo(Control *control, const base::State *source, base::State *dest)
Sample a control given that it will be applied to state state and the intention is to reach state des...
virtual unsigned int getBestControl(Control *control, const base::State *source, base::State *dest, const Control *previous)
Samples numControlSamples_ controls, and returns the control that brings the system the closest to ta...
SimpleDirectedControlSampler(const SpaceInformation *si, unsigned int k=1)
Constructor takes the state space to construct samples for as argument Optionally, a k value can be given to indicate the number of controls to try when directing a system toward a specific state. Default value is 1.
Abstract definition of a directed control sampler. Motion planners that need to sample controls that ...
Definition of an abstract state.