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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines