00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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/lexical_cast.hpp>
00041 #include <boost/bind.hpp>
00042 #include <numeric>
00043 #include <limits>
00044 #include <queue>
00045 #include <cmath>
00046 #include <list>
00047
00048 const std::string ompl::base::StateSpace::DEFAULT_PROJECTION_NAME = "";
00049
00051 namespace ompl
00052 {
00053 namespace base
00054 {
00055 struct AllocatedSpaces
00056 {
00057 std::list<StateSpace*> list_;
00058 boost::mutex lock_;
00059 };
00060
00061 static AllocatedSpaces& getAllocatedSpaces(void)
00062 {
00063 static AllocatedSpaces as;
00064 return as;
00065 }
00066 }
00067 }
00069
00070 ompl::base::StateSpace::StateSpace(void)
00071 {
00072
00073 static boost::mutex lock;
00074 static unsigned int m = 0;
00075
00076 lock.lock();
00077 m++;
00078 lock.unlock();
00079
00080 name_ = "Space" + boost::lexical_cast<std::string>(m);
00081 msg_.setPrefix(name_);
00082
00083 longestValidSegment_ = 0.0;
00084 longestValidSegmentFraction_ = 0.01;
00085 longestValidSegmentCountFactor_ = 1;
00086
00087 type_ = STATE_SPACE_UNKNOWN;
00088
00089 maxExtent_ = std::numeric_limits<double>::infinity();
00090
00091 params_.declareParam<double>("longest_valid_segment_fraction", msg_,
00092 boost::bind(&StateSpace::setLongestValidSegmentFraction, this, _1),
00093 boost::bind(&StateSpace::getLongestValidSegmentFraction, this));
00094
00095 params_.declareParam<unsigned int>("valid_segment_count_factor", msg_,
00096 boost::bind(&StateSpace::setValidSegmentCountFactor, this, _1),
00097 boost::bind(&StateSpace::getValidSegmentCountFactor, this));
00098 AllocatedSpaces &as = getAllocatedSpaces();
00099 boost::mutex::scoped_lock smLock(as.lock_);
00100 as.list_.push_back(this);
00101 }
00102
00103 ompl::base::StateSpace::~StateSpace(void)
00104 {
00105 AllocatedSpaces &as = getAllocatedSpaces();
00106 boost::mutex::scoped_lock smLock(as.lock_);
00107 as.list_.remove(this);
00108 }
00109
00111 namespace ompl
00112 {
00113 namespace base
00114 {
00115 static void computeStateSpaceSignatureHelper(const StateSpace *space, std::vector<int> &signature)
00116 {
00117 signature.push_back(space->getType());
00118 signature.push_back(space->getDimension());
00119
00120 if (space->isCompound())
00121 {
00122 unsigned int c = space->as<CompoundStateSpace>()->getSubSpaceCount();
00123 for (unsigned int i = 0 ; i < c ; ++i)
00124 computeStateSpaceSignatureHelper(space->as<CompoundStateSpace>()->getSubSpace(i).get(), signature);
00125 }
00126 }
00127
00128 void computeLocationsHelper(const StateSpace *s, std::vector<StateSpace::ValueLocation> &locationsArray,
00129 std::map<std::string, StateSpace::ValueLocation> &locationsMap, StateSpace::ValueLocation loc)
00130 {
00131 State *test = s->allocState();
00132 if (s->getValueAddressAtIndex(test, 0) != NULL)
00133 {
00134 loc.index = 0;
00135 loc.space = s;
00136 locationsMap[s->getName()] = loc;
00137
00138 if (!s->isCompound())
00139 {
00140 if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
00141 {
00142 const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(0);
00143 if (!name.empty())
00144 locationsMap[name] = loc;
00145 }
00146 locationsArray.push_back(loc);
00147 while (s->getValueAddressAtIndex(test, ++loc.index) != NULL)
00148 {
00149 if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
00150 {
00151 const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(loc.index);
00152 if (!name.empty())
00153 locationsMap[name] = loc;
00154 }
00155 locationsArray.push_back(loc);
00156 }
00157 }
00158 }
00159 s->freeState(test);
00160
00161 if (s->isCompound())
00162 for (unsigned int i = 0 ; i < s->as<base::CompoundStateSpace>()->getSubSpaceCount() ; ++i)
00163 {
00164 loc.chain.push_back(i);
00165 computeLocationsHelper(s->as<base::CompoundStateSpace>()->getSubSpace(i).get(), locationsArray, locationsMap, loc);
00166 loc.chain.pop_back();
00167 }
00168 }
00169
00170 void computeLocationsHelper(const StateSpace *s, std::vector<StateSpace::ValueLocation> &locationsArray,
00171 std::map<std::string, StateSpace::ValueLocation> &locationsMap)
00172 {
00173 locationsArray.clear();
00174 locationsMap.clear();
00175 computeLocationsHelper(s, locationsArray, locationsMap, StateSpace::ValueLocation());
00176 }
00177 }
00178 }
00180
00181 const std::string& ompl::base::StateSpace::getName(void) const
00182 {
00183 return name_;
00184 }
00185
00186 void ompl::base::StateSpace::setName(const std::string &name)
00187 {
00188 name_ = name;
00189 msg_.setPrefix(name_);
00190
00191
00192
00193
00194
00195 if (!valueLocationsInOrder_.empty())
00196 computeLocationsHelper(this, valueLocationsInOrder_, valueLocationsByName_);
00197 }
00198
00199 void ompl::base::StateSpace::computeLocations(void)
00200 {
00201 computeLocationsHelper(this, valueLocationsInOrder_, valueLocationsByName_);
00202 }
00203
00204 void ompl::base::StateSpace::computeSignature(std::vector<int> &signature) const
00205 {
00206 signature.clear();
00207 computeStateSpaceSignatureHelper(this, signature);
00208 signature.insert(signature.begin(), signature.size());
00209 }
00210
00211 void ompl::base::StateSpace::registerProjections(void)
00212 {
00213 }
00214
00215 void ompl::base::StateSpace::setup(void)
00216 {
00217 maxExtent_ = getMaximumExtent();
00218 longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
00219
00220 if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
00221 throw Exception("The longest valid segment for state space " + getName() + " must be positive");
00222
00223 computeLocationsHelper(this, valueLocationsInOrder_, valueLocationsByName_);
00224
00225
00226 std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
00227 registerProjections();
00228 for (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
00229 if (it->second->userConfigured())
00230 {
00231 std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
00232 if (o != projections_.end())
00233 if (!o->second->userConfigured())
00234 projections_[it->first] = it->second;
00235 }
00236
00237
00238 std::vector<std::string> pnames;
00239 params_.getParamNames(pnames);
00240 for (std::vector<std::string>::const_iterator it = pnames.begin() ; it != pnames.end() ; ++it)
00241 if (it->substr(0, 11) == "projection.")
00242 params_.remove(*it);
00243
00244
00245 for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00246 {
00247 it->second->setup();
00248 if (it->first == DEFAULT_PROJECTION_NAME)
00249 params_.include(it->second->params(), "projection");
00250 else
00251 params_.include(it->second->params(), "projection." + it->first);
00252 }
00253 }
00254
00255 double* ompl::base::StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00256 {
00257 return NULL;
00258 }
00259
00260 const double* ompl::base::StateSpace::getValueAddressAtIndex(const State *state, const unsigned int index) const
00261 {
00262 double *val = getValueAddressAtIndex(const_cast<State*>(state), index);
00263 return val;
00264 }
00265
00266 const std::vector<ompl::base::StateSpace::ValueLocation>& ompl::base::StateSpace::getValueLocations(void) const
00267 {
00268 return valueLocationsInOrder_;
00269 }
00270
00271 const std::map<std::string, ompl::base::StateSpace::ValueLocation>& ompl::base::StateSpace::getValueLocationsByName(void) const
00272 {
00273 return valueLocationsByName_;
00274 }
00275
00276 void ompl::base::StateSpace::copyToReals(std::vector<double> &reals, const State *source) const
00277 {
00278 reals.resize(valueLocationsInOrder_.size());
00279 for (std::size_t i = 0 ; i < valueLocationsInOrder_.size() ; ++i)
00280 reals[i] = *getValueAddressAtLocation(source, valueLocationsInOrder_[i]);
00281 }
00282
00283 void ompl::base::StateSpace::copyFromReals(State *destination, const std::vector<double> &reals) const
00284 {
00285 assert(reals.size() == valueLocationsInOrder_.size());
00286 for (std::size_t i = 0 ; i < reals.size() ; ++i)
00287 *getValueAddressAtLocation(destination, valueLocationsInOrder_[i]) = reals[i];
00288 }
00289
00290 double* ompl::base::StateSpace::getValueAddressAtLocation(State *state, const ValueLocation &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 loc.space->getValueAddressAtIndex(state, loc.index);
00296 }
00297
00298 const double* ompl::base::StateSpace::getValueAddressAtLocation(const State *state, const ValueLocation &loc) const
00299 {
00300 std::size_t index = 0;
00301 while (loc.chain.size() > index)
00302 state = state->as<CompoundState>()->components[loc.chain[index++]];
00303 return loc.space->getValueAddressAtIndex(state, loc.index);
00304 }
00305
00306 double* ompl::base::StateSpace::getValueAddressAtName(State *state, const std::string &name) const
00307 {
00308 std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
00309 return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
00310 }
00311
00312 const double* ompl::base::StateSpace::getValueAddressAtName(const State *state, const std::string &name) const
00313 {
00314 std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
00315 return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
00316 }
00317
00318 unsigned int ompl::base::StateSpace::getSerializationLength(void) const
00319 {
00320 return 0;
00321 }
00322
00323 void ompl::base::StateSpace::serialize(void *serialization, const State *state) const
00324 {
00325 }
00326
00327 void ompl::base::StateSpace::deserialize(State *state, const void *serialization) const
00328 {
00329 }
00330
00331 void ompl::base::StateSpace::printState(const State *state, std::ostream &out) const
00332 {
00333 out << "State instance [" << state << ']' << std::endl;
00334 }
00335
00336 void ompl::base::StateSpace::printSettings(std::ostream &out) const
00337 {
00338 out << "StateSpace '" << getName() << "' instance: " << this << std::endl;
00339 printProjections(out);
00340 }
00341
00342 void ompl::base::StateSpace::printProjections(std::ostream &out) const
00343 {
00344 if (projections_.empty())
00345 out << "No registered projections" << std::endl;
00346 else
00347 {
00348 out << "Registered projections:" << std::endl;
00349 for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00350 {
00351 out << " - ";
00352 if (it->first == DEFAULT_PROJECTION_NAME)
00353 out << "<default>";
00354 else
00355 out << it->first;
00356 out << std::endl;
00357 it->second->printSettings(out);
00358 }
00359 }
00360 }
00361
00363 namespace ompl
00364 {
00365 namespace base
00366 {
00367 static bool StateSpaceIncludes(const StateSpace *self, const StateSpace *other)
00368 {
00369 std::queue<const StateSpace*> q;
00370 q.push(self);
00371 while (!q.empty())
00372 {
00373 const StateSpace *m = q.front();
00374 q.pop();
00375 if (m->getName() == other->getName())
00376 return true;
00377 if (m->isCompound())
00378 {
00379 unsigned int c = m->as<CompoundStateSpace>()->getSubSpaceCount();
00380 for (unsigned int i = 0 ; i < c ; ++i)
00381 q.push(m->as<CompoundStateSpace>()->getSubSpace(i).get());
00382 }
00383 }
00384 return false;
00385 }
00386
00387 static bool StateSpaceCovers(const StateSpace *self, const StateSpace *other)
00388 {
00389 if (StateSpaceIncludes(self, other))
00390 return true;
00391 else
00392 if (other->isCompound())
00393 {
00394 unsigned int c = other->as<CompoundStateSpace>()->getSubSpaceCount();
00395 for (unsigned int i = 0 ; i < c ; ++i)
00396 if (!StateSpaceCovers(self, other->as<CompoundStateSpace>()->getSubSpace(i).get()))
00397 return false;
00398 return true;
00399 }
00400 return false;
00401 }
00402 }
00403 }
00404
00406
00407 bool ompl::base::StateSpace::covers(const StateSpacePtr &other) const
00408 {
00409 return StateSpaceCovers(this, other.get());
00410 }
00411
00412 bool ompl::base::StateSpace::includes(const StateSpacePtr &other) const
00413 {
00414 return StateSpaceIncludes(this, other.get());
00415 }
00416
00417 void ompl::base::StateSpace::List(std::ostream &out)
00418 {
00419 AllocatedSpaces &as = getAllocatedSpaces();
00420 boost::mutex::scoped_lock smLock(as.lock_);
00421 for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
00422 out << "@ " << *it << ": " << (*it)->getName() << std::endl;
00423 }
00424
00425 void ompl::base::StateSpace::Diagram(std::ostream &out)
00426 {
00427 AllocatedSpaces &as = getAllocatedSpaces();
00428 boost::mutex::scoped_lock smLock(as.lock_);
00429 out << "digraph StateSpaces {" << std::endl;
00430 for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
00431 {
00432 out << '"' << (*it)->getName() << '"' << std::endl;
00433 for (std::list<StateSpace*>::iterator jt = as.list_.begin() ; jt != as.list_.end(); ++jt)
00434 if (it != jt)
00435 {
00436 if ((*it)->isCompound() && (*it)->as<CompoundStateSpace>()->hasSubSpace((*jt)->getName()))
00437 out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [label=\"" <<
00438 boost::lexical_cast<std::string>((*it)->as<CompoundStateSpace>()->getSubSpaceWeight((*jt)->getName())) <<
00439 "\"];" << std::endl;
00440 else
00441 if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
00442 out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [style=dashed];" << std::endl;
00443 }
00444 }
00445 out << '}' << std::endl;
00446 }
00447
00448 void ompl::base::StateSpace::sanityChecks(void) const
00449 {
00450 sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), ~0);
00451 }
00452
00453 void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int flags) const
00454 {
00455
00456 {
00457 double maxExt = getMaximumExtent();
00458
00459 State *s1 = allocState();
00460 State *s2 = allocState();
00461 StateSamplerPtr ss = allocStateSampler();
00462
00463 for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
00464 {
00465 ss->sampleUniform(s1);
00466 if (flags & STATESPACE_DISTANCE_TO_SELF && distance(s1, s1) > eps)
00467 throw Exception("Distance from a state to itself should be 0");
00468 if (flags & STATESPACE_EQUAL_TO_SELF && !equalStates(s1, s1))
00469 throw Exception("A state should be equal to itself");
00470 ss->sampleUniform(s2);
00471 if (!equalStates(s1, s2))
00472 {
00473 double d12 = distance(s1, s2);
00474 if (flags & STATESPACE_DISTANCE_DIFFERENT_STATES && d12 < zero)
00475 throw Exception("Distance between different states should be above 0");
00476 double d21 = distance(s2, s1);
00477 if (flags & STATESPACE_DISTANCE_SYMMETRIC && fabs(d12 - d21) > eps)
00478 throw Exception("The distance function should be symmetric (A->B=" +
00479 boost::lexical_cast<std::string>(d12) + ", B->A=" +
00480 boost::lexical_cast<std::string>(d21) + ", difference is " +
00481 boost::lexical_cast<std::string>(fabs(d12 - d21)) + ")");
00482 if (flags & STATESPACE_DISTANCE_BOUND)
00483 if (d12 > maxExt + zero)
00484 throw Exception("The distance function should not report values larger than the maximum extent ("+
00485 boost::lexical_cast<std::string>(d12) + " > " + boost::lexical_cast<std::string>(maxExt) + ")");
00486 }
00487 }
00488
00489 freeState(s1);
00490 freeState(s2);
00491 }
00492
00493
00494
00495 if (!isDiscrete() && !isHybrid())
00496 {
00497 State *s1 = allocState();
00498 State *s2 = allocState();
00499 State *s3 = allocState();
00500 StateSamplerPtr ss = allocStateSampler();
00501
00502 for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
00503 {
00504 ss->sampleUniform(s1);
00505 ss->sampleUniform(s2);
00506 ss->sampleUniform(s3);
00507
00508 interpolate(s1, s2, 0.0, s3);
00509 if (flags & STATESPACE_INTERPOLATION && distance(s1, s3) > eps)
00510 throw Exception("Interpolation from a state at time 0 should be not change the original state");
00511
00512 interpolate(s1, s2, 1.0, s3);
00513 if (flags & STATESPACE_INTERPOLATION && distance(s2, s3) > eps)
00514 throw Exception("Interpolation to a state at time 1 should be the same as the final state");
00515
00516 interpolate(s1, s2, 0.5, s3);
00517 double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
00518 if (flags & STATESPACE_TRIANGLE_INEQUALITY && fabs(diff) > eps)
00519 throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle inequality (" +
00520 boost::lexical_cast<std::string>(diff) + " difference)");
00521
00522 interpolate(s3, s2, 0.5, s3);
00523 interpolate(s1, s2, 0.75, s2);
00524
00525 if (flags & STATESPACE_INTERPOLATION && distance(s2, s3) > eps)
00526 throw Exception("Continued interpolation does not work as expected. Please also check that interpolate() works with overlapping memory for its state arguments");
00527 }
00528 freeState(s1);
00529 freeState(s2);
00530 freeState(s3);
00531 }
00532 }
00533
00534 bool ompl::base::StateSpace::hasDefaultProjection(void) const
00535 {
00536 return hasProjection(DEFAULT_PROJECTION_NAME);
00537 }
00538
00539 bool ompl::base::StateSpace::hasProjection(const std::string &name) const
00540 {
00541 return projections_.find(name) != projections_.end();
00542 }
00543
00544 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getDefaultProjection(void) const
00545 {
00546 if (hasDefaultProjection())
00547 return getProjection(DEFAULT_PROJECTION_NAME);
00548 else
00549 {
00550 msg_.error("No default projection is set. Perhaps setup() needs to be called");
00551 return ProjectionEvaluatorPtr();
00552 }
00553 }
00554
00555 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getProjection(const std::string &name) const
00556 {
00557 std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
00558 if (it != projections_.end())
00559 return it->second;
00560 else
00561 {
00562 msg_.error("Projection '" + name + "' is not defined");
00563 return ProjectionEvaluatorPtr();
00564 }
00565 }
00566
00567 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr>& ompl::base::StateSpace::getRegisteredProjections(void) const
00568 {
00569 return projections_;
00570 }
00571
00572 void ompl::base::StateSpace::registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
00573 {
00574 registerProjection(DEFAULT_PROJECTION_NAME, projection);
00575 }
00576
00577 void ompl::base::StateSpace::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
00578 {
00579 if (projection)
00580 projections_[name] = projection;
00581 else
00582 msg_.error("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
00583 }
00584
00585 bool ompl::base::StateSpace::isCompound(void) const
00586 {
00587 return false;
00588 }
00589
00590 bool ompl::base::StateSpace::isDiscrete(void) const
00591 {
00592 return false;
00593 }
00594
00595 bool ompl::base::StateSpace::isHybrid(void) const
00596 {
00597 return false;
00598 }
00599
00600 void ompl::base::StateSpace::setStateSamplerAllocator(const StateSamplerAllocator &ssa)
00601 {
00602 ssa_ = ssa;
00603 }
00604
00605 void ompl::base::StateSpace::clearStateSamplerAllocator(void)
00606 {
00607 ssa_ = StateSamplerAllocator();
00608 }
00609
00610 ompl::base::StateSamplerPtr ompl::base::StateSpace::allocStateSampler(void) const
00611 {
00612 if (ssa_)
00613 return ssa_(this);
00614 else
00615 return allocDefaultStateSampler();
00616 }
00617
00618 void ompl::base::StateSpace::setValidSegmentCountFactor(unsigned int factor)
00619 {
00620 if (factor < 1)
00621 throw Exception("The multiplicative factor for the valid segment count between two states must be strictly positive");
00622 longestValidSegmentCountFactor_ = factor;
00623 }
00624
00625 void ompl::base::StateSpace::setLongestValidSegmentFraction(double segmentFraction)
00626 {
00627 if (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
00628 throw Exception("The fraction of the extent must be larger than 0 and less than 1");
00629 longestValidSegmentFraction_ = segmentFraction;
00630 }
00631
00632 unsigned int ompl::base::StateSpace::getValidSegmentCountFactor(void) const
00633 {
00634 return longestValidSegmentCountFactor_;
00635 }
00636
00637 double ompl::base::StateSpace::getLongestValidSegmentFraction(void) const
00638 {
00639 return longestValidSegmentFraction_;
00640 }
00641
00642 unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
00643 {
00644 return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
00645 }
00646
00647 ompl::base::CompoundStateSpace::CompoundStateSpace(void) : StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
00648 {
00649 setName("Compound" + getName());
00650 }
00651
00652 ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
00653 const std::vector<double> &weights) :
00654 StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
00655 {
00656 if (components.size() != weights.size())
00657 throw Exception("Number of component spaces and weights are not the same");
00658 setName("Compound" + getName());
00659 for (unsigned int i = 0 ; i < components.size() ; ++i)
00660 addSubSpace(components[i], weights[i]);
00661 }
00662
00663 void ompl::base::CompoundStateSpace::addSubSpace(const StateSpacePtr &component, double weight)
00664 {
00665 if (locked_)
00666 throw Exception("This state space is locked. No further components can be added");
00667 if (weight < 0.0)
00668 throw Exception("Subspace weight cannot be negative");
00669 components_.push_back(component);
00670 weights_.push_back(weight);
00671 weightSum_ += weight;
00672 componentCount_ = components_.size();
00673 }
00674
00675 bool ompl::base::CompoundStateSpace::isCompound(void) const
00676 {
00677 return true;
00678 }
00679
00680 bool ompl::base::CompoundStateSpace::isHybrid(void) const
00681 {
00682 bool c = false;
00683 bool d = false;
00684 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00685 {
00686 if (components_[i]->isHybrid())
00687 return true;
00688 if (components_[i]->isDiscrete())
00689 d = true;
00690 else
00691 c = true;
00692 }
00693 return c && d;
00694 }
00695
00696 unsigned int ompl::base::CompoundStateSpace::getSubSpaceCount(void) const
00697 {
00698 return componentCount_;
00699 }
00700
00701 const ompl::base::StateSpacePtr& ompl::base::CompoundStateSpace::getSubSpace(const unsigned int index) const
00702 {
00703 if (componentCount_ > index)
00704 return components_[index];
00705 else
00706 throw Exception("Subspace index does not exist");
00707 }
00708
00709 bool ompl::base::CompoundStateSpace::hasSubSpace(const std::string &name) const
00710 {
00711 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00712 if (components_[i]->getName() == name)
00713 return true;
00714 return false;
00715 }
00716
00717 unsigned int ompl::base::CompoundStateSpace::getSubSpaceIndex(const std::string& name) const
00718 {
00719 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00720 if (components_[i]->getName() == name)
00721 return i;
00722 throw Exception("Subspace " + name + " does not exist");
00723 }
00724
00725 const ompl::base::StateSpacePtr& ompl::base::CompoundStateSpace::getSubSpace(const std::string& name) const
00726 {
00727 return components_[getSubSpaceIndex(name)];
00728 }
00729
00730 double ompl::base::CompoundStateSpace::getSubSpaceWeight(const unsigned int index) const
00731 {
00732 if (componentCount_ > index)
00733 return weights_[index];
00734 else
00735 throw Exception("Subspace index does not exist");
00736 }
00737
00738 double ompl::base::CompoundStateSpace::getSubSpaceWeight(const std::string &name) const
00739 {
00740 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00741 if (components_[i]->getName() == name)
00742 return weights_[i];
00743 throw Exception("Subspace " + name + " does not exist");
00744 }
00745
00746 void ompl::base::CompoundStateSpace::setSubSpaceWeight(const unsigned int index, double weight)
00747 {
00748 if (weight < 0.0)
00749 throw Exception("Subspace weight cannot be negative");
00750 if (componentCount_ > index)
00751 {
00752 weightSum_ += weight - weights_[index];
00753 weights_[index] = weight;
00754 }
00755 else
00756 throw Exception("Subspace index does not exist");
00757 }
00758
00759 void ompl::base::CompoundStateSpace::setSubSpaceWeight(const std::string &name, double weight)
00760 {
00761 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00762 if (components_[i]->getName() == name)
00763 {
00764 setSubSpaceWeight(i, weight);
00765 return;
00766 }
00767 throw Exception("Subspace " + name + " does not exist");
00768 }
00769
00770 const std::vector<ompl::base::StateSpacePtr>& ompl::base::CompoundStateSpace::getSubSpaces(void) const
00771 {
00772 return components_;
00773 }
00774
00775 const std::vector<double>& ompl::base::CompoundStateSpace::getSubSpaceWeights(void) const
00776 {
00777 return weights_;
00778 }
00779
00780 unsigned int ompl::base::CompoundStateSpace::getDimension(void) const
00781 {
00782 unsigned int dim = 0;
00783 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00784 dim += components_[i]->getDimension();
00785 return dim;
00786 }
00787
00788 double ompl::base::CompoundStateSpace::getMaximumExtent(void) const
00789 {
00790 double e = 0.0;
00791 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00792 if (weights_[i] >= std::numeric_limits<double>::epsilon())
00793 e += weights_[i] * components_[i]->getMaximumExtent();
00794 return e;
00795 }
00796
00797 void ompl::base::CompoundStateSpace::enforceBounds(State *state) const
00798 {
00799 CompoundState *cstate = static_cast<CompoundState*>(state);
00800 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00801 components_[i]->enforceBounds(cstate->components[i]);
00802 }
00803
00804 bool ompl::base::CompoundStateSpace::satisfiesBounds(const State *state) const
00805 {
00806 const CompoundState *cstate = static_cast<const CompoundState*>(state);
00807 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00808 if (!components_[i]->satisfiesBounds(cstate->components[i]))
00809 return false;
00810 return true;
00811 }
00812
00813 void ompl::base::CompoundStateSpace::copyState(State *destination, const State *source) const
00814 {
00815 CompoundState *cdest = static_cast<CompoundState*>(destination);
00816 const CompoundState *csrc = static_cast<const CompoundState*>(source);
00817 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00818 components_[i]->copyState(cdest->components[i], csrc->components[i]);
00819 }
00820
00821 unsigned int ompl::base::CompoundStateSpace::getSerializationLength(void) const
00822 {
00823 unsigned int l = 0;
00824 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00825 l += components_[i]->getSerializationLength();
00826 return l;
00827 }
00828
00829 void ompl::base::CompoundStateSpace::serialize(void *serialization, const State *state) const
00830 {
00831 const CompoundState *cstate = static_cast<const CompoundState*>(state);
00832 unsigned int l = 0;
00833 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00834 {
00835 components_[i]->serialize(reinterpret_cast<char*>(serialization) + l, cstate->components[i]);
00836 l += components_[i]->getSerializationLength();
00837 }
00838 }
00839
00840 void ompl::base::CompoundStateSpace::deserialize(State *state, const void *serialization) const
00841 {
00842 CompoundState *cstate = static_cast<CompoundState*>(state);
00843 unsigned int l = 0;
00844 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00845 {
00846 components_[i]->deserialize(cstate->components[i], reinterpret_cast<const char*>(serialization) + l);
00847 l += components_[i]->getSerializationLength();
00848 }
00849 }
00850
00851 double ompl::base::CompoundStateSpace::distance(const State *state1, const State *state2) const
00852 {
00853 const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00854 const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00855 double dist = 0.0;
00856 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00857 dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
00858 return dist;
00859 }
00860
00861 void ompl::base::CompoundStateSpace::setLongestValidSegmentFraction(double segmentFraction)
00862 {
00863 StateSpace::setLongestValidSegmentFraction(segmentFraction);
00864 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00865 components_[i]->setLongestValidSegmentFraction(segmentFraction);
00866 }
00867
00868 unsigned int ompl::base::CompoundStateSpace::validSegmentCount(const State *state1, const State *state2) const
00869 {
00870 const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00871 const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00872 unsigned int sc = 0;
00873 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00874 {
00875 unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
00876 if (sci > sc)
00877 sc = sci;
00878 }
00879 return sc;
00880 }
00881
00882 bool ompl::base::CompoundStateSpace::equalStates(const State *state1, const State *state2) const
00883 {
00884 const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00885 const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00886 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00887 if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
00888 return false;
00889 return true;
00890 }
00891
00892 void ompl::base::CompoundStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00893 {
00894 const CompoundState *cfrom = static_cast<const CompoundState*>(from);
00895 const CompoundState *cto = static_cast<const CompoundState*>(to);
00896 CompoundState *cstate = static_cast<CompoundState*>(state);
00897 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00898 components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
00899 }
00900
00901 ompl::base::StateSamplerPtr ompl::base::CompoundStateSpace::allocDefaultStateSampler(void) const
00902 {
00903 CompoundStateSampler *ss = new CompoundStateSampler(this);
00904 if (weightSum_ < std::numeric_limits<double>::epsilon())
00905 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00906 ss->addSampler(components_[i]->allocStateSampler(), 1.0);
00907 else
00908 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00909 ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / weightSum_);
00910 return StateSamplerPtr(ss);
00911 }
00912
00913 ompl::base::State* ompl::base::CompoundStateSpace::allocState(void) const
00914 {
00915 CompoundState *state = new CompoundState();
00916 allocStateComponents(state);
00917 return static_cast<State*>(state);
00918 }
00919
00920 void ompl::base::CompoundStateSpace::allocStateComponents(CompoundState *state) const
00921 {
00922 state->components = new State*[componentCount_];
00923 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00924 state->components[i] = components_[i]->allocState();
00925 }
00926
00927 void ompl::base::CompoundStateSpace::freeState(State *state) const
00928 {
00929 CompoundState *cstate = static_cast<CompoundState*>(state);
00930 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00931 components_[i]->freeState(cstate->components[i]);
00932 delete[] cstate->components;
00933 delete cstate;
00934 }
00935
00936 void ompl::base::CompoundStateSpace::lock(void)
00937 {
00938 locked_ = true;
00939 }
00940
00941 bool ompl::base::CompoundStateSpace::isLocked(void) const
00942 {
00943 return locked_;
00944 }
00945
00946 double* ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00947 {
00948 CompoundState *cstate = static_cast<CompoundState*>(state);
00949 unsigned int idx = 0;
00950
00951 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00952 for (unsigned int j = 0 ; j <= index ; ++j)
00953 {
00954 double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
00955 if (va)
00956 {
00957 if (idx == index)
00958 return va;
00959 else
00960 idx++;
00961 }
00962 else
00963 break;
00964 }
00965 return NULL;
00966 }
00967
00968 void ompl::base::CompoundStateSpace::printState(const State *state, std::ostream &out) const
00969 {
00970 out << "Compound state [" << std::endl;
00971 const CompoundState *cstate = static_cast<const CompoundState*>(state);
00972 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00973 components_[i]->printState(cstate->components[i], out);
00974 out << "]" << std::endl;
00975 }
00976
00977 void ompl::base::CompoundStateSpace::printSettings(std::ostream &out) const
00978 {
00979 out << "Compound state space '" << getName() << "' of dimension " << getDimension() << (isLocked() ? " (locked)" : "") << " [" << std::endl;
00980 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00981 {
00982 components_[i]->printSettings(out);
00983 out << " of weight " << weights_[i] << std::endl;
00984 }
00985 out << "]" << std::endl;
00986 printProjections(out);
00987 }
00988
00989 void ompl::base::CompoundStateSpace::setup(void)
00990 {
00991 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00992 components_[i]->setup();
00993
00994 StateSpace::setup();
00995 }
00996
00997 void ompl::base::CompoundStateSpace::computeLocations(void)
00998 {
00999 StateSpace::computeLocations();
01000 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
01001 components_[i]->computeLocations();
01002 }
01003
01004 namespace ompl
01005 {
01006 namespace base
01007 {
01008
01010
01011 static const int NO_DATA_COPIED = 0;
01012 static const int SOME_DATA_COPIED = 1;
01013 static const int ALL_DATA_COPIED = 2;
01014
01016
01017
01018 int copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
01019 {
01020
01021 if (destS->getName() == sourceS->getName())
01022 {
01023 if (dest != source)
01024 destS->copyState(dest, source);
01025 return ALL_DATA_COPIED;
01026 }
01027
01028 int result = NO_DATA_COPIED;
01029
01030
01031 if (destS->isCompound())
01032 {
01033 const CompoundStateSpace *compoundDestS = destS->as<CompoundStateSpace>();
01034 CompoundState *compoundDest = dest->as<CompoundState>();
01035
01036
01037 for (unsigned int i = 0 ; i < compoundDestS->getSubSpaceCount() ; ++i)
01038 if (compoundDestS->getSubSpace(i)->getName() == sourceS->getName())
01039 {
01040 if (compoundDest->components[i] != source)
01041 compoundDestS->getSubSpace(i)->copyState(compoundDest->components[i], source);
01042 return ALL_DATA_COPIED;
01043 }
01044
01045
01046
01047 for (unsigned int i = 0 ; i < compoundDestS->getSubSpaceCount() ; ++i)
01048 {
01049 int res = copyStateData(compoundDestS->getSubSpace(i), compoundDest->components[i], sourceS, source);
01050
01051 if (res != NO_DATA_COPIED)
01052 result = SOME_DATA_COPIED;
01053
01054
01055 if (res == ALL_DATA_COPIED)
01056 return ALL_DATA_COPIED;
01057 }
01058 }
01059
01060
01061
01062 if (sourceS->isCompound())
01063 {
01064 const CompoundStateSpace *compoundSourceS = sourceS->as<CompoundStateSpace>();
01065 const CompoundState *compoundSource = source->as<CompoundState>();
01066
01067 unsigned int copiedComponents = 0;
01068
01069
01070 for (unsigned int i = 0 ; i < compoundSourceS->getSubSpaceCount() ; ++i)
01071 {
01072 int res = copyStateData(destS, dest, compoundSourceS->getSubSpace(i), compoundSource->components[i]);
01073 if (res == ALL_DATA_COPIED)
01074 copiedComponents++;
01075 if (res)
01076 result = SOME_DATA_COPIED;
01077 }
01078
01079
01080 if (copiedComponents == compoundSourceS->getSubSpaceCount())
01081 result = ALL_DATA_COPIED;
01082 }
01083
01084 return result;
01085 }
01086
01088 inline bool StateSpaceHasContent(const StateSpacePtr &m)
01089 {
01090 if (!m)
01091 return false;
01092 if (m->getDimension() == 0 && m->getType() == STATE_SPACE_UNKNOWN && m->isCompound())
01093 {
01094 const unsigned int nc = m->as<CompoundStateSpace>()->getSubSpaceCount();
01095 for (unsigned int i = 0 ; i < nc ; ++i)
01096 if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubSpace(i)))
01097 return true;
01098 return false;
01099 }
01100 return true;
01101 }
01103
01104 StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b)
01105 {
01106 if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
01107 return b;
01108
01109 if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
01110 return a;
01111
01112 std::vector<StateSpacePtr> components;
01113 std::vector<double> weights;
01114
01115 bool change = false;
01116 if (a)
01117 {
01118 bool used = false;
01119 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01120 if (!csm_a->isLocked())
01121 {
01122 used = true;
01123 for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
01124 {
01125 components.push_back(csm_a->getSubSpace(i));
01126 weights.push_back(csm_a->getSubSpaceWeight(i));
01127 }
01128 }
01129
01130 if (!used)
01131 {
01132 components.push_back(a);
01133 weights.push_back(1.0);
01134 }
01135 }
01136 if (b)
01137 {
01138 bool used = false;
01139 unsigned int size = components.size();
01140
01141 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01142 if (!csm_b->isLocked())
01143 {
01144 used = true;
01145 for (unsigned int i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
01146 {
01147 bool ok = true;
01148 for (unsigned int j = 0 ; j < size ; ++j)
01149 if (components[j]->getName() == csm_b->getSubSpace(i)->getName())
01150 {
01151 ok = false;
01152 break;
01153 }
01154 if (ok)
01155 {
01156 components.push_back(csm_b->getSubSpace(i));
01157 weights.push_back(csm_b->getSubSpaceWeight(i));
01158 change = true;
01159 }
01160 }
01161 if (components.size() == csm_b->getSubSpaceCount())
01162 return b;
01163 }
01164
01165 if (!used)
01166 {
01167 bool ok = true;
01168 for (unsigned int j = 0 ; j < size ; ++j)
01169 if (components[j]->getName() == b->getName())
01170 {
01171 ok = false;
01172 break;
01173 }
01174 if (ok)
01175 {
01176 components.push_back(b);
01177 weights.push_back(1.0);
01178 change = true;
01179 }
01180 }
01181 }
01182
01183 if (!change && a)
01184 return a;
01185
01186 if (components.size() == 1)
01187 return components[0];
01188
01189 return StateSpacePtr(new CompoundStateSpace(components, weights));
01190 }
01191
01192 StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
01193 {
01194 std::vector<StateSpacePtr> components_a;
01195 std::vector<double> weights_a;
01196 std::vector<StateSpacePtr> components_b;
01197
01198 if (a)
01199 {
01200 bool used = false;
01201 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01202 if (!csm_a->isLocked())
01203 {
01204 used = true;
01205 for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
01206 {
01207 components_a.push_back(csm_a->getSubSpace(i));
01208 weights_a.push_back(csm_a->getSubSpaceWeight(i));
01209 }
01210 }
01211
01212 if (!used)
01213 {
01214 components_a.push_back(a);
01215 weights_a.push_back(1.0);
01216 }
01217 }
01218
01219 if (b)
01220 {
01221 bool used = false;
01222 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01223 if (!csm_b->isLocked())
01224 {
01225 used = true;
01226 for (unsigned int i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
01227 components_b.push_back(csm_b->getSubSpace(i));
01228 }
01229 if (!used)
01230 components_b.push_back(b);
01231 }
01232
01233 bool change = false;
01234 for (unsigned int i = 0 ; i < components_b.size() ; ++i)
01235 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
01236 if (components_a[j]->getName() == components_b[i]->getName())
01237 {
01238 components_a.erase(components_a.begin() + j);
01239 weights_a.erase(weights_a.begin() + j);
01240 change = true;
01241 break;
01242 }
01243
01244 if (!change && a)
01245 return a;
01246
01247 if (components_a.size() == 1)
01248 return components_a[0];
01249
01250 return StateSpacePtr(new CompoundStateSpace(components_a, weights_a));
01251 }
01252
01253 StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name)
01254 {
01255 std::vector<StateSpacePtr> components;
01256 std::vector<double> weights;
01257
01258 bool change = false;
01259 if (a)
01260 {
01261 bool used = false;
01262 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01263 if (!csm_a->isLocked())
01264 {
01265 used = true;
01266 for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
01267 {
01268 if (csm_a->getSubSpace(i)->getName() == name)
01269 {
01270 change = true;
01271 continue;
01272 }
01273 components.push_back(csm_a->getSubSpace(i));
01274 weights.push_back(csm_a->getSubSpaceWeight(i));
01275 }
01276 }
01277
01278 if (!used)
01279 {
01280 if (a->getName() != name)
01281 {
01282 components.push_back(a);
01283 weights.push_back(1.0);
01284 }
01285 else
01286 change = true;
01287 }
01288 }
01289
01290 if (!change && a)
01291 return a;
01292
01293 if (components.size() == 1)
01294 return components[0];
01295
01296 return StateSpacePtr(new CompoundStateSpace(components, weights));
01297 }
01298
01299 StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b)
01300 {
01301 std::vector<StateSpacePtr> components_a;
01302 std::vector<double> weights_a;
01303 std::vector<StateSpacePtr> components_b;
01304 std::vector<double> weights_b;
01305
01306 if (a)
01307 {
01308 bool used = false;
01309 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01310 if (!csm_a->isLocked())
01311 {
01312 used = true;
01313 for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
01314 {
01315 components_a.push_back(csm_a->getSubSpace(i));
01316 weights_a.push_back(csm_a->getSubSpaceWeight(i));
01317 }
01318 }
01319
01320 if (!used)
01321 {
01322 components_a.push_back(a);
01323 weights_a.push_back(1.0);
01324 }
01325 }
01326
01327 if (b)
01328 {
01329 bool used = false;
01330 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01331 if (!csm_b->isLocked())
01332 {
01333 used = true;
01334 for (unsigned int i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
01335 {
01336 components_b.push_back(csm_b->getSubSpace(i));
01337 weights_b.push_back(csm_b->getSubSpaceWeight(i));
01338 }
01339 }
01340
01341 if (!used)
01342 {
01343 components_b.push_back(b);
01344 weights_b.push_back(1.0);
01345 }
01346 }
01347
01348 std::vector<StateSpacePtr> components;
01349 std::vector<double> weights;
01350
01351 for (unsigned int i = 0 ; i < components_b.size() ; ++i)
01352 {
01353 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
01354 if (components_a[j]->getName() == components_b[i]->getName())
01355 {
01356 components.push_back(components_b[i]);
01357 weights.push_back(std::max(weights_a[j], weights_b[i]));
01358 break;
01359 }
01360 }
01361
01362 if (a && components.size() == components_a.size())
01363 return a;
01364
01365 if (b && components.size() == components_b.size())
01366 return b;
01367
01368 if (components.size() == 1)
01369 return components[0];
01370
01371 return StateSpacePtr(new CompoundStateSpace(components, weights));
01372 }
01373 }
01374 }