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.states.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 bool valid = true;
00156 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00157 double res = si->getPropagationStepSize();
00158 base::State *dummy = si_->allocState();
00159 for (unsigned int i = 0 ; i < controls.size() ; ++i)
00160 {
00161 unsigned int steps = (unsigned int)floor(0.5 + controlDurations[i] / res);
00162 if (si->propagateWhileValid(states[i], controls[i], steps, dummy) != steps)
00163 {
00164 valid = false;
00165 break;
00166 }
00167 }
00168 si_->freeState(dummy);
00169
00170 if (valid)
00171 for (unsigned int j = 0 ; j < states.size() ; ++j)
00172 if (!si_->isValid(states[j]))
00173 throw Exception("Internal error. This should not ever happen. Please contact the developers.");
00174
00175 return valid;
00176 }
00177
00178 void ompl::control::PathControl::random(void)
00179 {
00180 freeMemory();
00181 states.resize(2);
00182 controlDurations.resize(1);
00183 controls.resize(1);
00184
00185 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00186 states[0] = si->allocState();
00187 states[1] = si->allocState();
00188 controls[0] = si->allocControl();
00189
00190 base::StateSamplerPtr ss = si->allocStateSampler();
00191 ss->sampleUniform(states[0]);
00192 ControlSamplerPtr cs = si->allocControlSampler();
00193 cs->sample(controls[0], states[0]);
00194 unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
00195 controlDurations[0] = steps * si->getPropagationStepSize();
00196 si->propagate(states[0], controls[0], steps, states[1]);
00197 }
00198
00199 bool ompl::control::PathControl::randomValid(unsigned int attempts)
00200 {
00201 freeMemory();
00202 states.resize(2);
00203 controlDurations.resize(1);
00204 controls.resize(1);
00205
00206 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00207 states[0] = si->allocState();
00208 states[1] = si->allocState();
00209 controls[0] = si->allocControl();
00210
00211 ControlSamplerPtr cs = si->allocControlSampler();
00212 base::UniformValidStateSampler *uvss = new base::UniformValidStateSampler(si);
00213 uvss->setNrAttempts(attempts);
00214 bool ok = false;
00215 for (unsigned int i = 0 ; i < attempts ; ++i)
00216 if (uvss->sample(states[0]))
00217 {
00218 cs->sample(controls[0], states[0]);
00219 unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
00220 controlDurations[0] = steps * si->getPropagationStepSize();
00221 if (si->propagateWhileValid(states[0], controls[0], steps, states[1]) == steps)
00222 {
00223 ok = true;
00224 break;
00225 }
00226 }
00227 delete uvss;
00228
00229 if (!ok)
00230 {
00231 freeMemory();
00232 states.clear();
00233 controls.clear();
00234 controlDurations.clear();
00235 }
00236 return ok;
00237 }
00238
00239 void ompl::control::PathControl::freeMemory(void)
00240 {
00241 for (unsigned int i = 0 ; i < states.size() ; ++i)
00242 si_->freeState(states[i]);
00243 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
00244 for (unsigned int i = 0 ; i < controls.size() ; ++i)
00245 si->freeControl(controls[i]);
00246 }