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/SpaceInformation.h"
00038 #include "ompl/util/Exception.h"
00039 #include <cassert>
00040 #include <utility>
00041 #include <limits>
00042
00043 void ompl::control::SpaceInformation::setup(void)
00044 {
00045 base::SpaceInformation::setup();
00046 if (minSteps_ > maxSteps_)
00047 throw Exception("The minimum number of steps cannot be larger than the maximum number of steps");
00048 if (minSteps_ == 0 && maxSteps_ == 0)
00049 {
00050 minSteps_ = 1;
00051 maxSteps_ = 10;
00052 msg_.warn("Assuming propagation will always have between %d and %d steps", minSteps_, maxSteps_);
00053 }
00054 if (minSteps_ < 1)
00055 throw Exception("The minimum number of steps must be at least 1");
00056
00057 if (stepSize_ < std::numeric_limits<double>::epsilon())
00058 {
00059 stepSize_ = getStateValidityCheckingResolution() * getMaximumExtent();
00060 if (stepSize_ < std::numeric_limits<double>::epsilon())
00061 throw Exception("The propagation step size must be larger than 0");
00062 msg_.warn("The propagation step size is assumed to be %f", stepSize_);
00063 }
00064
00065 controlManifold_->setup();
00066 if (controlManifold_->getDimension() <= 0)
00067 throw Exception("The dimension of the control manifold we plan in must be > 0");
00068 }
00069
00070 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control* control, unsigned int steps, base::State *result) const
00071 {
00072 if (steps == 0)
00073 {
00074 if (result != state)
00075 copyState(result, state);
00076 }
00077 else
00078 {
00079 controlManifold_->propagate(state, control, stepSize_, result);
00080 for (unsigned int i = 1 ; i < steps ; ++i)
00081 controlManifold_->propagate(result, control, stepSize_, result);
00082 }
00083 }
00084
00085 unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control* control, unsigned int steps, base::State *result) const
00086 {
00087 if (steps == 0)
00088 {
00089 if (result != state)
00090 copyState(result, state);
00091 return 0;
00092 }
00093
00094
00095 controlManifold_->propagate(state, control, stepSize_, result);
00096
00097
00098 if (isValid(result))
00099 {
00100 base::State *temp1 = result;
00101 base::State *temp2 = allocState();
00102 base::State *toDelete = temp2;
00103 unsigned int r = steps;
00104
00105
00106 for (unsigned int i = 1 ; i < steps ; ++i)
00107 {
00108 controlManifold_->propagate(temp1, control, stepSize_, temp2);
00109 if (isValid(temp2))
00110 std::swap(temp1, temp2);
00111 else
00112 {
00113
00114 r = i;
00115 break;
00116 }
00117 }
00118
00119
00120
00121 if (result != temp1)
00122 copyState(result, temp1);
00123
00124
00125 freeState(toDelete);
00126
00127 return r;
00128 }
00129
00130
00131 else
00132 {
00133 if (result != state)
00134 copyState(result, state);
00135 return 0;
00136 }
00137 }
00138
00139 void ompl::control::SpaceInformation::propagate(const base::State *state, const Control* control, unsigned int steps, std::vector<base::State*> &result, bool alloc) const
00140 {
00141 if (alloc)
00142 {
00143 result.resize(steps);
00144 for (unsigned int i = 0 ; i < result.size() ; ++i)
00145 result[i] = allocState();
00146 }
00147 else
00148 {
00149 if (result.empty())
00150 return;
00151 steps = std::min(steps, (unsigned int)result.size());
00152 }
00153
00154 unsigned int st = 0;
00155
00156 if (st < steps)
00157 {
00158 controlManifold_->propagate(state, control, stepSize_, result[st]);
00159 ++st;
00160
00161 while (st < steps)
00162 {
00163 controlManifold_->propagate(result[st-1], control, stepSize_, result[st]);
00164 ++st;
00165 }
00166 }
00167 }
00168
00169 unsigned int ompl::control::SpaceInformation::propagateWhileValid(const base::State *state, const Control* control, unsigned int steps, std::vector<base::State*> &result, bool alloc) const
00170 {
00171 if (alloc)
00172 result.resize(steps);
00173 else
00174 {
00175 if (result.empty())
00176 return 0;
00177 steps = std::min(steps, (unsigned int)result.size());
00178 }
00179
00180 unsigned int st = 0;
00181
00182 if (st < steps)
00183 {
00184 if (alloc)
00185 result[st] = allocState();
00186 controlManifold_->propagate(state, control, stepSize_, result[st]);
00187
00188 if (isValid(result[st]))
00189 {
00190 ++st;
00191 while (st < steps)
00192 {
00193 if (alloc)
00194 result[st] = allocState();
00195 controlManifold_->propagate(result[st-1], control, stepSize_, result[st]);
00196
00197 if (!isValid(result[st]))
00198 {
00199 if (alloc)
00200 {
00201 freeState(result[st]);
00202 result.resize(st);
00203 }
00204 break;
00205 }
00206 else
00207 ++st;
00208 }
00209 }
00210 else
00211 {
00212 if (alloc)
00213 {
00214 freeState(result[st]);
00215 result.resize(st);
00216 }
00217 }
00218 }
00219
00220 return st;
00221 }
00222
00223 void ompl::control::SpaceInformation::printSettings(std::ostream &out) const
00224 {
00225 base::SpaceInformation::printSettings(out);
00226 out << " - control manifold:" << std::endl;
00227 controlManifold_->printSettings(out);
00228 out << " - propagation step size: " << stepSize_ << std::endl;
00229 out << " - propagation duration: [" << minSteps_ << ", " << maxSteps_ << "]" << std::endl;
00230 }