All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
StateManifold.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 #include "ompl/base/StateManifold.h"
00036 #include "ompl/util/Exception.h"
00037 #include <boost/thread/mutex.hpp>
00038 #include <boost/lexical_cast.hpp>
00039 #include <numeric>
00040 #include <limits>
00041 #include <queue>
00042 #include <cmath>
00043 #include <list>
00044 
00045 const std::string ompl::base::StateManifold::DEFAULT_PROJECTION_NAME = "";
00046 
00048 namespace ompl
00049 {
00050     namespace base
00051     {
00052         static std::list<StateManifold*> STATE_MANIFOLD_LIST;
00053         static boost::mutex              STATE_MANIFOLD_LIST_LOCK;
00054     }
00055 }
00057 
00058 ompl::base::StateManifold::StateManifold(void)
00059 {
00060     // autocompute a unique name
00061     static boost::mutex lock;
00062     static unsigned int m = 0;
00063 
00064     lock.lock();
00065     m++;
00066     lock.unlock();
00067 
00068     name_ = "Manifold" + boost::lexical_cast<std::string>(m);
00069     boost::mutex::scoped_lock smLock(STATE_MANIFOLD_LIST_LOCK);
00070     STATE_MANIFOLD_LIST.push_back(this);
00071 
00072     longestValidSegment_ = 0.0;
00073     longestValidSegmentFraction_ = 0.01; // 1%
00074     longestValidSegmentCountFactor_ = 1;
00075 
00076     type_ = STATE_MANIFOLD_UNKNOWN;
00077 
00078     maxExtent_ = std::numeric_limits<double>::infinity();
00079 }
00080 
00081 ompl::base::StateManifold::~StateManifold(void)
00082 {
00083     boost::mutex::scoped_lock smLock(STATE_MANIFOLD_LIST_LOCK);
00084     STATE_MANIFOLD_LIST.remove(this);
00085 }
00086 
00087 const std::string& ompl::base::StateManifold::getName(void) const
00088 {
00089     return name_;
00090 }
00091 
00092 void ompl::base::StateManifold::setName(const std::string &name)
00093 {
00094     name_ = name;
00095 }
00096 
00097 void ompl::base::StateManifold::registerProjections(void)
00098 {
00099 }
00100 
00101 void ompl::base::StateManifold::setup(void)
00102 {
00103     maxExtent_ = getMaximumExtent();
00104     longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
00105 
00106     if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
00107         throw Exception("The longest valid segment for manifold " + getName() + " must be positive");
00108 
00109     // make sure we don't overwrite projections that have been configured by the user
00110     std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
00111     registerProjections();
00112     for (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
00113         if (it->second->userConfigured())
00114         {
00115             std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
00116             if (o != projections_.end())
00117                 if (!o->second->userConfigured())
00118                     projections_[it->first] = it->second;
00119         }
00120 
00121     for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00122         it->second->setup();
00123 }
00124 
00125 double* ompl::base::StateManifold::getValueAddressAtIndex(State *state, const unsigned int index) const
00126 {
00127     return NULL;
00128 }
00129 
00130 void ompl::base::StateManifold::printState(const State *state, std::ostream &out) const
00131 {
00132     out << "State instance [" << state << ']' << std::endl;
00133 }
00134 
00135 void ompl::base::StateManifold::printSettings(std::ostream &out) const
00136 {
00137     out << "StateManifold '" << getName() << "' instance: " << this << std::endl;
00138     printProjections(out);
00139 }
00140 
00141 void ompl::base::StateManifold::printProjections(std::ostream &out) const
00142 {
00143     if (projections_.empty())
00144         out << "No registered projections" << std::endl;
00145     else
00146     {
00147         out << "Registered projections:" << std::endl;
00148         for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00149         {
00150             out << "  - ";
00151             if (it->first == DEFAULT_PROJECTION_NAME)
00152                 out << "<default>";
00153             else
00154                 out << it->first;
00155             out << std::endl;
00156             it->second->printSettings(out);
00157         }
00158     }
00159 }
00160 
00162 namespace ompl
00163 {
00164     namespace base
00165     {
00166         static bool StateManifoldIncludes(const StateManifold *self, const StateManifold *other)
00167         {
00168             std::queue<const StateManifold*> q;
00169             q.push(self);
00170             while (!q.empty())
00171             {
00172                 const StateManifold *m = q.front();
00173                 q.pop();
00174                 if (m->getName() == other->getName())
00175                     return true;
00176                 if (m->isCompound())
00177                 {
00178                     unsigned int c = m->as<CompoundStateManifold>()->getSubManifoldCount();
00179                     for (unsigned int i = 0 ; i < c ; ++i)
00180                         q.push(m->as<CompoundStateManifold>()->getSubManifold(i).get());
00181                 }
00182             }
00183             return false;
00184         }
00185 
00186         static bool StateManifoldCovers(const StateManifold *self, const StateManifold *other)
00187         {
00188             if (StateManifoldIncludes(self, other))
00189                 return true;
00190             else
00191                 if (other->isCompound())
00192                 {
00193                     unsigned int c = other->as<CompoundStateManifold>()->getSubManifoldCount();
00194                     for (unsigned int i = 0 ; i < c ; ++i)
00195                         if (!StateManifoldCovers(self, other->as<CompoundStateManifold>()->getSubManifold(i).get()))
00196                             return false;
00197                     return true;
00198                 }
00199             return false;
00200         }
00201     }
00202 }
00203 
00205 
00206 bool ompl::base::StateManifold::covers(const StateManifoldPtr &other) const
00207 {
00208     return StateManifoldCovers(this, other.get());
00209 }
00210 
00211 bool ompl::base::StateManifold::includes(const StateManifoldPtr &other) const
00212 {
00213     return StateManifoldIncludes(this, other.get());
00214 }
00215 
00216 void ompl::base::StateManifold::diagram(std::ostream &out)
00217 {
00218     boost::mutex::scoped_lock smLock(STATE_MANIFOLD_LIST_LOCK);
00219     out << "digraph StateManifolds {" << std::endl;
00220     for (std::list<StateManifold*>::iterator it = STATE_MANIFOLD_LIST.begin() ; it != STATE_MANIFOLD_LIST.end(); ++it)
00221     {
00222         out << '"' << (*it)->getName() << '"' << std::endl;
00223         for (std::list<StateManifold*>::iterator jt = STATE_MANIFOLD_LIST.begin() ; jt != STATE_MANIFOLD_LIST.end(); ++jt)
00224             if (it != jt)
00225             {
00226                 if ((*it)->isCompound() && (*it)->as<CompoundStateManifold>()->hasSubManifold((*jt)->getName()))
00227                     out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [label=\"" <<
00228                         boost::lexical_cast<std::string>((*it)->as<CompoundStateManifold>()->getSubManifoldWeight((*jt)->getName())) <<
00229                         "\"];" << std::endl;
00230                 else
00231                     if (!StateManifoldIncludes(*it, *jt) && StateManifoldCovers(*it, *jt))
00232                         out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [style=dashed];" << std::endl;
00233             }
00234     }
00235     out << '}' << std::endl;
00236 }
00237 
00238 bool ompl::base::StateManifold::hasDefaultProjection(void) const
00239 {
00240     return hasProjection(DEFAULT_PROJECTION_NAME);
00241 }
00242 
00243 bool ompl::base::StateManifold::hasProjection(const std::string &name) const
00244 {
00245     return projections_.find(name) != projections_.end();
00246 }
00247 
00248 ompl::base::ProjectionEvaluatorPtr ompl::base::StateManifold::getDefaultProjection(void) const
00249 {
00250     if (hasDefaultProjection())
00251         return getProjection(DEFAULT_PROJECTION_NAME);
00252     else
00253     {
00254         msg_.error("No default projection is set");
00255         return ProjectionEvaluatorPtr();
00256     }
00257 }
00258 
00259 ompl::base::ProjectionEvaluatorPtr ompl::base::StateManifold::getProjection(const std::string &name) const
00260 {
00261     std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
00262     if (it != projections_.end())
00263         return it->second;
00264     else
00265     {
00266         msg_.error("Projection '" + name + "' is not defined");
00267         return ProjectionEvaluatorPtr();
00268     }
00269 }
00270 
00271 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr>& ompl::base::StateManifold::getRegisteredProjections(void) const
00272 {
00273     return projections_;
00274 }
00275 
00276 void ompl::base::StateManifold::registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
00277 {
00278     registerProjection(DEFAULT_PROJECTION_NAME, projection);
00279 }
00280 
00281 void ompl::base::StateManifold::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
00282 {
00283     if (projection)
00284         projections_[name] = projection;
00285     else
00286         msg_.error("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
00287 }
00288 
00289 bool ompl::base::StateManifold::isCompound(void) const
00290 {
00291     return false;
00292 }
00293 
00294 void ompl::base::StateManifold::setValidSegmentCountFactor(unsigned int factor)
00295 {
00296     if (factor < 1)
00297         throw Exception("The multiplicative factor for the valid segment count between two states must be strictly positive");
00298     longestValidSegmentCountFactor_ = factor;
00299 }
00300 
00301 void ompl::base::StateManifold::setLongestValidSegmentFraction(double segmentFraction)
00302 {
00303     if (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
00304         throw Exception("The fraction of the extent must be larger than 0 and less than 1");
00305     longestValidSegmentFraction_ = segmentFraction;
00306 }
00307 
00308 unsigned int ompl::base::StateManifold::getValidSegmentCountFactor(void) const
00309 {
00310     return longestValidSegmentCountFactor_;
00311 }
00312 
00313 double ompl::base::StateManifold::getLongestValidSegmentFraction(void) const
00314 {
00315     return longestValidSegmentFraction_;
00316 }
00317 
00318 unsigned int ompl::base::StateManifold::validSegmentCount(const State *state1, const State *state2) const
00319 {
00320     return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
00321 }
00322 
00323 ompl::base::CompoundStateManifold::CompoundStateManifold(void) : StateManifold(), componentCount_(0), locked_(false)
00324 {
00325     setName("Compound" + getName());
00326 }
00327 
00328 ompl::base::CompoundStateManifold::CompoundStateManifold(const std::vector<StateManifoldPtr> &components,
00329                                                          const std::vector<double> &weights) : StateManifold(), componentCount_(0), locked_(false)
00330 {
00331     if (components.size() != weights.size())
00332         throw Exception("Number of component manifolds and weights are not the same");
00333     setName("Compound" + getName());
00334     for (unsigned int i = 0 ; i < components.size() ; ++i)
00335         addSubManifold(components[i], weights[i]);
00336 }
00337 
00338 void ompl::base::CompoundStateManifold::addSubManifold(const StateManifoldPtr &component, double weight)
00339 {
00340     if (locked_)
00341         throw Exception("This manifold is locked. No further components can be added");
00342     if (weight < 0.0)
00343         throw Exception("Submanifold weight cannot be negative");
00344     components_.push_back(component);
00345     weights_.push_back(weight);
00346     componentCount_ = components_.size();
00347 }
00348 
00349 bool ompl::base::CompoundStateManifold::isCompound(void) const
00350 {
00351     return true;
00352 }
00353 
00354 unsigned int ompl::base::CompoundStateManifold::getSubManifoldCount(void) const
00355 {
00356     return componentCount_;
00357 }
00358 
00359 const ompl::base::StateManifoldPtr& ompl::base::CompoundStateManifold::getSubManifold(const unsigned int index) const
00360 {
00361     if (componentCount_ > index)
00362         return components_[index];
00363     else
00364         throw Exception("Submanifold index does not exist");
00365 }
00366 
00367 bool ompl::base::CompoundStateManifold::hasSubManifold(const std::string &name) const
00368 {
00369     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00370         if (components_[i]->getName() == name)
00371             return true;
00372     return false;
00373 }
00374 
00375 unsigned int ompl::base::CompoundStateManifold::getSubManifoldIndex(const std::string& name) const
00376 {
00377     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00378         if (components_[i]->getName() == name)
00379             return i;
00380     throw Exception("Submanifold " + name + " does not exist");
00381 }
00382 
00383 const ompl::base::StateManifoldPtr& ompl::base::CompoundStateManifold::getSubManifold(const std::string& name) const
00384 {
00385     return components_[getSubManifoldIndex(name)];
00386 }
00387 
00388 double ompl::base::CompoundStateManifold::getSubManifoldWeight(const unsigned int index) const
00389 {
00390     if (componentCount_ > index)
00391         return weights_[index];
00392     else
00393         throw Exception("Submanifold index does not exist");
00394 }
00395 
00396 double ompl::base::CompoundStateManifold::getSubManifoldWeight(const std::string &name) const
00397 {
00398     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00399         if (components_[i]->getName() == name)
00400             return weights_[i];
00401     throw Exception("Submanifold " + name + " does not exist");
00402 }
00403 
00404 void ompl::base::CompoundStateManifold::setSubManifoldWeight(const unsigned int index, double weight)
00405 {
00406     if (weight < 0.0)
00407         throw Exception("Submanifold weight cannot be negative");
00408     if (componentCount_ > index)
00409         weights_[index] = weight;
00410     else
00411         throw Exception("Submanifold index does not exist");
00412 }
00413 
00414 void ompl::base::CompoundStateManifold::setSubManifoldWeight(const std::string &name, double weight)
00415 {
00416     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00417         if (components_[i]->getName() == name)
00418         {
00419             setSubManifoldWeight(i, weight);
00420             return;
00421         }
00422     throw Exception("Submanifold " + name + " does not exist");
00423 }
00424 
00425 const std::vector<ompl::base::StateManifoldPtr>& ompl::base::CompoundStateManifold::getSubManifolds(void) const
00426 {
00427     return components_;
00428 }
00429 
00430 const std::vector<double>& ompl::base::CompoundStateManifold::getSubManifoldWeights(void) const
00431 {
00432     return weights_;
00433 }
00434 
00435 unsigned int ompl::base::CompoundStateManifold::getDimension(void) const
00436 {
00437     unsigned int dim = 0;
00438     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00439         dim += components_[i]->getDimension();
00440     return dim;
00441 }
00442 
00443 double ompl::base::CompoundStateManifold::getMaximumExtent(void) const
00444 {
00445     double e = 0.0;
00446     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00447         e += weights_[i] * components_[i]->getMaximumExtent();
00448     return e;
00449 }
00450 
00451 void ompl::base::CompoundStateManifold::enforceBounds(State *state) const
00452 {
00453     CompoundState *cstate = static_cast<CompoundState*>(state);
00454     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00455         components_[i]->enforceBounds(cstate->components[i]);
00456 }
00457 
00458 bool ompl::base::CompoundStateManifold::satisfiesBounds(const State *state) const
00459 {
00460     const CompoundState *cstate = static_cast<const CompoundState*>(state);
00461     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00462         if (!components_[i]->satisfiesBounds(cstate->components[i]))
00463             return false;
00464     return true;
00465 }
00466 
00467 void ompl::base::CompoundStateManifold::copyState(State *destination, const State *source) const
00468 {
00469     CompoundState      *cdest = static_cast<CompoundState*>(destination);
00470     const CompoundState *csrc = static_cast<const CompoundState*>(source);
00471     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00472         components_[i]->copyState(cdest->components[i], csrc->components[i]);
00473 }
00474 
00475 double ompl::base::CompoundStateManifold::distance(const State *state1, const State *state2) const
00476 {
00477     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00478     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00479     double dist = 0.0;
00480     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00481         dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
00482     return dist;
00483 }
00484 
00485 void ompl::base::CompoundStateManifold::setLongestValidSegmentFraction(double segmentFraction)
00486 {
00487     StateManifold::setLongestValidSegmentFraction(segmentFraction);
00488     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00489         components_[i]->setLongestValidSegmentFraction(segmentFraction);
00490 }
00491 
00492 unsigned int ompl::base::CompoundStateManifold::validSegmentCount(const State *state1, const State *state2) const
00493 {
00494     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00495     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00496     unsigned int sc = 0;
00497     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00498     {
00499         unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
00500         if (sci > sc)
00501             sc = sci;
00502     }
00503     return sc;
00504 }
00505 
00506 bool ompl::base::CompoundStateManifold::equalStates(const State *state1, const State *state2) const
00507 {
00508     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00509     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00510     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00511         if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
00512             return false;
00513     return true;
00514 }
00515 
00516 void ompl::base::CompoundStateManifold::interpolate(const State *from, const State *to, const double t, State *state) const
00517 {
00518     const CompoundState *cfrom  = static_cast<const CompoundState*>(from);
00519     const CompoundState *cto    = static_cast<const CompoundState*>(to);
00520     CompoundState       *cstate = static_cast<CompoundState*>(state);
00521     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00522         components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
00523 }
00524 
00525 ompl::base::ManifoldStateSamplerPtr ompl::base::CompoundStateManifold::allocStateSampler(void) const
00526 {
00527     double totalWeight = std::accumulate(weights_.begin(), weights_.end(), 0.0);
00528     if (totalWeight < std::numeric_limits<double>::epsilon())
00529         totalWeight = 1.0;
00530     CompoundManifoldStateSampler *ss = new CompoundManifoldStateSampler(this);
00531     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00532         ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / totalWeight);
00533     return ManifoldStateSamplerPtr(ss);
00534 }
00535 
00536 ompl::base::State* ompl::base::CompoundStateManifold::allocState(void) const
00537 {
00538     CompoundState *state = new CompoundState();
00539     allocStateComponents(state);
00540     return static_cast<State*>(state);
00541 }
00542 
00543 void ompl::base::CompoundStateManifold::allocStateComponents(CompoundState *state) const
00544 {
00545     state->components = new State*[componentCount_];
00546     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00547         state->components[i] = components_[i]->allocState();
00548 }
00549 
00550 void ompl::base::CompoundStateManifold::freeState(State *state) const
00551 {
00552     CompoundState *cstate = static_cast<CompoundState*>(state);
00553     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00554         components_[i]->freeState(cstate->components[i]);
00555     delete[] cstate->components;
00556     delete cstate;
00557 }
00558 
00559 void ompl::base::CompoundStateManifold::lock(void)
00560 {
00561     locked_ = true;
00562 }
00563 
00564 bool ompl::base::CompoundStateManifold::isLocked(void) const
00565 {
00566     return locked_;
00567 }
00568 
00569 double* ompl::base::CompoundStateManifold::getValueAddressAtIndex(State *state, const unsigned int index) const
00570 {
00571     CompoundState *cstate = static_cast<CompoundState*>(state);
00572     unsigned int idx = 0;
00573 
00574     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00575         for (unsigned int j = 0 ; j <= index ; ++j)
00576         {
00577             double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
00578             if (va)
00579             {
00580                 if (idx == index)
00581                     return va;
00582                 else
00583                     idx++;
00584             }
00585             else
00586                 break;
00587         }
00588     return NULL;
00589 }
00590 
00591 void ompl::base::CompoundStateManifold::printState(const State *state, std::ostream &out) const
00592 {
00593     out << "Compound state [" << std::endl;
00594     const CompoundState *cstate = static_cast<const CompoundState*>(state);
00595     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00596         components_[i]->printState(cstate->components[i], out);
00597     out << "]" << std::endl;
00598 }
00599 
00600 void ompl::base::CompoundStateManifold::printSettings(std::ostream &out) const
00601 {
00602     out << "Compound state manifold '" << getName() << "' of dimension " << getDimension() << (isLocked() ? " (locked)" : "") << " [" << std::endl;
00603     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00604     {
00605         components_[i]->printSettings(out);
00606         out << " of weight " << weights_[i] << std::endl;
00607     }
00608     out << "]" << std::endl;
00609     printProjections(out);
00610 }
00611 
00612 void ompl::base::CompoundStateManifold::setup(void)
00613 {
00614     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00615         components_[i]->setup();
00616 
00617     StateManifold::setup();
00618 }
00619 
00620 namespace ompl
00621 {
00622     namespace base
00623     {
00624 
00626 
00627         static const int NO_DATA_COPIED   = 0;
00628         static const int SOME_DATA_COPIED = 1;
00629         static const int ALL_DATA_COPIED  = 2;
00630 
00632 
00633         // return one of the constants defined above
00634         int copyStateData(const StateManifoldPtr &destM, State *dest, const StateManifoldPtr &sourceM, const State *source)
00635         {
00636             // if states correspond to the same manifold, simply do copy
00637             if (destM->getName() == sourceM->getName())
00638             {
00639                 if (dest != source)
00640                     destM->copyState(dest, source);
00641                 return ALL_DATA_COPIED;
00642             }
00643 
00644             int result = NO_DATA_COPIED;
00645 
00646             // if "to" state is compound
00647             if (destM->isCompound())
00648             {
00649                 const CompoundStateManifold *compoundDestM = destM->as<CompoundStateManifold>();
00650                 CompoundState *compoundDest = dest->as<CompoundState>();
00651 
00652                 // if there is a submanifold in "to" that corresponds to "from", set the data and return
00653                 for (unsigned int i = 0 ; i < compoundDestM->getSubManifoldCount() ; ++i)
00654                     if (compoundDestM->getSubManifold(i)->getName() == sourceM->getName())
00655                     {
00656                         if (compoundDest->components[i] != source)
00657                             compoundDestM->getSubManifold(i)->copyState(compoundDest->components[i], source);
00658                         return ALL_DATA_COPIED;
00659                     }
00660 
00661                 // it could be there are further levels of compound manifolds where the data can be set
00662                 // so we call this function recursively
00663                 for (unsigned int i = 0 ; i < compoundDestM->getSubManifoldCount() ; ++i)
00664                 {
00665                     int res = copyStateData(compoundDestM->getSubManifold(i), compoundDest->components[i], sourceM, source);
00666 
00667                     if (res != NO_DATA_COPIED)
00668                         result = SOME_DATA_COPIED;
00669 
00670                     // if all data was copied, we stop
00671                     if (res == ALL_DATA_COPIED)
00672                         return ALL_DATA_COPIED;
00673                 }
00674             }
00675 
00676             // if we got to this point, it means that the data in "from" could not be copied as a chunk to "to"
00677             // it could be the case "from" is from a compound manifold as well, so we can copy parts of "from", as needed
00678             if (sourceM->isCompound())
00679             {
00680                 const CompoundStateManifold *compoundSourceM = sourceM->as<CompoundStateManifold>();
00681                 const CompoundState *compoundSource = source->as<CompoundState>();
00682 
00683                 unsigned int copiedComponents = 0;
00684 
00685                 // if there is a submanifold in "to" that corresponds to "from", set the data and return
00686                 for (unsigned int i = 0 ; i < compoundSourceM->getSubManifoldCount() ; ++i)
00687                 {
00688                     int res = copyStateData(destM, dest, compoundSourceM->getSubManifold(i), compoundSource->components[i]);
00689                     if (res == ALL_DATA_COPIED)
00690                         copiedComponents++;
00691                     if (res)
00692                         result = SOME_DATA_COPIED;
00693                 }
00694 
00695                 // if each individual component got copied, then the entire data in "from" got copied
00696                 if (copiedComponents == compoundSourceM->getSubManifoldCount())
00697                     result = ALL_DATA_COPIED;
00698             }
00699 
00700             return result;
00701         }
00702 
00704         inline bool StateManifoldHasContent(const StateManifoldPtr &m)
00705         {
00706             if (!m)
00707                 return false;
00708             if (m->getDimension() == 0 && m->getType() == STATE_MANIFOLD_UNKNOWN && m->isCompound())
00709             {
00710                 const unsigned int nc = m->as<CompoundStateManifold>()->getSubManifoldCount();
00711                 for (unsigned int i = 0 ; i < nc ; ++i)
00712                     if (StateManifoldHasContent(m->as<CompoundStateManifold>()->getSubManifold(i)))
00713                         return true;
00714                 return false;
00715             }
00716             return true;
00717         }
00719 
00720         StateManifoldPtr operator+(const StateManifoldPtr &a, const StateManifoldPtr &b)
00721         {
00722             if (!StateManifoldHasContent(a) && StateManifoldHasContent(b))
00723                 return b;
00724 
00725             if (!StateManifoldHasContent(b) && StateManifoldHasContent(a))
00726                 return a;
00727 
00728             std::vector<StateManifoldPtr> components;
00729             std::vector<double>           weights;
00730 
00731             bool change = false;
00732             if (a)
00733             {
00734                 bool used = false;
00735                 if (CompoundStateManifold *csm_a = dynamic_cast<CompoundStateManifold*>(a.get()))
00736                     if (!csm_a->isLocked())
00737                     {
00738                         used = true;
00739                         for (unsigned int i = 0 ; i < csm_a->getSubManifoldCount() ; ++i)
00740                         {
00741                             components.push_back(csm_a->getSubManifold(i));
00742                             weights.push_back(csm_a->getSubManifoldWeight(i));
00743                         }
00744                     }
00745 
00746                 if (!used)
00747                 {
00748                     components.push_back(a);
00749                     weights.push_back(1.0);
00750                 }
00751             }
00752             if (b)
00753             {
00754                 bool used = false;
00755                 unsigned int size = components.size();
00756 
00757                 if (CompoundStateManifold *csm_b = dynamic_cast<CompoundStateManifold*>(b.get()))
00758                     if (!csm_b->isLocked())
00759                     {
00760                         used = true;
00761                         for (unsigned int i = 0 ; i < csm_b->getSubManifoldCount() ; ++i)
00762                         {
00763                             bool ok = true;
00764                             for (unsigned int j = 0 ; j < size ; ++j)
00765                                 if (components[j]->getName() == csm_b->getSubManifold(i)->getName())
00766                                 {
00767                                     ok = false;
00768                                     break;
00769                                 }
00770                             if (ok)
00771                             {
00772                                 components.push_back(csm_b->getSubManifold(i));
00773                                 weights.push_back(csm_b->getSubManifoldWeight(i));
00774                                 change = true;
00775                             }
00776                         }
00777                         if (components.size() == csm_b->getSubManifoldCount())
00778                             return b;
00779                     }
00780 
00781                 if (!used)
00782                 {
00783                     bool ok = true;
00784                     for (unsigned int j = 0 ; j < size ; ++j)
00785                         if (components[j]->getName() == b->getName())
00786                         {
00787                             ok = false;
00788                             break;
00789                         }
00790                     if (ok)
00791                     {
00792                         components.push_back(b);
00793                         weights.push_back(1.0);
00794                         change = true;
00795                     }
00796                 }
00797             }
00798 
00799             if (!change && a)
00800                 return a;
00801 
00802             if (components.size() == 1)
00803                 return components[0];
00804 
00805             return StateManifoldPtr(new CompoundStateManifold(components, weights));
00806         }
00807 
00808         StateManifoldPtr operator-(const StateManifoldPtr &a, const StateManifoldPtr &b)
00809         {
00810             std::vector<StateManifoldPtr> components_a;
00811             std::vector<double>           weights_a;
00812             std::vector<StateManifoldPtr> components_b;
00813 
00814             if (a)
00815             {
00816                 bool used = false;
00817                 if (CompoundStateManifold *csm_a = dynamic_cast<CompoundStateManifold*>(a.get()))
00818                     if (!csm_a->isLocked())
00819                     {
00820                         used = true;
00821                         for (unsigned int i = 0 ; i < csm_a->getSubManifoldCount() ; ++i)
00822                         {
00823                             components_a.push_back(csm_a->getSubManifold(i));
00824                             weights_a.push_back(csm_a->getSubManifoldWeight(i));
00825                         }
00826                     }
00827 
00828                 if (!used)
00829                 {
00830                     components_a.push_back(a);
00831                     weights_a.push_back(1.0);
00832                 }
00833             }
00834 
00835             if (b)
00836             {
00837                 bool used = false;
00838                 if (CompoundStateManifold *csm_b = dynamic_cast<CompoundStateManifold*>(b.get()))
00839                     if (!csm_b->isLocked())
00840                     {
00841                         used = true;
00842                         for (unsigned int i = 0 ; i < csm_b->getSubManifoldCount() ; ++i)
00843                             components_b.push_back(csm_b->getSubManifold(i));
00844                     }
00845                 if (!used)
00846                     components_b.push_back(b);
00847             }
00848 
00849             bool change = false;
00850             for (unsigned int i = 0 ; i < components_b.size() ; ++i)
00851                 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
00852                     if (components_a[j]->getName() == components_b[i]->getName())
00853                     {
00854                         components_a.erase(components_a.begin() + j);
00855                         weights_a.erase(weights_a.begin() + j);
00856                         change = true;
00857                         break;
00858                     }
00859 
00860             if (!change && a)
00861                 return a;
00862 
00863             if (components_a.size() == 1)
00864                 return components_a[0];
00865 
00866             return StateManifoldPtr(new CompoundStateManifold(components_a, weights_a));
00867         }
00868 
00869         StateManifoldPtr operator-(const StateManifoldPtr &a, const std::string &name)
00870         {
00871             std::vector<StateManifoldPtr> components;
00872             std::vector<double>           weights;
00873 
00874             bool change = false;
00875             if (a)
00876             {
00877                 bool used = false;
00878                 if (CompoundStateManifold *csm_a = dynamic_cast<CompoundStateManifold*>(a.get()))
00879                     if (!csm_a->isLocked())
00880                     {
00881                         used = true;
00882                         for (unsigned int i = 0 ; i < csm_a->getSubManifoldCount() ; ++i)
00883                         {
00884                             if (csm_a->getSubManifold(i)->getName() == name)
00885                             {
00886                                 change = true;
00887                                 continue;
00888                             }
00889                             components.push_back(csm_a->getSubManifold(i));
00890                             weights.push_back(csm_a->getSubManifoldWeight(i));
00891                         }
00892                     }
00893 
00894                 if (!used)
00895                 {
00896                     if (a->getName() != name)
00897                     {
00898                         components.push_back(a);
00899                         weights.push_back(1.0);
00900                     }
00901                     else
00902                         change = true;
00903                 }
00904             }
00905 
00906             if (!change && a)
00907                 return a;
00908 
00909             if (components.size() == 1)
00910                 return components[0];
00911 
00912             return StateManifoldPtr(new CompoundStateManifold(components, weights));
00913         }
00914 
00915         StateManifoldPtr operator*(const StateManifoldPtr &a, const StateManifoldPtr &b)
00916         {
00917             std::vector<StateManifoldPtr> components_a;
00918             std::vector<double>           weights_a;
00919             std::vector<StateManifoldPtr> components_b;
00920             std::vector<double>           weights_b;
00921 
00922             if (a)
00923             {
00924                 bool used = false;
00925                 if (CompoundStateManifold *csm_a = dynamic_cast<CompoundStateManifold*>(a.get()))
00926                     if (!csm_a->isLocked())
00927                     {
00928                         used = true;
00929                         for (unsigned int i = 0 ; i < csm_a->getSubManifoldCount() ; ++i)
00930                         {
00931                             components_a.push_back(csm_a->getSubManifold(i));
00932                             weights_a.push_back(csm_a->getSubManifoldWeight(i));
00933                         }
00934                     }
00935 
00936                 if (!used)
00937                 {
00938                     components_a.push_back(a);
00939                     weights_a.push_back(1.0);
00940                 }
00941             }
00942 
00943             if (b)
00944             {
00945                 bool used = false;
00946                 if (CompoundStateManifold *csm_b = dynamic_cast<CompoundStateManifold*>(b.get()))
00947                     if (!csm_b->isLocked())
00948                     {
00949                         used = true;
00950                         for (unsigned int i = 0 ; i < csm_b->getSubManifoldCount() ; ++i)
00951                         {
00952                             components_b.push_back(csm_b->getSubManifold(i));
00953                             weights_b.push_back(csm_b->getSubManifoldWeight(i));
00954                         }
00955                     }
00956 
00957                 if (!used)
00958                 {
00959                     components_b.push_back(b);
00960                     weights_b.push_back(1.0);
00961                 }
00962             }
00963 
00964             std::vector<StateManifoldPtr> components;
00965             std::vector<double>           weights;
00966 
00967             for (unsigned int i = 0 ; i < components_b.size() ; ++i)
00968             {
00969                 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
00970                     if (components_a[j]->getName() == components_b[i]->getName())
00971                     {
00972                         components.push_back(components_b[i]);
00973                         weights.push_back(std::max(weights_a[j], weights_b[i]));
00974                         break;
00975                     }
00976             }
00977 
00978             if (a && components.size() == components_a.size())
00979                 return a;
00980 
00981             if (b && components.size() == components_b.size())
00982                 return b;
00983 
00984             if (components.size() == 1)
00985                 return components[0];
00986 
00987             return StateManifoldPtr(new CompoundStateManifold(components, weights));
00988         }
00989 
00990     }
00991 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator