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/PathControl.h"
00038 #include "ompl/geometric/PathGeometric.h"
00039 #include "ompl/base/samplers/UniformValidStateSampler.h"
00040 #include "ompl/util/Exception.h"
00041 #include "ompl/util/Console.h"
00042 #include <numeric>
00043 #include <cmath>
00044
00045 ompl::control::PathControl::PathControl(const base::SpaceInformationPtr &si) : base::Path(si)
00046 {
00047 if (!dynamic_cast<const SpaceInformation*>(si_.get()))
00048 throw Exception("Cannot create a path with controls from a space that does not support controls");
00049 }
00050
00051 ompl::control::PathControl::PathControl(const PathControl &path) : base::Path(path.si_)
00052 {
00053 copyFrom(path);
00054 }
00055
00056 ompl::geometric::PathGeometric ompl::control::PathControl::asGeometric(void) const
00057 {
00058 PathControl pc(*this);
00059 pc.interpolate();
00060 geometric::PathGeometric pg(si_);
00061 pg.getStates().swap(pc.states_);
00062 return pg;
00063 }
00064
00065 ompl::control::PathControl& ompl::control::PathControl::operator=(const PathControl& other)
00066 {
00067 freeMemory();
00068 si_ = other.si_;
00069 copyFrom(other);
00070 return *this;
00071 }
00072
00073 void ompl::control::PathControl::copyFrom(const PathControl& other)
00074 {
00075 states_.resize(other.states_.size());
00076 controls_.resize(other.controls_.size());
00077
00078 for (unsigned int i = 0 ; i < states_.size() ; ++i)
00079 states_[i] = si_->cloneState(other.states_[i]);
00080
00081 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00082 for (unsigned int i = 0 ; i < controls_.size() ; ++i)
00083 controls_[i] = si->cloneControl(other.controls_[i]);
00084
00085 controlDurations_ = other.controlDurations_;
00086 }
00087
00088 double ompl::control::PathControl::length(void) const
00089 {
00090 return std::accumulate(controlDurations_.begin(), controlDurations_.end(), 0.0);
00091 }
00092
00093 void ompl::control::PathControl::print(std::ostream &out) const
00094 {
00095 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00096 double res = si->getPropagationStepSize();
00097 out << "Control path with " << states_.size() << " states" << std::endl;
00098 for (unsigned int i = 0 ; i < controls_.size() ; ++i)
00099 {
00100 out << "At state ";
00101 si_->printState(states_[i], out);
00102 out << " apply control ";
00103 si->printControl(controls_[i], out);
00104 out << " for " << (int)floor(0.5 + controlDurations_[i]/res) << " steps" << std::endl;
00105 }
00106 out << "Arrive at state ";
00107 si_->printState(states_[controls_.size()], out);
00108 out << std::endl;
00109 }
00110
00111 void ompl::control::PathControl::interpolate(void)
00112 {
00113 if (states_.size() <= controls_.size())
00114 {
00115 logError("Interpolation not performed. Number of states in the path should be strictly greater than the number of controls.");
00116 return;
00117 }
00118
00119 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00120 std::vector<base::State*> newStates;
00121 std::vector<Control*> newControls;
00122 std::vector<double> newControlDurations;
00123
00124 double res = si->getPropagationStepSize();
00125 for (unsigned int i = 0 ; i < controls_.size() ; ++i)
00126 {
00127 int steps = (int)floor(0.5 + controlDurations_[i] / res);
00128 assert(steps >= 0);
00129 if (steps <= 1)
00130 {
00131 newStates.push_back(states_[i]);
00132 newControls.push_back(controls_[i]);
00133 newControlDurations.push_back(controlDurations_[i]);
00134 continue;
00135 }
00136 std::vector<base::State*> istates;
00137 si->propagate(states_[i], controls_[i], steps, istates, true);
00138
00139 if (!istates.empty())
00140 {
00141 si_->freeState(istates.back());
00142 istates.pop_back();
00143 }
00144 newStates.push_back(states_[i]);
00145 newStates.insert(newStates.end(), istates.begin(), istates.end());
00146 newControls.push_back(controls_[i]);
00147 newControlDurations.push_back(res);
00148 for (int j = 1 ; j < steps; ++j)
00149 {
00150 newControls.push_back(si->cloneControl(controls_[i]));
00151 newControlDurations.push_back(res);
00152 }
00153 }
00154 newStates.push_back(states_[controls_.size()]);
00155 states_.swap(newStates);
00156 controls_.swap(newControls);
00157 controlDurations_.swap(newControlDurations);
00158 }
00159
00160 bool ompl::control::PathControl::check(void) const
00161 {
00162 if (controls_.empty())
00163 {
00164 if (states_.size() == 1)
00165 return si_->isValid(states_[0]);
00166 else
00167 return false;
00168 }
00169
00170 bool valid = true;
00171 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00172 double res = si->getPropagationStepSize();
00173 base::State *dummy = si_->allocState();
00174 for (unsigned int i = 0 ; valid && i < controls_.size() ; ++i)
00175 {
00176 unsigned int steps = (unsigned int)floor(0.5 + controlDurations_[i] / res);
00177 if (!si->isValid(states_[i]) || si->propagateWhileValid(states_[i], controls_[i], steps, dummy) != steps)
00178 valid = false;
00179 }
00180 si_->freeState(dummy);
00181
00182 return valid;
00183 }
00184
00185 void ompl::control::PathControl::append(const base::State *state)
00186 {
00187 states_.push_back(si_->cloneState(state));
00188 }
00189
00190 void ompl::control::PathControl::append(const base::State *state, const Control *control, double duration)
00191 {
00192 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00193 states_.push_back(si->cloneState(state));
00194 controls_.push_back(si->cloneControl(control));
00195 controlDurations_.push_back(duration);
00196 }
00197
00198 void ompl::control::PathControl::random(void)
00199 {
00200 freeMemory();
00201 states_.resize(2);
00202 controlDurations_.resize(1);
00203 controls_.resize(1);
00204
00205 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00206 states_[0] = si->allocState();
00207 states_[1] = si->allocState();
00208 controls_[0] = si->allocControl();
00209
00210 base::StateSamplerPtr ss = si->allocStateSampler();
00211 ss->sampleUniform(states_[0]);
00212 ControlSamplerPtr cs = si->allocControlSampler();
00213 cs->sample(controls_[0], states_[0]);
00214 unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
00215 controlDurations_[0] = steps * si->getPropagationStepSize();
00216 si->propagate(states_[0], controls_[0], steps, states_[1]);
00217 }
00218
00219 bool ompl::control::PathControl::randomValid(unsigned int attempts)
00220 {
00221 freeMemory();
00222 states_.resize(2);
00223 controlDurations_.resize(1);
00224 controls_.resize(1);
00225
00226 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00227 states_[0] = si->allocState();
00228 states_[1] = si->allocState();
00229 controls_[0] = si->allocControl();
00230
00231 ControlSamplerPtr cs = si->allocControlSampler();
00232 base::UniformValidStateSampler *uvss = new base::UniformValidStateSampler(si);
00233 uvss->setNrAttempts(attempts);
00234 bool ok = false;
00235 for (unsigned int i = 0 ; i < attempts ; ++i)
00236 if (uvss->sample(states_[0]))
00237 {
00238 cs->sample(controls_[0], states_[0]);
00239 unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
00240 controlDurations_[0] = steps * si->getPropagationStepSize();
00241 if (si->propagateWhileValid(states_[0], controls_[0], steps, states_[1]) == steps)
00242 {
00243 ok = true;
00244 break;
00245 }
00246 }
00247 delete uvss;
00248
00249 if (!ok)
00250 {
00251 freeMemory();
00252 states_.clear();
00253 controls_.clear();
00254 controlDurations_.clear();
00255 }
00256 return ok;
00257 }
00258
00259 void ompl::control::PathControl::freeMemory(void)
00260 {
00261 for (unsigned int i = 0 ; i < states_.size() ; ++i)
00262 si_->freeState(states_[i]);
00263 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00264 for (unsigned int i = 0 ; i < controls_.size() ; ++i)
00265 si->freeControl(controls_[i]);
00266 }