ompl/base/src/StateSpace.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/StateSpace.h"
00036 #include "ompl/util/Exception.h"
00037 #include "ompl/tools/config/MagicConstants.h"
00038 #include "ompl/base/spaces/RealVectorStateSpace.h"
00039 #include <boost/thread/mutex.hpp>
00040 #include <boost/thread/once.hpp>
00041 #include <boost/lexical_cast.hpp>
00042 #include <boost/scoped_ptr.hpp>
00043 #include <boost/bind.hpp>
00044 #include <numeric>
00045 #include <limits>
00046 #include <queue>
00047 #include <cmath>
00048 #include <list>
00049 #include <set>
00050 
00051 const std::string ompl::base::StateSpace::DEFAULT_PROJECTION_NAME = "";
00052 
00054 namespace ompl
00055 {
00056     namespace base
00057     {
00058         namespace
00059         {
00060             struct AllocatedSpaces
00061             {
00062                 AllocatedSpaces() : counter_(0)
00063                 {
00064             }
00065                 std::list<StateSpace*> list_;
00066                 boost::mutex           lock_;
00067                 unsigned int           counter_;
00068             };
00069 
00070             static boost::scoped_ptr<AllocatedSpaces> g_allocatedSpaces;
00071             static boost::once_flag g_once = BOOST_ONCE_INIT;
00072 
00073             void initAllocatedSpaces()
00074             {
00075                 g_allocatedSpaces.reset(new AllocatedSpaces);
00076             }
00077 
00078             AllocatedSpaces& getAllocatedSpaces()
00079             {
00080                 boost::call_once(&initAllocatedSpaces, g_once);
00081                 return *g_allocatedSpaces;
00082             }
00083         }  // namespace
00084     }
00085 }
00087 
00088 ompl::base::StateSpace::StateSpace()
00089 {
00090     AllocatedSpaces &as = getAllocatedSpaces();
00091     boost::mutex::scoped_lock smLock(as.lock_);
00092 
00093     // autocompute a unique name
00094     name_ = "Space" + boost::lexical_cast<std::string>(as.counter_++);
00095 
00096     longestValidSegment_ = 0.0;
00097     longestValidSegmentFraction_ = 0.01; // 1%
00098     longestValidSegmentCountFactor_ = 1;
00099 
00100     type_ = STATE_SPACE_UNKNOWN;
00101 
00102     maxExtent_ = std::numeric_limits<double>::infinity();
00103 
00104     params_.declareParam<double>("longest_valid_segment_fraction",
00105                                  boost::bind(&StateSpace::setLongestValidSegmentFraction, this, _1),
00106                                  boost::bind(&StateSpace::getLongestValidSegmentFraction, this));
00107 
00108     params_.declareParam<unsigned int>("valid_segment_count_factor",
00109                                        boost::bind(&StateSpace::setValidSegmentCountFactor, this, _1),
00110                                        boost::bind(&StateSpace::getValidSegmentCountFactor, this));
00111     as.list_.push_back(this);
00112 }
00113 
00114 ompl::base::StateSpace::~StateSpace()
00115 {
00116     AllocatedSpaces &as = getAllocatedSpaces();
00117     boost::mutex::scoped_lock smLock(as.lock_);
00118     as.list_.remove(this);
00119 }
00120 
00122 namespace ompl
00123 {
00124     namespace base
00125     {
00126         static void computeStateSpaceSignatureHelper(const StateSpace *space, std::vector<int> &signature)
00127         {
00128             signature.push_back(space->getType());
00129             signature.push_back(space->getDimension());
00130 
00131             if (space->isCompound())
00132             {
00133                 unsigned int c = space->as<CompoundStateSpace>()->getSubspaceCount();
00134                 for (unsigned int i = 0 ; i < c ; ++i)
00135                     computeStateSpaceSignatureHelper(space->as<CompoundStateSpace>()->getSubspace(i).get(), signature);
00136             }
00137         }
00138 
00139         void computeLocationsHelper(const StateSpace *s,
00140                                     std::map<std::string, StateSpace::SubstateLocation> &substateMap,
00141                                     std::vector<StateSpace::ValueLocation> &locationsArray,
00142                                     std::map<std::string, StateSpace::ValueLocation> &locationsMap, StateSpace::ValueLocation loc)
00143         {
00144             loc.stateLocation.space = s;
00145             substateMap[s->getName()] = loc.stateLocation;
00146             State *test = s->allocState();
00147             if (s->getValueAddressAtIndex(test, 0) != NULL)
00148             {
00149                 loc.index = 0;
00150                 locationsMap[s->getName()] = loc;
00151                 // if the space is compound, we will find this value again in the first subspace
00152                 if (!s->isCompound())
00153                 {
00154                     if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
00155                     {
00156                         const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(0);
00157                         if (!name.empty())
00158                             locationsMap[name] = loc;
00159                     }
00160                     locationsArray.push_back(loc);
00161                     while (s->getValueAddressAtIndex(test, ++loc.index) != NULL)
00162                     {
00163                         if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
00164                         {
00165                             const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(loc.index);
00166                             if (!name.empty())
00167                                 locationsMap[name] = loc;
00168                         }
00169                         locationsArray.push_back(loc);
00170                     }
00171                 }
00172             }
00173             s->freeState(test);
00174 
00175             if (s->isCompound())
00176                 for (unsigned int i = 0 ; i < s->as<base::CompoundStateSpace>()->getSubspaceCount() ; ++i)
00177                 {
00178                     loc.stateLocation.chain.push_back(i);
00179                     computeLocationsHelper(s->as<base::CompoundStateSpace>()->getSubspace(i).get(), substateMap, locationsArray, locationsMap, loc);
00180                     loc.stateLocation.chain.pop_back();
00181                 }
00182         }
00183 
00184         void computeLocationsHelper(const StateSpace *s,
00185                                     std::map<std::string, StateSpace::SubstateLocation> &substateMap,
00186                                     std::vector<StateSpace::ValueLocation> &locationsArray,
00187                                     std::map<std::string, StateSpace::ValueLocation> &locationsMap)
00188         {
00189             substateMap.clear();
00190             locationsArray.clear();
00191             locationsMap.clear();
00192             computeLocationsHelper(s, substateMap, locationsArray, locationsMap, StateSpace::ValueLocation());
00193         }
00194     }
00195 }
00197 
00198 const std::string& ompl::base::StateSpace::getName() const
00199 {
00200     return name_;
00201 }
00202 
00203 void ompl::base::StateSpace::setName(const std::string &name)
00204 {
00205     name_ = name;
00206 
00207     // we don't want to call this function during the state space construction because calls to virtual functions are made,
00208     // so we check if any values were previously inserted as value locations;
00209     // if none were, then we either have none (so no need to call this function again)
00210     // or setup() was not yet called
00211     if (!valueLocationsInOrder_.empty())
00212       computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
00213 }
00214 
00215 void ompl::base::StateSpace::computeLocations()
00216 {
00217     computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
00218 }
00219 
00220 void ompl::base::StateSpace::computeSignature(std::vector<int> &signature) const
00221 {
00222     signature.clear();
00223     computeStateSpaceSignatureHelper(this, signature);
00224     signature.insert(signature.begin(), signature.size());
00225 }
00226 
00227 void ompl::base::StateSpace::registerProjections()
00228 {
00229 }
00230 
00231 void ompl::base::StateSpace::setup()
00232 {
00233     maxExtent_ = getMaximumExtent();
00234     longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
00235 
00236     if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
00237     {
00238         std::stringstream error;
00239         error << "The longest valid segment for state space " + getName() + " must be positive." << std::endl;
00240         error << "Space settings:" << std::endl;
00241         printSettings(error);
00242         throw Exception(error.str());
00243     }
00244 
00245     computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
00246 
00247     // make sure we don't overwrite projections that have been configured by the user
00248     std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
00249     registerProjections();
00250     for (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
00251         if (it->second->userConfigured())
00252         {
00253             std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
00254             if (o != projections_.end())
00255                 if (!o->second->userConfigured())
00256                     projections_[it->first] = it->second;
00257         }
00258 
00259     // remove previously set parameters for projections
00260     std::vector<std::string> pnames;
00261     params_.getParamNames(pnames);
00262     for (std::vector<std::string>::const_iterator it = pnames.begin() ; it != pnames.end() ; ++it)
00263         if (it->substr(0, 11) == "projection.")
00264             params_.remove(*it);
00265 
00266     // setup projections and add their parameters
00267     for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00268     {
00269         it->second->setup();
00270         if (it->first == DEFAULT_PROJECTION_NAME)
00271             params_.include(it->second->params(), "projection");
00272         else
00273             params_.include(it->second->params(), "projection." + it->first);
00274     }
00275 }
00276 
00277 const std::map<std::string, ompl::base::StateSpace::SubstateLocation>& ompl::base::StateSpace::getSubstateLocationsByName() const
00278 {
00279     return substateLocationsByName_;
00280 }
00281 
00282 ompl::base::State* ompl::base::StateSpace::getSubstateAtLocation(State *state, const SubstateLocation &loc) const
00283 {
00284     std::size_t index = 0;
00285     while (loc.chain.size() > index)
00286         state = state->as<CompoundState>()->components[loc.chain[index++]];
00287     return state;
00288 }
00289 
00290 const ompl::base::State* ompl::base::StateSpace::getSubstateAtLocation(const State *state, const SubstateLocation &loc) const
00291 {
00292     std::size_t index = 0;
00293     while (loc.chain.size() > index)
00294         state = state->as<CompoundState>()->components[loc.chain[index++]];
00295     return state;
00296 }
00297 
00298 double* ompl::base::StateSpace::getValueAddressAtIndex(State* /*state*/, const unsigned int /*index*/) const
00299 {
00300     return NULL;
00301 }
00302 
00303 const double* ompl::base::StateSpace::getValueAddressAtIndex(const State *state, const unsigned int index) const
00304 {
00305     double *val = getValueAddressAtIndex(const_cast<State*>(state), index); // this const-cast does not hurt, since the state is not modified
00306     return val;
00307 }
00308 
00309 const std::vector<ompl::base::StateSpace::ValueLocation>& ompl::base::StateSpace::getValueLocations() const
00310 {
00311     return valueLocationsInOrder_;
00312 }
00313 
00314 const std::map<std::string, ompl::base::StateSpace::ValueLocation>& ompl::base::StateSpace::getValueLocationsByName() const
00315 {
00316     return valueLocationsByName_;
00317 }
00318 
00319 void ompl::base::StateSpace::copyToReals(std::vector<double> &reals, const State *source) const
00320 {
00321     reals.resize(valueLocationsInOrder_.size());
00322     for (std::size_t i = 0 ; i < valueLocationsInOrder_.size() ; ++i)
00323         reals[i] = *getValueAddressAtLocation(source, valueLocationsInOrder_[i]);
00324 }
00325 
00326 void ompl::base::StateSpace::copyFromReals(State *destination, const std::vector<double> &reals) const
00327 {
00328     assert(reals.size() == valueLocationsInOrder_.size());
00329     for (std::size_t i = 0 ; i < reals.size() ; ++i)
00330         *getValueAddressAtLocation(destination, valueLocationsInOrder_[i]) = reals[i];
00331 }
00332 
00333 double* ompl::base::StateSpace::getValueAddressAtLocation(State *state, const ValueLocation &loc) const
00334 {
00335     std::size_t index = 0;
00336     while (loc.stateLocation.chain.size() > index)
00337         state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
00338     return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
00339 }
00340 
00341 const double* ompl::base::StateSpace::getValueAddressAtLocation(const State *state, const ValueLocation &loc) const
00342 {
00343     std::size_t index = 0;
00344     while (loc.stateLocation.chain.size() > index)
00345         state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
00346     return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
00347 }
00348 
00349 double* ompl::base::StateSpace::getValueAddressAtName(State *state, const std::string &name) const
00350 {
00351     std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
00352     return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
00353 }
00354 
00355 const double* ompl::base::StateSpace::getValueAddressAtName(const State *state, const std::string &name) const
00356 {
00357     std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
00358     return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
00359 }
00360 
00361 unsigned int ompl::base::StateSpace::getSerializationLength() const
00362 {
00363     return 0;
00364 }
00365 
00366 void ompl::base::StateSpace::serialize(void* /*serialization*/, const State* /*state*/) const
00367 {
00368 }
00369 
00370 void ompl::base::StateSpace::deserialize(State* /*state*/, const void* /*serialization*/) const
00371 {
00372 }
00373 
00374 void ompl::base::StateSpace::printState(const State *state, std::ostream &out) const
00375 {
00376     out << "State instance [" << state << ']' << std::endl;
00377 }
00378 
00379 void ompl::base::StateSpace::printSettings(std::ostream &out) const
00380 {
00381     out << "StateSpace '" << getName() << "' instance: " << this << std::endl;
00382     printProjections(out);
00383 }
00384 
00385 void ompl::base::StateSpace::printProjections(std::ostream &out) const
00386 {
00387     if (projections_.empty())
00388         out << "No registered projections" << std::endl;
00389     else
00390     {
00391         out << "Registered projections:" << std::endl;
00392         for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00393         {
00394             out << "  - ";
00395             if (it->first == DEFAULT_PROJECTION_NAME)
00396                 out << "<default>";
00397             else
00398                 out << it->first;
00399             out << std::endl;
00400             it->second->printSettings(out);
00401         }
00402     }
00403 }
00404 
00406 namespace ompl
00407 {
00408     namespace base
00409     {
00410         static bool StateSpaceIncludes(const StateSpace *self, const StateSpace *other)
00411         {
00412             std::queue<const StateSpace*> q;
00413             q.push(self);
00414             while (!q.empty())
00415             {
00416                 const StateSpace *m = q.front();
00417                 q.pop();
00418                 if (m->getName() == other->getName())
00419                     return true;
00420                 if (m->isCompound())
00421                 {
00422                     unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
00423                     for (unsigned int i = 0 ; i < c ; ++i)
00424                         q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
00425                 }
00426             }
00427             return false;
00428         }
00429 
00430         static bool StateSpaceCovers(const StateSpace *self, const StateSpace *other)
00431         {
00432             if (StateSpaceIncludes(self, other))
00433                 return true;
00434             else
00435                 if (other->isCompound())
00436                 {
00437                     unsigned int c = other->as<CompoundStateSpace>()->getSubspaceCount();
00438                     for (unsigned int i = 0 ; i < c ; ++i)
00439                         if (!StateSpaceCovers(self, other->as<CompoundStateSpace>()->getSubspace(i).get()))
00440                             return false;
00441                     return true;
00442                 }
00443             return false;
00444         }
00445 
00446         struct CompareSubstateLocation
00447         {
00448             bool operator()(const StateSpace::SubstateLocation &a, const StateSpace::SubstateLocation &b) const
00449             {
00450               if (a.space->getDimension() != b.space->getDimension())
00451                   return a.space->getDimension() > b.space->getDimension();
00452               return a.space->getName() > b.space->getName();
00453             }
00454         };
00455 
00456     }
00457 }
00458 
00460 
00461 bool ompl::base::StateSpace::covers(const StateSpacePtr &other) const
00462 {
00463     return StateSpaceCovers(this, other.get());
00464 }
00465 
00466 bool ompl::base::StateSpace::includes(const StateSpacePtr &other) const
00467 {
00468     return StateSpaceIncludes(this, other.get());
00469 }
00470 
00471 bool ompl::base::StateSpace::covers(const StateSpace *other) const
00472 {
00473     return StateSpaceCovers(this, other);
00474 }
00475 
00476 bool ompl::base::StateSpace::includes(const StateSpace *other) const
00477 {
00478     return StateSpaceIncludes(this, other);
00479 }
00480 
00481 void ompl::base::StateSpace::getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const
00482 {
00483     getCommonSubspaces(other.get(), subspaces);
00484 }
00485 
00486 void ompl::base::StateSpace::getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const
00487 {
00488     std::set<StateSpace::SubstateLocation, CompareSubstateLocation> intersection;
00489     const std::map<std::string, StateSpace::SubstateLocation> &S = other->getSubstateLocationsByName();
00490     for (std::map<std::string, StateSpace::SubstateLocation>::const_iterator it = substateLocationsByName_.begin() ; it != substateLocationsByName_.end() ; ++it)
00491     {
00492         if (S.find(it->first) != S.end())
00493             intersection.insert(it->second);
00494     }
00495 
00496     bool found = true;
00497     while (found)
00498     {
00499       found = false;
00500       for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator it = intersection.begin() ; it != intersection.end() ; ++it)
00501           for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator jt = intersection.begin() ; jt != intersection.end() ; ++jt)
00502               if (it != jt)
00503                   if (StateSpaceCovers(it->space, jt->space))
00504                   {
00505                       intersection.erase(jt);
00506                       found = true;
00507                       break;
00508                   }
00509     }
00510     subspaces.clear();
00511     for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator it = intersection.begin() ; it != intersection.end() ; ++it)
00512         subspaces.push_back(it->space->getName());
00513 }
00514 
00515 void ompl::base::StateSpace::List(std::ostream &out)
00516 {
00517     AllocatedSpaces &as = getAllocatedSpaces();
00518     boost::mutex::scoped_lock smLock(as.lock_);
00519     for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
00520         out << "@ " << *it << ": " << (*it)->getName() << std::endl;
00521 }
00522 
00523 void ompl::base::StateSpace::list(std::ostream &out) const
00524 {
00525     std::queue<const StateSpace*> q;
00526     q.push(this);
00527     while (!q.empty())
00528     {
00529         const StateSpace *m = q.front();
00530         q.pop();
00531         out << "@ " << m << ": " << m->getName() << std::endl;
00532         if (m->isCompound())
00533         {
00534             unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
00535             for (unsigned int i = 0 ; i < c ; ++i)
00536                 q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
00537         }
00538     }
00539 }
00540 
00541 void ompl::base::StateSpace::diagram(std::ostream &out) const
00542 {
00543     out << "digraph StateSpace {" << std::endl;
00544     out << '"' << getName() << '"' << std::endl;
00545 
00546     std::queue<const StateSpace*> q;
00547     q.push(this);
00548     while (!q.empty())
00549     {
00550         const StateSpace *m = q.front();
00551         q.pop();
00552         if (m->isCompound())
00553         {
00554             unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
00555             for (unsigned int i = 0 ; i < c ; ++i)
00556             {
00557                 const StateSpace *s = m->as<CompoundStateSpace>()->getSubspace(i).get();
00558                 q.push(s);
00559                 out << '"' << m->getName() << "\" -> \"" << s->getName() << "\" [label=\"" <<
00560                     boost::lexical_cast<std::string>(m->as<CompoundStateSpace>()->getSubspaceWeight(i)) << "\"];" << std::endl;
00561             }
00562         }
00563     }
00564 
00565     out << '}' << std::endl;
00566 }
00567 
00568 void ompl::base::StateSpace::Diagram(std::ostream &out)
00569 {
00570     AllocatedSpaces &as = getAllocatedSpaces();
00571     boost::mutex::scoped_lock smLock(as.lock_);
00572     out << "digraph StateSpaces {" << std::endl;
00573     for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
00574     {
00575         out << '"' << (*it)->getName() << '"' << std::endl;
00576         for (std::list<StateSpace*>::iterator jt = as.list_.begin() ; jt != as.list_.end(); ++jt)
00577             if (it != jt)
00578             {
00579                 if ((*it)->isCompound() && (*it)->as<CompoundStateSpace>()->hasSubspace((*jt)->getName()))
00580                     out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [label=\"" <<
00581                         boost::lexical_cast<std::string>((*it)->as<CompoundStateSpace>()->getSubspaceWeight((*jt)->getName())) <<
00582                         "\"];" << std::endl;
00583                 else
00584                     if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
00585                         out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [style=dashed];" << std::endl;
00586             }
00587     }
00588     out << '}' << std::endl;
00589 }
00590 
00591 void ompl::base::StateSpace::sanityChecks() const
00592 {
00593     unsigned int flags = isMetricSpace() ? ~0 : ~(STATESPACE_DISTANCE_SYMMETRIC | STATESPACE_TRIANGLE_INEQUALITY);
00594     sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), flags);
00595 }
00596 
00597 void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int flags) const
00598 {
00599     {
00600         double maxExt = getMaximumExtent();
00601 
00602         State *s1 = allocState();
00603         State *s2 = allocState();
00604         StateSamplerPtr ss = allocStateSampler();
00605         char *serialization = NULL;
00606         if ((flags & STATESPACE_SERIALIZATION) && getSerializationLength() > 0)
00607             serialization = new char[getSerializationLength()];
00608         for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
00609         {
00610             ss->sampleUniform(s1);
00611             if (distance(s1, s1) > eps)
00612                 throw Exception("Distance from a state to itself should be 0");
00613             if (!equalStates(s1, s1))
00614                 throw Exception("A state should be equal to itself");
00615             if ((flags & STATESPACE_RESPECT_BOUNDS) && !satisfiesBounds(s1))
00616                 throw Exception("Sampled states should be within bounds");
00617             copyState(s2, s1);
00618             if (!equalStates(s1, s2))
00619                 throw Exception("Copy of a state is not the same as the original state. copyState() may not work correctly.");
00620             if (flags & STATESPACE_ENFORCE_BOUNDS_NO_OP)
00621             {
00622                 enforceBounds(s1);
00623                 if  (!equalStates(s1, s2))
00624                     throw Exception("enforceBounds() seems to modify states that are in fact within bounds.");
00625             }
00626             if (flags & STATESPACE_SERIALIZATION)
00627             {
00628                 ss->sampleUniform(s2);
00629                 serialize(serialization, s1);
00630                 deserialize(s2, serialization);
00631                 if  (!equalStates(s1, s2))
00632                     throw Exception("Serialization/deserialization operations do not seem to work as expected.");
00633             }
00634             ss->sampleUniform(s2);
00635             if (!equalStates(s1, s2))
00636             {
00637                 double d12 = distance(s1, s2);
00638                 if ((flags & STATESPACE_DISTANCE_DIFFERENT_STATES) && d12 < zero)
00639                     throw Exception("Distance between different states should be above 0");
00640                 double d21 = distance(s2, s1);
00641                 if ((flags & STATESPACE_DISTANCE_SYMMETRIC) && fabs(d12 - d21) > eps)
00642                     throw Exception("The distance function should be symmetric (A->B=" +
00643                                     boost::lexical_cast<std::string>(d12) + ", B->A=" +
00644                                     boost::lexical_cast<std::string>(d21) + ", difference is " +
00645                                     boost::lexical_cast<std::string>(fabs(d12 - d21)) + ")");
00646                 if (flags & STATESPACE_DISTANCE_BOUND)
00647                     if (d12 > maxExt + zero)
00648                         throw Exception("The distance function should not report values larger than the maximum extent ("+
00649                                         boost::lexical_cast<std::string>(d12) + " > " + boost::lexical_cast<std::string>(maxExt) + ")");
00650             }
00651         }
00652         if (serialization)
00653             delete[] serialization;
00654         freeState(s1);
00655         freeState(s2);
00656     }
00657 
00658 
00659     // Test that interpolation works as expected and also test triangle inequality
00660     if (!isDiscrete() && !isHybrid())
00661     {
00662         State *s1 = allocState();
00663         State *s2 = allocState();
00664         State *s3 = allocState();
00665         StateSamplerPtr ss = allocStateSampler();
00666 
00667         for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
00668         {
00669             ss->sampleUniform(s1);
00670             ss->sampleUniform(s2);
00671             ss->sampleUniform(s3);
00672 
00673             interpolate(s1, s2, 0.0, s3);
00674             if ((flags & STATESPACE_INTERPOLATION) && distance(s1, s3) > eps)
00675                 throw Exception("Interpolation from a state at time 0 should be not change the original state");
00676 
00677             interpolate(s1, s2, 1.0, s3);
00678             if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
00679                 throw Exception("Interpolation to a state at time 1 should be the same as the final state");
00680 
00681             interpolate(s1, s2, 0.5, s3);
00682             double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
00683             if ((flags & STATESPACE_TRIANGLE_INEQUALITY) && fabs(diff) > eps)
00684                 throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle inequality (" +
00685                                 boost::lexical_cast<std::string>(diff) + " difference)");
00686 
00687             interpolate(s3, s2, 0.5, s3);
00688             interpolate(s1, s2, 0.75, s2);
00689 
00690             if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
00691                 throw Exception("Continued interpolation does not work as expected. Please also check that interpolate() works with overlapping memory for its state arguments");
00692         }
00693         freeState(s1);
00694         freeState(s2);
00695         freeState(s3);
00696     }
00697 }
00698 
00699 bool ompl::base::StateSpace::hasDefaultProjection() const
00700 {
00701     return hasProjection(DEFAULT_PROJECTION_NAME);
00702 }
00703 
00704 bool ompl::base::StateSpace::hasProjection(const std::string &name) const
00705 {
00706     return projections_.find(name) != projections_.end();
00707 }
00708 
00709 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getDefaultProjection() const
00710 {
00711     if (hasDefaultProjection())
00712         return getProjection(DEFAULT_PROJECTION_NAME);
00713     else
00714     {
00715         OMPL_ERROR("No default projection is set. Perhaps setup() needs to be called");
00716         return ProjectionEvaluatorPtr();
00717     }
00718 }
00719 
00720 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getProjection(const std::string &name) const
00721 {
00722     std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
00723     if (it != projections_.end())
00724         return it->second;
00725     else
00726     {
00727         OMPL_ERROR("Projection '%s' is not defined", name.c_str());
00728         return ProjectionEvaluatorPtr();
00729     }
00730 }
00731 
00732 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr>& ompl::base::StateSpace::getRegisteredProjections() const
00733 {
00734     return projections_;
00735 }
00736 
00737 void ompl::base::StateSpace::registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
00738 {
00739     registerProjection(DEFAULT_PROJECTION_NAME, projection);
00740 }
00741 
00742 void ompl::base::StateSpace::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
00743 {
00744     if (projection)
00745         projections_[name] = projection;
00746     else
00747         OMPL_ERROR("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
00748 }
00749 
00750 bool ompl::base::StateSpace::isCompound() const
00751 {
00752     return false;
00753 }
00754 
00755 bool ompl::base::StateSpace::isDiscrete() const
00756 {
00757     return false;
00758 }
00759 
00760 bool ompl::base::StateSpace::isHybrid() const
00761 {
00762     return false;
00763 }
00764 
00765 bool ompl::base::StateSpace::hasSymmetricDistance() const
00766 {
00767     return true;
00768 }
00769 
00770 bool ompl::base::StateSpace::hasSymmetricInterpolate() const
00771 {
00772     return true;
00773 }
00774 
00775 void ompl::base::StateSpace::setStateSamplerAllocator(const StateSamplerAllocator &ssa)
00776 {
00777     ssa_ = ssa;
00778 }
00779 
00780 void ompl::base::StateSpace::clearStateSamplerAllocator()
00781 {
00782     ssa_ = StateSamplerAllocator();
00783 }
00784 
00785 ompl::base::StateSamplerPtr ompl::base::StateSpace::allocStateSampler() const
00786 {
00787     if (ssa_)
00788         return ssa_(this);
00789     else
00790         return allocDefaultStateSampler();
00791 }
00792 
00793 ompl::base::StateSamplerPtr ompl::base::StateSpace::allocSubspaceStateSampler(const StateSpacePtr &subspace) const
00794 {
00795     return allocSubspaceStateSampler(subspace.get());
00796 }
00797 
00798 ompl::base::StateSamplerPtr ompl::base::StateSpace::allocSubspaceStateSampler(const StateSpace *subspace) const
00799 {
00800     if (subspace->getName() == getName())
00801         return allocStateSampler();
00802     return StateSamplerPtr(new SubspaceStateSampler(this, subspace, 1.0));
00803 }
00804 
00805 void ompl::base::StateSpace::setValidSegmentCountFactor(unsigned int factor)
00806 {
00807     if (factor < 1)
00808         throw Exception("The multiplicative factor for the valid segment count between two states must be strictly positive");
00809     longestValidSegmentCountFactor_ = factor;
00810 }
00811 
00812 void ompl::base::StateSpace::setLongestValidSegmentFraction(double segmentFraction)
00813 {
00814     if (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
00815         throw Exception("The fraction of the extent must be larger than 0 and less than 1");
00816     longestValidSegmentFraction_ = segmentFraction;
00817 }
00818 
00819 unsigned int ompl::base::StateSpace::getValidSegmentCountFactor() const
00820 {
00821     return longestValidSegmentCountFactor_;
00822 }
00823 
00824 double ompl::base::StateSpace::getLongestValidSegmentFraction() const
00825 {
00826     return longestValidSegmentFraction_;
00827 }
00828 
00829 double ompl::base::StateSpace::getLongestValidSegmentLength() const
00830 {
00831     return longestValidSegment_;
00832 }
00833 
00834 unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
00835 {
00836     return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
00837 }
00838 
00839 ompl::base::CompoundStateSpace::CompoundStateSpace() : StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
00840 {
00841     setName("Compound" + getName());
00842 }
00843 
00844 ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
00845                                                    const std::vector<double> &weights) :
00846     StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
00847 {
00848     if (components.size() != weights.size())
00849         throw Exception("Number of component spaces and weights are not the same");
00850     setName("Compound" + getName());
00851     for (unsigned int i = 0 ; i < components.size() ; ++i)
00852         addSubspace(components[i], weights[i]);
00853 }
00854 
00855 void ompl::base::CompoundStateSpace::addSubspace(const StateSpacePtr &component, double weight)
00856 {
00857     if (locked_)
00858         throw Exception("This state space is locked. No further components can be added");
00859     if (weight < 0.0)
00860         throw Exception("Subspace weight cannot be negative");
00861     components_.push_back(component);
00862     weights_.push_back(weight);
00863     weightSum_ += weight;
00864     componentCount_ = components_.size();
00865 }
00866 
00867 bool ompl::base::CompoundStateSpace::isCompound() const
00868 {
00869     return true;
00870 }
00871 
00872 bool ompl::base::CompoundStateSpace::isHybrid() const
00873 {
00874     bool c = false;
00875     bool d = false;
00876     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00877     {
00878         if (components_[i]->isHybrid())
00879             return true;
00880         if (components_[i]->isDiscrete())
00881             d = true;
00882         else
00883             c = true;
00884     }
00885     return c && d;
00886 }
00887 
00888 unsigned int ompl::base::CompoundStateSpace::getSubspaceCount() const
00889 {
00890     return componentCount_;
00891 }
00892 
00893 const ompl::base::StateSpacePtr& ompl::base::CompoundStateSpace::getSubspace(const unsigned int index) const
00894 {
00895     if (componentCount_ > index)
00896         return components_[index];
00897     else
00898         throw Exception("Subspace index does not exist");
00899 }
00900 
00901 bool ompl::base::CompoundStateSpace::hasSubspace(const std::string &name) const
00902 {
00903     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00904         if (components_[i]->getName() == name)
00905             return true;
00906     return false;
00907 }
00908 
00909 unsigned int ompl::base::CompoundStateSpace::getSubspaceIndex(const std::string& name) const
00910 {
00911     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00912         if (components_[i]->getName() == name)
00913             return i;
00914     throw Exception("Subspace " + name + " does not exist");
00915 }
00916 
00917 const ompl::base::StateSpacePtr& ompl::base::CompoundStateSpace::getSubspace(const std::string& name) const
00918 {
00919     return components_[getSubspaceIndex(name)];
00920 }
00921 
00922 double ompl::base::CompoundStateSpace::getSubspaceWeight(const unsigned int index) const
00923 {
00924     if (componentCount_ > index)
00925         return weights_[index];
00926     else
00927         throw Exception("Subspace index does not exist");
00928 }
00929 
00930 double ompl::base::CompoundStateSpace::getSubspaceWeight(const std::string &name) const
00931 {
00932     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00933         if (components_[i]->getName() == name)
00934             return weights_[i];
00935     throw Exception("Subspace " + name + " does not exist");
00936 }
00937 
00938 void ompl::base::CompoundStateSpace::setSubspaceWeight(const unsigned int index, double weight)
00939 {
00940     if (weight < 0.0)
00941         throw Exception("Subspace weight cannot be negative");
00942     if (componentCount_ > index)
00943     {
00944         weightSum_ += weight - weights_[index];
00945         weights_[index] = weight;
00946     }
00947     else
00948         throw Exception("Subspace index does not exist");
00949 }
00950 
00951 void ompl::base::CompoundStateSpace::setSubspaceWeight(const std::string &name, double weight)
00952 {
00953     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00954         if (components_[i]->getName() == name)
00955         {
00956             setSubspaceWeight(i, weight);
00957             return;
00958         }
00959     throw Exception("Subspace " + name + " does not exist");
00960 }
00961 
00962 const std::vector<ompl::base::StateSpacePtr>& ompl::base::CompoundStateSpace::getSubspaces() const
00963 {
00964     return components_;
00965 }
00966 
00967 const std::vector<double>& ompl::base::CompoundStateSpace::getSubspaceWeights() const
00968 {
00969     return weights_;
00970 }
00971 
00972 unsigned int ompl::base::CompoundStateSpace::getDimension() const
00973 {
00974     unsigned int dim = 0;
00975     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00976         dim += components_[i]->getDimension();
00977     return dim;
00978 }
00979 
00980 double ompl::base::CompoundStateSpace::getMaximumExtent() const
00981 {
00982     double e = 0.0;
00983     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00984         if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
00985             e += weights_[i] * components_[i]->getMaximumExtent();
00986     return e;
00987 }
00988 
00989 double ompl::base::CompoundStateSpace::getMeasure() const
00990 {
00991     double m = 1.0;
00992     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00993         if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
00994             m *= weights_[i] * components_[i]->getMeasure();
00995     return m;
00996 }
00997 
00998 void ompl::base::CompoundStateSpace::enforceBounds(State *state) const
00999 {
01000     CompoundState *cstate = static_cast<CompoundState*>(state);
01001     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01002         components_[i]->enforceBounds(cstate->components[i]);
01003 }
01004 
01005 bool ompl::base::CompoundStateSpace::satisfiesBounds(const State *state) const
01006 {
01007     const CompoundState *cstate = static_cast<const CompoundState*>(state);
01008     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01009         if (!components_[i]->satisfiesBounds(cstate->components[i]))
01010             return false;
01011     return true;
01012 }
01013 
01014 void ompl::base::CompoundStateSpace::copyState(State *destination, const State *source) const
01015 {
01016     CompoundState      *cdest = static_cast<CompoundState*>(destination);
01017     const CompoundState *csrc = static_cast<const CompoundState*>(source);
01018     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01019         components_[i]->copyState(cdest->components[i], csrc->components[i]);
01020 }
01021 
01022 unsigned int ompl::base::CompoundStateSpace::getSerializationLength() const
01023 {
01024     unsigned int l = 0;
01025     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01026         l += components_[i]->getSerializationLength();
01027     return l;
01028 }
01029 
01030 void ompl::base::CompoundStateSpace::serialize(void *serialization, const State *state) const
01031 {
01032     const CompoundState *cstate = static_cast<const CompoundState*>(state);
01033     unsigned int l = 0;
01034     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01035     {
01036         components_[i]->serialize(reinterpret_cast<char*>(serialization) + l, cstate->components[i]);
01037         l += components_[i]->getSerializationLength();
01038     }
01039 }
01040 
01041 void ompl::base::CompoundStateSpace::deserialize(State *state, const void *serialization) const
01042 {
01043     CompoundState *cstate = static_cast<CompoundState*>(state);
01044     unsigned int l = 0;
01045     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01046     {
01047         components_[i]->deserialize(cstate->components[i], reinterpret_cast<const char*>(serialization) + l);
01048         l += components_[i]->getSerializationLength();
01049     }
01050 }
01051 
01052 double ompl::base::CompoundStateSpace::distance(const State *state1, const State *state2) const
01053 {
01054     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
01055     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
01056     double dist = 0.0;
01057     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01058         dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
01059     return dist;
01060 }
01061 
01062 void ompl::base::CompoundStateSpace::setLongestValidSegmentFraction(double segmentFraction)
01063 {
01064     StateSpace::setLongestValidSegmentFraction(segmentFraction);
01065     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01066         components_[i]->setLongestValidSegmentFraction(segmentFraction);
01067 }
01068 
01069 unsigned int ompl::base::CompoundStateSpace::validSegmentCount(const State *state1, const State *state2) const
01070 {
01071     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
01072     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
01073     unsigned int sc = 0;
01074     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01075     {
01076         unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
01077         if (sci > sc)
01078             sc = sci;
01079     }
01080     return sc;
01081 }
01082 
01083 bool ompl::base::CompoundStateSpace::equalStates(const State *state1, const State *state2) const
01084 {
01085     const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
01086     const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
01087     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01088         if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
01089             return false;
01090     return true;
01091 }
01092 
01093 void ompl::base::CompoundStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
01094 {
01095     const CompoundState *cfrom  = static_cast<const CompoundState*>(from);
01096     const CompoundState *cto    = static_cast<const CompoundState*>(to);
01097     CompoundState       *cstate = static_cast<CompoundState*>(state);
01098     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01099         components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
01100 }
01101 
01102 ompl::base::StateSamplerPtr ompl::base::CompoundStateSpace::allocDefaultStateSampler() const
01103 {
01104     CompoundStateSampler *ss = new CompoundStateSampler(this);
01105     if (weightSum_ < std::numeric_limits<double>::epsilon())
01106         for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01107             ss->addSampler(components_[i]->allocStateSampler(), 1.0);
01108     else
01109         for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01110             ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / weightSum_);
01111     return StateSamplerPtr(ss);
01112 }
01113 
01114 ompl::base::StateSamplerPtr ompl::base::CompoundStateSpace::allocSubspaceStateSampler(const StateSpace *subspace) const
01115 {
01116     if (subspace->getName() == getName())
01117         return allocStateSampler();
01118     if (hasSubspace(subspace->getName()))
01119         return StateSamplerPtr(new SubspaceStateSampler(this, subspace, getSubspaceWeight(subspace->getName()) / weightSum_));
01120     return StateSpace::allocSubspaceStateSampler(subspace);
01121 }
01122 
01123 ompl::base::State* ompl::base::CompoundStateSpace::allocState() const
01124 {
01125     CompoundState *state = new CompoundState();
01126     allocStateComponents(state);
01127     return static_cast<State*>(state);
01128 }
01129 
01130 void ompl::base::CompoundStateSpace::allocStateComponents(CompoundState *state) const
01131 {
01132     state->components = new State*[componentCount_];
01133     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01134         state->components[i] = components_[i]->allocState();
01135 }
01136 
01137 void ompl::base::CompoundStateSpace::freeState(State *state) const
01138 {
01139     CompoundState *cstate = static_cast<CompoundState*>(state);
01140     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01141         components_[i]->freeState(cstate->components[i]);
01142     delete[] cstate->components;
01143     delete cstate;
01144 }
01145 
01146 void ompl::base::CompoundStateSpace::lock()
01147 {
01148     locked_ = true;
01149 }
01150 
01151 bool ompl::base::CompoundStateSpace::isLocked() const
01152 {
01153     return locked_;
01154 }
01155 
01156 double* ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
01157 {
01158     CompoundState *cstate = static_cast<CompoundState*>(state);
01159     unsigned int idx = 0;
01160 
01161     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01162         for (unsigned int j = 0 ; j <= index ; ++j)
01163         {
01164             double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
01165             if (va)
01166             {
01167                 if (idx == index)
01168                     return va;
01169                 else
01170                     idx++;
01171             }
01172             else
01173                 break;
01174         }
01175     return NULL;
01176 }
01177 
01178 void ompl::base::CompoundStateSpace::printState(const State *state, std::ostream &out) const
01179 {
01180     out << "Compound state [" << std::endl;
01181     const CompoundState *cstate = static_cast<const CompoundState*>(state);
01182     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01183         components_[i]->printState(cstate->components[i], out);
01184     out << "]" << std::endl;
01185 }
01186 
01187 void ompl::base::CompoundStateSpace::printSettings(std::ostream &out) const
01188 {
01189     out << "Compound state space '" << getName() << "' of dimension " << getDimension() << (isLocked() ? " (locked)" : "") << " [" << std::endl;
01190     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01191     {
01192         components_[i]->printSettings(out);
01193         out << " of weight " << weights_[i] << std::endl;
01194     }
01195     out << "]" << std::endl;
01196     printProjections(out);
01197 }
01198 
01199 void ompl::base::CompoundStateSpace::setup()
01200 {
01201     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01202         components_[i]->setup();
01203 
01204     StateSpace::setup();
01205 }
01206 
01207 void ompl::base::CompoundStateSpace::computeLocations()
01208 {
01209     StateSpace::computeLocations();
01210     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01211         components_[i]->computeLocations();
01212 }
01213 
01214 namespace ompl
01215 {
01216     namespace base
01217     {
01218 
01219         AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
01220         {
01221           return copyStateData(destS.get(), dest, sourceS.get(), source);
01222         }
01223 
01224         AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS, const State *source)
01225         {
01226             // if states correspond to the same space, simply do copy
01227             if (destS->getName() == sourceS->getName())
01228             {
01229                 if (dest != source)
01230                     destS->copyState(dest, source);
01231                 return ALL_DATA_COPIED;
01232             }
01233 
01234             AdvancedStateCopyOperation result = NO_DATA_COPIED;
01235 
01236             // if "to" state is compound
01237             if (destS->isCompound())
01238             {
01239                 const CompoundStateSpace *compoundDestS = destS->as<CompoundStateSpace>();
01240                 CompoundState *compoundDest = dest->as<CompoundState>();
01241 
01242                 // if there is a subspace in "to" that corresponds to "from", set the data and return
01243                 for (unsigned int i = 0 ; i < compoundDestS->getSubspaceCount() ; ++i)
01244                     if (compoundDestS->getSubspace(i)->getName() == sourceS->getName())
01245                     {
01246                         if (compoundDest->components[i] != source)
01247                             compoundDestS->getSubspace(i)->copyState(compoundDest->components[i], source);
01248                         return ALL_DATA_COPIED;
01249                     }
01250 
01251                 // it could be there are further levels of compound spaces where the data can be set
01252                 // so we call this function recursively
01253                 for (unsigned int i = 0 ; i < compoundDestS->getSubspaceCount() ; ++i)
01254                 {
01255                     AdvancedStateCopyOperation res = copyStateData(compoundDestS->getSubspace(i).get(), compoundDest->components[i], sourceS, source);
01256 
01257                     if (res != NO_DATA_COPIED)
01258                         result = SOME_DATA_COPIED;
01259 
01260                     // if all data was copied, we stop
01261                     if (res == ALL_DATA_COPIED)
01262                         return ALL_DATA_COPIED;
01263                 }
01264             }
01265 
01266             // if we got to this point, it means that the data in "from" could not be copied as a chunk to "to"
01267             // it could be the case "from" is from a compound space as well, so we can copy parts of "from", as needed
01268             if (sourceS->isCompound())
01269             {
01270                 const CompoundStateSpace *compoundSourceS = sourceS->as<CompoundStateSpace>();
01271                 const CompoundState *compoundSource = source->as<CompoundState>();
01272 
01273                 unsigned int copiedComponents = 0;
01274 
01275                 // if there is a subspace in "to" that corresponds to "from", set the data and return
01276                 for (unsigned int i = 0 ; i < compoundSourceS->getSubspaceCount() ; ++i)
01277                 {
01278                     AdvancedStateCopyOperation res = copyStateData(destS, dest, compoundSourceS->getSubspace(i).get(), compoundSource->components[i]);
01279                     if (res == ALL_DATA_COPIED)
01280                         copiedComponents++;
01281                     if (res != NO_DATA_COPIED)
01282                         result = SOME_DATA_COPIED;
01283                 }
01284 
01285                 // if each individual component got copied, then the entire data in "from" got copied
01286                 if (copiedComponents == compoundSourceS->getSubspaceCount())
01287                     result = ALL_DATA_COPIED;
01288             }
01289 
01290             return result;
01291         }
01292 
01293         AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
01294                                                  const StateSpacePtr &sourceS, const State *source,
01295                                                  const std::vector<std::string> &subspaces)
01296         {
01297             return copyStateData(destS.get(), dest, sourceS.get(), source, subspaces);
01298         }
01299 
01300         AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
01301                                                  const StateSpace *sourceS, const State *source,
01302                                                  const std::vector<std::string> &subspaces)
01303         {
01304             std::size_t copyCount = 0;
01305             const std::map<std::string, StateSpace::SubstateLocation> &destLoc = destS->getSubstateLocationsByName();
01306             const std::map<std::string, StateSpace::SubstateLocation> &sourceLoc = sourceS->getSubstateLocationsByName();
01307             for (std::size_t i = 0 ; i < subspaces.size() ; ++i)
01308             {
01309                 std::map<std::string, StateSpace::SubstateLocation>::const_iterator dt = destLoc.find(subspaces[i]);
01310                 if (dt != destLoc.end())
01311                 {
01312                     std::map<std::string, StateSpace::SubstateLocation>::const_iterator st = sourceLoc.find(subspaces[i]);
01313                     if (st != sourceLoc.end())
01314                     {
01315                         dt->second.space->copyState(destS->getSubstateAtLocation(dest, dt->second), sourceS->getSubstateAtLocation(source, st->second));
01316                         ++copyCount;
01317                     }
01318                 }
01319             }
01320             if (copyCount == subspaces.size())
01321                 return ALL_DATA_COPIED;
01322             if (copyCount > 0)
01323                 return SOME_DATA_COPIED;
01324             return NO_DATA_COPIED;
01325         }
01326 
01328         inline bool StateSpaceHasContent(const StateSpacePtr &m)
01329         {
01330             if (!m)
01331                 return false;
01332             if (m->getDimension() == 0 && m->getType() == STATE_SPACE_UNKNOWN && m->isCompound())
01333             {
01334                 const unsigned int nc = m->as<CompoundStateSpace>()->getSubspaceCount();
01335                 for (unsigned int i = 0 ; i < nc ; ++i)
01336                     if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubspace(i)))
01337                         return true;
01338                 return false;
01339             }
01340             return true;
01341         }
01343 
01344         StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b)
01345         {
01346             if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
01347                 return b;
01348 
01349             if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
01350                 return a;
01351 
01352             std::vector<StateSpacePtr> components;
01353             std::vector<double>           weights;
01354 
01355             bool change = false;
01356             if (a)
01357             {
01358                 bool used = false;
01359                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01360                     if (!csm_a->isLocked())
01361                     {
01362                         used = true;
01363                         for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
01364                         {
01365                             components.push_back(csm_a->getSubspace(i));
01366                             weights.push_back(csm_a->getSubspaceWeight(i));
01367                         }
01368                     }
01369 
01370                 if (!used)
01371                 {
01372                     components.push_back(a);
01373                     weights.push_back(1.0);
01374                 }
01375             }
01376             if (b)
01377             {
01378                 bool used = false;
01379                 unsigned int size = components.size();
01380 
01381                 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01382                     if (!csm_b->isLocked())
01383                     {
01384                         used = true;
01385                         for (unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
01386                         {
01387                             bool ok = true;
01388                             for (unsigned int j = 0 ; j < size ; ++j)
01389                                 if (components[j]->getName() == csm_b->getSubspace(i)->getName())
01390                                 {
01391                                     ok = false;
01392                                     break;
01393                                 }
01394                             if (ok)
01395                             {
01396                                 components.push_back(csm_b->getSubspace(i));
01397                                 weights.push_back(csm_b->getSubspaceWeight(i));
01398                                 change = true;
01399                             }
01400                         }
01401                         if (components.size() == csm_b->getSubspaceCount())
01402                             return b;
01403                     }
01404 
01405                 if (!used)
01406                 {
01407                     bool ok = true;
01408                     for (unsigned int j = 0 ; j < size ; ++j)
01409                         if (components[j]->getName() == b->getName())
01410                         {
01411                             ok = false;
01412                             break;
01413                         }
01414                     if (ok)
01415                     {
01416                         components.push_back(b);
01417                         weights.push_back(1.0);
01418                         change = true;
01419                     }
01420                 }
01421             }
01422 
01423             if (!change && a)
01424                 return a;
01425 
01426             if (components.size() == 1)
01427                 return components[0];
01428 
01429             return StateSpacePtr(new CompoundStateSpace(components, weights));
01430         }
01431 
01432         StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
01433         {
01434             std::vector<StateSpacePtr> components_a;
01435             std::vector<double>           weights_a;
01436             std::vector<StateSpacePtr> components_b;
01437 
01438             if (a)
01439             {
01440                 bool used = false;
01441                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01442                     if (!csm_a->isLocked())
01443                     {
01444                         used = true;
01445                         for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
01446                         {
01447                             components_a.push_back(csm_a->getSubspace(i));
01448                             weights_a.push_back(csm_a->getSubspaceWeight(i));
01449                         }
01450                     }
01451 
01452                 if (!used)
01453                 {
01454                     components_a.push_back(a);
01455                     weights_a.push_back(1.0);
01456                 }
01457             }
01458 
01459             if (b)
01460             {
01461                 bool used = false;
01462                 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01463                     if (!csm_b->isLocked())
01464                     {
01465                         used = true;
01466                         for (unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
01467                             components_b.push_back(csm_b->getSubspace(i));
01468                     }
01469                 if (!used)
01470                     components_b.push_back(b);
01471             }
01472 
01473             bool change = false;
01474             for (unsigned int i = 0 ; i < components_b.size() ; ++i)
01475                 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
01476                     if (components_a[j]->getName() == components_b[i]->getName())
01477                     {
01478                         components_a.erase(components_a.begin() + j);
01479                         weights_a.erase(weights_a.begin() + j);
01480                         change = true;
01481                         break;
01482                     }
01483 
01484             if (!change && a)
01485                 return a;
01486 
01487             if (components_a.size() == 1)
01488                 return components_a[0];
01489 
01490             return StateSpacePtr(new CompoundStateSpace(components_a, weights_a));
01491         }
01492 
01493         StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name)
01494         {
01495             std::vector<StateSpacePtr> components;
01496             std::vector<double>           weights;
01497 
01498             bool change = false;
01499             if (a)
01500             {
01501                 bool used = false;
01502                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01503                     if (!csm_a->isLocked())
01504                     {
01505                         used = true;
01506                         for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
01507                         {
01508                             if (csm_a->getSubspace(i)->getName() == name)
01509                             {
01510                                 change = true;
01511                                 continue;
01512                             }
01513                             components.push_back(csm_a->getSubspace(i));
01514                             weights.push_back(csm_a->getSubspaceWeight(i));
01515                         }
01516                     }
01517 
01518                 if (!used)
01519                 {
01520                     if (a->getName() != name)
01521                     {
01522                         components.push_back(a);
01523                         weights.push_back(1.0);
01524                     }
01525                     else
01526                         change = true;
01527                 }
01528             }
01529 
01530             if (!change && a)
01531                 return a;
01532 
01533             if (components.size() == 1)
01534                 return components[0];
01535 
01536             return StateSpacePtr(new CompoundStateSpace(components, weights));
01537         }
01538 
01539         StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b)
01540         {
01541             std::vector<StateSpacePtr> components_a;
01542             std::vector<double>           weights_a;
01543             std::vector<StateSpacePtr> components_b;
01544             std::vector<double>           weights_b;
01545 
01546             if (a)
01547             {
01548                 bool used = false;
01549                 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01550                     if (!csm_a->isLocked())
01551                     {
01552                         used = true;
01553                         for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
01554                         {
01555                             components_a.push_back(csm_a->getSubspace(i));
01556                             weights_a.push_back(csm_a->getSubspaceWeight(i));
01557                         }
01558                     }
01559 
01560                 if (!used)
01561                 {
01562                     components_a.push_back(a);
01563                     weights_a.push_back(1.0);
01564                 }
01565             }
01566 
01567             if (b)
01568             {
01569                 bool used = false;
01570                 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01571                     if (!csm_b->isLocked())
01572                     {
01573                         used = true;
01574                         for (unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
01575                         {
01576                             components_b.push_back(csm_b->getSubspace(i));
01577                             weights_b.push_back(csm_b->getSubspaceWeight(i));
01578                         }
01579                     }
01580 
01581                 if (!used)
01582                 {
01583                     components_b.push_back(b);
01584                     weights_b.push_back(1.0);
01585                 }
01586             }
01587 
01588             std::vector<StateSpacePtr> components;
01589             std::vector<double>           weights;
01590 
01591             for (unsigned int i = 0 ; i < components_b.size() ; ++i)
01592             {
01593                 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
01594                     if (components_a[j]->getName() == components_b[i]->getName())
01595                     {
01596                         components.push_back(components_b[i]);
01597                         weights.push_back(std::max(weights_a[j], weights_b[i]));
01598                         break;
01599                     }
01600             }
01601 
01602             if (a && components.size() == components_a.size())
01603                 return a;
01604 
01605             if (b && components.size() == components_b.size())
01606                 return b;
01607 
01608             if (components.size() == 1)
01609                 return components[0];
01610 
01611             return StateSpacePtr(new CompoundStateSpace(components, weights));
01612         }
01613     }
01614 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines