35 #include "ompl/base/StateSpace.h"
36 #include "ompl/util/Exception.h"
37 #include "ompl/tools/config/MagicConstants.h"
38 #include "ompl/base/spaces/RealVectorStateSpace.h"
39 #include <boost/thread/mutex.hpp>
40 #include <boost/lexical_cast.hpp>
41 #include <boost/bind.hpp>
56 struct AllocatedSpaces
58 std::list<StateSpace*> list_;
62 static AllocatedSpaces& getAllocatedSpaces(
void)
64 static AllocatedSpaces as;
74 static boost::mutex lock;
75 static unsigned int m = 0;
81 name_ =
"Space" + boost::lexical_cast<std::string>(m);
89 maxExtent_ = std::numeric_limits<double>::infinity();
98 AllocatedSpaces &
as = getAllocatedSpaces();
99 boost::mutex::scoped_lock smLock(as.lock_);
100 as.list_.push_back(
this);
103 ompl::base::StateSpace::~StateSpace(
void)
105 AllocatedSpaces &as = getAllocatedSpaces();
106 boost::mutex::scoped_lock smLock(as.lock_);
107 as.list_.remove(
this);
115 static void computeStateSpaceSignatureHelper(
const StateSpace *space, std::vector<int> &signature)
117 signature.push_back(space->getType());
118 signature.push_back(space->getDimension());
120 if (space->isCompound())
122 unsigned int c = space->as<CompoundStateSpace>()->getSubspaceCount();
123 for (
unsigned int i = 0 ; i < c ; ++i)
124 computeStateSpaceSignatureHelper(space->as<CompoundStateSpace>()->getSubspace(i).get(), signature);
128 void computeLocationsHelper(
const StateSpace *s,
129 std::map<std::string, StateSpace::SubstateLocation> &substateMap,
130 std::vector<StateSpace::ValueLocation> &locationsArray,
131 std::map<std::string, StateSpace::ValueLocation> &locationsMap, StateSpace::ValueLocation loc)
133 loc.stateLocation.space = s;
134 substateMap[s->getName()] = loc.stateLocation;
135 State *test = s->allocState();
136 if (s->getValueAddressAtIndex(test, 0) != NULL)
139 locationsMap[s->getName()] = loc;
141 if (!s->isCompound())
145 const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(0);
147 locationsMap[name] = loc;
149 locationsArray.push_back(loc);
150 while (s->getValueAddressAtIndex(test, ++loc.index) != NULL)
154 const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(loc.index);
156 locationsMap[name] = loc;
158 locationsArray.push_back(loc);
165 for (
unsigned int i = 0 ; i < s->as<base::CompoundStateSpace>()->getSubspaceCount() ; ++i)
167 loc.stateLocation.chain.push_back(i);
168 computeLocationsHelper(s->as<base::CompoundStateSpace>()->getSubspace(i).get(), substateMap, locationsArray, locationsMap, loc);
169 loc.stateLocation.chain.pop_back();
173 void computeLocationsHelper(
const StateSpace *s,
174 std::map<std::string, StateSpace::SubstateLocation> &substateMap,
175 std::vector<StateSpace::ValueLocation> &locationsArray,
176 std::map<std::string, StateSpace::ValueLocation> &locationsMap)
179 locationsArray.clear();
180 locationsMap.clear();
181 computeLocationsHelper(s, substateMap, locationsArray, locationsMap, StateSpace::ValueLocation());
200 if (!valueLocationsInOrder_.empty())
201 computeLocationsHelper(
this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
206 computeLocationsHelper(
this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
212 computeStateSpaceSignatureHelper(
this, signature);
213 signature.insert(signature.begin(), signature.size());
222 maxExtent_ = getMaximumExtent();
223 longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
225 if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
226 throw Exception(
"The longest valid segment for state space " + getName() +
" must be positive");
228 computeLocationsHelper(
this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
231 std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
232 registerProjections();
233 for (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
234 if (it->second->userConfigured())
236 std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
237 if (o != projections_.end())
238 if (!o->second->userConfigured())
239 projections_[it->first] = it->second;
243 std::vector<std::string> pnames;
244 params_.getParamNames(pnames);
245 for (std::vector<std::string>::const_iterator it = pnames.begin() ; it != pnames.end() ; ++it)
246 if (it->substr(0, 11) ==
"projection.")
250 for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
253 if (it->first == DEFAULT_PROJECTION_NAME)
254 params_.include(it->second->params(),
"projection");
256 params_.include(it->second->params(),
"projection." + it->first);
262 return substateLocationsByName_;
267 std::size_t index = 0;
268 while (loc.
chain.size() > index)
275 std::size_t index = 0;
276 while (loc.
chain.size() > index)
288 double *val = getValueAddressAtIndex(const_cast<State*>(state), index);
294 return valueLocationsInOrder_;
299 return valueLocationsByName_;
304 reals.resize(valueLocationsInOrder_.size());
305 for (std::size_t i = 0 ; i < valueLocationsInOrder_.size() ; ++i)
306 reals[i] = *getValueAddressAtLocation(source, valueLocationsInOrder_[i]);
311 assert(reals.size() == valueLocationsInOrder_.size());
312 for (std::size_t i = 0 ; i < reals.size() ; ++i)
313 *getValueAddressAtLocation(destination, valueLocationsInOrder_[i]) = reals[i];
318 std::size_t index = 0;
326 std::size_t index = 0;
334 std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
335 return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
340 std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
341 return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
359 out <<
"State instance [" << state <<
']' << std::endl;
364 out <<
"StateSpace '" << getName() <<
"' instance: " <<
this << std::endl;
365 printProjections(out);
370 if (projections_.empty())
371 out <<
"No registered projections" << std::endl;
374 out <<
"Registered projections:" << std::endl;
375 for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
378 if (it->first == DEFAULT_PROJECTION_NAME)
383 it->second->printSettings(out);
395 std::queue<const StateSpace*> q;
406 for (
unsigned int i = 0 ; i < c ; ++i)
413 static bool StateSpaceCovers(
const StateSpace *
self,
const StateSpace *other)
415 if (StateSpaceIncludes(
self, other))
418 if (other->isCompound())
420 unsigned int c = other->as<CompoundStateSpace>()->getSubspaceCount();
421 for (
unsigned int i = 0 ; i < c ; ++i)
422 if (!StateSpaceCovers(
self, other->as<CompoundStateSpace>()->getSubspace(i).get()))
429 struct CompareSubstateLocation
431 bool operator()(
const StateSpace::SubstateLocation &a,
const StateSpace::SubstateLocation &b)
const
433 if (a.space->getDimension() != b.space->getDimension())
434 return a.space->getDimension() > b.space->getDimension();
435 return a.space->getName() > b.space->getName();
446 return StateSpaceCovers(
this, other.get());
451 return StateSpaceIncludes(
this, other.get());
456 return StateSpaceCovers(
this, other);
461 return StateSpaceIncludes(
this, other);
466 getCommonSubspaces(other.get(), subspaces);
471 std::set<StateSpace::SubstateLocation, CompareSubstateLocation> intersection;
473 for (std::map<std::string, StateSpace::SubstateLocation>::const_iterator it = substateLocationsByName_.begin() ; it != substateLocationsByName_.end() ; ++it)
475 if (S.find(it->first) != S.end())
476 intersection.insert(it->second);
483 for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator it = intersection.begin() ; it != intersection.end() ; ++it)
484 for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator jt = intersection.begin() ; jt != intersection.end() ; ++jt)
486 if (StateSpaceCovers(it->space, jt->space))
488 intersection.erase(jt);
494 for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator it = intersection.begin() ; it != intersection.end() ; ++it)
495 subspaces.push_back(it->space->getName());
500 AllocatedSpaces &as = getAllocatedSpaces();
501 boost::mutex::scoped_lock smLock(as.lock_);
502 for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
503 out <<
"@ " << *it <<
": " << (*it)->getName() << std::endl;
508 std::queue<const StateSpace*> q;
514 out <<
"@ " << m <<
": " << m->
getName() << std::endl;
518 for (
unsigned int i = 0 ; i < c ; ++i)
526 out <<
"digraph StateSpace {" << std::endl;
527 out <<
'"' << getName() <<
'"' << std::endl;
529 std::queue<const StateSpace*> q;
538 for (
unsigned int i = 0 ; i < c ; ++i)
542 out <<
'"' << m->
getName() <<
"\" -> \"" << s->
getName() <<
"\" [label=\"" <<
543 boost::lexical_cast<std::string>(m->
as<
CompoundStateSpace>()->getSubspaceWeight(i)) <<
"\"];" << std::endl;
548 out <<
'}' << std::endl;
553 AllocatedSpaces &as = getAllocatedSpaces();
554 boost::mutex::scoped_lock smLock(as.lock_);
555 out <<
"digraph StateSpaces {" << std::endl;
556 for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
558 out <<
'"' << (*it)->getName() <<
'"' << std::endl;
559 for (std::list<StateSpace*>::iterator jt = as.list_.begin() ; jt != as.list_.end(); ++jt)
562 if ((*it)->isCompound() && (*it)->as<
CompoundStateSpace>()->hasSubspace((*jt)->getName()))
563 out <<
'"' << (*it)->getName() <<
"\" -> \"" << (*jt)->getName() <<
"\" [label=\"" <<
564 boost::lexical_cast<std::string>((*it)->as<
CompoundStateSpace>()->getSubspaceWeight((*jt)->getName())) <<
567 if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
568 out <<
'"' << (*it)->getName() <<
"\" -> \"" << (*jt)->getName() <<
"\" [style=dashed];" << std::endl;
571 out <<
'}' << std::endl;
576 sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), ~0);
582 double maxExt = getMaximumExtent();
584 State *s1 = allocState();
585 State *s2 = allocState();
587 char *serialization = NULL;
588 if ((flags & STATESPACE_SERIALIZATION) && getSerializationLength() > 0)
589 serialization =
new char[getSerializationLength()];
592 ss->sampleUniform(s1);
593 if (distance(s1, s1) > eps)
594 throw Exception(
"Distance from a state to itself should be 0");
595 if (!equalStates(s1, s1))
596 throw Exception(
"A state should be equal to itself");
597 if ((flags & STATESPACE_RESPECT_BOUNDS) && !satisfiesBounds(s1))
598 throw Exception(
"Sampled states should be within bounds");
600 if (!equalStates(s1, s2))
601 throw Exception(
"Copy of a state is not the same as the original state. copyState() may not work correctly.");
602 if (flags & STATESPACE_ENFORCE_BOUNDS_NO_OP)
605 if (!equalStates(s1, s2))
606 throw Exception(
"enforceBounds() seems to modify states that are in fact within bounds.");
608 if (flags & STATESPACE_SERIALIZATION)
610 ss->sampleUniform(s2);
611 serialize(serialization, s1);
612 deserialize(s2, serialization);
613 if (!equalStates(s1, s2))
614 throw Exception(
"Serialization/deserialization operations do not seem to work as expected.");
616 ss->sampleUniform(s2);
617 if (!equalStates(s1, s2))
619 double d12 = distance(s1, s2);
620 if ((flags & STATESPACE_DISTANCE_DIFFERENT_STATES) && d12 < zero)
621 throw Exception(
"Distance between different states should be above 0");
622 double d21 = distance(s2, s1);
623 if ((flags & STATESPACE_DISTANCE_SYMMETRIC) && fabs(d12 - d21) > eps)
624 throw Exception(
"The distance function should be symmetric (A->B=" +
625 boost::lexical_cast<std::string>(d12) +
", B->A=" +
626 boost::lexical_cast<std::string>(d21) +
", difference is " +
627 boost::lexical_cast<std::string>(fabs(d12 - d21)) +
")");
628 if (flags & STATESPACE_DISTANCE_BOUND)
629 if (d12 > maxExt + zero)
630 throw Exception(
"The distance function should not report values larger than the maximum extent ("+
631 boost::lexical_cast<std::string>(d12) +
" > " + boost::lexical_cast<std::string>(maxExt) +
")");
635 delete[] serialization;
642 if (!isDiscrete() && !isHybrid())
644 State *s1 = allocState();
645 State *s2 = allocState();
646 State *s3 = allocState();
651 ss->sampleUniform(s1);
652 ss->sampleUniform(s2);
653 ss->sampleUniform(s3);
655 interpolate(s1, s2, 0.0, s3);
656 if ((flags & STATESPACE_INTERPOLATION) && distance(s1, s3) > eps)
657 throw Exception(
"Interpolation from a state at time 0 should be not change the original state");
659 interpolate(s1, s2, 1.0, s3);
660 if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
661 throw Exception(
"Interpolation to a state at time 1 should be the same as the final state");
663 interpolate(s1, s2, 0.5, s3);
664 double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
665 if ((flags & STATESPACE_TRIANGLE_INEQUALITY) && fabs(diff) > eps)
666 throw Exception(
"Interpolation to midpoint state does not lead to distances that satisfy the triangle inequality (" +
667 boost::lexical_cast<std::string>(diff) +
" difference)");
669 interpolate(s3, s2, 0.5, s3);
670 interpolate(s1, s2, 0.75, s2);
672 if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
673 throw Exception(
"Continued interpolation does not work as expected. Please also check that interpolate() works with overlapping memory for its state arguments");
683 return hasProjection(DEFAULT_PROJECTION_NAME);
688 return projections_.find(name) != projections_.end();
693 if (hasDefaultProjection())
694 return getProjection(DEFAULT_PROJECTION_NAME);
697 logError(
"No default projection is set. Perhaps setup() needs to be called");
704 std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
705 if (it != projections_.end())
709 logError(
"Projection '%s' is not defined", name.c_str());
721 registerProjection(DEFAULT_PROJECTION_NAME, projection);
727 projections_[name] = projection;
729 logError(
"Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
762 return allocDefaultStateSampler();
767 return allocSubspaceStateSampler(subspace.get());
772 if (subspace->
getName() == getName())
773 return allocStateSampler();
780 throw Exception(
"The multiplicative factor for the valid segment count between two states must be strictly positive");
781 longestValidSegmentCountFactor_ = factor;
786 if (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
787 throw Exception(
"The fraction of the extent must be larger than 0 and less than 1");
788 longestValidSegmentFraction_ = segmentFraction;
793 return longestValidSegmentCountFactor_;
798 return longestValidSegmentFraction_;
803 return longestValidSegmentCountFactor_ * (
unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
812 const std::vector<double> &weights) :
813 StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
815 if (components.size() != weights.size())
816 throw Exception(
"Number of component spaces and weights are not the same");
818 for (
unsigned int i = 0 ; i < components.size() ; ++i)
825 throw Exception(
"This state space is locked. No further components can be added");
827 throw Exception(
"Subspace weight cannot be negative");
828 components_.push_back(component);
829 weights_.push_back(weight);
830 weightSum_ += weight;
831 componentCount_ = components_.size();
843 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
845 if (components_[i]->isHybrid())
847 if (components_[i]->isDiscrete())
857 return componentCount_;
862 if (componentCount_ > index)
863 return components_[index];
865 throw Exception(
"Subspace index does not exist");
870 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
871 if (components_[i]->getName() == name)
878 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
879 if (components_[i]->getName() == name)
881 throw Exception(
"Subspace " + name +
" does not exist");
886 return components_[getSubspaceIndex(name)];
891 if (componentCount_ > index)
892 return weights_[index];
894 throw Exception(
"Subspace index does not exist");
899 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
900 if (components_[i]->getName() == name)
902 throw Exception(
"Subspace " + name +
" does not exist");
908 throw Exception(
"Subspace weight cannot be negative");
909 if (componentCount_ > index)
911 weightSum_ += weight - weights_[index];
912 weights_[index] = weight;
915 throw Exception(
"Subspace index does not exist");
920 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
921 if (components_[i]->getName() == name)
923 setSubspaceWeight(i, weight);
926 throw Exception(
"Subspace " + name +
" does not exist");
941 unsigned int dim = 0;
942 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
943 dim += components_[i]->getDimension();
950 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
951 if (weights_[i] >= std::numeric_limits<double>::epsilon())
952 e += weights_[i] * components_[i]->getMaximumExtent();
959 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
960 components_[i]->enforceBounds(cstate->
components[i]);
966 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
967 if (!components_[i]->satisfiesBounds(cstate->
components[i]))
976 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
983 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
984 l += components_[i]->getSerializationLength();
992 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
994 components_[i]->serialize(reinterpret_cast<char*>(serialization) + l, cstate->
components[i]);
995 l += components_[i]->getSerializationLength();
1003 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1005 components_[i]->deserialize(cstate->
components[i], reinterpret_cast<const char*>(serialization) + l);
1006 l += components_[i]->getSerializationLength();
1015 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1016 dist += weights_[i] * components_[i]->distance(cstate1->
components[i], cstate2->
components[i]);
1023 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1024 components_[i]->setLongestValidSegmentFraction(segmentFraction);
1031 unsigned int sc = 0;
1032 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1034 unsigned int sci = components_[i]->validSegmentCount(cstate1->
components[i], cstate2->
components[i]);
1045 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1056 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1063 if (weightSum_ < std::numeric_limits<double>::epsilon())
1064 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1065 ss->
addSampler(components_[i]->allocStateSampler(), 1.0);
1067 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1068 ss->
addSampler(components_[i]->allocStateSampler(), weights_[i] / weightSum_);
1074 if (subspace->
getName() == getName())
1075 return allocStateSampler();
1076 if (hasSubspace(subspace->
getName()))
1084 allocStateComponents(state);
1085 return static_cast<State*
>(state);
1091 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1092 state->
components[i] = components_[i]->allocState();
1098 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1099 components_[i]->freeState(cstate->
components[i]);
1117 unsigned int idx = 0;
1119 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1120 for (
unsigned int j = 0 ; j <= index ; ++j)
1122 double *va = components_[i]->getValueAddressAtIndex(cstate->
components[i], j);
1138 out <<
"Compound state [" << std::endl;
1140 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1141 components_[i]->printState(cstate->
components[i], out);
1142 out <<
"]" << std::endl;
1147 out <<
"Compound state space '" << getName() <<
"' of dimension " << getDimension() << (isLocked() ?
" (locked)" :
"") <<
" [" << std::endl;
1148 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1150 components_[i]->printSettings(out);
1151 out <<
" of weight " << weights_[i] << std::endl;
1153 out <<
"]" << std::endl;
1154 printProjections(out);
1159 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1160 components_[i]->setup();
1168 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1169 components_[i]->computeLocations();
1179 return copyStateData(destS.get(), dest, sourceS.get(), source);
1204 if (compoundDest->components[i] != source)
1205 compoundDestS->
getSubspace(i)->copyState(compoundDest->components[i], source);
1231 unsigned int copiedComponents = 0;
1253 const std::vector<std::string> &subspaces)
1255 return copyStateData(destS.get(), dest, sourceS.get(), source, subspaces);
1260 const std::vector<std::string> &subspaces)
1262 std::size_t copyCount = 0;
1265 for (std::size_t i = 0 ; i < subspaces.size() ; ++i)
1267 std::map<std::string, StateSpace::SubstateLocation>::const_iterator dt = destLoc.find(subspaces[i]);
1268 if (dt != destLoc.end())
1270 std::map<std::string, StateSpace::SubstateLocation>::const_iterator st = sourceLoc.find(subspaces[i]);
1271 if (st != sourceLoc.end())
1278 if (copyCount == subspaces.size())
1286 inline bool StateSpaceHasContent(
const StateSpacePtr &m)
1292 const unsigned int nc = m->as<CompoundStateSpace>()->getSubspaceCount();
1293 for (
unsigned int i = 0 ; i < nc ; ++i)
1294 if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubspace(i)))
1304 if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
1307 if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
1310 std::vector<StateSpacePtr> components;
1311 std::vector<double> weights;
1313 bool change =
false;
1318 if (!csm_a->isLocked())
1321 for (
unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1323 components.push_back(csm_a->getSubspace(i));
1324 weights.push_back(csm_a->getSubspaceWeight(i));
1330 components.push_back(a);
1331 weights.push_back(1.0);
1337 unsigned int size = components.size();
1340 if (!csm_b->isLocked())
1343 for (
unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
1346 for (
unsigned int j = 0 ; j < size ; ++j)
1347 if (components[j]->getName() == csm_b->getSubspace(i)->getName())
1354 components.push_back(csm_b->getSubspace(i));
1355 weights.push_back(csm_b->getSubspaceWeight(i));
1359 if (components.size() == csm_b->getSubspaceCount())
1366 for (
unsigned int j = 0 ; j < size ; ++j)
1367 if (components[j]->getName() == b->getName())
1374 components.push_back(b);
1375 weights.push_back(1.0);
1384 if (components.size() == 1)
1385 return components[0];
1392 std::vector<StateSpacePtr> components_a;
1393 std::vector<double> weights_a;
1394 std::vector<StateSpacePtr> components_b;
1400 if (!csm_a->isLocked())
1403 for (
unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1405 components_a.push_back(csm_a->getSubspace(i));
1406 weights_a.push_back(csm_a->getSubspaceWeight(i));
1412 components_a.push_back(a);
1413 weights_a.push_back(1.0);
1421 if (!csm_b->isLocked())
1424 for (
unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
1425 components_b.push_back(csm_b->getSubspace(i));
1428 components_b.push_back(b);
1431 bool change =
false;
1432 for (
unsigned int i = 0 ; i < components_b.size() ; ++i)
1433 for (
unsigned int j = 0 ; j < components_a.size() ; ++j)
1434 if (components_a[j]->getName() == components_b[i]->getName())
1436 components_a.erase(components_a.begin() + j);
1437 weights_a.erase(weights_a.begin() + j);
1445 if (components_a.size() == 1)
1446 return components_a[0];
1453 std::vector<StateSpacePtr> components;
1454 std::vector<double> weights;
1456 bool change =
false;
1461 if (!csm_a->isLocked())
1464 for (
unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1466 if (csm_a->getSubspace(i)->getName() == name)
1471 components.push_back(csm_a->getSubspace(i));
1472 weights.push_back(csm_a->getSubspaceWeight(i));
1478 if (a->getName() != name)
1480 components.push_back(a);
1481 weights.push_back(1.0);
1491 if (components.size() == 1)
1492 return components[0];
1499 std::vector<StateSpacePtr> components_a;
1500 std::vector<double> weights_a;
1501 std::vector<StateSpacePtr> components_b;
1502 std::vector<double> weights_b;
1508 if (!csm_a->isLocked())
1511 for (
unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1513 components_a.push_back(csm_a->getSubspace(i));
1514 weights_a.push_back(csm_a->getSubspaceWeight(i));
1520 components_a.push_back(a);
1521 weights_a.push_back(1.0);
1529 if (!csm_b->isLocked())
1532 for (
unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
1534 components_b.push_back(csm_b->getSubspace(i));
1535 weights_b.push_back(csm_b->getSubspaceWeight(i));
1541 components_b.push_back(b);
1542 weights_b.push_back(1.0);
1546 std::vector<StateSpacePtr> components;
1547 std::vector<double> weights;
1549 for (
unsigned int i = 0 ; i < components_b.size() ; ++i)
1551 for (
unsigned int j = 0 ; j < components_a.size() ; ++j)
1552 if (components_a[j]->getName() == components_b[i]->getName())
1554 components.push_back(components_b[i]);
1555 weights.push_back(std::max(weights_a[j], weights_b[i]));
1560 if (a && components.size() == components_a.size())
1563 if (b && components.size() == components_b.size())
1566 if (components.size() == 1)
1567 return components[0];