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 }