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 
52 namespace 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 
93  longestValidSegmentFraction_ = 0.01; // 1%
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 
110 ompl::base::StateSpace::~StateSpace()
111 {
112  AllocatedSpaces &as = getAllocatedSpaces();
113  std::lock_guard<std::mutex> smLock(as.lock_);
114  as.list_.remove(this);
115 }
116 
118 namespace 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,
139  StateSpace::ValueLocation loc)
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 
196 const std::string &ompl::base::StateSpace::getName() const
197 {
198  return name_;
199 }
200 
201 void 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())
211  computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
212 }
213 
215 {
216  computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
217 }
218 
219 void 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 
234 {
235 }
236 
238 {
239  maxExtent_ = getMaximumExtent();
240  longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
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 
251  computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
252 
253  // make sure we don't overwrite projections that have been configured by the user
254  std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
255  registerProjections();
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 
283 const std::map<std::string, ompl::base::StateSpace::SubstateLocation> &
285 {
286  return substateLocationsByName_;
287 }
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 
306 double *ompl::base::StateSpace::getValueAddressAtIndex(State * /*state*/, const unsigned int /*index*/) const
307 {
308  return nullptr;
309 }
310 
311 const 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 
318 const std::vector<ompl::base::StateSpace::ValueLocation> &ompl::base::StateSpace::getValueLocations() const
319 {
320  return valueLocationsInOrder_;
321 }
322 
323 const std::map<std::string, ompl::base::StateSpace::ValueLocation> &
325 {
326  return valueLocationsByName_;
327 }
328 
329 void 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 
337 void 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 
353 const 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 
361 double *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 
368 const 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 
380 void ompl::base::StateSpace::serialize(void * /*serialization*/, const State * /*state*/) const
381 {
382 }
383 
384 void ompl::base::StateSpace::deserialize(State * /*state*/, const void * /*serialization*/) const
385 {
386 }
387 
388 void ompl::base::StateSpace::printState(const State *state, std::ostream &out) const
389 {
390  out << "State instance [" << state << ']' << std::endl;
391 }
392 
393 void ompl::base::StateSpace::printSettings(std::ostream &out) const
394 {
395  out << "StateSpace '" << getName() << "' instance: " << this << std::endl;
396  printProjections(out);
397 }
398 
399 void 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 
420 namespace 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 
484 {
485  return StateSpaceCovers(this, other);
486 }
487 
489 {
490  return StateSpaceIncludes(this, other);
491 }
492 
493 void ompl::base::StateSpace::getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const
494 {
495  getCommonSubspaces(other.get(), subspaces);
496 }
497 
498 void 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 
527 void 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 
535 void 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 
553 void 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 
580 void 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 {
605  unsigned int flags = isMetricSpace() ? ~0 : ~(STATESPACE_DISTANCE_SYMMETRIC | STATESPACE_TRIANGLE_INEQUALITY);
606  sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), flags);
607 }
608 
609 void 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();
616  StateSamplerPtr ss = allocStateSampler();
617  char *serialization = nullptr;
618  if ((flags & STATESPACE_SERIALIZATION) && getSerializationLength() > 0)
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.");
633  if (flags & STATESPACE_ENFORCE_BOUNDS_NO_OP)
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
672  if (!isDiscrete() && !isHybrid() && (flags & (STATESPACE_INTERPOLATION | STATESPACE_TRIANGLE_INEQUALITY)))
673  {
674  State *s1 = allocState();
675  State *s2 = allocState();
676  State *s3 = allocState();
677  StateSamplerPtr ss = allocStateSampler();
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) && fabs(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 
714 {
715  return hasProjection(DEFAULT_PROJECTION_NAME);
716 }
717 
718 bool ompl::base::StateSpace::hasProjection(const std::string &name) const
719 {
720  return projections_.find(name) != projections_.end();
721 }
722 
724 {
725  if (hasDefaultProjection())
726  return getProjection(DEFAULT_PROJECTION_NAME);
727  else
728  {
729  OMPL_ERROR("No default projection is set. Perhaps setup() needs to be called");
730  return ProjectionEvaluatorPtr();
731  }
732 }
733 
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 
746 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr> &
748 {
749  return projections_;
750 }
751 
752 void ompl::base::StateSpace::registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
753 {
754  registerProjection(DEFAULT_PROJECTION_NAME, projection);
755 }
756 
757 void 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 
791 {
792  ssa_ = ssa;
793 }
794 
796 {
797  ssa_ = StateSamplerAllocator();
798 }
799 
801 {
802  if (ssa_)
803  return ssa_(this);
804  else
805  return allocDefaultStateSampler();
806 }
807 
809 {
810  return allocSubspaceStateSampler(subspace.get());
811 }
812 
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");
825  longestValidSegmentCountFactor_ = factor;
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 
837 {
838  return longestValidSegmentCountFactor_;
839 }
840 
842 {
843  return longestValidSegmentFraction_;
844 }
845 
847 {
848  return longestValidSegment_;
849 }
850 
851 unsigned 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 
857 {
858  setName("Compound" + getName());
859 }
860 
861 ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
862  const std::vector<double> &weights)
863  : StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
864 {
865  if (components.size() != weights.size())
866  throw Exception("Number of component spaces and weights are not the same");
867  setName("Compound" + getName());
868  for (unsigned int i = 0; i < components.size(); ++i)
869  addSubspace(components[i], weights[i]);
870 }
871 
872 void ompl::base::CompoundStateSpace::addSubspace(const StateSpacePtr &component, double weight)
873 {
874  if (locked_)
875  throw Exception("This state space is locked. No further components can be added");
876  if (weight < 0.0)
877  throw Exception("Subspace weight cannot be negative");
878  components_.push_back(component);
879  weights_.push_back(weight);
880  weightSum_ += weight;
881  componentCount_ = components_.size();
882 }
883 
885 {
886  return true;
887 }
888 
890 {
891  bool c = false;
892  bool d = false;
893  for (unsigned int i = 0; i < componentCount_; ++i)
894  {
895  if (components_[i]->isHybrid())
896  return true;
897  if (components_[i]->isDiscrete())
898  d = true;
899  else
900  c = true;
901  }
902  return c && d;
903 }
904 
906 {
907  return componentCount_;
908 }
909 
910 const ompl::base::StateSpacePtr &ompl::base::CompoundStateSpace::getSubspace(const unsigned int index) const
911 {
912  if (componentCount_ > index)
913  return components_[index];
914  else
915  throw Exception("Subspace index does not exist");
916 }
917 
918 bool ompl::base::CompoundStateSpace::hasSubspace(const std::string &name) const
919 {
920  for (unsigned int i = 0; i < componentCount_; ++i)
921  if (components_[i]->getName() == name)
922  return true;
923  return false;
924 }
925 
926 unsigned int ompl::base::CompoundStateSpace::getSubspaceIndex(const std::string &name) const
927 {
928  for (unsigned int i = 0; i < componentCount_; ++i)
929  if (components_[i]->getName() == name)
930  return i;
931  throw Exception("Subspace " + name + " does not exist");
932 }
933 
935 {
936  return components_[getSubspaceIndex(name)];
937 }
938 
939 double ompl::base::CompoundStateSpace::getSubspaceWeight(const unsigned int index) const
940 {
941  if (componentCount_ > index)
942  return weights_[index];
943  else
944  throw Exception("Subspace index does not exist");
945 }
946 
947 double ompl::base::CompoundStateSpace::getSubspaceWeight(const std::string &name) const
948 {
949  for (unsigned int i = 0; i < componentCount_; ++i)
950  if (components_[i]->getName() == name)
951  return weights_[i];
952  throw Exception("Subspace " + name + " does not exist");
953 }
954 
955 void ompl::base::CompoundStateSpace::setSubspaceWeight(const unsigned int index, double weight)
956 {
957  if (weight < 0.0)
958  throw Exception("Subspace weight cannot be negative");
959  if (componentCount_ > index)
960  {
961  weightSum_ += weight - weights_[index];
962  weights_[index] = weight;
963  }
964  else
965  throw Exception("Subspace index does not exist");
966 }
967 
968 void ompl::base::CompoundStateSpace::setSubspaceWeight(const std::string &name, double weight)
969 {
970  for (unsigned int i = 0; i < componentCount_; ++i)
971  if (components_[i]->getName() == name)
972  {
973  setSubspaceWeight(i, weight);
974  return;
975  }
976  throw Exception("Subspace " + name + " does not exist");
977 }
978 
979 const std::vector<ompl::base::StateSpacePtr> &ompl::base::CompoundStateSpace::getSubspaces() const
980 {
981  return components_;
982 }
983 
984 const std::vector<double> &ompl::base::CompoundStateSpace::getSubspaceWeights() const
985 {
986  return weights_;
987 }
988 
990 {
991  unsigned int dim = 0;
992  for (unsigned int i = 0; i < componentCount_; ++i)
993  dim += components_[i]->getDimension();
994  return dim;
995 }
996 
998 {
999  double e = 0.0;
1000  for (unsigned int i = 0; i < componentCount_; ++i)
1001  if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
1002  e += weights_[i] * components_[i]->getMaximumExtent();
1003  return e;
1004 }
1005 
1007 {
1008  double m = 1.0;
1009  for (unsigned int i = 0; i < componentCount_; ++i)
1010  if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
1011  m *= weights_[i] * components_[i]->getMeasure();
1012  return m;
1013 }
1014 
1015 void ompl::base::CompoundStateSpace::enforceBounds(State *state) const
1016 {
1017  auto *cstate = static_cast<CompoundState *>(state);
1018  for (unsigned int i = 0; i < componentCount_; ++i)
1019  components_[i]->enforceBounds(cstate->components[i]);
1020 }
1021 
1022 bool ompl::base::CompoundStateSpace::satisfiesBounds(const State *state) const
1023 {
1024  const auto *cstate = static_cast<const CompoundState *>(state);
1025  for (unsigned int i = 0; i < componentCount_; ++i)
1026  if (!components_[i]->satisfiesBounds(cstate->components[i]))
1027  return false;
1028  return true;
1029 }
1030 
1031 void ompl::base::CompoundStateSpace::copyState(State *destination, const State *source) const
1032 {
1033  auto *cdest = static_cast<CompoundState *>(destination);
1034  const auto *csrc = static_cast<const CompoundState *>(source);
1035  for (unsigned int i = 0; i < componentCount_; ++i)
1036  components_[i]->copyState(cdest->components[i], csrc->components[i]);
1037 }
1038 
1040 {
1041  unsigned int l = 0;
1042  for (unsigned int i = 0; i < componentCount_; ++i)
1043  l += components_[i]->getSerializationLength();
1044  return l;
1045 }
1046 
1047 void ompl::base::CompoundStateSpace::serialize(void *serialization, const State *state) const
1048 {
1049  const auto *cstate = static_cast<const CompoundState *>(state);
1050  unsigned int l = 0;
1051  for (unsigned int i = 0; i < componentCount_; ++i)
1052  {
1053  components_[i]->serialize(reinterpret_cast<char *>(serialization) + l, cstate->components[i]);
1054  l += components_[i]->getSerializationLength();
1055  }
1056 }
1057 
1058 void ompl::base::CompoundStateSpace::deserialize(State *state, const void *serialization) const
1059 {
1060  auto *cstate = static_cast<CompoundState *>(state);
1061  unsigned int l = 0;
1062  for (unsigned int i = 0; i < componentCount_; ++i)
1063  {
1064  components_[i]->deserialize(cstate->components[i], reinterpret_cast<const char *>(serialization) + l);
1065  l += components_[i]->getSerializationLength();
1066  }
1067 }
1068 
1069 double ompl::base::CompoundStateSpace::distance(const State *state1, const State *state2) const
1070 {
1071  const auto *cstate1 = static_cast<const CompoundState *>(state1);
1072  const auto *cstate2 = static_cast<const CompoundState *>(state2);
1073  double dist = 0.0;
1074  for (unsigned int i = 0; i < componentCount_; ++i)
1075  dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
1076  return dist;
1077 }
1078 
1080 {
1082  for (unsigned int i = 0; i < componentCount_; ++i)
1083  components_[i]->setLongestValidSegmentFraction(segmentFraction);
1084 }
1085 
1086 unsigned int ompl::base::CompoundStateSpace::validSegmentCount(const State *state1, const State *state2) const
1087 {
1088  const auto *cstate1 = static_cast<const CompoundState *>(state1);
1089  const auto *cstate2 = static_cast<const CompoundState *>(state2);
1090  unsigned int sc = 0;
1091  for (unsigned int i = 0; i < componentCount_; ++i)
1092  {
1093  unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
1094  if (sci > sc)
1095  sc = sci;
1096  }
1097  return sc;
1098 }
1099 
1100 bool ompl::base::CompoundStateSpace::equalStates(const State *state1, const State *state2) const
1101 {
1102  const auto *cstate1 = static_cast<const CompoundState *>(state1);
1103  const auto *cstate2 = static_cast<const CompoundState *>(state2);
1104  for (unsigned int i = 0; i < componentCount_; ++i)
1105  if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
1106  return false;
1107  return true;
1108 }
1109 
1110 void ompl::base::CompoundStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
1111 {
1112  const auto *cfrom = static_cast<const CompoundState *>(from);
1113  const auto *cto = static_cast<const CompoundState *>(to);
1114  auto *cstate = static_cast<CompoundState *>(state);
1115  for (unsigned int i = 0; i < componentCount_; ++i)
1116  components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
1117 }
1118 
1120 {
1121  auto ss(std::make_shared<CompoundStateSampler>(this));
1122  if (weightSum_ < std::numeric_limits<double>::epsilon())
1123  for (unsigned int i = 0; i < componentCount_; ++i)
1124  ss->addSampler(components_[i]->allocStateSampler(), 1.0);
1125  else
1126  for (unsigned int i = 0; i < componentCount_; ++i)
1127  ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / weightSum_);
1128  return ss;
1129 }
1130 
1132 {
1133  if (subspace->getName() == getName())
1134  return allocStateSampler();
1135  if (hasSubspace(subspace->getName()))
1136  return std::make_shared<SubspaceStateSampler>(this, subspace,
1137  getSubspaceWeight(subspace->getName()) / weightSum_);
1138  return StateSpace::allocSubspaceStateSampler(subspace);
1139 }
1140 
1142 {
1143  auto *state = new CompoundState();
1144  allocStateComponents(state);
1145  return static_cast<State *>(state);
1146 }
1147 
1148 void ompl::base::CompoundStateSpace::allocStateComponents(CompoundState *state) const
1149 {
1150  state->components = new State *[componentCount_];
1151  for (unsigned int i = 0; i < componentCount_; ++i)
1152  state->components[i] = components_[i]->allocState();
1153 }
1154 
1155 void ompl::base::CompoundStateSpace::freeState(State *state) const
1156 {
1157  auto *cstate = static_cast<CompoundState *>(state);
1158  for (unsigned int i = 0; i < componentCount_; ++i)
1159  components_[i]->freeState(cstate->components[i]);
1160  delete[] cstate->components;
1161  delete cstate;
1162 }
1163 
1165 {
1166  locked_ = true;
1167 }
1168 
1170 {
1171  return locked_;
1172 }
1173 
1174 double *ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
1175 {
1176  auto *cstate = static_cast<CompoundState *>(state);
1177  unsigned int idx = 0;
1178 
1179  for (unsigned int i = 0; i < componentCount_; ++i)
1180  for (unsigned int j = 0; j <= index; ++j)
1181  {
1182  double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
1183  if (va)
1184  {
1185  if (idx == index)
1186  return va;
1187  else
1188  idx++;
1189  }
1190  else
1191  break;
1192  }
1193  return nullptr;
1194 }
1195 
1196 void ompl::base::CompoundStateSpace::printState(const State *state, std::ostream &out) const
1197 {
1198  out << "Compound state [" << std::endl;
1199  const auto *cstate = static_cast<const CompoundState *>(state);
1200  for (unsigned int i = 0; i < componentCount_; ++i)
1201  components_[i]->printState(cstate->components[i], out);
1202  out << "]" << std::endl;
1203 }
1204 
1205 void ompl::base::CompoundStateSpace::printSettings(std::ostream &out) const
1206 {
1207  out << "Compound state space '" << getName() << "' of dimension " << getDimension()
1208  << (isLocked() ? " (locked)" : "") << " [" << std::endl;
1209  for (unsigned int i = 0; i < componentCount_; ++i)
1210  {
1211  components_[i]->printSettings(out);
1212  out << " of weight " << weights_[i] << std::endl;
1213  }
1214  out << "]" << std::endl;
1215  printProjections(out);
1216 }
1217 
1219 {
1220  for (unsigned int i = 0; i < componentCount_; ++i)
1221  components_[i]->setup();
1222 
1224 }
1225 
1227 {
1229  for (unsigned int i = 0; i < componentCount_; ++i)
1230  components_[i]->computeLocations();
1231 }
1232 
1233 namespace ompl
1234 {
1235  namespace base
1236  {
1237  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
1238  const State *source)
1239  {
1240  return copyStateData(destS.get(), dest, sourceS.get(), source);
1241  }
1242 
1243  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
1244  const State *source)
1245  {
1246  // if states correspond to the same space, simply do copy
1247  if (destS->getName() == sourceS->getName())
1248  {
1249  if (dest != source)
1250  destS->copyState(dest, source);
1251  return ALL_DATA_COPIED;
1252  }
1253 
1255 
1256  // if "to" state is compound
1257  if (destS->isCompound())
1258  {
1259  const auto *compoundDestS = destS->as<CompoundStateSpace>();
1260  auto *compoundDest = dest->as<CompoundState>();
1261 
1262  // if there is a subspace in "to" that corresponds to "from", set the data and return
1263  for (unsigned int i = 0; i < compoundDestS->getSubspaceCount(); ++i)
1264  if (compoundDestS->getSubspace(i)->getName() == sourceS->getName())
1265  {
1266  if (compoundDest->components[i] != source)
1267  compoundDestS->getSubspace(i)->copyState(compoundDest->components[i], source);
1268  return ALL_DATA_COPIED;
1269  }
1270 
1271  // it could be there are further levels of compound spaces where the data can be set
1272  // so we call this function recursively
1273  for (unsigned int i = 0; i < compoundDestS->getSubspaceCount(); ++i)
1274  {
1275  AdvancedStateCopyOperation res = copyStateData(compoundDestS->getSubspace(i).get(),
1276  compoundDest->components[i], sourceS, source);
1277 
1278  if (res != NO_DATA_COPIED)
1279  result = SOME_DATA_COPIED;
1280 
1281  // if all data was copied, we stop
1282  if (res == ALL_DATA_COPIED)
1283  return ALL_DATA_COPIED;
1284  }
1285  }
1286 
1287  // if we got to this point, it means that the data in "from" could not be copied as a chunk to "to"
1288  // it could be the case "from" is from a compound space as well, so we can copy parts of "from", as needed
1289  if (sourceS->isCompound())
1290  {
1291  const auto *compoundSourceS = sourceS->as<CompoundStateSpace>();
1292  const auto *compoundSource = source->as<CompoundState>();
1293 
1294  unsigned int copiedComponents = 0;
1295 
1296  // if there is a subspace in "to" that corresponds to "from", set the data and return
1297  for (unsigned int i = 0; i < compoundSourceS->getSubspaceCount(); ++i)
1298  {
1299  AdvancedStateCopyOperation res = copyStateData(destS, dest, compoundSourceS->getSubspace(i).get(),
1300  compoundSource->components[i]);
1301  if (res == ALL_DATA_COPIED)
1302  copiedComponents++;
1303  if (res != NO_DATA_COPIED)
1304  result = SOME_DATA_COPIED;
1305  }
1306 
1307  // if each individual component got copied, then the entire data in "from" got copied
1308  if (copiedComponents == compoundSourceS->getSubspaceCount())
1309  result = ALL_DATA_COPIED;
1310  }
1311 
1312  return result;
1313  }
1314 
1315  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
1316  const State *source, const std::vector<std::string> &subspaces)
1317  {
1318  return copyStateData(destS.get(), dest, sourceS.get(), source, subspaces);
1319  }
1320 
1321  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
1322  const State *source, const std::vector<std::string> &subspaces)
1323  {
1324  std::size_t copyCount = 0;
1325  const std::map<std::string, StateSpace::SubstateLocation> &destLoc = destS->getSubstateLocationsByName();
1326  const std::map<std::string, StateSpace::SubstateLocation> &sourceLoc =
1327  sourceS->getSubstateLocationsByName();
1328  for (const auto &subspace : subspaces)
1329  {
1330  auto dt = destLoc.find(subspace);
1331  if (dt != destLoc.end())
1332  {
1333  auto st = sourceLoc.find(subspace);
1334  if (st != sourceLoc.end())
1335  {
1336  dt->second.space->copyState(destS->getSubstateAtLocation(dest, dt->second),
1337  sourceS->getSubstateAtLocation(source, st->second));
1338  ++copyCount;
1339  }
1340  }
1341  }
1342  if (copyCount == subspaces.size())
1343  return ALL_DATA_COPIED;
1344  if (copyCount > 0)
1345  return SOME_DATA_COPIED;
1346  return NO_DATA_COPIED;
1347  }
1348 
1350  inline bool StateSpaceHasContent(const StateSpacePtr &m)
1351  {
1352  if (!m)
1353  return false;
1354  if (m->getDimension() == 0 && m->getType() == STATE_SPACE_UNKNOWN && m->isCompound())
1355  {
1356  const unsigned int nc = m->as<CompoundStateSpace>()->getSubspaceCount();
1357  for (unsigned int i = 0; i < nc; ++i)
1358  if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubspace(i)))
1359  return true;
1360  return false;
1361  }
1362  return true;
1363  }
1365 
1366  StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b)
1367  {
1368  if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
1369  return b;
1370 
1371  if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
1372  return a;
1373 
1374  std::vector<StateSpacePtr> components;
1375  std::vector<double> weights;
1376 
1377  bool change = false;
1378  if (a)
1379  {
1380  bool used = false;
1381  if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1382  if (!csm_a->isLocked())
1383  {
1384  used = true;
1385  for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1386  {
1387  components.push_back(csm_a->getSubspace(i));
1388  weights.push_back(csm_a->getSubspaceWeight(i));
1389  }
1390  }
1391 
1392  if (!used)
1393  {
1394  components.push_back(a);
1395  weights.push_back(1.0);
1396  }
1397  }
1398  if (b)
1399  {
1400  bool used = false;
1401  unsigned int size = components.size();
1402 
1403  if (auto *csm_b = dynamic_cast<CompoundStateSpace *>(b.get()))
1404  if (!csm_b->isLocked())
1405  {
1406  used = true;
1407  for (unsigned int i = 0; i < csm_b->getSubspaceCount(); ++i)
1408  {
1409  bool ok = true;
1410  for (unsigned int j = 0; j < size; ++j)
1411  if (components[j]->getName() == csm_b->getSubspace(i)->getName())
1412  {
1413  ok = false;
1414  break;
1415  }
1416  if (ok)
1417  {
1418  components.push_back(csm_b->getSubspace(i));
1419  weights.push_back(csm_b->getSubspaceWeight(i));
1420  change = true;
1421  }
1422  }
1423  if (components.size() == csm_b->getSubspaceCount())
1424  return b;
1425  }
1426 
1427  if (!used)
1428  {
1429  bool ok = true;
1430  for (unsigned int j = 0; j < size; ++j)
1431  if (components[j]->getName() == b->getName())
1432  {
1433  ok = false;
1434  break;
1435  }
1436  if (ok)
1437  {
1438  components.push_back(b);
1439  weights.push_back(1.0);
1440  change = true;
1441  }
1442  }
1443  }
1444 
1445  if (!change && a)
1446  return a;
1447 
1448  if (components.size() == 1)
1449  return components[0];
1450 
1451  return std::make_shared<CompoundStateSpace>(components, weights);
1452  }
1453 
1454  StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
1455  {
1456  std::vector<StateSpacePtr> components_a;
1457  std::vector<double> weights_a;
1458  std::vector<StateSpacePtr> components_b;
1459 
1460  if (a)
1461  {
1462  bool used = false;
1463  if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1464  if (!csm_a->isLocked())
1465  {
1466  used = true;
1467  for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1468  {
1469  components_a.push_back(csm_a->getSubspace(i));
1470  weights_a.push_back(csm_a->getSubspaceWeight(i));
1471  }
1472  }
1473 
1474  if (!used)
1475  {
1476  components_a.push_back(a);
1477  weights_a.push_back(1.0);
1478  }
1479  }
1480 
1481  if (b)
1482  {
1483  bool used = false;
1484  if (auto *csm_b = dynamic_cast<CompoundStateSpace *>(b.get()))
1485  if (!csm_b->isLocked())
1486  {
1487  used = true;
1488  for (unsigned int i = 0; i < csm_b->getSubspaceCount(); ++i)
1489  components_b.push_back(csm_b->getSubspace(i));
1490  }
1491  if (!used)
1492  components_b.push_back(b);
1493  }
1494 
1495  bool change = false;
1496  for (auto &i : components_b)
1497  for (unsigned int j = 0; j < components_a.size(); ++j)
1498  if (components_a[j]->getName() == i->getName())
1499  {
1500  components_a.erase(components_a.begin() + j);
1501  weights_a.erase(weights_a.begin() + j);
1502  change = true;
1503  break;
1504  }
1505 
1506  if (!change && a)
1507  return a;
1508 
1509  if (components_a.size() == 1)
1510  return components_a[0];
1511 
1512  return std::make_shared<CompoundStateSpace>(components_a, weights_a);
1513  }
1514 
1515  StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name)
1516  {
1517  std::vector<StateSpacePtr> components;
1518  std::vector<double> weights;
1519 
1520  bool change = false;
1521  if (a)
1522  {
1523  bool used = false;
1524  if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1525  if (!csm_a->isLocked())
1526  {
1527  used = true;
1528  for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1529  {
1530  if (csm_a->getSubspace(i)->getName() == name)
1531  {
1532  change = true;
1533  continue;
1534  }
1535  components.push_back(csm_a->getSubspace(i));
1536  weights.push_back(csm_a->getSubspaceWeight(i));
1537  }
1538  }
1539 
1540  if (!used)
1541  {
1542  if (a->getName() != name)
1543  {
1544  components.push_back(a);
1545  weights.push_back(1.0);
1546  }
1547  else
1548  change = true;
1549  }
1550  }
1551 
1552  if (!change && a)
1553  return a;
1554 
1555  if (components.size() == 1)
1556  return components[0];
1557 
1558  return std::make_shared<CompoundStateSpace>(components, weights);
1559  }
1560 
1561  StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b)
1562  {
1563  std::vector<StateSpacePtr> components_a;
1564  std::vector<double> weights_a;
1565  std::vector<StateSpacePtr> components_b;
1566  std::vector<double> weights_b;
1567 
1568  if (a)
1569  {
1570  bool used = false;
1571  if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1572  if (!csm_a->isLocked())
1573  {
1574  used = true;
1575  for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1576  {
1577  components_a.push_back(csm_a->getSubspace(i));
1578  weights_a.push_back(csm_a->getSubspaceWeight(i));
1579  }
1580  }
1581 
1582  if (!used)
1583  {
1584  components_a.push_back(a);
1585  weights_a.push_back(1.0);
1586  }
1587  }
1588 
1589  if (b)
1590  {
1591  bool used = false;
1592  if (auto *csm_b = dynamic_cast<CompoundStateSpace *>(b.get()))
1593  if (!csm_b->isLocked())
1594  {
1595  used = true;
1596  for (unsigned int i = 0; i < csm_b->getSubspaceCount(); ++i)
1597  {
1598  components_b.push_back(csm_b->getSubspace(i));
1599  weights_b.push_back(csm_b->getSubspaceWeight(i));
1600  }
1601  }
1602 
1603  if (!used)
1604  {
1605  components_b.push_back(b);
1606  weights_b.push_back(1.0);
1607  }
1608  }
1609 
1610  std::vector<StateSpacePtr> components;
1611  std::vector<double> weights;
1612 
1613  for (unsigned int i = 0; i < components_b.size(); ++i)
1614  {
1615  for (unsigned int j = 0; j < components_a.size(); ++j)
1616  if (components_a[j]->getName() == components_b[i]->getName())
1617  {
1618  components.push_back(components_b[i]);
1619  weights.push_back(std::max(weights_a[j], weights_b[i]));
1620  break;
1621  }
1622  }
1623 
1624  if (a && components.size() == components_a.size())
1625  return a;
1626 
1627  if (b && components.size() == components_b.size())
1628  return b;
1629 
1630  if (components.size() == 1)
1631  return components[0];
1632 
1633  return std::make_shared<CompoundStateSpace>(components, weights);
1634  }
1635  }
1636 }
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:361
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:201
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:226
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:219
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:345
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:535
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:84
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:380
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:478
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:214
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:233
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:196
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:324
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:473
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:329
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:384
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:393
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:388
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:375
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:237
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:306
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:284
State * getSubstateAtLocation(State *state, const SubstateLocation &loc) const
Get the substate of state that is pointed to by loc.
Definition: StateSpace.cpp:289
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.
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:337
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:318
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=[] { return T();})
This function declares a parameter name, and specifies the setter and getter functions.
Definition: GenericParam.h:231
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:399
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:527
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::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
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:553
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:493
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.