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 <mutex>
40 #include <boost/scoped_ptr.hpp>
41 #include <numeric>
42 #include <limits>
43 #include <queue>
44 #include <cmath>
45 #include <list>
46 #include <set>
47 
49 
51 namespace ompl
52 {
53  namespace base
54  {
55  namespace
56  {
57  struct AllocatedSpaces
58  {
59  AllocatedSpaces() = default;
60  std::list<StateSpace *> list_;
61  std::mutex lock_;
62  unsigned int counter_{0};
63  };
64 
65  static boost::scoped_ptr<AllocatedSpaces> g_allocatedSpaces;
66  static std::once_flag g_once;
67 
68  void initAllocatedSpaces()
69  {
70  g_allocatedSpaces.reset(new AllocatedSpaces);
71  }
72 
73  AllocatedSpaces &getAllocatedSpaces()
74  {
75  std::call_once(g_once, &initAllocatedSpaces);
76  return *g_allocatedSpaces;
77  }
78  } // namespace
79  }
80 }
82 
84 {
85  AllocatedSpaces &as = getAllocatedSpaces();
86  std::lock_guard<std::mutex> smLock(as.lock_);
87 
88  // autocompute a unique name
89  name_ = "Space" + std::to_string(as.counter_++);
90 
92  longestValidSegmentFraction_ = 0.01; // 1%
94 
96 
97  maxExtent_ = std::numeric_limits<double>::infinity();
98 
99  params_.declareParam<double>("longest_valid_segment_fraction",
100  [this](double segmentFraction) { setLongestValidSegmentFraction(segmentFraction); },
101  [this] { return getLongestValidSegmentFraction(); });
102 
103  params_.declareParam<unsigned int>("valid_segment_count_factor",
104  [this](unsigned int factor) { setValidSegmentCountFactor(factor); },
105  [this] { return getValidSegmentCountFactor(); });
106  as.list_.push_back(this);
107 }
108 
109 ompl::base::StateSpace::~StateSpace()
110 {
111  AllocatedSpaces &as = getAllocatedSpaces();
112  std::lock_guard<std::mutex> smLock(as.lock_);
113  as.list_.remove(this);
114 }
115 
117 namespace ompl
118 {
119  namespace base
120  {
121  static void computeStateSpaceSignatureHelper(const StateSpace *space, std::vector<int> &signature)
122  {
123  signature.push_back(space->getType());
124  signature.push_back(space->getDimension());
125 
126  if (space->isCompound())
127  {
128  unsigned int c = space->as<CompoundStateSpace>()->getSubspaceCount();
129  for (unsigned int i = 0; i < c; ++i)
130  computeStateSpaceSignatureHelper(space->as<CompoundStateSpace>()->getSubspace(i).get(), signature);
131  }
132  }
133 
134  void computeLocationsHelper(const StateSpace *s,
135  std::map<std::string, StateSpace::SubstateLocation> &substateMap,
136  std::vector<StateSpace::ValueLocation> &locationsArray,
137  std::map<std::string, StateSpace::ValueLocation> &locationsMap,
138  StateSpace::ValueLocation loc)
139  {
140  loc.stateLocation.space = s;
141  substateMap[s->getName()] = loc.stateLocation;
142  State *test = s->allocState();
143  if (s->getValueAddressAtIndex(test, 0) != nullptr)
144  {
145  loc.index = 0;
146  locationsMap[s->getName()] = loc;
147  // if the space is compound, we will find this value again in the first subspace
148  if (!s->isCompound())
149  {
150  if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
151  {
152  const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(0);
153  if (!name.empty())
154  locationsMap[name] = loc;
155  }
156  locationsArray.push_back(loc);
157  while (s->getValueAddressAtIndex(test, ++loc.index) != nullptr)
158  {
159  if (s->getType() == base::STATE_SPACE_REAL_VECTOR)
160  {
161  const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(loc.index);
162  if (!name.empty())
163  locationsMap[name] = loc;
164  }
165  locationsArray.push_back(loc);
166  }
167  }
168  }
169  s->freeState(test);
170 
171  if (s->isCompound())
172  for (unsigned int i = 0; i < s->as<base::CompoundStateSpace>()->getSubspaceCount(); ++i)
173  {
174  loc.stateLocation.chain.push_back(i);
175  computeLocationsHelper(s->as<base::CompoundStateSpace>()->getSubspace(i).get(), substateMap,
176  locationsArray, locationsMap, loc);
177  loc.stateLocation.chain.pop_back();
178  }
179  }
180 
181  void computeLocationsHelper(const StateSpace *s,
182  std::map<std::string, StateSpace::SubstateLocation> &substateMap,
183  std::vector<StateSpace::ValueLocation> &locationsArray,
184  std::map<std::string, StateSpace::ValueLocation> &locationsMap)
185  {
186  substateMap.clear();
187  locationsArray.clear();
188  locationsMap.clear();
189  computeLocationsHelper(s, substateMap, locationsArray, locationsMap, StateSpace::ValueLocation());
190  }
191  }
192 }
194 
195 const std::string &ompl::base::StateSpace::getName() const
196 {
197  return name_;
198 }
199 
200 void ompl::base::StateSpace::setName(const std::string &name)
201 {
202  name_ = name;
203 
204  // we don't want to call this function during the state space construction because calls to virtual functions are
205  // made,
206  // so we check if any values were previously inserted as value locations;
207  // if none were, then we either have none (so no need to call this function again)
208  // or setup() was not yet called
209  if (!valueLocationsInOrder_.empty())
210  computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
211 }
212 
214 {
215  computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
216 }
217 
218 void ompl::base::StateSpace::computeSignature(std::vector<int> &signature) const
219 {
220  signature.clear();
221  computeStateSpaceSignatureHelper(this, signature);
222  signature.insert(signature.begin(), signature.size());
223 }
224 
226 {
227  State *copy = allocState();
228  copyState(copy, source);
229  return copy;
230 }
231 
233 {
234 }
235 
237 {
238  maxExtent_ = getMaximumExtent();
239  longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
240 
241  if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
242  {
243  std::stringstream error;
244  error << "The longest valid segment for state space " + getName() + " must be positive." << std::endl;
245  error << "Space settings:" << std::endl;
246  printSettings(error);
247  throw Exception(error.str());
248  }
249 
250  computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
251 
252  // make sure we don't overwrite projections that have been configured by the user
253  std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
254  registerProjections();
255  for (auto &oldProjection : oldProjections)
256  if (oldProjection.second->userConfigured())
257  {
258  auto o = projections_.find(oldProjection.first);
259  if (o != projections_.end())
260  if (!o->second->userConfigured())
261  projections_[oldProjection.first] = oldProjection.second;
262  }
263 
264  // remove previously set parameters for projections
265  std::vector<std::string> pnames;
266  params_.getParamNames(pnames);
267  for (const auto &pname : pnames)
268  if (pname.substr(0, 11) == "projection.")
269  params_.remove(pname);
270 
271  // setup projections and add their parameters
272  for (const auto &projection : projections_)
273  {
274  projection.second->setup();
275  if (projection.first == DEFAULT_PROJECTION_NAME)
276  params_.include(projection.second->params(), "projection");
277  else
278  params_.include(projection.second->params(), "projection." + projection.first);
279  }
280 }
281 
282 const std::map<std::string, ompl::base::StateSpace::SubstateLocation> &
284 {
285  return substateLocationsByName_;
286 }
287 
289 {
290  std::size_t index = 0;
291  while (loc.chain.size() > index)
292  state = state->as<CompoundState>()->components[loc.chain[index++]];
293  return state;
294 }
295 
297  const SubstateLocation &loc) const
298 {
299  std::size_t index = 0;
300  while (loc.chain.size() > index)
301  state = state->as<CompoundState>()->components[loc.chain[index++]];
302  return state;
303 }
304 
305 double *ompl::base::StateSpace::getValueAddressAtIndex(State * /*state*/, const unsigned int /*index*/) const
306 {
307  return nullptr;
308 }
309 
310 const double *ompl::base::StateSpace::getValueAddressAtIndex(const State *state, const unsigned int index) const
311 {
312  double *val = getValueAddressAtIndex(const_cast<State *>(state),
313  index); // this const-cast does not hurt, since the state is not modified
314  return val;
315 }
316 
317 const std::vector<ompl::base::StateSpace::ValueLocation> &ompl::base::StateSpace::getValueLocations() const
318 {
319  return valueLocationsInOrder_;
320 }
321 
322 const std::map<std::string, ompl::base::StateSpace::ValueLocation> &
324 {
325  return valueLocationsByName_;
326 }
327 
328 void ompl::base::StateSpace::copyToReals(std::vector<double> &reals, const State *source) const
329 {
330  const auto &locations = getValueLocations();
331  reals.resize(locations.size());
332  for (std::size_t i = 0; i < locations.size(); ++i)
333  reals[i] = *getValueAddressAtLocation(source, locations[i]);
334 }
335 
336 void ompl::base::StateSpace::copyFromReals(State *destination, const std::vector<double> &reals) const
337 {
338  const auto &locations = getValueLocations();
339  assert(reals.size() == locations.size());
340  for (std::size_t i = 0; i < reals.size(); ++i)
341  *getValueAddressAtLocation(destination, locations[i]) = reals[i];
342 }
343 
345 {
346  std::size_t index = 0;
347  while (loc.stateLocation.chain.size() > index)
348  state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
349  return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
350 }
351 
352 const double *ompl::base::StateSpace::getValueAddressAtLocation(const State *state, const ValueLocation &loc) const
353 {
354  std::size_t index = 0;
355  while (loc.stateLocation.chain.size() > index)
356  state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
357  return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
358 }
359 
360 double *ompl::base::StateSpace::getValueAddressAtName(State *state, const std::string &name) const
361 {
362  const auto &locations = getValueLocationsByName();
363  auto it = locations.find(name);
364  return (it != locations.end()) ? getValueAddressAtLocation(state, it->second) : nullptr;
365 }
366 
367 const double *ompl::base::StateSpace::getValueAddressAtName(const State *state, const std::string &name) const
368 {
369  const auto &locations = getValueLocationsByName();
370  auto it = locations.find(name);
371  return (it != locations.end()) ? getValueAddressAtLocation(state, it->second) : nullptr;
372 }
373 
375 {
376  return 0;
377 }
378 
379 void ompl::base::StateSpace::serialize(void * /*serialization*/, const State * /*state*/) const
380 {
381 }
382 
383 void ompl::base::StateSpace::deserialize(State * /*state*/, const void * /*serialization*/) const
384 {
385 }
386 
387 void ompl::base::StateSpace::printState(const State *state, std::ostream &out) const
388 {
389  out << "State instance [" << state << ']' << std::endl;
390 }
391 
392 void ompl::base::StateSpace::printSettings(std::ostream &out) const
393 {
394  out << "StateSpace '" << getName() << "' instance: " << this << std::endl;
395  printProjections(out);
396 }
397 
398 void ompl::base::StateSpace::printProjections(std::ostream &out) const
399 {
400  if (projections_.empty())
401  out << "No registered projections" << std::endl;
402  else
403  {
404  out << "Registered projections:" << std::endl;
405  for (const auto &projection : projections_)
406  {
407  out << " - ";
408  if (projection.first == DEFAULT_PROJECTION_NAME)
409  out << "<default>";
410  else
411  out << projection.first;
412  out << std::endl;
413  projection.second->printSettings(out);
414  }
415  }
416 }
417 
419 namespace ompl
420 {
421  namespace base
422  {
423  static bool StateSpaceIncludes(const StateSpace *self, const StateSpace *other)
424  {
425  std::queue<const StateSpace *> q;
426  q.push(self);
427  while (!q.empty())
428  {
429  const StateSpace *m = q.front();
430  q.pop();
431  if (m->getName() == other->getName())
432  return true;
433  if (m->isCompound())
434  {
435  unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
436  for (unsigned int i = 0; i < c; ++i)
437  q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
438  }
439  }
440  return false;
441  }
442 
443  static bool StateSpaceCovers(const StateSpace *self, const StateSpace *other)
444  {
445  if (StateSpaceIncludes(self, other))
446  return true;
447  else if (other->isCompound())
448  {
449  unsigned int c = other->as<CompoundStateSpace>()->getSubspaceCount();
450  for (unsigned int i = 0; i < c; ++i)
451  if (!StateSpaceCovers(self, other->as<CompoundStateSpace>()->getSubspace(i).get()))
452  return false;
453  return true;
454  }
455  return false;
456  }
457 
458  struct CompareSubstateLocation
459  {
460  bool operator()(const StateSpace::SubstateLocation &a, const StateSpace::SubstateLocation &b) const
461  {
462  if (a.space->getDimension() != b.space->getDimension())
463  return a.space->getDimension() > b.space->getDimension();
464  return a.space->getName() > b.space->getName();
465  }
466  };
467  }
468 }
469 
471 
473 {
474  return StateSpaceCovers(this, other.get());
475 }
476 
478 {
479  return StateSpaceIncludes(this, other.get());
480 }
481 
483 {
484  return StateSpaceCovers(this, other);
485 }
486 
488 {
489  return StateSpaceIncludes(this, other);
490 }
491 
492 void ompl::base::StateSpace::getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const
493 {
494  getCommonSubspaces(other.get(), subspaces);
495 }
496 
497 void ompl::base::StateSpace::getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const
498 {
499  std::set<StateSpace::SubstateLocation, CompareSubstateLocation> intersection;
500  const std::map<std::string, StateSpace::SubstateLocation> &S = other->getSubstateLocationsByName();
501  for (const auto &it : substateLocationsByName_)
502  {
503  if (S.find(it.first) != S.end())
504  intersection.insert(it.second);
505  }
506 
507  bool found = true;
508  while (found)
509  {
510  found = false;
511  for (auto it = intersection.begin(); it != intersection.end(); ++it)
512  for (auto jt = intersection.begin(); jt != intersection.end(); ++jt)
513  if (it != jt)
514  if (StateSpaceCovers(it->space, jt->space))
515  {
516  intersection.erase(jt);
517  found = true;
518  break;
519  }
520  }
521  subspaces.clear();
522  for (const auto &it : intersection)
523  subspaces.push_back(it.space->getName());
524 }
525 
526 void ompl::base::StateSpace::List(std::ostream &out)
527 {
528  AllocatedSpaces &as = getAllocatedSpaces();
529  std::lock_guard<std::mutex> smLock(as.lock_);
530  for (auto &it : as.list_)
531  out << "@ " << it << ": " << it->getName() << std::endl;
532 }
533 
534 void ompl::base::StateSpace::list(std::ostream &out) const
535 {
536  std::queue<const StateSpace *> q;
537  q.push(this);
538  while (!q.empty())
539  {
540  const StateSpace *m = q.front();
541  q.pop();
542  out << "@ " << m << ": " << m->getName() << std::endl;
543  if (m->isCompound())
544  {
545  unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
546  for (unsigned int i = 0; i < c; ++i)
547  q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
548  }
549  }
550 }
551 
552 void ompl::base::StateSpace::diagram(std::ostream &out) const
553 {
554  out << "digraph StateSpace {" << std::endl;
555  out << '"' << getName() << '"' << std::endl;
556 
557  std::queue<const StateSpace *> q;
558  q.push(this);
559  while (!q.empty())
560  {
561  const StateSpace *m = q.front();
562  q.pop();
563  if (m->isCompound())
564  {
565  unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
566  for (unsigned int i = 0; i < c; ++i)
567  {
568  const StateSpace *s = m->as<CompoundStateSpace>()->getSubspace(i).get();
569  q.push(s);
570  out << '"' << m->getName() << R"(" -> ")" << s->getName() << R"(" [label=")"
571  << std::to_string(m->as<CompoundStateSpace>()->getSubspaceWeight(i)) << R"("];)" << std::endl;
572  }
573  }
574  }
575 
576  out << '}' << std::endl;
577 }
578 
579 void ompl::base::StateSpace::Diagram(std::ostream &out)
580 {
581  AllocatedSpaces &as = getAllocatedSpaces();
582  std::lock_guard<std::mutex> smLock(as.lock_);
583  out << "digraph StateSpaces {" << std::endl;
584  for (auto it = as.list_.begin(); it != as.list_.end(); ++it)
585  {
586  out << '"' << (*it)->getName() << '"' << std::endl;
587  for (auto jt = as.list_.begin(); jt != as.list_.end(); ++jt)
588  if (it != jt)
589  {
590  if ((*it)->isCompound() && (*it)->as<CompoundStateSpace>()->hasSubspace((*jt)->getName()))
591  out << '"' << (*it)->getName() << R"(" -> ")" << (*jt)->getName() << R"(" [label=")"
592  << std::to_string((*it)->as<CompoundStateSpace>()->getSubspaceWeight((*jt)->getName())) <<
593  R"("];)" << std::endl;
594  else if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
595  out << '"' << (*it)->getName() << R"(" -> ")" << (*jt)->getName() << R"(" [style=dashed];)"
596  << std::endl;
597  }
598  }
599  out << '}' << std::endl;
600 }
601 
603 {
604  unsigned int flags = isMetricSpace() ? ~0 : ~(STATESPACE_DISTANCE_SYMMETRIC | STATESPACE_TRIANGLE_INEQUALITY);
605  sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), flags);
606 }
607 
608 void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int flags) const
609 {
610  {
611  double maxExt = getMaximumExtent();
612 
613  State *s1 = allocState();
614  State *s2 = allocState();
615  StateSamplerPtr ss = allocStateSampler();
616  char *serialization = nullptr;
617  if ((flags & STATESPACE_SERIALIZATION) && getSerializationLength() > 0)
618  serialization = new char[getSerializationLength()];
619  for (unsigned int i = 0; i < magic::TEST_STATE_COUNT; ++i)
620  {
621  ss->sampleUniform(s1);
622  if (distance(s1, s1) > eps)
623  throw Exception("Distance from a state to itself should be 0");
624  if (!equalStates(s1, s1))
625  throw Exception("A state should be equal to itself");
626  if ((flags & STATESPACE_RESPECT_BOUNDS) && !satisfiesBounds(s1))
627  throw Exception("Sampled states should be within bounds");
628  copyState(s2, s1);
629  if (!equalStates(s1, s2))
630  throw Exception("Copy of a state is not the same as the original state. copyState() may not work "
631  "correctly.");
632  if (flags & STATESPACE_ENFORCE_BOUNDS_NO_OP)
633  {
634  enforceBounds(s1);
635  if (!equalStates(s1, s2))
636  throw Exception("enforceBounds() seems to modify states that are in fact within bounds.");
637  }
638  if (flags & STATESPACE_SERIALIZATION)
639  {
640  ss->sampleUniform(s2);
641  serialize(serialization, s1);
642  deserialize(s2, serialization);
643  if (!equalStates(s1, s2))
644  throw Exception("Serialization/deserialization operations do not seem to work as expected.");
645  }
646  ss->sampleUniform(s2);
647  if (!equalStates(s1, s2))
648  {
649  double d12 = distance(s1, s2);
650  if ((flags & STATESPACE_DISTANCE_DIFFERENT_STATES) && d12 < zero)
651  throw Exception("Distance between different states should be above 0");
652  double d21 = distance(s2, s1);
653  if ((flags & STATESPACE_DISTANCE_SYMMETRIC) && fabs(d12 - d21) > eps)
654  throw Exception("The distance function should be symmetric (A->B=" + std::to_string(d12) +
655  ", B->A=" + std::to_string(d21) + ", difference is " +
656  std::to_string(fabs(d12 - d21)) + ")");
657  if (flags & STATESPACE_DISTANCE_BOUND)
658  if (d12 > maxExt + zero)
659  throw Exception("The distance function should not report values larger than the maximum extent "
660  "(" +
661  std::to_string(d12) + " > " + std::to_string(maxExt) + ")");
662  }
663  }
664  if (serialization)
665  delete[] serialization;
666  freeState(s1);
667  freeState(s2);
668  }
669 
670  // Test that interpolation works as expected and also test triangle inequality
671  if (!isDiscrete() && !isHybrid() && (flags & (STATESPACE_INTERPOLATION | STATESPACE_TRIANGLE_INEQUALITY)))
672  {
673  State *s1 = allocState();
674  State *s2 = allocState();
675  State *s3 = allocState();
676  StateSamplerPtr ss = allocStateSampler();
677 
678  for (unsigned int i = 0; i < magic::TEST_STATE_COUNT; ++i)
679  {
680  ss->sampleUniform(s1);
681  ss->sampleUniform(s2);
682  ss->sampleUniform(s3);
683 
684  interpolate(s1, s2, 0.0, s3);
685  if ((flags & STATESPACE_INTERPOLATION) && distance(s1, s3) > eps)
686  throw Exception("Interpolation from a state at time 0 should be not change the original state");
687 
688  interpolate(s1, s2, 1.0, s3);
689  if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
690  throw Exception("Interpolation to a state at time 1 should be the same as the final state");
691 
692  interpolate(s1, s2, 0.5, s3);
693  double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
694  if ((flags & STATESPACE_TRIANGLE_INEQUALITY) && fabs(diff) > eps)
695  throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle "
696  "inequality (" +
697  std::to_string(diff) + " difference)");
698 
699  interpolate(s3, s2, 0.5, s3);
700  interpolate(s1, s2, 0.75, s2);
701 
702  if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
703  throw Exception("Continued interpolation does not work as expected. Please also check that "
704  "interpolate() works with overlapping memory for its state arguments");
705  }
706  freeState(s1);
707  freeState(s2);
708  freeState(s3);
709  }
710 }
711 
713 {
714  return hasProjection(DEFAULT_PROJECTION_NAME);
715 }
716 
717 bool ompl::base::StateSpace::hasProjection(const std::string &name) const
718 {
719  return projections_.find(name) != projections_.end();
720 }
721 
723 {
724  if (hasDefaultProjection())
725  return getProjection(DEFAULT_PROJECTION_NAME);
726  else
727  {
728  OMPL_ERROR("No default projection is set. Perhaps setup() needs to be called");
729  return ProjectionEvaluatorPtr();
730  }
731 }
732 
734 {
735  auto it = projections_.find(name);
736  if (it != projections_.end())
737  return it->second;
738  else
739  {
740  OMPL_ERROR("Projection '%s' is not defined", name.c_str());
741  return ProjectionEvaluatorPtr();
742  }
743 }
744 
745 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr> &
747 {
748  return projections_;
749 }
750 
751 void ompl::base::StateSpace::registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
752 {
753  registerProjection(DEFAULT_PROJECTION_NAME, projection);
754 }
755 
756 void ompl::base::StateSpace::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
757 {
758  if (projection)
759  projections_[name] = projection;
760  else
761  OMPL_ERROR("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
762 }
763 
765 {
766  return false;
767 }
768 
770 {
771  return false;
772 }
773 
775 {
776  return false;
777 }
778 
780 {
781  return true;
782 }
783 
785 {
786  return true;
787 }
788 
790 {
791  ssa_ = ssa;
792 }
793 
795 {
796  ssa_ = StateSamplerAllocator();
797 }
798 
800 {
801  if (ssa_)
802  return ssa_(this);
803  else
804  return allocDefaultStateSampler();
805 }
806 
808 {
809  return allocSubspaceStateSampler(subspace.get());
810 }
811 
813 {
814  if (subspace->getName() == getName())
815  return allocStateSampler();
816  return std::make_shared<SubspaceStateSampler>(this, subspace, 1.0);
817 }
818 
820 {
821  if (factor < 1)
822  throw Exception("The multiplicative factor for the valid segment count between two states must be strictly "
823  "positive");
824  longestValidSegmentCountFactor_ = factor;
825 }
826 
828 {
829  if (segmentFraction < std::numeric_limits<double>::epsilon() ||
830  segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
831  throw Exception("The fraction of the extent must be larger than 0 and less than 1");
832  longestValidSegmentFraction_ = segmentFraction;
833 }
834 
836 {
837  return longestValidSegmentCountFactor_;
838 }
839 
841 {
842  return longestValidSegmentFraction_;
843 }
844 
846 {
847  return longestValidSegment_;
848 }
849 
850 unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
851 {
852  return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
853 }
854 
856 {
857  setName("Compound" + getName());
858 }
859 
860 ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
861  const std::vector<double> &weights)
862  : StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
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 
871 void ompl::base::CompoundStateSpace::addSubspace(const StateSpacePtr &component, double weight)
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;
880  componentCount_ = components_.size();
881 }
882 
884 {
885  return true;
886 }
887 
889 {
890  bool c = false;
891  bool d = false;
892  for (unsigned int i = 0; i < componentCount_; ++i)
893  {
894  if (components_[i]->isHybrid())
895  return true;
896  if (components_[i]->isDiscrete())
897  d = true;
898  else
899  c = true;
900  }
901  return c && d;
902 }
903 
905 {
906  return componentCount_;
907 }
908 
909 const 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 
917 bool ompl::base::CompoundStateSpace::hasSubspace(const std::string &name) const
918 {
919  for (unsigned int i = 0; i < componentCount_; ++i)
920  if (components_[i]->getName() == name)
921  return true;
922  return false;
923 }
924 
925 unsigned 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 
934 {
935  return components_[getSubspaceIndex(name)];
936 }
937 
938 double 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 
946 double 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 
954 void 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 
967 void 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 
978 const std::vector<ompl::base::StateSpacePtr> &ompl::base::CompoundStateSpace::getSubspaces() const
979 {
980  return components_;
981 }
982 
983 const std::vector<double> &ompl::base::CompoundStateSpace::getSubspaceWeights() const
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 
1014 void ompl::base::CompoundStateSpace::enforceBounds(State *state) const
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 
1021 bool ompl::base::CompoundStateSpace::satisfiesBounds(const State *state) const
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 
1030 void 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 (unsigned int i = 0; i < componentCount_; ++i)
1042  l += components_[i]->getSerializationLength();
1043  return l;
1044 }
1045 
1046 void 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 
1057 void 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 
1068 double 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 (unsigned int i = 0; i < componentCount_; ++i)
1082  components_[i]->setLongestValidSegmentFraction(segmentFraction);
1083 }
1084 
1085 unsigned 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 
1099 bool 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 
1109 void 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 
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_);
1137  return StateSpace::allocSubspaceStateSampler(subspace);
1138 }
1139 
1141 {
1142  auto *state = new CompoundState();
1143  allocStateComponents(state);
1144  return static_cast<State *>(state);
1145 }
1146 
1147 void ompl::base::CompoundStateSpace::allocStateComponents(CompoundState *state) const
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 
1154 void ompl::base::CompoundStateSpace::freeState(State *state) const
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 
1173 double *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 
1195 void 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 
1204 void ompl::base::CompoundStateSpace::printSettings(std::ostream &out) const
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 (unsigned int i = 0; i < componentCount_; ++i)
1220  components_[i]->setup();
1221 
1223 }
1224 
1226 {
1228  for (unsigned int i = 0; i < componentCount_; ++i)
1229  components_[i]->computeLocations();
1230 }
1231 
1232 namespace ompl
1233 {
1234  namespace base
1235  {
1236  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
1237  const State *source)
1238  {
1239  return copyStateData(destS.get(), dest, sourceS.get(), source);
1240  }
1241 
1242  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
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 
1314  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
1315  const State *source, const std::vector<std::string> &subspaces)
1316  {
1317  return copyStateData(destS.get(), dest, sourceS.get(), source, subspaces);
1318  }
1319 
1320  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
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 
1365  StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b)
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 
1453  StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
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 
1560  StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b)
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 }
double * getValueAddressAtName(State *state, const std::string &name) const
Get a pointer to the double value in state that name points to.
Definition: StateSpace.cpp:360
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:200
State * allocState() const override
Allocate a state that can store a point in the described space.
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
State * cloneState(const State *source) const
Clone a state.
Definition: StateSpace.cpp:225
int type_
A type assigned for this state space.
Definition: StateSpace.h:531
ParamSet params_
The set of parameters for this space.
Definition: StateSpace.h:553
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...
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
Definition of a compound state.
Definition: State.h:86
ProjectionEvaluatorPtr getDefaultProjection() const
Get the default projection.
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume) ...
virtual 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.
Definition: StateSpace.cpp:218
static const std::string DEFAULT_PROJECTION_NAME
The name used for the default projection.
Definition: StateSpace.h:528
std::size_t index
The index of the value to be accessed, within the substate location above.
Definition: StateSpace.h:128
const std::map< std::string, ProjectionEvaluatorPtr > & getRegisteredProjections() const
Get all the registered projections.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
Allocate a sampler that actually samples only components that are part of subspace.
bool hasDefaultProjection() const
Check if a default projection is available.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
A shared pointer wrapper for ompl::base::StateSpace.
OptimizationObjectivePtr operator*(double weight, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
double * getValueAddressAtLocation(State *state, const ValueLocation &loc) const
Get a pointer to the double value in state that loc points to.
Definition: StateSpace.cpp:344
A shared pointer wrapper for ompl::base::StateSampler.
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
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
void list(std::ostream &out) const
Print the list of all contained state space instances.
Definition: StateSpace.cpp:534
Representation of the address of a value in a state. This structure stores the indexing information n...
Definition: StateSpace.h:122
No data was copied.
Definition: StateSpace.h:789
StateSpace()
Constructor. Assigns a unique name to the space.
Definition: StateSpace.cpp:83
void setSubspaceWeight(unsigned int index, double weight)
Set the weight of a subspace in the compound state space (used in distance computation) ...
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 void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
Definition: StateSpace.cpp:379
bool includes(const StateSpacePtr &other) const
Return true if other is a space included (perhaps equal, perhaps a subspace) in this one...
Definition: StateSpace.cpp:477
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 registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
Register a projection for this state space under a specified name.
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 isCompound() const override
Check if the state space is compound.
virtual void computeLocations()
Compute the location information for various components of the state space. Either this function or s...
Definition: StateSpace.cpp:213
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:232
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(). Rarely used but useful for things l...
Definition: StateSpace.h:547
double getSubspaceWeight(unsigned int index) const
Get the weight of a subspace from the compound state space (used in distance computation) ...
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:195
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
const std::map< std::string, ValueLocation > & getValueLocationsByName() 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.
Definition: StateSpace.cpp:323
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
All data was copied.
Definition: StateSpace.h:795
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...
Definition: StateSpace.cpp:472
void computeLocations() override
Compute the location information for various components of the state space. Either this function or s...
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
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() ...
Definition: StateSpace.cpp:328
virtual unsigned int getValidSegmentCountFactor() const
Get the value used to multiply the return value of validSegmentCount().
virtual bool hasSymmetricInterpolate() 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 deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
Definition: StateSpace.cpp:383
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
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 satisfiesBounds(const State *state) const override
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
Definition: StateSpace.cpp:392
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
virtual void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
Definition: StateSpace.cpp:387
A space to allow the composition of state spaces.
Definition: StateSpace.h:573
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Representation of the address of a substate in a state. This structure stores the indexing informatio...
Definition: StateSpace.h:107
virtual bool isCompound() const
Check if the state space is compound.
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
virtual bool isDiscrete() const
Check if the set of states is discrete.
bool hasProjection(const std::string &name) const
Check if a projection with a specified name is available.
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
Definition: StateSpace.cpp:374
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
ProjectionEvaluatorPtr getProjection(const std::string &name) const
Get the projection registered under a specific name.
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.
Definition: StateSpace.h:70
virtual void setup()
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.
Definition: StateSpace.cpp:236
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
Definition of an abstract state.
Definition: State.h:49
const StateSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound state space.
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...
Definition: StateSpace.cpp:305
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
const std::map< std::string, SubstateLocation > & getSubstateLocationsByName() const
Get the list of known substate locations (keys of the map corrspond to names of subspaces) ...
Definition: StateSpace.cpp:283
State * getSubstateAtLocation(State *state, const SubstateLocation &loc) const
Get the substate of state that is pointed to by loc.
Definition: StateSpace.cpp:288
void setStateSamplerAllocator(const StateSamplerAllocator &ssa)
Set the sampler allocator to use.
Some data was copied.
Definition: StateSpace.h:792
virtual double getMeasure() const =0
Get a measure of the space (this can be thought of as a generalization of volume) ...
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.
Definition: Exception.h:46
double maxExtent_
The extent of this space at the time setup() was called.
Definition: StateSpace.h:537
bool hasSubspace(const std::string &name) const
Check if a specific subspace is contained in this state space.
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.
Definition: GenericParam.h:232
virtual double getLongestValidSegmentLength() const
Get the longest valid segment at the time setup() was called.
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
bool isLocked() const
Return true if the state space is locked. A value of true means that no further spaces can be added a...
virtual void copyFromReals(State *destination, const std::vector< double > &reals) const
Copy the values from reals to the state destination using getValueAddressAtLocation() ...
Definition: StateSpace.cpp:336
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...
Definition: StateSpace.cpp:317
ompl::base::RealVectorStateSpace
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
SubstateLocation stateLocation
Location of the substate that contains the pointed to value.
Definition: StateSpace.h:125
double longestValidSegmentFraction_
The fraction of the longest valid segment.
Definition: StateSpace.h:540
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings() ...
Definition: StateSpace.cpp:398
State ** components
The components that make up a compound state.
Definition: State.h:128
static void List(std::ostream &out)
Print the list of available state space instances.
Definition: StateSpace.cpp:526
virtual bool isHybrid() const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
virtual bool hasSymmetricDistance() const
Check if the distance function on this state space is symmetric, i.e. distance(s1,s2) = distance(s2,s1). Default implementation returns true.
void freeState(State *state) const override
Free the memory of the allocated state.
const std::vector< double > & getSubspaceWeights() const
Get the list of component weights.
double longestValidSegment_
The longest valid segment at the time setup() was called.
Definition: StateSpace.h:543
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
CompoundStateSpace()
Construct an empty compound state space.
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
std::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Definition: StateSampler.h:191
unsigned int getSubspaceIndex(const std::string &name) const
Get the index of a specific subspace from the compound state space.
void diagram(std::ostream &out) const
Print a Graphviz digraph that represents the containment diagram for the state space.
Definition: StateSpace.cpp:552
const StateSpace * space
The space that is reached if the chain above is followed on the state space.
Definition: StateSpace.h:117
AdvancedStateCopyOperation
The possible outputs for an advanced copy operation.
Definition: StateSpace.h:786
static void Diagram(std::ostream &out)
Print a Graphviz digraph that represents the containment diagram for all the instantiated 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...
Definition: StateSpace.cpp:492
virtual void setValidSegmentCountFactor(unsigned int factor)
Set factor to be the value to multiply the return value of validSegmentCount(). By default...
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...
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...
void clearStateSamplerAllocator()
Clear the state sampler allocator (reset to default)
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces...
virtual double getLongestValidSegmentFraction() const
When performing discrete validation of motions, the length of the longest segment that does not requi...
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
void setup() override
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.
Unset type; this is the default type.