00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/control/SimpleDirectedControlSampler.h"
00038 #include "ompl/control/SpaceInformation.h"
00039
00040 ompl::control::SimpleDirectedControlSampler::SimpleDirectedControlSampler(const SpaceInformation *si, unsigned int k) :
00041 DirectedControlSampler(si), cs_(si->allocControlSampler()), numControlSamples_(k)
00042 {
00043 }
00044
00045 ompl::control::SimpleDirectedControlSampler::~SimpleDirectedControlSampler(void)
00046 {
00047 }
00048
00049 unsigned int ompl::control::SimpleDirectedControlSampler::sampleTo(Control *control, const base::State *source, const base::State *target)
00050 {
00051 return getBestControl(control, source, target, NULL);
00052 }
00053
00054 unsigned int ompl::control::SimpleDirectedControlSampler::sampleTo(Control *control, const Control *previous, const base::State *source, const base::State *target)
00055 {
00056 return getBestControl(control, source, target, previous);
00057 }
00058
00059 unsigned int ompl::control::SimpleDirectedControlSampler::getBestControl (Control *control, const base::State *source, const base::State *target, const Control *previous)
00060 {
00061
00062 if (previous)
00063 cs_->sampleNext(control, previous, source);
00064 else
00065 cs_->sample(control, source);
00066
00067 const double minDuration = si_->getMinControlDuration();
00068 const double maxDuration = si_->getMaxControlDuration();
00069
00070 unsigned int steps = cs_->sampleStepCount(minDuration, maxDuration);
00071
00072 if (numControlSamples_ > 1)
00073 {
00074 Control *tempControl = si_->allocControl();
00075 base::State *tempState = si_->allocState();
00076
00077
00078 si_->propagateWhileValid(source, control, steps, tempState);
00079 double bestDistance = si_->distance(tempState, target);
00080
00081
00082 for (unsigned int i = 1; i < numControlSamples_; ++i)
00083 {
00084 unsigned int sampleSteps = cs_->sampleStepCount(minDuration, maxDuration);
00085 if (previous)
00086 cs_->sampleNext(tempControl, previous, source);
00087 else
00088 cs_->sample(tempControl, source);
00089
00090 si_->propagateWhileValid(source, tempControl, sampleSteps, tempState);
00091 double tempDistance = si_->distance(tempState, target);
00092 if (tempDistance < bestDistance)
00093 {
00094 si_->copyControl(control, tempControl);
00095 bestDistance = tempDistance;
00096 steps = sampleSteps;
00097 }
00098 }
00099
00100 si_->freeControl(tempControl);
00101 si_->freeState(tempState);
00102 }
00103 return steps;
00104 }