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