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())
227 std::stringstream error;
228 error <<
"The longest valid segment for state space " + getName() +
" must be positive." << std::endl;
229 error <<
"Space settings:" << std::endl;
230 printSettings(error);
234 computeLocationsHelper(
this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
237 std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
238 registerProjections();
239 for (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
240 if (it->second->userConfigured())
242 std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
243 if (o != projections_.end())
244 if (!o->second->userConfigured())
245 projections_[it->first] = it->second;
249 std::vector<std::string> pnames;
250 params_.getParamNames(pnames);
251 for (std::vector<std::string>::const_iterator it = pnames.begin() ; it != pnames.end() ; ++it)
252 if (it->substr(0, 11) ==
"projection.")
256 for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
259 if (it->first == DEFAULT_PROJECTION_NAME)
260 params_.include(it->second->params(),
"projection");
262 params_.include(it->second->params(),
"projection." + it->first);
268 return substateLocationsByName_;
273 std::size_t index = 0;
274 while (loc.
chain.size() > index)
281 std::size_t index = 0;
282 while (loc.
chain.size() > index)
294 double *val = getValueAddressAtIndex(const_cast<State*>(state), index);
300 return valueLocationsInOrder_;
305 return valueLocationsByName_;
310 reals.resize(valueLocationsInOrder_.size());
311 for (std::size_t i = 0 ; i < valueLocationsInOrder_.size() ; ++i)
312 reals[i] = *getValueAddressAtLocation(source, valueLocationsInOrder_[i]);
317 assert(reals.size() == valueLocationsInOrder_.size());
318 for (std::size_t i = 0 ; i < reals.size() ; ++i)
319 *getValueAddressAtLocation(destination, valueLocationsInOrder_[i]) = reals[i];
324 std::size_t index = 0;
332 std::size_t index = 0;
340 std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
341 return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
346 std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
347 return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
365 out <<
"State instance [" << state <<
']' << std::endl;
370 out <<
"StateSpace '" << getName() <<
"' instance: " <<
this << std::endl;
371 printProjections(out);
376 if (projections_.empty())
377 out <<
"No registered projections" << std::endl;
380 out <<
"Registered projections:" << std::endl;
381 for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
384 if (it->first == DEFAULT_PROJECTION_NAME)
389 it->second->printSettings(out);
401 std::queue<const StateSpace*> q;
412 for (
unsigned int i = 0 ; i < c ; ++i)
419 static bool StateSpaceCovers(
const StateSpace *
self,
const StateSpace *other)
421 if (StateSpaceIncludes(
self, other))
424 if (other->isCompound())
426 unsigned int c = other->as<CompoundStateSpace>()->getSubspaceCount();
427 for (
unsigned int i = 0 ; i < c ; ++i)
428 if (!StateSpaceCovers(
self, other->as<CompoundStateSpace>()->getSubspace(i).get()))
435 struct CompareSubstateLocation
437 bool operator()(
const StateSpace::SubstateLocation &a,
const StateSpace::SubstateLocation &b)
const
439 if (a.space->getDimension() != b.space->getDimension())
440 return a.space->getDimension() > b.space->getDimension();
441 return a.space->getName() > b.space->getName();
452 return StateSpaceCovers(
this, other.get());
457 return StateSpaceIncludes(
this, other.get());
462 return StateSpaceCovers(
this, other);
467 return StateSpaceIncludes(
this, other);
472 getCommonSubspaces(other.get(), subspaces);
477 std::set<StateSpace::SubstateLocation, CompareSubstateLocation> intersection;
479 for (std::map<std::string, StateSpace::SubstateLocation>::const_iterator it = substateLocationsByName_.begin() ; it != substateLocationsByName_.end() ; ++it)
481 if (S.find(it->first) != S.end())
482 intersection.insert(it->second);
489 for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator it = intersection.begin() ; it != intersection.end() ; ++it)
490 for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator jt = intersection.begin() ; jt != intersection.end() ; ++jt)
492 if (StateSpaceCovers(it->space, jt->space))
494 intersection.erase(jt);
500 for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator it = intersection.begin() ; it != intersection.end() ; ++it)
501 subspaces.push_back(it->space->getName());
506 AllocatedSpaces &as = getAllocatedSpaces();
507 boost::mutex::scoped_lock smLock(as.lock_);
508 for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
509 out <<
"@ " << *it <<
": " << (*it)->getName() << std::endl;
514 std::queue<const StateSpace*> q;
520 out <<
"@ " << m <<
": " << m->
getName() << std::endl;
524 for (
unsigned int i = 0 ; i < c ; ++i)
532 out <<
"digraph StateSpace {" << std::endl;
533 out <<
'"' << getName() <<
'"' << std::endl;
535 std::queue<const StateSpace*> q;
544 for (
unsigned int i = 0 ; i < c ; ++i)
548 out <<
'"' << m->
getName() <<
"\" -> \"" << s->
getName() <<
"\" [label=\"" <<
549 boost::lexical_cast<std::string>(m->
as<
CompoundStateSpace>()->getSubspaceWeight(i)) <<
"\"];" << std::endl;
554 out <<
'}' << std::endl;
559 AllocatedSpaces &as = getAllocatedSpaces();
560 boost::mutex::scoped_lock smLock(as.lock_);
561 out <<
"digraph StateSpaces {" << std::endl;
562 for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
564 out <<
'"' << (*it)->getName() <<
'"' << std::endl;
565 for (std::list<StateSpace*>::iterator jt = as.list_.begin() ; jt != as.list_.end(); ++jt)
568 if ((*it)->isCompound() && (*it)->as<
CompoundStateSpace>()->hasSubspace((*jt)->getName()))
569 out <<
'"' << (*it)->getName() <<
"\" -> \"" << (*jt)->getName() <<
"\" [label=\"" <<
570 boost::lexical_cast<std::string>((*it)->as<
CompoundStateSpace>()->getSubspaceWeight((*jt)->getName())) <<
573 if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
574 out <<
'"' << (*it)->getName() <<
"\" -> \"" << (*jt)->getName() <<
"\" [style=dashed];" << std::endl;
577 out <<
'}' << std::endl;
582 unsigned int flags = isMetricSpace() ? ~0 : ~(STATESPACE_DISTANCE_SYMMETRIC | STATESPACE_TRIANGLE_INEQUALITY);
583 sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), flags);
589 double maxExt = getMaximumExtent();
591 State *s1 = allocState();
592 State *s2 = allocState();
594 char *serialization = NULL;
595 if ((flags & STATESPACE_SERIALIZATION) && getSerializationLength() > 0)
596 serialization =
new char[getSerializationLength()];
599 ss->sampleUniform(s1);
600 if (distance(s1, s1) > eps)
601 throw Exception(
"Distance from a state to itself should be 0");
602 if (!equalStates(s1, s1))
603 throw Exception(
"A state should be equal to itself");
604 if ((flags & STATESPACE_RESPECT_BOUNDS) && !satisfiesBounds(s1))
605 throw Exception(
"Sampled states should be within bounds");
607 if (!equalStates(s1, s2))
608 throw Exception(
"Copy of a state is not the same as the original state. copyState() may not work correctly.");
609 if (flags & STATESPACE_ENFORCE_BOUNDS_NO_OP)
612 if (!equalStates(s1, s2))
613 throw Exception(
"enforceBounds() seems to modify states that are in fact within bounds.");
615 if (flags & STATESPACE_SERIALIZATION)
617 ss->sampleUniform(s2);
618 serialize(serialization, s1);
619 deserialize(s2, serialization);
620 if (!equalStates(s1, s2))
621 throw Exception(
"Serialization/deserialization operations do not seem to work as expected.");
623 ss->sampleUniform(s2);
624 if (!equalStates(s1, s2))
626 double d12 = distance(s1, s2);
627 if ((flags & STATESPACE_DISTANCE_DIFFERENT_STATES) && d12 < zero)
628 throw Exception(
"Distance between different states should be above 0");
629 double d21 = distance(s2, s1);
630 if ((flags & STATESPACE_DISTANCE_SYMMETRIC) && fabs(d12 - d21) > eps)
631 throw Exception(
"The distance function should be symmetric (A->B=" +
632 boost::lexical_cast<std::string>(d12) +
", B->A=" +
633 boost::lexical_cast<std::string>(d21) +
", difference is " +
634 boost::lexical_cast<std::string>(fabs(d12 - d21)) +
")");
635 if (flags & STATESPACE_DISTANCE_BOUND)
636 if (d12 > maxExt + zero)
637 throw Exception(
"The distance function should not report values larger than the maximum extent ("+
638 boost::lexical_cast<std::string>(d12) +
" > " + boost::lexical_cast<std::string>(maxExt) +
")");
642 delete[] serialization;
649 if (!isDiscrete() && !isHybrid())
651 State *s1 = allocState();
652 State *s2 = allocState();
653 State *s3 = allocState();
658 ss->sampleUniform(s1);
659 ss->sampleUniform(s2);
660 ss->sampleUniform(s3);
662 interpolate(s1, s2, 0.0, s3);
663 if ((flags & STATESPACE_INTERPOLATION) && distance(s1, s3) > eps)
664 throw Exception(
"Interpolation from a state at time 0 should be not change the original state");
666 interpolate(s1, s2, 1.0, s3);
667 if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
668 throw Exception(
"Interpolation to a state at time 1 should be the same as the final state");
670 interpolate(s1, s2, 0.5, s3);
671 double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
672 if ((flags & STATESPACE_TRIANGLE_INEQUALITY) && fabs(diff) > eps)
673 throw Exception(
"Interpolation to midpoint state does not lead to distances that satisfy the triangle inequality (" +
674 boost::lexical_cast<std::string>(diff) +
" difference)");
676 interpolate(s3, s2, 0.5, s3);
677 interpolate(s1, s2, 0.75, s2);
679 if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
680 throw Exception(
"Continued interpolation does not work as expected. Please also check that interpolate() works with overlapping memory for its state arguments");
690 return hasProjection(DEFAULT_PROJECTION_NAME);
695 return projections_.find(name) != projections_.end();
700 if (hasDefaultProjection())
701 return getProjection(DEFAULT_PROJECTION_NAME);
704 OMPL_ERROR(
"No default projection is set. Perhaps setup() needs to be called");
711 std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
712 if (it != projections_.end())
716 OMPL_ERROR(
"Projection '%s' is not defined", name.c_str());
728 registerProjection(DEFAULT_PROJECTION_NAME, projection);
734 projections_[name] = projection;
736 OMPL_ERROR(
"Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
779 return allocDefaultStateSampler();
784 return allocSubspaceStateSampler(subspace.get());
789 if (subspace->
getName() == getName())
790 return allocStateSampler();
797 throw Exception(
"The multiplicative factor for the valid segment count between two states must be strictly positive");
798 longestValidSegmentCountFactor_ = factor;
803 if (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
804 throw Exception(
"The fraction of the extent must be larger than 0 and less than 1");
805 longestValidSegmentFraction_ = segmentFraction;
810 return longestValidSegmentCountFactor_;
815 return longestValidSegmentFraction_;
820 return longestValidSegmentCountFactor_ * (
unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
829 const std::vector<double> &weights) :
830 StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
832 if (components.size() != weights.size())
833 throw Exception(
"Number of component spaces and weights are not the same");
835 for (
unsigned int i = 0 ; i < components.size() ; ++i)
842 throw Exception(
"This state space is locked. No further components can be added");
844 throw Exception(
"Subspace weight cannot be negative");
845 components_.push_back(component);
846 weights_.push_back(weight);
847 weightSum_ += weight;
848 componentCount_ = components_.size();
860 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
862 if (components_[i]->isHybrid())
864 if (components_[i]->isDiscrete())
874 return componentCount_;
879 if (componentCount_ > index)
880 return components_[index];
882 throw Exception(
"Subspace index does not exist");
887 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
888 if (components_[i]->getName() == name)
895 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
896 if (components_[i]->getName() == name)
898 throw Exception(
"Subspace " + name +
" does not exist");
903 return components_[getSubspaceIndex(name)];
908 if (componentCount_ > index)
909 return weights_[index];
911 throw Exception(
"Subspace index does not exist");
916 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
917 if (components_[i]->getName() == name)
919 throw Exception(
"Subspace " + name +
" does not exist");
925 throw Exception(
"Subspace weight cannot be negative");
926 if (componentCount_ > index)
928 weightSum_ += weight - weights_[index];
929 weights_[index] = weight;
932 throw Exception(
"Subspace index does not exist");
937 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
938 if (components_[i]->getName() == name)
940 setSubspaceWeight(i, weight);
943 throw Exception(
"Subspace " + name +
" does not exist");
958 unsigned int dim = 0;
959 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
960 dim += components_[i]->getDimension();
967 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
968 if (weights_[i] >= std::numeric_limits<double>::epsilon())
969 e += weights_[i] * components_[i]->getMaximumExtent();
976 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
977 components_[i]->enforceBounds(cstate->
components[i]);
983 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
984 if (!components_[i]->satisfiesBounds(cstate->
components[i]))
993 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1000 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1001 l += components_[i]->getSerializationLength();
1009 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1011 components_[i]->serialize(reinterpret_cast<char*>(serialization) + l, cstate->
components[i]);
1012 l += components_[i]->getSerializationLength();
1020 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1022 components_[i]->deserialize(cstate->
components[i], reinterpret_cast<const char*>(serialization) + l);
1023 l += components_[i]->getSerializationLength();
1032 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1033 dist += weights_[i] * components_[i]->distance(cstate1->
components[i], cstate2->
components[i]);
1040 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1041 components_[i]->setLongestValidSegmentFraction(segmentFraction);
1048 unsigned int sc = 0;
1049 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1051 unsigned int sci = components_[i]->validSegmentCount(cstate1->
components[i], cstate2->
components[i]);
1062 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1073 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1080 if (weightSum_ < std::numeric_limits<double>::epsilon())
1081 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1082 ss->
addSampler(components_[i]->allocStateSampler(), 1.0);
1084 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1085 ss->
addSampler(components_[i]->allocStateSampler(), weights_[i] / weightSum_);
1091 if (subspace->
getName() == getName())
1092 return allocStateSampler();
1093 if (hasSubspace(subspace->
getName()))
1101 allocStateComponents(state);
1102 return static_cast<State*
>(state);
1108 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1109 state->
components[i] = components_[i]->allocState();
1115 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1116 components_[i]->freeState(cstate->
components[i]);
1134 unsigned int idx = 0;
1136 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1137 for (
unsigned int j = 0 ; j <= index ; ++j)
1139 double *va = components_[i]->getValueAddressAtIndex(cstate->
components[i], j);
1155 out <<
"Compound state [" << std::endl;
1157 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1158 components_[i]->printState(cstate->
components[i], out);
1159 out <<
"]" << std::endl;
1164 out <<
"Compound state space '" << getName() <<
"' of dimension " << getDimension() << (isLocked() ?
" (locked)" :
"") <<
" [" << std::endl;
1165 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1167 components_[i]->printSettings(out);
1168 out <<
" of weight " << weights_[i] << std::endl;
1170 out <<
"]" << std::endl;
1171 printProjections(out);
1176 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1177 components_[i]->setup();
1185 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
1186 components_[i]->computeLocations();
1196 return copyStateData(destS.get(), dest, sourceS.get(), source);
1221 if (compoundDest->components[i] != source)
1222 compoundDestS->
getSubspace(i)->copyState(compoundDest->components[i], source);
1248 unsigned int copiedComponents = 0;
1270 const std::vector<std::string> &subspaces)
1272 return copyStateData(destS.get(), dest, sourceS.get(), source, subspaces);
1277 const std::vector<std::string> &subspaces)
1279 std::size_t copyCount = 0;
1282 for (std::size_t i = 0 ; i < subspaces.size() ; ++i)
1284 std::map<std::string, StateSpace::SubstateLocation>::const_iterator dt = destLoc.find(subspaces[i]);
1285 if (dt != destLoc.end())
1287 std::map<std::string, StateSpace::SubstateLocation>::const_iterator st = sourceLoc.find(subspaces[i]);
1288 if (st != sourceLoc.end())
1295 if (copyCount == subspaces.size())
1303 inline bool StateSpaceHasContent(
const StateSpacePtr &m)
1309 const unsigned int nc = m->as<CompoundStateSpace>()->getSubspaceCount();
1310 for (
unsigned int i = 0 ; i < nc ; ++i)
1311 if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubspace(i)))
1321 if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
1324 if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
1327 std::vector<StateSpacePtr> components;
1328 std::vector<double> weights;
1330 bool change =
false;
1335 if (!csm_a->isLocked())
1338 for (
unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1340 components.push_back(csm_a->getSubspace(i));
1341 weights.push_back(csm_a->getSubspaceWeight(i));
1347 components.push_back(a);
1348 weights.push_back(1.0);
1354 unsigned int size = components.size();
1357 if (!csm_b->isLocked())
1360 for (
unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
1363 for (
unsigned int j = 0 ; j < size ; ++j)
1364 if (components[j]->getName() == csm_b->getSubspace(i)->getName())
1371 components.push_back(csm_b->getSubspace(i));
1372 weights.push_back(csm_b->getSubspaceWeight(i));
1376 if (components.size() == csm_b->getSubspaceCount())
1383 for (
unsigned int j = 0 ; j < size ; ++j)
1384 if (components[j]->getName() == b->getName())
1391 components.push_back(b);
1392 weights.push_back(1.0);
1401 if (components.size() == 1)
1402 return components[0];
1409 std::vector<StateSpacePtr> components_a;
1410 std::vector<double> weights_a;
1411 std::vector<StateSpacePtr> components_b;
1417 if (!csm_a->isLocked())
1420 for (
unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1422 components_a.push_back(csm_a->getSubspace(i));
1423 weights_a.push_back(csm_a->getSubspaceWeight(i));
1429 components_a.push_back(a);
1430 weights_a.push_back(1.0);
1438 if (!csm_b->isLocked())
1441 for (
unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
1442 components_b.push_back(csm_b->getSubspace(i));
1445 components_b.push_back(b);
1448 bool change =
false;
1449 for (
unsigned int i = 0 ; i < components_b.size() ; ++i)
1450 for (
unsigned int j = 0 ; j < components_a.size() ; ++j)
1451 if (components_a[j]->getName() == components_b[i]->getName())
1453 components_a.erase(components_a.begin() + j);
1454 weights_a.erase(weights_a.begin() + j);
1462 if (components_a.size() == 1)
1463 return components_a[0];
1470 std::vector<StateSpacePtr> components;
1471 std::vector<double> weights;
1473 bool change =
false;
1478 if (!csm_a->isLocked())
1481 for (
unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1483 if (csm_a->getSubspace(i)->getName() == name)
1488 components.push_back(csm_a->getSubspace(i));
1489 weights.push_back(csm_a->getSubspaceWeight(i));
1495 if (a->getName() != name)
1497 components.push_back(a);
1498 weights.push_back(1.0);
1508 if (components.size() == 1)
1509 return components[0];
1516 std::vector<StateSpacePtr> components_a;
1517 std::vector<double> weights_a;
1518 std::vector<StateSpacePtr> components_b;
1519 std::vector<double> weights_b;
1525 if (!csm_a->isLocked())
1528 for (
unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1530 components_a.push_back(csm_a->getSubspace(i));
1531 weights_a.push_back(csm_a->getSubspaceWeight(i));
1537 components_a.push_back(a);
1538 weights_a.push_back(1.0);
1546 if (!csm_b->isLocked())
1549 for (
unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
1551 components_b.push_back(csm_b->getSubspace(i));
1552 weights_b.push_back(csm_b->getSubspaceWeight(i));
1558 components_b.push_back(b);
1559 weights_b.push_back(1.0);
1563 std::vector<StateSpacePtr> components;
1564 std::vector<double> weights;
1566 for (
unsigned int i = 0 ; i < components_b.size() ; ++i)
1568 for (
unsigned int j = 0 ; j < components_a.size() ; ++j)
1569 if (components_a[j]->getName() == components_b[i]->getName())
1571 components.push_back(components_b[i]);
1572 weights.push_back(std::max(weights_a[j], weights_b[i]));
1577 if (a && components.size() == components_a.size())
1580 if (b && components.size() == components_b.size())
1583 if (components.size() == 1)
1584 return components[0];
virtual bool isCompound(void) const
Check if the state space is compound.
State * getSubstateAtLocation(State *state, const SubstateLocation &loc) const
Get the substate of state that is pointed to by loc.
void setName(const std::string &name)
Set the name of the state space.
bool isLocked(void) const
Return true if the state space is locked. A value of true means that no further spaces can be added a...
boost::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
int type_
A type assigned for this state space.
ParamSet params_
The set of parameters for this space.
virtual unsigned int validSegmentCount(const State *state1, const State *state2) const
Count how many segments of the "longest valid length" fit on the motion from state1 to state2...
Definition of a compound state.
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
virtual double * getValueAddressAtIndex(State *state, const unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
virtual unsigned int getSerializationLength(void) const
Get the number of chars in the serialization of a state in this space.
static const std::string DEFAULT_PROJECTION_NAME
The name used for the default projection.
std::size_t index
The index of the value to be accessed, within the substate location above.
unsigned int getSubspaceIndex(const std::string &name) const
Get the index of a specific subspace from the compound state space.
const std::vector< double > & getSubspaceWeights(void) const
Get the list of component weights.
bool includes(const StateSpacePtr &other) const
Return true if other is a space included (perhaps equal, perhaps a subspace) in this one...
static void Diagram(std::ostream &out)
Print a Graphviz digraph that represents the containment diagram for all the instantiated state space...
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings() ...
A boost shared pointer wrapper for ompl::base::StateSpace.
double getSubspaceWeight(const unsigned int index) const
Get the weight of a subspace from the compound state space (used in distance computation) ...
A boost shared pointer wrapper for ompl::base::StateSampler.
virtual unsigned int getSerializationLength(void) const
Get the number of chars in the serialization of a state in this space.
std::vector< std::size_t > chain
In a complex state space there may be multiple compound state spaces that make up an even larger comp...
virtual bool isHybrid(void) const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
Representation of the address of a value in a state. This structure stores the indexing information n...
virtual bool isHybrid(void) const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
virtual State * allocState(void) const
Allocate a state that can store a point in the described space.
AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
Copy data from source (state from space sourceS) to dest (state from space destS) on a component by c...
virtual double getLongestValidSegmentFraction(void) const
When performing discrete validation of motions, the length of the longest segment that does not requi...
const std::map< std::string, ValueLocation > & getValueLocationsByName(void) const
Get the named locations of values of type double contained in a state from this space. The setup() function must have been previously called.
virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
void diagram(std::ostream &out) const
Print a Graphviz digraph that represents the containment diagram for the state space.
void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
Register a projection for this state space under a specified name.
void copyFromReals(State *destination, const std::vector< double > &reals) const
Copy the values from reals to the state destination using getValueAddressAtLocation() ...
void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the state space. The first element of the signature is the number of integers that follow.
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
virtual StateSamplerPtr allocDefaultStateSampler(void) const
Allocate an instance of the default uniform state sampler for this space.
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
unsigned int longestValidSegmentCountFactor_
The factor to multiply the value returned by validSegmentCount()
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces...
virtual double * getValueAddressAtIndex(State *state, const unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
const StateSpacePtr & getSubspace(const unsigned int index) const
Get a specific subspace from the compound state space.
const T * as(void) const
Cast this instance to a desired type.
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
virtual void addSampler(const StateSamplerPtr &sampler, double weightImportance)
Add a sampler as part of the new compound sampler. This sampler is used to sample part of the compoun...
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
const std::vector< ValueLocation > & getValueLocations(void) const
Get the locations of values of type double contained in a state from this space. The order of the val...
virtual bool hasSymmetricInterpolate(void) const
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from, to, t, state) = interpolate(to, from, 1-t, state). Default implementation returns true.
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
StateSpace(void)
Constructor. Assigns a unique name to the space.
A space to allow the composition of state spaces.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Representation of the address of a substate in a state. This structure stores the indexing informatio...
const std::string & getName(void) const
Get the name of the state space.
unsigned int getSubspaceCount(void) const
Get the number of state spaces that make up the compound state space.
void setSubspaceWeight(const unsigned int index, double weight)
Set the weight of a subspace in the compound state space (used in distance computation) ...
virtual double getMaximumExtent(void) const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
Construct a compound state space that contains subspaces only from a. If a is compound, b (or the components from b, if b is compound) are removed and the remaining components are returned as a compound state space. If the compound space would end up containing solely one component, that component is returned instead.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
ProjectionEvaluatorPtr getDefaultProjection(void) const
Get the default projection.
virtual void freeState(State *state) const
Free the memory of the allocated state.
Definition of an abstract state.
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
void setValidSegmentCountFactor(unsigned int factor)
Set factor to be the value to multiply the return value of validSegmentCount(). By default...
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
CompoundStateSpace(void)
Construct an empty compound state space.
StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
void lock(void)
Lock this state space. This means no further spaces can be added as components. This function can be ...
void setStateSamplerAllocator(const StateSamplerAllocator &ssa)
Set the sampler allocator to use.
const std::map< std::string, ProjectionEvaluatorPtr > & getRegisteredProjections(void) const
Get all the registered projections.
bool covers(const StateSpacePtr &other) const
Return true if other is a space that is either included (perhaps equal, perhaps a subspace) in this o...
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
The exception type for ompl.
double maxExtent_
The extent of this space at the time setup() was called.
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=typename SpecificParam< T >::GetterFn())
This function declares a parameter name, and specifies the setter and getter functions.
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
virtual StateSamplerPtr allocStateSampler(void) const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
virtual bool isDiscrete(void) const
Check if the set of states is discrete.
bool hasSubspace(const std::string &name) const
Check if a specific subspace is contained in this state space.
void getCommonSubspaces(const StateSpacePtr &other, std::vector< std::string > &subspaces) const
Get the set of subspaces that this space and other have in common. The computed list of subspaces doe...
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
ompl::base::RealVectorStateSpace
virtual bool hasSymmetricDistance(void) const
Check if the distance function on this state space is symmetric, i.e. distance(s1,s2) = distance(s2,s1). Default implementation returns true.
SubstateLocation stateLocation
Location of the substate that contains the pointed to value.
virtual unsigned int validSegmentCount(const State *state1, const State *state2) const
Count how many segments of the "longest valid length" fit on the motion from state1 to state2...
virtual void sanityChecks(void) const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
double longestValidSegmentFraction_
The fraction of the longest valid segment.
State ** components
The components that make up a compound state.
unsigned int getValidSegmentCountFactor(void) const
Get the value used to multiply the return value of validSegmentCount().
bool hasProjection(const std::string &name) const
Check if a projection with a specified name is available.
static void List(std::ostream &out)
Print the list of available state space instances.
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
virtual bool isCompound(void) const
Check if the state space is compound.
double longestValidSegment_
The longest valid segment at the time setup() was called.
Definition of a compound state sampler. This is useful to construct samplers for compound states...
virtual void computeLocations(void)
Compute the location information for various components of the state space. Either this function or s...
bool hasDefaultProjection(void) const
Check if a default projection is available.
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
virtual void enforceBounds(State *state) const
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
double * getValueAddressAtLocation(State *state, const ValueLocation &loc) const
Get a pointer to the double value in state that loc points to.
double * getValueAddressAtName(State *state, const std::string &name) const
Get a pointer to the double value in state that name points to.
virtual void copyState(State *destination, const State *source) const =0
Copy a state to another. The memory of source and destination should NOT overlap. ...
virtual unsigned int getDimension(void) const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
void clearStateSamplerAllocator(void)
Clear the state sampler allocator (reset to default)
const StateSpace * space
The space that is reached if the chain above is followed on the state space.
AdvancedStateCopyOperation
The possible outputs for an advanced copy operation.
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
const std::vector< StateSpacePtr > & getSubspaces(void) const
Get the list of components.
void copyToReals(std::vector< double > &reals, const State *source) const
Copy all the real values from a state source to the array reals using getValueAddressAtLocation() ...
Construct a sampler that samples only within a subspace of the space.
ProjectionEvaluatorPtr getProjection(const std::string &name) const
Get the projection registered under a specific name.
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
T * as(void)
Cast this instance to a desired type.
virtual void computeLocations(void)
Compute the location information for various components of the state space. Either this function or s...
OptimizationObjectivePtr operator*(double w, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
const std::map< std::string, SubstateLocation > & getSubstateLocationsByName(void) const
Get the list of known substate locations (keys of the map corrspond to names of subspaces) ...
virtual bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
void list(std::ostream &out) const
Print the list of all contained state space instances.
Unset type; this is the default type.