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