Loading...
Searching...
No Matches
StateSpace.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2010, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
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 "ompl/util/String.h"
40#include <mutex>
41#include <boost/scoped_ptr.hpp>
42#include <numeric>
43#include <limits>
44#include <queue>
45#include <cmath>
46#include <list>
47#include <set>
48
50
52namespace ompl
53{
54 namespace base
55 {
56 namespace
57 {
58 struct AllocatedSpaces
59 {
60 AllocatedSpaces() = default;
61 std::list<StateSpace *> list_;
62 std::mutex lock_;
63 unsigned int counter_{0};
64 };
65
66 static boost::scoped_ptr<AllocatedSpaces> g_allocatedSpaces;
67 static std::once_flag g_once;
68
69 void initAllocatedSpaces()
70 {
71 g_allocatedSpaces.reset(new AllocatedSpaces);
72 }
73
74 AllocatedSpaces &getAllocatedSpaces()
75 {
76 std::call_once(g_once, &initAllocatedSpaces);
77 return *g_allocatedSpaces;
78 }
79 } // namespace
80 }
81}
83
85{
86 AllocatedSpaces &as = getAllocatedSpaces();
87 std::lock_guard<std::mutex> smLock(as.lock_);
88
89 // autocompute a unique name
90 name_ = "Space" + std::to_string(as.counter_++);
91
95
97
98 maxExtent_ = std::numeric_limits<double>::infinity();
99
100 params_.declareParam<double>("longest_valid_segment_fraction",
101 [this](double segmentFraction) { setLongestValidSegmentFraction(segmentFraction); },
102 [this] { return getLongestValidSegmentFraction(); });
103
104 params_.declareParam<unsigned int>("valid_segment_count_factor",
105 [this](unsigned int factor) { setValidSegmentCountFactor(factor); },
106 [this] { return getValidSegmentCountFactor(); });
107 as.list_.push_back(this);
108}
109
110ompl::base::StateSpace::~StateSpace()
111{
112 AllocatedSpaces &as = getAllocatedSpaces();
113 std::lock_guard<std::mutex> smLock(as.lock_);
114 as.list_.remove(this);
115}
116
118namespace ompl
119{
120 namespace base
121 {
122 static void computeStateSpaceSignatureHelper(const StateSpace *space, std::vector<int> &signature)
123 {
124 signature.push_back(space->getType());
125 signature.push_back(space->getDimension());
126
127 if (space->isCompound())
128 {
129 unsigned int c = space->as<CompoundStateSpace>()->getSubspaceCount();
130 for (unsigned int i = 0; i < c; ++i)
131 computeStateSpaceSignatureHelper(space->as<CompoundStateSpace>()->getSubspace(i).get(), signature);
132 }
133 }
134
135 void computeLocationsHelper(const StateSpace *s,
136 std::map<std::string, StateSpace::SubstateLocation> &substateMap,
137 std::vector<StateSpace::ValueLocation> &locationsArray,
138 std::map<std::string, StateSpace::ValueLocation> &locationsMap,
140 {
141 loc.stateLocation.space = s;
142 substateMap[s->getName()] = loc.stateLocation;
143 State *test = s->allocState();
144 if (s->getValueAddressAtIndex(test, 0) != nullptr)
145 {
146 loc.index = 0;
147 locationsMap[s->getName()] = loc;
148 // if the space is compound, we will find this value again in the first subspace
149 if (!s->isCompound())
150 {
151 if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
152 {
153 const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(0);
154 if (!name.empty())
155 locationsMap[name] = loc;
156 }
157 locationsArray.push_back(loc);
158 while (s->getValueAddressAtIndex(test, ++loc.index) != nullptr)
159 {
160 if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
161 {
162 const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(loc.index);
163 if (!name.empty())
164 locationsMap[name] = loc;
165 }
166 locationsArray.push_back(loc);
167 }
168 }
169 }
170 s->freeState(test);
171
172 if (s->isCompound())
173 for (unsigned int i = 0; i < s->as<base::CompoundStateSpace>()->getSubspaceCount(); ++i)
174 {
175 loc.stateLocation.chain.push_back(i);
176 computeLocationsHelper(s->as<base::CompoundStateSpace>()->getSubspace(i).get(), substateMap,
177 locationsArray, locationsMap, loc);
178 loc.stateLocation.chain.pop_back();
179 }
180 }
181
182 void computeLocationsHelper(const StateSpace *s,
183 std::map<std::string, StateSpace::SubstateLocation> &substateMap,
184 std::vector<StateSpace::ValueLocation> &locationsArray,
185 std::map<std::string, StateSpace::ValueLocation> &locationsMap)
186 {
187 substateMap.clear();
188 locationsArray.clear();
189 locationsMap.clear();
190 computeLocationsHelper(s, substateMap, locationsArray, locationsMap, StateSpace::ValueLocation());
191 }
192 }
193}
195
196const std::string &ompl::base::StateSpace::getName() const
197{
198 return name_;
199}
200
201void ompl::base::StateSpace::setName(const std::string &name)
202{
203 name_ = name;
204
205 // we don't want to call this function during the state space construction because calls to virtual functions are
206 // made,
207 // so we check if any values were previously inserted as value locations;
208 // if none were, then we either have none (so no need to call this function again)
209 // or setup() was not yet called
210 if (!valueLocationsInOrder_.empty())
212}
213
218
219void ompl::base::StateSpace::computeSignature(std::vector<int> &signature) const
220{
221 signature.clear();
222 computeStateSpaceSignatureHelper(this, signature);
223 signature.insert(signature.begin(), signature.size());
224}
225
227{
228 State *copy = allocState();
229 copyState(copy, source);
230 return copy;
231}
232
236
238{
241
242 if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
243 {
244 std::stringstream error;
245 error << "The longest valid segment for state space " + getName() + " must be positive." << std::endl;
246 error << "Space settings:" << std::endl;
247 printSettings(error);
248 throw Exception(error.str());
249 }
250
252
253 // make sure we don't overwrite projections that have been configured by the user
254 std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
256 for (auto &oldProjection : oldProjections)
257 if (oldProjection.second->userConfigured())
258 {
259 auto o = projections_.find(oldProjection.first);
260 if (o != projections_.end())
261 if (!o->second->userConfigured())
262 projections_[oldProjection.first] = oldProjection.second;
263 }
264
265 // remove previously set parameters for projections
266 std::vector<std::string> pnames;
267 params_.getParamNames(pnames);
268 for (const auto &pname : pnames)
269 if (pname.substr(0, 11) == "projection.")
270 params_.remove(pname);
271
272 // setup projections and add their parameters
273 for (const auto &projection : projections_)
274 {
275 projection.second->setup();
276 if (projection.first == DEFAULT_PROJECTION_NAME)
277 params_.include(projection.second->params(), "projection");
278 else
279 params_.include(projection.second->params(), "projection." + projection.first);
280 }
281}
282
283const std::map<std::string, ompl::base::StateSpace::SubstateLocation> &
288
290{
291 std::size_t index = 0;
292 while (loc.chain.size() > index)
293 state = state->as<CompoundState>()->components[loc.chain[index++]];
294 return state;
295}
296
298 const SubstateLocation &loc) const
299{
300 std::size_t index = 0;
301 while (loc.chain.size() > index)
302 state = state->as<CompoundState>()->components[loc.chain[index++]];
303 return state;
304}
305
306double *ompl::base::StateSpace::getValueAddressAtIndex(State * /*state*/, const unsigned int /*index*/) const
307{
308 return nullptr;
309}
310
311const double *ompl::base::StateSpace::getValueAddressAtIndex(const State *state, const unsigned int index) const
312{
313 double *val = getValueAddressAtIndex(const_cast<State *>(state),
314 index); // this const-cast does not hurt, since the state is not modified
315 return val;
316}
317
318const std::vector<ompl::base::StateSpace::ValueLocation> &ompl::base::StateSpace::getValueLocations() const
319{
321}
322
323const std::map<std::string, ompl::base::StateSpace::ValueLocation> &
328
329void ompl::base::StateSpace::copyToReals(std::vector<double> &reals, const State *source) const
330{
331 const auto &locations = getValueLocations();
332 reals.resize(locations.size());
333 for (std::size_t i = 0; i < locations.size(); ++i)
334 reals[i] = *getValueAddressAtLocation(source, locations[i]);
335}
336
337void ompl::base::StateSpace::copyFromReals(State *destination, const std::vector<double> &reals) const
338{
339 const auto &locations = getValueLocations();
340 assert(reals.size() == locations.size());
341 for (std::size_t i = 0; i < reals.size(); ++i)
342 *getValueAddressAtLocation(destination, locations[i]) = reals[i];
343}
344
346{
347 std::size_t index = 0;
348 while (loc.stateLocation.chain.size() > index)
349 state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
350 return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
351}
352
353const double *ompl::base::StateSpace::getValueAddressAtLocation(const State *state, const ValueLocation &loc) const
354{
355 std::size_t index = 0;
356 while (loc.stateLocation.chain.size() > index)
357 state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
358 return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
359}
360
361double *ompl::base::StateSpace::getValueAddressAtName(State *state, const std::string &name) const
362{
363 const auto &locations = getValueLocationsByName();
364 auto it = locations.find(name);
365 return (it != locations.end()) ? getValueAddressAtLocation(state, it->second) : nullptr;
366}
367
368const double *ompl::base::StateSpace::getValueAddressAtName(const State *state, const std::string &name) const
369{
370 const auto &locations = getValueLocationsByName();
371 auto it = locations.find(name);
372 return (it != locations.end()) ? getValueAddressAtLocation(state, it->second) : nullptr;
373}
374
376{
377 return 0;
378}
379
380void ompl::base::StateSpace::serialize(void * /*serialization*/, const State * /*state*/) const
381{
382}
383
384void ompl::base::StateSpace::deserialize(State * /*state*/, const void * /*serialization*/) const
385{
386}
387
388void ompl::base::StateSpace::printState(const State *state, std::ostream &out) const
389{
390 out << "State instance [" << state << ']' << std::endl;
391}
392
393void ompl::base::StateSpace::printSettings(std::ostream &out) const
394{
395 out << "StateSpace '" << getName() << "' instance: " << this << std::endl;
396 printProjections(out);
397}
398
399void ompl::base::StateSpace::printProjections(std::ostream &out) const
400{
401 if (projections_.empty())
402 out << "No registered projections" << std::endl;
403 else
404 {
405 out << "Registered projections:" << std::endl;
406 for (const auto &projection : projections_)
407 {
408 out << " - ";
409 if (projection.first == DEFAULT_PROJECTION_NAME)
410 out << "<default>";
411 else
412 out << projection.first;
413 out << std::endl;
414 projection.second->printSettings(out);
415 }
416 }
417}
418
420namespace ompl
421{
422 namespace base
423 {
424 static bool StateSpaceIncludes(const StateSpace *self, const StateSpace *other)
425 {
426 std::queue<const StateSpace *> q;
427 q.push(self);
428 while (!q.empty())
429 {
430 const StateSpace *m = q.front();
431 q.pop();
432 if (m->getName() == other->getName())
433 return true;
434 if (m->isCompound())
435 {
436 unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
437 for (unsigned int i = 0; i < c; ++i)
438 q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
439 }
440 }
441 return false;
442 }
443
444 static bool StateSpaceCovers(const StateSpace *self, const StateSpace *other)
445 {
446 if (StateSpaceIncludes(self, other))
447 return true;
448 else if (other->isCompound())
449 {
450 unsigned int c = other->as<CompoundStateSpace>()->getSubspaceCount();
451 for (unsigned int i = 0; i < c; ++i)
452 if (!StateSpaceCovers(self, other->as<CompoundStateSpace>()->getSubspace(i).get()))
453 return false;
454 return true;
455 }
456 return false;
457 }
458
459 struct CompareSubstateLocation
460 {
461 bool operator()(const StateSpace::SubstateLocation &a, const StateSpace::SubstateLocation &b) const
462 {
463 if (a.space->getDimension() != b.space->getDimension())
464 return a.space->getDimension() > b.space->getDimension();
465 return a.space->getName() > b.space->getName();
466 }
467 };
468 }
469}
470
472
474{
475 return StateSpaceCovers(this, other.get());
476}
477
479{
480 return StateSpaceIncludes(this, other.get());
481}
482
483bool ompl::base::StateSpace::covers(const StateSpace *other) const
484{
485 return StateSpaceCovers(this, other);
486}
487
488bool ompl::base::StateSpace::includes(const StateSpace *other) const
489{
490 return StateSpaceIncludes(this, other);
491}
492
493void ompl::base::StateSpace::getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const
494{
495 getCommonSubspaces(other.get(), subspaces);
496}
497
498void ompl::base::StateSpace::getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const
499{
500 std::set<StateSpace::SubstateLocation, CompareSubstateLocation> intersection;
501 const std::map<std::string, StateSpace::SubstateLocation> &S = other->getSubstateLocationsByName();
502 for (const auto &it : substateLocationsByName_)
503 {
504 if (S.find(it.first) != S.end())
505 intersection.insert(it.second);
506 }
507
508 bool found = true;
509 while (found)
510 {
511 found = false;
512 for (auto it = intersection.begin(); it != intersection.end(); ++it)
513 for (auto jt = intersection.begin(); jt != intersection.end(); ++jt)
514 if (it != jt)
515 if (StateSpaceCovers(it->space, jt->space))
516 {
517 intersection.erase(jt);
518 found = true;
519 break;
520 }
521 }
522 subspaces.clear();
523 for (const auto &it : intersection)
524 subspaces.push_back(it.space->getName());
525}
526
527void ompl::base::StateSpace::List(std::ostream &out)
528{
529 AllocatedSpaces &as = getAllocatedSpaces();
530 std::lock_guard<std::mutex> smLock(as.lock_);
531 for (auto &it : as.list_)
532 out << "@ " << it << ": " << it->getName() << std::endl;
533}
534
535void ompl::base::StateSpace::list(std::ostream &out) const
536{
537 std::queue<const StateSpace *> q;
538 q.push(this);
539 while (!q.empty())
540 {
541 const StateSpace *m = q.front();
542 q.pop();
543 out << "@ " << m << ": " << m->getName() << std::endl;
544 if (m->isCompound())
545 {
546 unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
547 for (unsigned int i = 0; i < c; ++i)
548 q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
549 }
550 }
551}
552
553void ompl::base::StateSpace::diagram(std::ostream &out) const
554{
555 out << "digraph StateSpace {" << std::endl;
556 out << '"' << getName() << '"' << std::endl;
557
558 std::queue<const StateSpace *> q;
559 q.push(this);
560 while (!q.empty())
561 {
562 const StateSpace *m = q.front();
563 q.pop();
564 if (m->isCompound())
565 {
566 unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
567 for (unsigned int i = 0; i < c; ++i)
568 {
569 const StateSpace *s = m->as<CompoundStateSpace>()->getSubspace(i).get();
570 q.push(s);
571 out << '"' << m->getName() << R"(" -> ")" << s->getName() << R"(" [label=")"
572 << ompl::toString(m->as<CompoundStateSpace>()->getSubspaceWeight(i)) << R"("];)" << std::endl;
573 }
574 }
575 }
576
577 out << '}' << std::endl;
578}
579
580void ompl::base::StateSpace::Diagram(std::ostream &out)
581{
582 AllocatedSpaces &as = getAllocatedSpaces();
583 std::lock_guard<std::mutex> smLock(as.lock_);
584 out << "digraph StateSpaces {" << std::endl;
585 for (auto it = as.list_.begin(); it != as.list_.end(); ++it)
586 {
587 out << '"' << (*it)->getName() << '"' << std::endl;
588 for (auto jt = as.list_.begin(); jt != as.list_.end(); ++jt)
589 if (it != jt)
590 {
591 if ((*it)->isCompound() && (*it)->as<CompoundStateSpace>()->hasSubspace((*jt)->getName()))
592 out << '"' << (*it)->getName() << R"(" -> ")" << (*jt)->getName() << R"(" [label=")"
593 << ompl::toString((*it)->as<CompoundStateSpace>()->getSubspaceWeight((*jt)->getName())) <<
594 R"("];)" << std::endl;
595 else if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
596 out << '"' << (*it)->getName() << R"(" -> ")" << (*jt)->getName() << R"(" [style=dashed];)"
597 << std::endl;
598 }
599 }
600 out << '}' << std::endl;
601}
602
604{
606 sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), flags);
607}
608
609void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int flags) const
610{
611 {
612 double maxExt = getMaximumExtent();
613
614 State *s1 = allocState();
615 State *s2 = allocState();
617 char *serialization = nullptr;
619 serialization = new char[getSerializationLength()];
620 for (unsigned int i = 0; i < magic::TEST_STATE_COUNT; ++i)
621 {
622 ss->sampleUniform(s1);
623 if (distance(s1, s1) > eps)
624 throw Exception("Distance from a state to itself should be 0");
625 if (!equalStates(s1, s1))
626 throw Exception("A state should be equal to itself");
627 if ((flags & STATESPACE_RESPECT_BOUNDS) && !satisfiesBounds(s1))
628 throw Exception("Sampled states should be within bounds");
629 copyState(s2, s1);
630 if (!equalStates(s1, s2))
631 throw Exception("Copy of a state is not the same as the original state. copyState() may not work "
632 "correctly.");
634 {
635 enforceBounds(s1);
636 if (!equalStates(s1, s2))
637 throw Exception("enforceBounds() seems to modify states that are in fact within bounds.");
638 }
639 if (flags & STATESPACE_SERIALIZATION)
640 {
641 ss->sampleUniform(s2);
642 serialize(serialization, s1);
643 deserialize(s2, serialization);
644 if (!equalStates(s1, s2))
645 throw Exception("Serialization/deserialization operations do not seem to work as expected.");
646 }
647 ss->sampleUniform(s2);
648 if (!equalStates(s1, s2))
649 {
650 double d12 = distance(s1, s2);
651 if ((flags & STATESPACE_DISTANCE_DIFFERENT_STATES) && d12 < zero)
652 throw Exception("Distance between different states should be above 0");
653 double d21 = distance(s2, s1);
654 if ((flags & STATESPACE_DISTANCE_SYMMETRIC) && fabs(d12 - d21) > eps)
655 throw Exception("The distance function should be symmetric (A->B=" + ompl::toString(d12) +
656 ", B->A=" + ompl::toString(d21) + ", difference is " +
657 ompl::toString(fabs(d12 - d21)) + ")");
658 if (flags & STATESPACE_DISTANCE_BOUND)
659 if (d12 > maxExt + zero)
660 throw Exception("The distance function should not report values larger than the maximum extent "
661 "(" +
662 ompl::toString(d12) + " > " + ompl::toString(maxExt) + ")");
663 }
664 }
665 if (serialization)
666 delete[] serialization;
667 freeState(s1);
668 freeState(s2);
669 }
670
671 // Test that interpolation works as expected and also test triangle inequality
673 {
674 State *s1 = allocState();
675 State *s2 = allocState();
676 State *s3 = allocState();
678
679 for (unsigned int i = 0; i < magic::TEST_STATE_COUNT; ++i)
680 {
681 ss->sampleUniform(s1);
682 ss->sampleUniform(s2);
683 ss->sampleUniform(s3);
684
685 interpolate(s1, s2, 0.0, s3);
686 if ((flags & STATESPACE_INTERPOLATION) && distance(s1, s3) > eps)
687 throw Exception("Interpolation from a state at time 0 should be not change the original state");
688
689 interpolate(s1, s2, 1.0, s3);
690 if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
691 throw Exception("Interpolation to a state at time 1 should be the same as the final state");
692
693 interpolate(s1, s2, 0.5, s3);
694 double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
695 if ((flags & STATESPACE_TRIANGLE_INEQUALITY) && diff < -eps)
696 throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle "
697 "inequality (" +
698 ompl::toString(diff) + " difference)");
699
700 interpolate(s3, s2, 0.5, s3);
701 interpolate(s1, s2, 0.75, s2);
702
703 if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
704 throw Exception("Continued interpolation does not work as expected. Please also check that "
705 "interpolate() works with overlapping memory for its state arguments");
706 }
707 freeState(s1);
708 freeState(s2);
709 freeState(s3);
710 }
711}
712
717
718bool ompl::base::StateSpace::hasProjection(const std::string &name) const
719{
720 return projections_.find(name) != projections_.end();
721}
722
723ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getDefaultProjection() const
724{
727 else
728 {
729 OMPL_ERROR("No default projection is set. Perhaps setup() needs to be called");
730 return ProjectionEvaluatorPtr();
731 }
732}
733
734ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getProjection(const std::string &name) const
735{
736 auto it = projections_.find(name);
737 if (it != projections_.end())
738 return it->second;
739 else
740 {
741 OMPL_ERROR("Projection '%s' is not defined", name.c_str());
742 return ProjectionEvaluatorPtr();
743 }
744}
745
746const std::map<std::string, ompl::base::ProjectionEvaluatorPtr> &
751
756
757void ompl::base::StateSpace::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
758{
759 if (projection)
760 projections_[name] = projection;
761 else
762 OMPL_ERROR("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
763}
764
766{
767 return false;
768}
769
771{
772 return false;
773}
774
776{
777 return false;
778}
779
781{
782 return true;
783}
784
786{
787 return true;
788}
789
794
799
800ompl::base::StateSamplerPtr ompl::base::StateSpace::allocStateSampler() const
801{
802 if (ssa_)
803 return ssa_(this);
804 else
806}
807
808ompl::base::StateSamplerPtr ompl::base::StateSpace::allocSubspaceStateSampler(const StateSpacePtr &subspace) const
809{
810 return allocSubspaceStateSampler(subspace.get());
811}
812
813ompl::base::StateSamplerPtr ompl::base::StateSpace::allocSubspaceStateSampler(const StateSpace *subspace) const
814{
815 if (subspace->getName() == getName())
816 return allocStateSampler();
817 return std::make_shared<SubspaceStateSampler>(this, subspace, 1.0);
818}
819
821{
822 if (factor < 1)
823 throw Exception("The multiplicative factor for the valid segment count between two states must be strictly "
824 "positive");
826}
827
829{
830 if (segmentFraction < std::numeric_limits<double>::epsilon() ||
831 segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
832 throw Exception("The fraction of the extent must be larger than 0 and less than 1");
833 longestValidSegmentFraction_ = segmentFraction;
834}
835
840
845
850
851unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
852{
853 return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
854}
855
860
861ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
862 const std::vector<double> &weights)
863{
864 if (components.size() != weights.size())
865 throw Exception("Number of component spaces and weights are not the same");
866 setName("Compound" + getName());
867 for (unsigned int i = 0; i < components.size(); ++i)
868 addSubspace(components[i], weights[i]);
869}
870
872{
873 if (locked_)
874 throw Exception("This state space is locked. No further components can be added");
875 if (weight < 0.0)
876 throw Exception("Subspace weight cannot be negative");
877 components_.push_back(component);
878 weights_.push_back(weight);
879 weightSum_ += weight;
881}
882
884{
885 return true;
886}
887
889{
890 bool c = false;
891 bool d = false;
892 for (const auto &component : components_)
893 {
894 if (component->isHybrid())
895 return true;
896 if (component->isDiscrete())
897 d = true;
898 else
899 c = true;
900 }
901 return c && d;
902}
903
905{
906 return componentCount_;
907}
908
909const ompl::base::StateSpacePtr &ompl::base::CompoundStateSpace::getSubspace(const unsigned int index) const
910{
911 if (componentCount_ > index)
912 return components_[index];
913 else
914 throw Exception("Subspace index does not exist");
915}
916
917bool ompl::base::CompoundStateSpace::hasSubspace(const std::string &name) const
918{
919 for (const auto &component : components_)
920 if (component->getName() == name)
921 return true;
922 return false;
923}
924
925unsigned int ompl::base::CompoundStateSpace::getSubspaceIndex(const std::string &name) const
926{
927 for (unsigned int i = 0; i < componentCount_; ++i)
928 if (components_[i]->getName() == name)
929 return i;
930 throw Exception("Subspace " + name + " does not exist");
931}
932
933const ompl::base::StateSpacePtr &ompl::base::CompoundStateSpace::getSubspace(const std::string &name) const
934{
935 return components_[getSubspaceIndex(name)];
936}
937
938double ompl::base::CompoundStateSpace::getSubspaceWeight(const unsigned int index) const
939{
940 if (componentCount_ > index)
941 return weights_[index];
942 else
943 throw Exception("Subspace index does not exist");
944}
945
946double ompl::base::CompoundStateSpace::getSubspaceWeight(const std::string &name) const
947{
948 for (unsigned int i = 0; i < componentCount_; ++i)
949 if (components_[i]->getName() == name)
950 return weights_[i];
951 throw Exception("Subspace " + name + " does not exist");
952}
953
954void ompl::base::CompoundStateSpace::setSubspaceWeight(const unsigned int index, double weight)
955{
956 if (weight < 0.0)
957 throw Exception("Subspace weight cannot be negative");
958 if (componentCount_ > index)
959 {
960 weightSum_ += weight - weights_[index];
961 weights_[index] = weight;
962 }
963 else
964 throw Exception("Subspace index does not exist");
965}
966
967void ompl::base::CompoundStateSpace::setSubspaceWeight(const std::string &name, double weight)
968{
969 for (unsigned int i = 0; i < componentCount_; ++i)
970 if (components_[i]->getName() == name)
971 {
972 setSubspaceWeight(i, weight);
973 return;
974 }
975 throw Exception("Subspace " + name + " does not exist");
976}
977
978const std::vector<ompl::base::StateSpacePtr> &ompl::base::CompoundStateSpace::getSubspaces() const
979{
980 return components_;
981}
982
984{
985 return weights_;
986}
987
989{
990 unsigned int dim = 0;
991 for (unsigned int i = 0; i < componentCount_; ++i)
992 dim += components_[i]->getDimension();
993 return dim;
994}
995
997{
998 double e = 0.0;
999 for (unsigned int i = 0; i < componentCount_; ++i)
1000 if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
1001 e += weights_[i] * components_[i]->getMaximumExtent();
1002 return e;
1003}
1004
1006{
1007 double m = 1.0;
1008 for (unsigned int i = 0; i < componentCount_; ++i)
1009 if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
1010 m *= weights_[i] * components_[i]->getMeasure();
1011 return m;
1012}
1013
1015{
1016 auto *cstate = static_cast<CompoundState *>(state);
1017 for (unsigned int i = 0; i < componentCount_; ++i)
1018 components_[i]->enforceBounds(cstate->components[i]);
1019}
1020
1022{
1023 const auto *cstate = static_cast<const CompoundState *>(state);
1024 for (unsigned int i = 0; i < componentCount_; ++i)
1025 if (!components_[i]->satisfiesBounds(cstate->components[i]))
1026 return false;
1027 return true;
1028}
1029
1030void ompl::base::CompoundStateSpace::copyState(State *destination, const State *source) const
1031{
1032 auto *cdest = static_cast<CompoundState *>(destination);
1033 const auto *csrc = static_cast<const CompoundState *>(source);
1034 for (unsigned int i = 0; i < componentCount_; ++i)
1035 components_[i]->copyState(cdest->components[i], csrc->components[i]);
1036}
1037
1039{
1040 unsigned int l = 0;
1041 for (const auto &component : components_)
1042 l += component->getSerializationLength();
1043 return l;
1044}
1045
1046void ompl::base::CompoundStateSpace::serialize(void *serialization, const State *state) const
1047{
1048 const auto *cstate = static_cast<const CompoundState *>(state);
1049 unsigned int l = 0;
1050 for (unsigned int i = 0; i < componentCount_; ++i)
1051 {
1052 components_[i]->serialize(reinterpret_cast<char *>(serialization) + l, cstate->components[i]);
1053 l += components_[i]->getSerializationLength();
1054 }
1055}
1056
1057void ompl::base::CompoundStateSpace::deserialize(State *state, const void *serialization) const
1058{
1059 auto *cstate = static_cast<CompoundState *>(state);
1060 unsigned int l = 0;
1061 for (unsigned int i = 0; i < componentCount_; ++i)
1062 {
1063 components_[i]->deserialize(cstate->components[i], reinterpret_cast<const char *>(serialization) + l);
1064 l += components_[i]->getSerializationLength();
1065 }
1066}
1067
1068double ompl::base::CompoundStateSpace::distance(const State *state1, const State *state2) const
1069{
1070 const auto *cstate1 = static_cast<const CompoundState *>(state1);
1071 const auto *cstate2 = static_cast<const CompoundState *>(state2);
1072 double dist = 0.0;
1073 for (unsigned int i = 0; i < componentCount_; ++i)
1074 dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
1075 return dist;
1076}
1077
1079{
1081 for (const auto &component : components_)
1082 component->setLongestValidSegmentFraction(segmentFraction);
1083}
1084
1085unsigned int ompl::base::CompoundStateSpace::validSegmentCount(const State *state1, const State *state2) const
1086{
1087 const auto *cstate1 = static_cast<const CompoundState *>(state1);
1088 const auto *cstate2 = static_cast<const CompoundState *>(state2);
1089 unsigned int sc = 0;
1090 for (unsigned int i = 0; i < componentCount_; ++i)
1091 {
1092 unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
1093 if (sci > sc)
1094 sc = sci;
1095 }
1096 return sc;
1097}
1098
1099bool ompl::base::CompoundStateSpace::equalStates(const State *state1, const State *state2) const
1100{
1101 const auto *cstate1 = static_cast<const CompoundState *>(state1);
1102 const auto *cstate2 = static_cast<const CompoundState *>(state2);
1103 for (unsigned int i = 0; i < componentCount_; ++i)
1104 if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
1105 return false;
1106 return true;
1107}
1108
1109void ompl::base::CompoundStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
1110{
1111 const auto *cfrom = static_cast<const CompoundState *>(from);
1112 const auto *cto = static_cast<const CompoundState *>(to);
1113 auto *cstate = static_cast<CompoundState *>(state);
1114 for (unsigned int i = 0; i < componentCount_; ++i)
1115 components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
1116}
1117
1119{
1120 auto ss(std::make_shared<CompoundStateSampler>(this));
1121 if (weightSum_ < std::numeric_limits<double>::epsilon())
1122 for (unsigned int i = 0; i < componentCount_; ++i)
1123 ss->addSampler(components_[i]->allocStateSampler(), 1.0);
1124 else
1125 for (unsigned int i = 0; i < componentCount_; ++i)
1126 ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / weightSum_);
1127 return ss;
1128}
1129
1130ompl::base::StateSamplerPtr ompl::base::CompoundStateSpace::allocSubspaceStateSampler(const StateSpace *subspace) const
1131{
1132 if (subspace->getName() == getName())
1133 return allocStateSampler();
1134 if (hasSubspace(subspace->getName()))
1135 return std::make_shared<SubspaceStateSampler>(this, subspace,
1136 getSubspaceWeight(subspace->getName()) / weightSum_);
1138}
1139
1141{
1142 auto *state = new CompoundState();
1143 allocStateComponents(state);
1144 return static_cast<State *>(state);
1145}
1146
1148{
1149 state->components = new State *[componentCount_];
1150 for (unsigned int i = 0; i < componentCount_; ++i)
1151 state->components[i] = components_[i]->allocState();
1152}
1153
1155{
1156 auto *cstate = static_cast<CompoundState *>(state);
1157 for (unsigned int i = 0; i < componentCount_; ++i)
1158 components_[i]->freeState(cstate->components[i]);
1159 delete[] cstate->components;
1160 delete cstate;
1161}
1162
1164{
1165 locked_ = true;
1166}
1167
1169{
1170 return locked_;
1171}
1172
1173double *ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
1174{
1175 auto *cstate = static_cast<CompoundState *>(state);
1176 unsigned int idx = 0;
1177
1178 for (unsigned int i = 0; i < componentCount_; ++i)
1179 for (unsigned int j = 0; j <= index; ++j)
1180 {
1181 double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
1182 if (va)
1183 {
1184 if (idx == index)
1185 return va;
1186 else
1187 idx++;
1188 }
1189 else
1190 break;
1191 }
1192 return nullptr;
1193}
1194
1195void ompl::base::CompoundStateSpace::printState(const State *state, std::ostream &out) const
1196{
1197 out << "Compound state [" << std::endl;
1198 const auto *cstate = static_cast<const CompoundState *>(state);
1199 for (unsigned int i = 0; i < componentCount_; ++i)
1200 components_[i]->printState(cstate->components[i], out);
1201 out << "]" << std::endl;
1202}
1203
1205{
1206 out << "Compound state space '" << getName() << "' of dimension " << getDimension()
1207 << (isLocked() ? " (locked)" : "") << " [" << std::endl;
1208 for (unsigned int i = 0; i < componentCount_; ++i)
1209 {
1210 components_[i]->printSettings(out);
1211 out << " of weight " << weights_[i] << std::endl;
1212 }
1213 out << "]" << std::endl;
1214 printProjections(out);
1215}
1216
1218{
1219 for (const auto &component : components_)
1220 component->setup();
1221
1223}
1224
1226{
1228 for (const auto &component : components_)
1229 component->computeLocations();
1230}
1231
1232namespace ompl
1233{
1234 namespace base
1235 {
1237 const State *source)
1238 {
1239 return copyStateData(destS.get(), dest, sourceS.get(), source);
1240 }
1241
1243 const State *source)
1244 {
1245 // if states correspond to the same space, simply do copy
1246 if (destS->getName() == sourceS->getName())
1247 {
1248 if (dest != source)
1249 destS->copyState(dest, source);
1250 return ALL_DATA_COPIED;
1251 }
1252
1254
1255 // if "to" state is compound
1256 if (destS->isCompound())
1257 {
1258 const auto *compoundDestS = destS->as<CompoundStateSpace>();
1259 auto *compoundDest = dest->as<CompoundState>();
1260
1261 // if there is a subspace in "to" that corresponds to "from", set the data and return
1262 for (unsigned int i = 0; i < compoundDestS->getSubspaceCount(); ++i)
1263 if (compoundDestS->getSubspace(i)->getName() == sourceS->getName())
1264 {
1265 if (compoundDest->components[i] != source)
1266 compoundDestS->getSubspace(i)->copyState(compoundDest->components[i], source);
1267 return ALL_DATA_COPIED;
1268 }
1269
1270 // it could be there are further levels of compound spaces where the data can be set
1271 // so we call this function recursively
1272 for (unsigned int i = 0; i < compoundDestS->getSubspaceCount(); ++i)
1273 {
1274 AdvancedStateCopyOperation res = copyStateData(compoundDestS->getSubspace(i).get(),
1275 compoundDest->components[i], sourceS, source);
1276
1277 if (res != NO_DATA_COPIED)
1278 result = SOME_DATA_COPIED;
1279
1280 // if all data was copied, we stop
1281 if (res == ALL_DATA_COPIED)
1282 return ALL_DATA_COPIED;
1283 }
1284 }
1285
1286 // if we got to this point, it means that the data in "from" could not be copied as a chunk to "to"
1287 // it could be the case "from" is from a compound space as well, so we can copy parts of "from", as needed
1288 if (sourceS->isCompound())
1289 {
1290 const auto *compoundSourceS = sourceS->as<CompoundStateSpace>();
1291 const auto *compoundSource = source->as<CompoundState>();
1292
1293 unsigned int copiedComponents = 0;
1294
1295 // if there is a subspace in "to" that corresponds to "from", set the data and return
1296 for (unsigned int i = 0; i < compoundSourceS->getSubspaceCount(); ++i)
1297 {
1298 AdvancedStateCopyOperation res = copyStateData(destS, dest, compoundSourceS->getSubspace(i).get(),
1299 compoundSource->components[i]);
1300 if (res == ALL_DATA_COPIED)
1301 copiedComponents++;
1302 if (res != NO_DATA_COPIED)
1303 result = SOME_DATA_COPIED;
1304 }
1305
1306 // if each individual component got copied, then the entire data in "from" got copied
1307 if (copiedComponents == compoundSourceS->getSubspaceCount())
1308 result = ALL_DATA_COPIED;
1309 }
1310
1311 return result;
1312 }
1313
1315 const State *source, const std::vector<std::string> &subspaces)
1316 {
1317 return copyStateData(destS.get(), dest, sourceS.get(), source, subspaces);
1318 }
1319
1321 const State *source, const std::vector<std::string> &subspaces)
1322 {
1323 std::size_t copyCount = 0;
1324 const std::map<std::string, StateSpace::SubstateLocation> &destLoc = destS->getSubstateLocationsByName();
1325 const std::map<std::string, StateSpace::SubstateLocation> &sourceLoc =
1326 sourceS->getSubstateLocationsByName();
1327 for (const auto &subspace : subspaces)
1328 {
1329 auto dt = destLoc.find(subspace);
1330 if (dt != destLoc.end())
1331 {
1332 auto st = sourceLoc.find(subspace);
1333 if (st != sourceLoc.end())
1334 {
1335 dt->second.space->copyState(destS->getSubstateAtLocation(dest, dt->second),
1336 sourceS->getSubstateAtLocation(source, st->second));
1337 ++copyCount;
1338 }
1339 }
1340 }
1341 if (copyCount == subspaces.size())
1342 return ALL_DATA_COPIED;
1343 if (copyCount > 0)
1344 return SOME_DATA_COPIED;
1345 return NO_DATA_COPIED;
1346 }
1347
1349 inline bool StateSpaceHasContent(const StateSpacePtr &m)
1350 {
1351 if (!m)
1352 return false;
1353 if (m->getDimension() == 0 && m->getType() == STATE_SPACE_UNKNOWN && m->isCompound())
1354 {
1355 const unsigned int nc = m->as<CompoundStateSpace>()->getSubspaceCount();
1356 for (unsigned int i = 0; i < nc; ++i)
1357 if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubspace(i)))
1358 return true;
1359 return false;
1360 }
1361 return true;
1362 }
1364
1366 {
1367 if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
1368 return b;
1369
1370 if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
1371 return a;
1372
1373 std::vector<StateSpacePtr> components;
1374 std::vector<double> weights;
1375
1376 bool change = false;
1377 if (a)
1378 {
1379 bool used = false;
1380 if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1381 if (!csm_a->isLocked())
1382 {
1383 used = true;
1384 for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1385 {
1386 components.push_back(csm_a->getSubspace(i));
1387 weights.push_back(csm_a->getSubspaceWeight(i));
1388 }
1389 }
1390
1391 if (!used)
1392 {
1393 components.push_back(a);
1394 weights.push_back(1.0);
1395 }
1396 }
1397 if (b)
1398 {
1399 bool used = false;
1400 unsigned int size = components.size();
1401
1402 if (auto *csm_b = dynamic_cast<CompoundStateSpace *>(b.get()))
1403 if (!csm_b->isLocked())
1404 {
1405 used = true;
1406 for (unsigned int i = 0; i < csm_b->getSubspaceCount(); ++i)
1407 {
1408 bool ok = true;
1409 for (unsigned int j = 0; j < size; ++j)
1410 if (components[j]->getName() == csm_b->getSubspace(i)->getName())
1411 {
1412 ok = false;
1413 break;
1414 }
1415 if (ok)
1416 {
1417 components.push_back(csm_b->getSubspace(i));
1418 weights.push_back(csm_b->getSubspaceWeight(i));
1419 change = true;
1420 }
1421 }
1422 if (components.size() == csm_b->getSubspaceCount())
1423 return b;
1424 }
1425
1426 if (!used)
1427 {
1428 bool ok = true;
1429 for (unsigned int j = 0; j < size; ++j)
1430 if (components[j]->getName() == b->getName())
1431 {
1432 ok = false;
1433 break;
1434 }
1435 if (ok)
1436 {
1437 components.push_back(b);
1438 weights.push_back(1.0);
1439 change = true;
1440 }
1441 }
1442 }
1443
1444 if (!change && a)
1445 return a;
1446
1447 if (components.size() == 1)
1448 return components[0];
1449
1450 return std::make_shared<CompoundStateSpace>(components, weights);
1451 }
1452
1454 {
1455 std::vector<StateSpacePtr> components_a;
1456 std::vector<double> weights_a;
1457 std::vector<StateSpacePtr> components_b;
1458
1459 if (a)
1460 {
1461 bool used = false;
1462 if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1463 if (!csm_a->isLocked())
1464 {
1465 used = true;
1466 for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1467 {
1468 components_a.push_back(csm_a->getSubspace(i));
1469 weights_a.push_back(csm_a->getSubspaceWeight(i));
1470 }
1471 }
1472
1473 if (!used)
1474 {
1475 components_a.push_back(a);
1476 weights_a.push_back(1.0);
1477 }
1478 }
1479
1480 if (b)
1481 {
1482 bool used = false;
1483 if (auto *csm_b = dynamic_cast<CompoundStateSpace *>(b.get()))
1484 if (!csm_b->isLocked())
1485 {
1486 used = true;
1487 for (unsigned int i = 0; i < csm_b->getSubspaceCount(); ++i)
1488 components_b.push_back(csm_b->getSubspace(i));
1489 }
1490 if (!used)
1491 components_b.push_back(b);
1492 }
1493
1494 bool change = false;
1495 for (auto &i : components_b)
1496 for (unsigned int j = 0; j < components_a.size(); ++j)
1497 if (components_a[j]->getName() == i->getName())
1498 {
1499 components_a.erase(components_a.begin() + j);
1500 weights_a.erase(weights_a.begin() + j);
1501 change = true;
1502 break;
1503 }
1504
1505 if (!change && a)
1506 return a;
1507
1508 if (components_a.size() == 1)
1509 return components_a[0];
1510
1511 return std::make_shared<CompoundStateSpace>(components_a, weights_a);
1512 }
1513
1514 StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name)
1515 {
1516 std::vector<StateSpacePtr> components;
1517 std::vector<double> weights;
1518
1519 bool change = false;
1520 if (a)
1521 {
1522 bool used = false;
1523 if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1524 if (!csm_a->isLocked())
1525 {
1526 used = true;
1527 for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1528 {
1529 if (csm_a->getSubspace(i)->getName() == name)
1530 {
1531 change = true;
1532 continue;
1533 }
1534 components.push_back(csm_a->getSubspace(i));
1535 weights.push_back(csm_a->getSubspaceWeight(i));
1536 }
1537 }
1538
1539 if (!used)
1540 {
1541 if (a->getName() != name)
1542 {
1543 components.push_back(a);
1544 weights.push_back(1.0);
1545 }
1546 else
1547 change = true;
1548 }
1549 }
1550
1551 if (!change && a)
1552 return a;
1553
1554 if (components.size() == 1)
1555 return components[0];
1556
1557 return std::make_shared<CompoundStateSpace>(components, weights);
1558 }
1559
1561 {
1562 std::vector<StateSpacePtr> components_a;
1563 std::vector<double> weights_a;
1564 std::vector<StateSpacePtr> components_b;
1565 std::vector<double> weights_b;
1566
1567 if (a)
1568 {
1569 bool used = false;
1570 if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1571 if (!csm_a->isLocked())
1572 {
1573 used = true;
1574 for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1575 {
1576 components_a.push_back(csm_a->getSubspace(i));
1577 weights_a.push_back(csm_a->getSubspaceWeight(i));
1578 }
1579 }
1580
1581 if (!used)
1582 {
1583 components_a.push_back(a);
1584 weights_a.push_back(1.0);
1585 }
1586 }
1587
1588 if (b)
1589 {
1590 bool used = false;
1591 if (auto *csm_b = dynamic_cast<CompoundStateSpace *>(b.get()))
1592 if (!csm_b->isLocked())
1593 {
1594 used = true;
1595 for (unsigned int i = 0; i < csm_b->getSubspaceCount(); ++i)
1596 {
1597 components_b.push_back(csm_b->getSubspace(i));
1598 weights_b.push_back(csm_b->getSubspaceWeight(i));
1599 }
1600 }
1601
1602 if (!used)
1603 {
1604 components_b.push_back(b);
1605 weights_b.push_back(1.0);
1606 }
1607 }
1608
1609 std::vector<StateSpacePtr> components;
1610 std::vector<double> weights;
1611
1612 for (unsigned int i = 0; i < components_b.size(); ++i)
1613 {
1614 for (unsigned int j = 0; j < components_a.size(); ++j)
1615 if (components_a[j]->getName() == components_b[i]->getName())
1616 {
1617 components.push_back(components_b[i]);
1618 weights.push_back(std::max(weights_a[j], weights_b[i]));
1619 break;
1620 }
1621 }
1622
1623 if (a && components.size() == components_a.size())
1624 return a;
1625
1626 if (b && components.size() == components_b.size())
1627 return b;
1628
1629 if (components.size() == 1)
1630 return components[0];
1631
1632 return std::make_shared<CompoundStateSpace>(components, weights);
1633 }
1634 }
1635}
The exception type for ompl.
Definition Exception.h:47
A space to allow the composition of state spaces.
Definition StateSpace.h:574
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
unsigned int validSegmentCount(const State *state1, const State *state2) const override
Count how many segments of the "longest valid length" fit on the motion from state1 to state2....
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
void setSubspaceWeight(unsigned int index, double weight)
Set the weight of a subspace in the compound state space (used in distance computation)
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
CompoundStateSpace()
Construct an empty compound state space.
bool isLocked() const
Return true if the state space is locked. A value of true means that no further spaces can be added a...
const StateSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound state space.
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces.
double weightSum_
The sum of all the weights in weights_.
Definition StateSpace.h:744
bool locked_
Flag indicating whether adding further components is allowed or not.
Definition StateSpace.h:747
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
double getSubspaceWeight(unsigned int index) const
Get the weight of a subspace from the compound state space (used in distance computation)
bool isCompound() const override
Check if the state space is compound.
unsigned int getSubspaceIndex(const std::string &name) const
Get the index of a specific subspace from the compound state space.
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
void computeLocations() override
Compute the location information for various components of the state space. Either this function or s...
const std::vector< double > & getSubspaceWeights() const
Get the list of component weights.
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
unsigned int componentCount_
The number of components.
Definition StateSpace.h:738
StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
Allocate a sampler that actually samples only components that are part of subspace.
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
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...
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
double * getValueAddressAtIndex(State *state, unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
bool satisfiesBounds(const State *state) const override
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
State * allocState() const override
Allocate a state that can store a point in the described space.
void freeState(State *state) const override
Free the memory of the allocated state.
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
std::vector< double > weights_
The weight assigned to each component of the state space when computing the compound distance.
Definition StateSpace.h:741
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition StateSpace.h:735
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
void enforceBounds(State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
bool hasSubspace(const std::string &name) const
Check if a specific subspace is contained in this state space.
Definition of a compound state.
Definition State.h:87
State ** components
The components that make up a compound state.
Definition State.h:128
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
virtual bool satisfiesBounds(const State *state) const =0
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
virtual bool hasSymmetricInterpolate() const
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from,...
virtual void sanityChecks(double zero, double eps, unsigned int flags) const
Perform sanity checks for this state space. Throws an exception if failures are found.
virtual bool hasSymmetricDistance() const
Check if the distance function on this state space is symmetric, i.e. distance(s1,...
ParamSet params_
The set of parameters for this space.
Definition StateSpace.h:553
virtual void computeLocations()
Compute the location information for various components of the state space. Either this function or s...
virtual void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
std::map< std::string, SubstateLocation > substateLocationsByName_
All the known substat locations, by name.
Definition StateSpace.h:565
virtual double getLongestValidSegmentFraction() const
When performing discrete validation of motions, the length of the longest segment that does not requi...
ProjectionEvaluatorPtr getDefaultProjection() const
Get the default projection.
StateSamplerAllocator ssa_
An optional state sampler allocator.
Definition StateSpace.h:534
void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
Register a projection for this state space under a specified name.
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings()
virtual double * getValueAddressAtIndex(State *state, unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
virtual void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the state 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.
static const std::string DEFAULT_PROJECTION_NAME
The name used for the default projection.
Definition StateSpace.h:528
double * getValueAddressAtName(State *state, const std::string &name) const
Get a pointer to the double value in state that name points to.
virtual double getLongestValidSegmentLength() const
Get the longest valid segment at the time setup() was called.
void list(std::ostream &out) const
Print the list of all contained state space instances.
@ STATESPACE_DISTANCE_BOUND
Check whether the StateSpace::distance() is bounded by StateSpace::getExtent()
Definition StateSpace.h:152
@ STATESPACE_DISTANCE_DIFFERENT_STATES
Check whether the distances between non-equal states is strictly positive (StateSpace::distance())
Definition StateSpace.h:139
@ STATESPACE_INTERPOLATION
Check whether calling StateSpace::interpolate() works as expected.
Definition StateSpace.h:145
@ STATESPACE_RESPECT_BOUNDS
Check whether sampled states are always within bounds.
Definition StateSpace.h:155
@ STATESPACE_TRIANGLE_INEQUALITY
Check whether the triangle inequality holds when using StateSpace::interpolate() and StateSpace::dist...
Definition StateSpace.h:149
@ STATESPACE_DISTANCE_SYMMETRIC
Check whether the distance function is symmetric (StateSpace::distance())
Definition StateSpace.h:142
@ STATESPACE_SERIALIZATION
Check whether the StateSpace::serialize() and StateSpace::deserialize() work as expected.
Definition StateSpace.h:161
@ STATESPACE_ENFORCE_BOUNDS_NO_OP
Check that enforceBounds() does not modify the contents of states that are within bounds.
Definition StateSpace.h:158
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...
int type_
A type assigned for this state space.
Definition StateSpace.h:531
virtual bool equalStates(const State *state1, const State *state2) const =0
Checks whether two states are equal.
T * as()
Cast this instance to a desired type.
Definition StateSpace.h:87
virtual void copyFromReals(State *destination, const std::vector< double > &reals) const
Copy the values from reals to the state destination using getValueAddressAtLocation()
virtual bool isCompound() const
Check if the state space is compound.
virtual void enforceBounds(State *state) const =0
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
virtual StateSamplerPtr allocDefaultStateSampler() const =0
Allocate an instance of the default uniform state sampler for this space.
virtual unsigned int getValidSegmentCountFactor() const
Get the value used to multiply the return value of validSegmentCount().
StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
virtual bool isMetricSpace() const
Return true if the distance function associated with the space is a metric.
Definition StateSpace.h:183
double longestValidSegment_
The longest valid segment at the time setup() was called.
Definition StateSpace.h:543
virtual bool isDiscrete() const
Check if the set of states is discrete.
const std::map< std::string, ValueLocation > & getValueLocationsByName() const
Get the named locations of values of type double contained in a state from this space....
static void Diagram(std::ostream &out)
Print a Graphviz digraph that represents the containment diagram for all the instantiated state space...
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation....
virtual bool isHybrid() const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
const std::map< std::string, SubstateLocation > & getSubstateLocationsByName() const
Get the list of known substate locations (keys of the map corrspond to names of subspaces)
virtual void setValidSegmentCountFactor(unsigned int factor)
Set factor to be the value to multiply the return value of validSegmentCount(). By default,...
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection....
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...
State * getSubstateAtLocation(State *state, const SubstateLocation &loc) const
Get the substate of state that is pointed to by loc.
unsigned int longestValidSegmentCountFactor_
The factor to multiply the value returned by validSegmentCount(). Rarely used but useful for things l...
Definition StateSpace.h:547
void setStateSamplerAllocator(const StateSamplerAllocator &ssa)
Set the sampler allocator to use.
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
double * getValueAddressAtLocation(State *state, const ValueLocation &loc) const
Get a pointer to the double value in state that loc points to.
std::map< std::string, ProjectionEvaluatorPtr > projections_
List of available projections.
Definition StateSpace.h:550
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
bool hasDefaultProjection() const
Check if a default projection is available.
void clearStateSamplerAllocator()
Clear the state sampler allocator (reset to default)
virtual void copyState(State *destination, const State *source) const =0
Copy a state to another. The memory of source and destination should NOT overlap.
std::vector< ValueLocation > valueLocationsInOrder_
The value locations for all varliables of type double contained in a state; The locations point to va...
Definition StateSpace.h:557
std::map< std::string, ValueLocation > valueLocationsByName_
All the known value locations, by name. The names of state spaces access the first element of a state...
Definition StateSpace.h:562
virtual 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()
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
State * cloneState(const State *source) const
Clone a state.
virtual double distance(const State *state1, const State *state2) const =0
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
StateSpace()
Constructor. Assigns a unique name to the space.
void setName(const std::string &name)
Set the name of the state space.
static void List(std::ostream &out)
Print the list of available state space instances.
bool includes(const StateSpacePtr &other) const
Return true if other is a space included (perhaps equal, perhaps a subspace) in this one.
const std::map< std::string, ProjectionEvaluatorPtr > & getRegisteredProjections() const
Get all the registered projections.
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() const
Get the locations of values of type double contained in a state from this space. The order of the val...
const std::string & getName() const
Get the name of the state space.
ProjectionEvaluatorPtr getProjection(const std::string &name) const
Get the projection registered under a specific name.
virtual void interpolate(const State *from, const State *to, double t, State *state) const =0
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
virtual State * allocState() const =0
Allocate a state that can store a point in the described space.
double longestValidSegmentFraction_
The fraction of the longest valid segment.
Definition StateSpace.h:540
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
double maxExtent_
The extent of this space at the time setup() was called.
Definition StateSpace.h:537
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
virtual double getMaximumExtent() const =0
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
void diagram(std::ostream &out) const
Print a Graphviz digraph that represents the containment diagram for the state space.
bool hasProjection(const std::string &name) const
Check if a projection with a specified name is available.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
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...
AdvancedStateCopyOperation
The possible outputs for an advanced copy operation.
Definition StateSpace.h:787
@ ALL_DATA_COPIED
All data was copied.
Definition StateSpace.h:795
@ SOME_DATA_COPIED
Some data was copied.
Definition StateSpace.h:792
@ NO_DATA_COPIED
No data was copied.
Definition StateSpace.h:789
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
Construct a compound state space that contains subspaces only from a. If a is compound,...
This namespace contains sampling based planning routines shared by both planning under geometric cons...
std::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
@ STATE_SPACE_UNKNOWN
Unset type; this is the default type.
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
OptimizationObjectivePtr operator*(double weight, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
Main namespace. Contains everything in this library.
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition String.cpp:82
Representation of the address of a substate in a state. This structure stores the indexing informatio...
Definition StateSpace.h:108
const StateSpace * space
The space that is reached if the chain above is followed on the state space.
Definition StateSpace.h:117
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...
Definition StateSpace.h:114
Representation of the address of a value in a state. This structure stores the indexing information n...
Definition StateSpace.h:123
SubstateLocation stateLocation
Location of the substate that contains the pointed to value.
Definition StateSpace.h:125
std::size_t index
The index of the value to be accessed, within the substate location above.
Definition StateSpace.h:128