ompl/control/src/PathControl.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 00036 00037 #include "ompl/control/PathControl.h" 00038 #include "ompl/control/spaces/DiscreteControlSpace.h" 00039 #include "ompl/geometric/PathGeometric.h" 00040 #include "ompl/base/samplers/UniformValidStateSampler.h" 00041 #include "ompl/base/OptimizationObjective.h" 00042 #include "ompl/util/Exception.h" 00043 #include "ompl/util/Console.h" 00044 #include <numeric> 00045 #include <cmath> 00046 00047 namespace 00048 { 00049 unsigned int getNumberOfDiscreteControls(const ompl::control::ControlSpace *cs) 00050 { 00051 if (cs->isCompound()) 00052 { 00053 const ompl::control::CompoundControlSpace *ccs 00054 = cs->as<ompl::control::CompoundControlSpace>(); 00055 unsigned int num = 0; 00056 for (unsigned int i = 0; i < ccs->getSubspaceCount(); ++i) 00057 num += getNumberOfDiscreteControls(ccs->getSubspace(i).get()); 00058 00059 return num; 00060 } 00061 else 00062 if (dynamic_cast<const ompl::control::DiscreteControlSpace*>(cs)) 00063 return 1; 00064 return 0; 00065 } 00066 00067 void printDiscreteControls(std::ostream &out, const ompl::control::ControlSpace *cs, 00068 const ompl::control::Control *c) 00069 { 00070 if (cs->isCompound()) 00071 { 00072 const ompl::control::CompoundControlSpace *ccs 00073 = cs->as<ompl::control::CompoundControlSpace>(); 00074 for (unsigned int i = 0; i < ccs->getSubspaceCount(); ++i) 00075 printDiscreteControls(out, ccs->getSubspace(i).get(), 00076 c->as<ompl::control::CompoundControl>()->components[i]); 00077 } 00078 else if (dynamic_cast<const ompl::control::DiscreteControlSpace*>(cs)) 00079 out << c->as<ompl::control::DiscreteControlSpace::ControlType>()->value << ' '; 00080 } 00081 } 00082 00083 ompl::control::PathControl::PathControl(const base::SpaceInformationPtr &si) : base::Path(si) 00084 { 00085 if (!dynamic_cast<const SpaceInformation*>(si_.get())) 00086 throw Exception("Cannot create a path with controls from a space that does not support controls"); 00087 } 00088 00089 ompl::control::PathControl::PathControl(const PathControl &path) : base::Path(path.si_) 00090 { 00091 copyFrom(path); 00092 } 00093 00094 ompl::geometric::PathGeometric ompl::control::PathControl::asGeometric() const 00095 { 00096 PathControl pc(*this); 00097 pc.interpolate(); 00098 geometric::PathGeometric pg(si_); 00099 pg.getStates().swap(pc.states_); 00100 return pg; 00101 } 00102 00103 ompl::control::PathControl& ompl::control::PathControl::operator=(const PathControl &other) 00104 { 00105 freeMemory(); 00106 si_ = other.si_; 00107 copyFrom(other); 00108 return *this; 00109 } 00110 00111 void ompl::control::PathControl::copyFrom(const PathControl &other) 00112 { 00113 states_.resize(other.states_.size()); 00114 controls_.resize(other.controls_.size()); 00115 00116 for (unsigned int i = 0 ; i < states_.size() ; ++i) 00117 states_[i] = si_->cloneState(other.states_[i]); 00118 00119 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get()); 00120 for (unsigned int i = 0 ; i < controls_.size() ; ++i) 00121 controls_[i] = si->cloneControl(other.controls_[i]); 00122 00123 controlDurations_ = other.controlDurations_; 00124 } 00125 00126 ompl::base::Cost ompl::control::PathControl::cost(const base::OptimizationObjectivePtr &opt) const 00127 { 00128 OMPL_ERROR("Error: Cost computation is only implemented for paths of type PathGeometric."); 00129 return opt->identityCost(); 00130 } 00131 00132 double ompl::control::PathControl::length() const 00133 { 00134 return std::accumulate(controlDurations_.begin(), controlDurations_.end(), 0.0); 00135 } 00136 00137 void ompl::control::PathControl::print(std::ostream &out) const 00138 { 00139 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get()); 00140 double res = si->getPropagationStepSize(); 00141 out << "Control path with " << states_.size() << " states" << std::endl; 00142 for (unsigned int i = 0 ; i < controls_.size() ; ++i) 00143 { 00144 out << "At state "; 00145 si_->printState(states_[i], out); 00146 out << " apply control "; 00147 si->printControl(controls_[i], out); 00148 out << " for " << (int)floor(0.5 + controlDurations_[i]/res) << " steps" << std::endl; 00149 } 00150 out << "Arrive at state "; 00151 si_->printState(states_[controls_.size()], out); 00152 out << std::endl; 00153 } 00154 00155 void ompl::control::PathControl::printAsMatrix(std::ostream &out) const 00156 { 00157 if (states_.empty()) 00158 return; 00159 const base::StateSpace* space(si_->getStateSpace().get()); 00160 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get()); 00161 const ControlSpace* cspace(si->getControlSpace().get()); 00162 std::vector<double> reals; 00163 00164 space->copyToReals(reals, states_[0]); 00165 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " ")); 00166 if (controls_.empty()) 00167 return; 00168 00169 const ControlSpace *cs = static_cast<const SpaceInformation*>(si_.get())->getControlSpace().get(); 00170 unsigned int n = 0, m = getNumberOfDiscreteControls(cs); 00171 double *val; 00172 while ((val = cspace->getValueAddressAtIndex(controls_[0], n))) 00173 ++n; 00174 for (unsigned int i = 0 ; i < n + m; ++i) 00175 out << "0 "; 00176 out << '0' << std::endl; 00177 for (unsigned int i = 0 ; i < controls_.size(); ++i) 00178 { 00179 space->copyToReals(reals, states_[i + 1]); 00180 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " ")); 00181 // print discrete controls 00182 printDiscreteControls(out, cs, controls_[i]); 00183 // print real-valued controls 00184 for (unsigned int j = 0; j < n; ++j) 00185 out << *cspace->getValueAddressAtIndex(controls_[i], j) << ' '; 00186 out << controlDurations_[i] << std::endl; 00187 } 00188 } 00189 00190 void ompl::control::PathControl::interpolate() 00191 { 00192 if (states_.size() <= controls_.size()) 00193 { 00194 OMPL_ERROR("Interpolation not performed. Number of states in the path should be strictly greater than the number of controls."); 00195 return; 00196 } 00197 00198 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get()); 00199 std::vector<base::State*> newStates; 00200 std::vector<Control*> newControls; 00201 std::vector<double> newControlDurations; 00202 00203 double res = si->getPropagationStepSize(); 00204 for (unsigned int i = 0 ; i < controls_.size() ; ++i) 00205 { 00206 int steps = (int)floor(0.5 + controlDurations_[i] / res); 00207 assert(steps >= 0); 00208 if (steps <= 1) 00209 { 00210 newStates.push_back(states_[i]); 00211 newControls.push_back(controls_[i]); 00212 newControlDurations.push_back(controlDurations_[i]); 00213 continue; 00214 } 00215 std::vector<base::State*> istates; 00216 si->propagate(states_[i], controls_[i], steps, istates, true); 00217 // last state is already in the non-interpolated path 00218 if (!istates.empty()) 00219 { 00220 si_->freeState(istates.back()); 00221 istates.pop_back(); 00222 } 00223 newStates.push_back(states_[i]); 00224 newStates.insert(newStates.end(), istates.begin(), istates.end()); 00225 newControls.push_back(controls_[i]); 00226 newControlDurations.push_back(res); 00227 for (int j = 1 ; j < steps; ++j) 00228 { 00229 newControls.push_back(si->cloneControl(controls_[i])); 00230 newControlDurations.push_back(res); 00231 } 00232 } 00233 newStates.push_back(states_[controls_.size()]); 00234 states_.swap(newStates); 00235 controls_.swap(newControls); 00236 controlDurations_.swap(newControlDurations); 00237 } 00238 00239 bool ompl::control::PathControl::check() const 00240 { 00241 if (controls_.empty()) 00242 { 00243 if (states_.size() == 1) 00244 return si_->isValid(states_[0]); 00245 else 00246 return false; 00247 } 00248 00249 bool valid = true; 00250 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get()); 00251 double res = si->getPropagationStepSize(); 00252 base::State *next = si_->allocState(); 00253 for (unsigned int i = 0 ; valid && i < controls_.size() ; ++i) 00254 { 00255 unsigned int steps = (unsigned int)floor(0.5 + controlDurations_[i] / res); 00256 if (!si->isValid(states_[i]) || 00257 si->propagateWhileValid(states_[i], controls_[i], steps, next) != steps || 00258 si->distance(next, states_[i + 1]) > std::numeric_limits<float>::epsilon()) 00259 valid = false; 00260 } 00261 si_->freeState(next); 00262 00263 return valid; 00264 } 00265 00266 void ompl::control::PathControl::append(const base::State *state) 00267 { 00268 states_.push_back(si_->cloneState(state)); 00269 } 00270 00271 void ompl::control::PathControl::append(const base::State *state, const Control *control, double duration) 00272 { 00273 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get()); 00274 states_.push_back(si->cloneState(state)); 00275 controls_.push_back(si->cloneControl(control)); 00276 controlDurations_.push_back(duration); 00277 } 00278 00279 void ompl::control::PathControl::random() 00280 { 00281 freeMemory(); 00282 states_.resize(2); 00283 controlDurations_.resize(1); 00284 controls_.resize(1); 00285 00286 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get()); 00287 states_[0] = si->allocState(); 00288 states_[1] = si->allocState(); 00289 controls_[0] = si->allocControl(); 00290 00291 base::StateSamplerPtr ss = si->allocStateSampler(); 00292 ss->sampleUniform(states_[0]); 00293 ControlSamplerPtr cs = si->allocControlSampler(); 00294 cs->sample(controls_[0], states_[0]); 00295 unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration()); 00296 controlDurations_[0] = steps * si->getPropagationStepSize(); 00297 si->propagate(states_[0], controls_[0], steps, states_[1]); 00298 } 00299 00300 bool ompl::control::PathControl::randomValid(unsigned int attempts) 00301 { 00302 freeMemory(); 00303 states_.resize(2); 00304 controlDurations_.resize(1); 00305 controls_.resize(1); 00306 00307 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get()); 00308 states_[0] = si->allocState(); 00309 states_[1] = si->allocState(); 00310 controls_[0] = si->allocControl(); 00311 00312 ControlSamplerPtr cs = si->allocControlSampler(); 00313 base::UniformValidStateSampler *uvss = new base::UniformValidStateSampler(si); 00314 uvss->setNrAttempts(attempts); 00315 bool ok = false; 00316 for (unsigned int i = 0 ; i < attempts ; ++i) 00317 if (uvss->sample(states_[0])) 00318 { 00319 cs->sample(controls_[0], states_[0]); 00320 unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration()); 00321 controlDurations_[0] = steps * si->getPropagationStepSize(); 00322 if (si->propagateWhileValid(states_[0], controls_[0], steps, states_[1]) == steps) 00323 { 00324 ok = true; 00325 break; 00326 } 00327 } 00328 delete uvss; 00329 00330 if (!ok) 00331 { 00332 freeMemory(); 00333 states_.clear(); 00334 controls_.clear(); 00335 controlDurations_.clear(); 00336 } 00337 return ok; 00338 } 00339 00340 void ompl::control::PathControl::freeMemory() 00341 { 00342 for (unsigned int i = 0 ; i < states_.size() ; ++i) 00343 si_->freeState(states_[i]); 00344 const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get()); 00345 for (unsigned int i = 0 ; i < controls_.size() ; ++i) 00346 si->freeControl(controls_[i]); 00347 }