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/ControlSampler.h"
00038 #include "ompl/control/ControlSpace.h"
00039
00040 void ompl::control::ControlSampler::sample(Control *control, const base::State * )
00041 {
00042 sample(control);
00043 }
00044
00045 void ompl::control::ControlSampler::sampleNext(Control *control, const Control * )
00046 {
00047 sample(control);
00048 }
00049
00050 void ompl::control::ControlSampler::sampleNext(Control *control, const Control * , const base::State * )
00051 {
00052 sample(control);
00053 }
00054
00055 void ompl::control::ControlSampler::sampleTo(Control *control, const base::State *source, const base::State *target)
00056 {
00057 sample(control);
00058 }
00059
00060 void ompl::control::ControlSampler::sampleTo(Control *control, const Control * ,
00061 const base::State * , const base::State * )
00062 {
00063 sample(control);
00064 }
00065
00066 unsigned int ompl::control::ControlSampler::sampleTo(Control *control, unsigned int minSteps, unsigned int maxSteps,
00067 const base::State * , const base::State * )
00068 {
00069 sample(control);
00070 return sampleStepCount(minSteps, maxSteps);
00071 }
00072
00073 unsigned int ompl::control::ControlSampler::sampleTo(Control *control, unsigned int minSteps, unsigned int maxSteps,
00074 const Control * , const base::State * , const base::State * )
00075 {
00076 sample(control);
00077 return sampleStepCount(minSteps, maxSteps);
00078 }
00079
00080 unsigned int ompl::control::ControlSampler::sampleStepCount(unsigned int minSteps, unsigned int maxSteps)
00081 {
00082 return rng_.uniformInt(minSteps, maxSteps);
00083 }
00084
00085 void ompl::control::CompoundControlSampler::addSampler(const ControlSamplerPtr &sampler)
00086 {
00087 samplers_.push_back(sampler);
00088 samplerCount_ = samplers_.size();
00089 }
00090
00091 void ompl::control::CompoundControlSampler::sample(Control *control)
00092 {
00093 Control **comps = static_cast<CompoundControl*>(control)->components;
00094 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00095 samplers_[i]->sample(comps[i]);
00096 }
00097
00098 void ompl::control::CompoundControlSampler::sample(Control *control, const base::State *state)
00099 {
00100 Control **comps = static_cast<CompoundControl*>(control)->components;
00101 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00102 samplers_[i]->sample(comps[i], state);
00103 }
00104
00105 void ompl::control::CompoundControlSampler::sampleNext(Control *control, const Control *previous)
00106 {
00107 Control **comps = static_cast<CompoundControl*>(control)->components;
00108 const Control * const *prev = static_cast<const CompoundControl*>(previous)->components;
00109 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00110 samplers_[i]->sampleNext(comps[i], prev[i]);
00111 }
00112
00113 void ompl::control::CompoundControlSampler::sampleNext(Control *control, const Control *previous, const base::State *state)
00114 {
00115 Control **comps = static_cast<CompoundControl*>(control)->components;
00116 const Control * const *prev = static_cast<const CompoundControl*>(previous)->components;
00117 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00118 samplers_[i]->sampleNext(comps[i], prev[i], state);
00119 }
00120
00121 void ompl::control::CompoundControlSampler::sampleTo(Control *control, const base::State *source, const base::State *target)
00122 {
00123 Control **comps = static_cast<CompoundControl*>(control)->components;
00124 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00125 samplers_[i]->sampleTo(comps[i], source, target);
00126 }
00127
00128 void ompl::control::CompoundControlSampler::sampleTo(Control *control, const Control *previous,
00129 const base::State *source, const base::State *target)
00130 {
00131 Control **comps = static_cast<CompoundControl*>(control)->components;
00132 const Control * const *prev = static_cast<const CompoundControl*>(previous)->components;
00133 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00134 samplers_[i]->sampleTo(comps[i], prev[i], source, target);
00135 }
00136
00137 unsigned int ompl::control::CompoundControlSampler::sampleTo(Control *control, unsigned int minSteps, unsigned int maxSteps,
00138 const base::State *source, const base::State *target)
00139 {
00140
00141 Control **comps = static_cast<CompoundControl*>(control)->components;
00142 unsigned int sc = 0;
00143 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00144 {
00145 unsigned int c = samplers_[i]->sampleTo(comps[i], minSteps, maxSteps, source, target);
00146 if (i == 0 || sc > c)
00147 sc = c;
00148 }
00149 return sc;
00150 }
00151
00152 unsigned int ompl::control::CompoundControlSampler::sampleTo(Control *control, unsigned int minSteps, unsigned int maxSteps,
00153 const Control *previous, const base::State *source, const base::State *target)
00154 {
00155
00156 Control **comps = static_cast<CompoundControl*>(control)->components;
00157 const Control * const *prev = static_cast<const CompoundControl*>(previous)->components;
00158 unsigned int sc = 0;
00159 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00160 {
00161 unsigned int c = samplers_[i]->sampleTo(comps[i], minSteps, maxSteps, prev[i], source, target);
00162 if (i == 0 || sc > c)
00163 sc = c;
00164 }
00165 return sc;
00166 }