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 
91  longestValidSegment_ = 0.0;
92  longestValidSegmentFraction_ = 0.01; // 1%
93  longestValidSegmentCountFactor_ = 1;
94 
95  type_ = STATE_SPACE_UNKNOWN;
96 
97  maxExtent_ = std::numeric_limits<double>::infinity();
98 
99  params_.declareParam<double>("longest_valid_segment_fraction",
100  [this](double segmentFraction)
101  {
102  setLongestValidSegmentFraction(segmentFraction);
103  },
104  [this]
105  {
106  return getLongestValidSegmentFraction();
107  });
108 
109  params_.declareParam<unsigned int>("valid_segment_count_factor",
110  [this](unsigned int factor)
111  {
112  setValidSegmentCountFactor(factor);
113  },
114  [this]
115  {
116  return getValidSegmentCountFactor();
117  });
118  as.list_.push_back(this);
119 }
120 
121 ompl::base::StateSpace::~StateSpace()
122 {
123  AllocatedSpaces &as = getAllocatedSpaces();
124  std::lock_guard<std::mutex> smLock(as.lock_);
125  as.list_.remove(this);
126 }
127 
129 namespace ompl
130 {
131  namespace base
132  {
133  static void computeStateSpaceSignatureHelper(const StateSpace *space, std::vector<int> &signature)
134  {
135  signature.push_back(space->getType());
136  signature.push_back(space->getDimension());
137 
138  if (space->isCompound())
139  {
140  unsigned int c = space->as<CompoundStateSpace>()->getSubspaceCount();
141  for (unsigned int i = 0; i < c; ++i)
142  computeStateSpaceSignatureHelper(space->as<CompoundStateSpace>()->getSubspace(i).get(), signature);
143  }
144  }
145 
146  void computeLocationsHelper(const StateSpace *s,
147  std::map<std::string, StateSpace::SubstateLocation> &substateMap,
148  std::vector<StateSpace::ValueLocation> &locationsArray,
149  std::map<std::string, StateSpace::ValueLocation> &locationsMap,
151  {
152  loc.stateLocation.space = s;
153  substateMap[s->getName()] = loc.stateLocation;
154  State *test = s->allocState();
155  if (s->getValueAddressAtIndex(test, 0) != nullptr)
156  {
157  loc.index = 0;
158  locationsMap[s->getName()] = loc;
159  // if the space is compound, we will find this value again in the first subspace
160  if (!s->isCompound())
161  {
163  {
164  const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(0);
165  if (!name.empty())
166  locationsMap[name] = loc;
167  }
168  locationsArray.push_back(loc);
169  while (s->getValueAddressAtIndex(test, ++loc.index) != nullptr)
170  {
172  {
173  const std::string &name = s->as<base::RealVectorStateSpace>()->getDimensionName(loc.index);
174  if (!name.empty())
175  locationsMap[name] = loc;
176  }
177  locationsArray.push_back(loc);
178  }
179  }
180  }
181  s->freeState(test);
182 
183  if (s->isCompound())
184  for (unsigned int i = 0; i < s->as<base::CompoundStateSpace>()->getSubspaceCount(); ++i)
185  {
186  loc.stateLocation.chain.push_back(i);
187  computeLocationsHelper(s->as<base::CompoundStateSpace>()->getSubspace(i).get(), substateMap,
188  locationsArray, locationsMap, loc);
189  loc.stateLocation.chain.pop_back();
190  }
191  }
192 
193  void computeLocationsHelper(const StateSpace *s,
194  std::map<std::string, StateSpace::SubstateLocation> &substateMap,
195  std::vector<StateSpace::ValueLocation> &locationsArray,
196  std::map<std::string, StateSpace::ValueLocation> &locationsMap)
197  {
198  substateMap.clear();
199  locationsArray.clear();
200  locationsMap.clear();
201  computeLocationsHelper(s, substateMap, locationsArray, locationsMap, StateSpace::ValueLocation());
202  }
203  }
204 }
206 
207 const std::string &ompl::base::StateSpace::getName() const
208 {
209  return name_;
210 }
211 
212 void ompl::base::StateSpace::setName(const std::string &name)
213 {
214  name_ = name;
215 
216  // we don't want to call this function during the state space construction because calls to virtual functions are
217  // made,
218  // so we check if any values were previously inserted as value locations;
219  // if none were, then we either have none (so no need to call this function again)
220  // or setup() was not yet called
221  if (!valueLocationsInOrder_.empty())
222  computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
223 }
224 
226 {
227  computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
228 }
229 
230 void ompl::base::StateSpace::computeSignature(std::vector<int> &signature) const
231 {
232  signature.clear();
233  computeStateSpaceSignatureHelper(this, signature);
234  signature.insert(signature.begin(), signature.size());
235 }
236 
238 {
239  State *copy = allocState();
240  copyState(copy, source);
241  return copy;
242 }
243 
245 {
246 }
247 
249 {
250  maxExtent_ = getMaximumExtent();
251  longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
252 
253  if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
254  {
255  std::stringstream error;
256  error << "The longest valid segment for state space " + getName() + " must be positive." << std::endl;
257  error << "Space settings:" << std::endl;
258  printSettings(error);
259  throw Exception(error.str());
260  }
261 
262  computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
263 
264  // make sure we don't overwrite projections that have been configured by the user
265  std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
266  registerProjections();
267  for (auto &oldProjection : oldProjections)
268  if (oldProjection.second->userConfigured())
269  {
270  auto o = projections_.find(oldProjection.first);
271  if (o != projections_.end())
272  if (!o->second->userConfigured())
273  projections_[oldProjection.first] = oldProjection.second;
274  }
275 
276  // remove previously set parameters for projections
277  std::vector<std::string> pnames;
278  params_.getParamNames(pnames);
279  for (const auto &pname : pnames)
280  if (pname.substr(0, 11) == "projection.")
281  params_.remove(pname);
282 
283  // setup projections and add their parameters
284  for (const auto &projection : projections_)
285  {
286  projection.second->setup();
287  if (projection.first == DEFAULT_PROJECTION_NAME)
288  params_.include(projection.second->params(), "projection");
289  else
290  params_.include(projection.second->params(), "projection." + projection.first);
291  }
292 }
293 
294 const std::map<std::string, ompl::base::StateSpace::SubstateLocation> &
296 {
297  return substateLocationsByName_;
298 }
299 
301 {
302  std::size_t index = 0;
303  while (loc.chain.size() > index)
304  state = state->as<CompoundState>()->components[loc.chain[index++]];
305  return state;
306 }
307 
309  const SubstateLocation &loc) const
310 {
311  std::size_t index = 0;
312  while (loc.chain.size() > index)
313  state = state->as<CompoundState>()->components[loc.chain[index++]];
314  return state;
315 }
316 
317 double *ompl::base::StateSpace::getValueAddressAtIndex(State * /*state*/, const unsigned int /*index*/) const
318 {
319  return nullptr;
320 }
321 
322 const double *ompl::base::StateSpace::getValueAddressAtIndex(const State *state, const unsigned int index) const
323 {
324  double *val = getValueAddressAtIndex(const_cast<State *>(state),
325  index); // this const-cast does not hurt, since the state is not modified
326  return val;
327 }
328 
329 const std::vector<ompl::base::StateSpace::ValueLocation> &ompl::base::StateSpace::getValueLocations() const
330 {
331  return valueLocationsInOrder_;
332 }
333 
334 const std::map<std::string, ompl::base::StateSpace::ValueLocation> &
336 {
337  return valueLocationsByName_;
338 }
339 
340 void ompl::base::StateSpace::copyToReals(std::vector<double> &reals, const State *source) const
341 {
342  reals.resize(valueLocationsInOrder_.size());
343  for (std::size_t i = 0; i < valueLocationsInOrder_.size(); ++i)
344  reals[i] = *getValueAddressAtLocation(source, valueLocationsInOrder_[i]);
345 }
346 
347 void ompl::base::StateSpace::copyFromReals(State *destination, const std::vector<double> &reals) const
348 {
349  assert(reals.size() == valueLocationsInOrder_.size());
350  for (std::size_t i = 0; i < reals.size(); ++i)
351  *getValueAddressAtLocation(destination, valueLocationsInOrder_[i]) = reals[i];
352 }
353 
355 {
356  std::size_t index = 0;
357  while (loc.stateLocation.chain.size() > index)
358  state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
359  return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
360 }
361 
362 const double *ompl::base::StateSpace::getValueAddressAtLocation(const State *state, const ValueLocation &loc) const
363 {
364  std::size_t index = 0;
365  while (loc.stateLocation.chain.size() > index)
366  state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
367  return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
368 }
369 
370 double *ompl::base::StateSpace::getValueAddressAtName(State *state, const std::string &name) const
371 {
372  auto it = valueLocationsByName_.find(name);
373  return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : nullptr;
374 }
375 
376 const double *ompl::base::StateSpace::getValueAddressAtName(const State *state, const std::string &name) const
377 {
378  auto it = valueLocationsByName_.find(name);
379  return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : nullptr;
380 }
381 
383 {
384  return 0;
385 }
386 
387 void ompl::base::StateSpace::serialize(void * /*serialization*/, const State * /*state*/) const
388 {
389 }
390 
391 void ompl::base::StateSpace::deserialize(State * /*state*/, const void * /*serialization*/) const
392 {
393 }
394 
395 void ompl::base::StateSpace::printState(const State *state, std::ostream &out) const
396 {
397  out << "State instance [" << state << ']' << std::endl;
398 }
399 
400 void ompl::base::StateSpace::printSettings(std::ostream &out) const
401 {
402  out << "StateSpace '" << getName() << "' instance: " << this << std::endl;
403  printProjections(out);
404 }
405 
406 void ompl::base::StateSpace::printProjections(std::ostream &out) const
407 {
408  if (projections_.empty())
409  out << "No registered projections" << std::endl;
410  else
411  {
412  out << "Registered projections:" << std::endl;
413  for (const auto &projection : projections_)
414  {
415  out << " - ";
416  if (projection.first == DEFAULT_PROJECTION_NAME)
417  out << "<default>";
418  else
419  out << projection.first;
420  out << std::endl;
421  projection.second->printSettings(out);
422  }
423  }
424 }
425 
427 namespace ompl
428 {
429  namespace base
430  {
431  static bool StateSpaceIncludes(const StateSpace *self, const StateSpace *other)
432  {
433  std::queue<const StateSpace *> q;
434  q.push(self);
435  while (!q.empty())
436  {
437  const StateSpace *m = q.front();
438  q.pop();
439  if (m->getName() == other->getName())
440  return true;
441  if (m->isCompound())
442  {
443  unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
444  for (unsigned int i = 0; i < c; ++i)
445  q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
446  }
447  }
448  return false;
449  }
450 
451  static bool StateSpaceCovers(const StateSpace *self, const StateSpace *other)
452  {
453  if (StateSpaceIncludes(self, other))
454  return true;
455  else if (other->isCompound())
456  {
457  unsigned int c = other->as<CompoundStateSpace>()->getSubspaceCount();
458  for (unsigned int i = 0; i < c; ++i)
459  if (!StateSpaceCovers(self, other->as<CompoundStateSpace>()->getSubspace(i).get()))
460  return false;
461  return true;
462  }
463  return false;
464  }
465 
466  struct CompareSubstateLocation
467  {
468  bool operator()(const StateSpace::SubstateLocation &a, const StateSpace::SubstateLocation &b) const
469  {
470  if (a.space->getDimension() != b.space->getDimension())
471  return a.space->getDimension() > b.space->getDimension();
472  return a.space->getName() > b.space->getName();
473  }
474  };
475  }
476 }
477 
479 
481 {
482  return StateSpaceCovers(this, other.get());
483 }
484 
486 {
487  return StateSpaceIncludes(this, other.get());
488 }
489 
491 {
492  return StateSpaceCovers(this, other);
493 }
494 
496 {
497  return StateSpaceIncludes(this, other);
498 }
499 
500 void ompl::base::StateSpace::getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const
501 {
502  getCommonSubspaces(other.get(), subspaces);
503 }
504 
505 void ompl::base::StateSpace::getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const
506 {
507  std::set<StateSpace::SubstateLocation, CompareSubstateLocation> intersection;
508  const std::map<std::string, StateSpace::SubstateLocation> &S = other->getSubstateLocationsByName();
509  for (const auto &it : substateLocationsByName_)
510  {
511  if (S.find(it.first) != S.end())
512  intersection.insert(it.second);
513  }
514 
515  bool found = true;
516  while (found)
517  {
518  found = false;
519  for (auto it = intersection.begin(); it != intersection.end(); ++it)
520  for (auto jt = intersection.begin(); jt != intersection.end(); ++jt)
521  if (it != jt)
522  if (StateSpaceCovers(it->space, jt->space))
523  {
524  intersection.erase(jt);
525  found = true;
526  break;
527  }
528  }
529  subspaces.clear();
530  for (const auto &it : intersection)
531  subspaces.push_back(it.space->getName());
532 }
533 
534 void ompl::base::StateSpace::List(std::ostream &out)
535 {
536  AllocatedSpaces &as = getAllocatedSpaces();
537  std::lock_guard<std::mutex> smLock(as.lock_);
538  for (auto &it : as.list_)
539  out << "@ " << it << ": " << it->getName() << std::endl;
540 }
541 
542 void ompl::base::StateSpace::list(std::ostream &out) const
543 {
544  std::queue<const StateSpace *> q;
545  q.push(this);
546  while (!q.empty())
547  {
548  const StateSpace *m = q.front();
549  q.pop();
550  out << "@ " << m << ": " << m->getName() << std::endl;
551  if (m->isCompound())
552  {
553  unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
554  for (unsigned int i = 0; i < c; ++i)
555  q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
556  }
557  }
558 }
559 
560 void ompl::base::StateSpace::diagram(std::ostream &out) const
561 {
562  out << "digraph StateSpace {" << std::endl;
563  out << '"' << getName() << '"' << std::endl;
564 
565  std::queue<const StateSpace *> q;
566  q.push(this);
567  while (!q.empty())
568  {
569  const StateSpace *m = q.front();
570  q.pop();
571  if (m->isCompound())
572  {
573  unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
574  for (unsigned int i = 0; i < c; ++i)
575  {
576  const StateSpace *s = m->as<CompoundStateSpace>()->getSubspace(i).get();
577  q.push(s);
578  out << '"' << m->getName() << R"(" -> ")" << s->getName() << R"(" [label=")"
579  << std::to_string(m->as<CompoundStateSpace>()->getSubspaceWeight(i)) << R"("];)" << std::endl;
580  }
581  }
582  }
583 
584  out << '}' << std::endl;
585 }
586 
587 void ompl::base::StateSpace::Diagram(std::ostream &out)
588 {
589  AllocatedSpaces &as = getAllocatedSpaces();
590  std::lock_guard<std::mutex> smLock(as.lock_);
591  out << "digraph StateSpaces {" << std::endl;
592  for (auto it = as.list_.begin(); it != as.list_.end(); ++it)
593  {
594  out << '"' << (*it)->getName() << '"' << std::endl;
595  for (auto jt = as.list_.begin(); jt != as.list_.end(); ++jt)
596  if (it != jt)
597  {
598  if ((*it)->isCompound() && (*it)->as<CompoundStateSpace>()->hasSubspace((*jt)->getName()))
599  out << '"' << (*it)->getName() << R"(" -> ")" << (*jt)->getName() << R"(" [label=")"
600  << std::to_string((*it)->as<CompoundStateSpace>()->getSubspaceWeight((*jt)->getName())) <<
601  R"("];)" << std::endl;
602  else if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
603  out << '"' << (*it)->getName() << R"(" -> ")" << (*jt)->getName() << R"(" [style=dashed];)"
604  << std::endl;
605  }
606  }
607  out << '}' << std::endl;
608 }
609 
611 {
612  unsigned int flags = isMetricSpace() ? ~0 : ~(STATESPACE_DISTANCE_SYMMETRIC | STATESPACE_TRIANGLE_INEQUALITY);
613  sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), flags);
614 }
615 
616 void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int flags) const
617 {
618  {
619  double maxExt = getMaximumExtent();
620 
621  State *s1 = allocState();
622  State *s2 = allocState();
623  StateSamplerPtr ss = allocStateSampler();
624  char *serialization = nullptr;
625  if ((flags & STATESPACE_SERIALIZATION) && getSerializationLength() > 0)
626  serialization = new char[getSerializationLength()];
627  for (unsigned int i = 0; i < magic::TEST_STATE_COUNT; ++i)
628  {
629  ss->sampleUniform(s1);
630  if (distance(s1, s1) > eps)
631  throw Exception("Distance from a state to itself should be 0");
632  if (!equalStates(s1, s1))
633  throw Exception("A state should be equal to itself");
634  if ((flags & STATESPACE_RESPECT_BOUNDS) && !satisfiesBounds(s1))
635  throw Exception("Sampled states should be within bounds");
636  copyState(s2, s1);
637  if (!equalStates(s1, s2))
638  throw Exception("Copy of a state is not the same as the original state. copyState() may not work "
639  "correctly.");
640  if (flags & STATESPACE_ENFORCE_BOUNDS_NO_OP)
641  {
642  enforceBounds(s1);
643  if (!equalStates(s1, s2))
644  throw Exception("enforceBounds() seems to modify states that are in fact within bounds.");
645  }
646  if (flags & STATESPACE_SERIALIZATION)
647  {
648  ss->sampleUniform(s2);
649  serialize(serialization, s1);
650  deserialize(s2, serialization);
651  if (!equalStates(s1, s2))
652  throw Exception("Serialization/deserialization operations do not seem to work as expected.");
653  }
654  ss->sampleUniform(s2);
655  if (!equalStates(s1, s2))
656  {
657  double d12 = distance(s1, s2);
658  if ((flags & STATESPACE_DISTANCE_DIFFERENT_STATES) && d12 < zero)
659  throw Exception("Distance between different states should be above 0");
660  double d21 = distance(s2, s1);
661  if ((flags & STATESPACE_DISTANCE_SYMMETRIC) && fabs(d12 - d21) > eps)
662  throw Exception("The distance function should be symmetric (A->B=" + std::to_string(d12) +
663  ", B->A=" + std::to_string(d21) + ", difference is " +
664  std::to_string(fabs(d12 - d21)) + ")");
665  if (flags & STATESPACE_DISTANCE_BOUND)
666  if (d12 > maxExt + zero)
667  throw Exception("The distance function should not report values larger than the maximum extent "
668  "(" +
669  std::to_string(d12) + " > " + std::to_string(maxExt) + ")");
670  }
671  }
672  if (serialization)
673  delete[] serialization;
674  freeState(s1);
675  freeState(s2);
676  }
677 
678  // Test that interpolation works as expected and also test triangle inequality
679  if (!isDiscrete() && !isHybrid())
680  {
681  State *s1 = allocState();
682  State *s2 = allocState();
683  State *s3 = allocState();
684  StateSamplerPtr ss = allocStateSampler();
685 
686  for (unsigned int i = 0; i < magic::TEST_STATE_COUNT; ++i)
687  {
688  ss->sampleUniform(s1);
689  ss->sampleUniform(s2);
690  ss->sampleUniform(s3);
691 
692  interpolate(s1, s2, 0.0, s3);
693  if ((flags & STATESPACE_INTERPOLATION) && distance(s1, s3) > eps)
694  throw Exception("Interpolation from a state at time 0 should be not change the original state");
695 
696  interpolate(s1, s2, 1.0, s3);
697  if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
698  throw Exception("Interpolation to a state at time 1 should be the same as the final state");
699 
700  interpolate(s1, s2, 0.5, s3);
701  double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
702  if ((flags & STATESPACE_TRIANGLE_INEQUALITY) && fabs(diff) > eps)
703  throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle "
704  "inequality (" +
705  std::to_string(diff) + " difference)");
706 
707  interpolate(s3, s2, 0.5, s3);
708  interpolate(s1, s2, 0.75, s2);
709 
710  if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
711  throw Exception("Continued interpolation does not work as expected. Please also check that "
712  "interpolate() works with overlapping memory for its state arguments");
713  }
714  freeState(s1);
715  freeState(s2);
716  freeState(s3);
717  }
718 }
719 
721 {
722  return hasProjection(DEFAULT_PROJECTION_NAME);
723 }
724 
725 bool ompl::base::StateSpace::hasProjection(const std::string &name) const
726 {
727  return projections_.find(name) != projections_.end();
728 }
729 
731 {
732  if (hasDefaultProjection())
733  return getProjection(DEFAULT_PROJECTION_NAME);
734  else
735  {
736  OMPL_ERROR("No default projection is set. Perhaps setup() needs to be called");
737  return ProjectionEvaluatorPtr();
738  }
739 }
740 
742 {
743  auto it = projections_.find(name);
744  if (it != projections_.end())
745  return it->second;
746  else
747  {
748  OMPL_ERROR("Projection '%s' is not defined", name.c_str());
749  return ProjectionEvaluatorPtr();
750  }
751 }
752 
753 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr> &
755 {
756  return projections_;
757 }
758 
760 {
761  registerProjection(DEFAULT_PROJECTION_NAME, projection);
762 }
763 
764 void ompl::base::StateSpace::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
765 {
766  if (projection)
767  projections_[name] = projection;
768  else
769  OMPL_ERROR("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
770 }
771 
773 {
774  return false;
775 }
776 
778 {
779  return false;
780 }
781 
783 {
784  return false;
785 }
786 
788 {
789  return true;
790 }
791 
793 {
794  return true;
795 }
796 
798 {
799  ssa_ = ssa;
800 }
801 
803 {
804  ssa_ = StateSamplerAllocator();
805 }
806 
808 {
809  if (ssa_)
810  return ssa_(this);
811  else
812  return allocDefaultStateSampler();
813 }
814 
816 {
817  return allocSubspaceStateSampler(subspace.get());
818 }
819 
821 {
822  if (subspace->getName() == getName())
823  return allocStateSampler();
824  return std::make_shared<SubspaceStateSampler>(this, subspace, 1.0);
825 }
826 
828 {
829  if (factor < 1)
830  throw Exception("The multiplicative factor for the valid segment count between two states must be strictly "
831  "positive");
832  longestValidSegmentCountFactor_ = factor;
833 }
834 
836 {
837  if (segmentFraction < std::numeric_limits<double>::epsilon() ||
838  segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
839  throw Exception("The fraction of the extent must be larger than 0 and less than 1");
840  longestValidSegmentFraction_ = segmentFraction;
841 }
842 
844 {
845  return longestValidSegmentCountFactor_;
846 }
847 
849 {
850  return longestValidSegmentFraction_;
851 }
852 
854 {
855  return longestValidSegment_;
856 }
857 
858 unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
859 {
860  return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
861 }
862 
864 {
865  setName("Compound" + getName());
866 }
867 
868 ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
869  const std::vector<double> &weights)
870  : StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
871 {
872  if (components.size() != weights.size())
873  throw Exception("Number of component spaces and weights are not the same");
874  setName("Compound" + getName());
875  for (unsigned int i = 0; i < components.size(); ++i)
876  addSubspace(components[i], weights[i]);
877 }
878 
879 void ompl::base::CompoundStateSpace::addSubspace(const StateSpacePtr &component, double weight)
880 {
881  if (locked_)
882  throw Exception("This state space is locked. No further components can be added");
883  if (weight < 0.0)
884  throw Exception("Subspace weight cannot be negative");
885  components_.push_back(component);
886  weights_.push_back(weight);
887  weightSum_ += weight;
888  componentCount_ = components_.size();
889 }
890 
892 {
893  return true;
894 }
895 
897 {
898  bool c = false;
899  bool d = false;
900  for (unsigned int i = 0; i < componentCount_; ++i)
901  {
902  if (components_[i]->isHybrid())
903  return true;
904  if (components_[i]->isDiscrete())
905  d = true;
906  else
907  c = true;
908  }
909  return c && d;
910 }
911 
913 {
914  return componentCount_;
915 }
916 
917 const ompl::base::StateSpacePtr &ompl::base::CompoundStateSpace::getSubspace(const unsigned int index) const
918 {
919  if (componentCount_ > index)
920  return components_[index];
921  else
922  throw Exception("Subspace index does not exist");
923 }
924 
925 bool ompl::base::CompoundStateSpace::hasSubspace(const std::string &name) const
926 {
927  for (unsigned int i = 0; i < componentCount_; ++i)
928  if (components_[i]->getName() == name)
929  return true;
930  return false;
931 }
932 
933 unsigned int ompl::base::CompoundStateSpace::getSubspaceIndex(const std::string &name) const
934 {
935  for (unsigned int i = 0; i < componentCount_; ++i)
936  if (components_[i]->getName() == name)
937  return i;
938  throw Exception("Subspace " + name + " does not exist");
939 }
940 
942 {
943  return components_[getSubspaceIndex(name)];
944 }
945 
946 double ompl::base::CompoundStateSpace::getSubspaceWeight(const unsigned int index) const
947 {
948  if (componentCount_ > index)
949  return weights_[index];
950  else
951  throw Exception("Subspace index does not exist");
952 }
953 
954 double ompl::base::CompoundStateSpace::getSubspaceWeight(const std::string &name) const
955 {
956  for (unsigned int i = 0; i < componentCount_; ++i)
957  if (components_[i]->getName() == name)
958  return weights_[i];
959  throw Exception("Subspace " + name + " does not exist");
960 }
961 
962 void ompl::base::CompoundStateSpace::setSubspaceWeight(const unsigned int index, double weight)
963 {
964  if (weight < 0.0)
965  throw Exception("Subspace weight cannot be negative");
966  if (componentCount_ > index)
967  {
968  weightSum_ += weight - weights_[index];
969  weights_[index] = weight;
970  }
971  else
972  throw Exception("Subspace index does not exist");
973 }
974 
975 void ompl::base::CompoundStateSpace::setSubspaceWeight(const std::string &name, double weight)
976 {
977  for (unsigned int i = 0; i < componentCount_; ++i)
978  if (components_[i]->getName() == name)
979  {
980  setSubspaceWeight(i, weight);
981  return;
982  }
983  throw Exception("Subspace " + name + " does not exist");
984 }
985 
986 const std::vector<ompl::base::StateSpacePtr> &ompl::base::CompoundStateSpace::getSubspaces() const
987 {
988  return components_;
989 }
990 
991 const std::vector<double> &ompl::base::CompoundStateSpace::getSubspaceWeights() const
992 {
993  return weights_;
994 }
995 
997 {
998  unsigned int dim = 0;
999  for (unsigned int i = 0; i < componentCount_; ++i)
1000  dim += components_[i]->getDimension();
1001  return dim;
1002 }
1003 
1005 {
1006  double e = 0.0;
1007  for (unsigned int i = 0; i < componentCount_; ++i)
1008  if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
1009  e += weights_[i] * components_[i]->getMaximumExtent();
1010  return e;
1011 }
1012 
1014 {
1015  double m = 1.0;
1016  for (unsigned int i = 0; i < componentCount_; ++i)
1017  if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
1018  m *= weights_[i] * components_[i]->getMeasure();
1019  return m;
1020 }
1021 
1023 {
1024  auto *cstate = static_cast<CompoundState *>(state);
1025  for (unsigned int i = 0; i < componentCount_; ++i)
1026  components_[i]->enforceBounds(cstate->components[i]);
1027 }
1028 
1030 {
1031  const auto *cstate = static_cast<const CompoundState *>(state);
1032  for (unsigned int i = 0; i < componentCount_; ++i)
1033  if (!components_[i]->satisfiesBounds(cstate->components[i]))
1034  return false;
1035  return true;
1036 }
1037 
1038 void ompl::base::CompoundStateSpace::copyState(State *destination, const State *source) const
1039 {
1040  auto *cdest = static_cast<CompoundState *>(destination);
1041  const auto *csrc = static_cast<const CompoundState *>(source);
1042  for (unsigned int i = 0; i < componentCount_; ++i)
1043  components_[i]->copyState(cdest->components[i], csrc->components[i]);
1044 }
1045 
1047 {
1048  unsigned int l = 0;
1049  for (unsigned int i = 0; i < componentCount_; ++i)
1050  l += components_[i]->getSerializationLength();
1051  return l;
1052 }
1053 
1054 void ompl::base::CompoundStateSpace::serialize(void *serialization, const State *state) const
1055 {
1056  const auto *cstate = static_cast<const CompoundState *>(state);
1057  unsigned int l = 0;
1058  for (unsigned int i = 0; i < componentCount_; ++i)
1059  {
1060  components_[i]->serialize(reinterpret_cast<char *>(serialization) + l, cstate->components[i]);
1061  l += components_[i]->getSerializationLength();
1062  }
1063 }
1064 
1065 void ompl::base::CompoundStateSpace::deserialize(State *state, const void *serialization) const
1066 {
1067  auto *cstate = static_cast<CompoundState *>(state);
1068  unsigned int l = 0;
1069  for (unsigned int i = 0; i < componentCount_; ++i)
1070  {
1071  components_[i]->deserialize(cstate->components[i], reinterpret_cast<const char *>(serialization) + l);
1072  l += components_[i]->getSerializationLength();
1073  }
1074 }
1075 
1076 double ompl::base::CompoundStateSpace::distance(const State *state1, const State *state2) const
1077 {
1078  const auto *cstate1 = static_cast<const CompoundState *>(state1);
1079  const auto *cstate2 = static_cast<const CompoundState *>(state2);
1080  double dist = 0.0;
1081  for (unsigned int i = 0; i < componentCount_; ++i)
1082  dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
1083  return dist;
1084 }
1085 
1087 {
1089  for (unsigned int i = 0; i < componentCount_; ++i)
1090  components_[i]->setLongestValidSegmentFraction(segmentFraction);
1091 }
1092 
1093 unsigned int ompl::base::CompoundStateSpace::validSegmentCount(const State *state1, const State *state2) const
1094 {
1095  const auto *cstate1 = static_cast<const CompoundState *>(state1);
1096  const auto *cstate2 = static_cast<const CompoundState *>(state2);
1097  unsigned int sc = 0;
1098  for (unsigned int i = 0; i < componentCount_; ++i)
1099  {
1100  unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
1101  if (sci > sc)
1102  sc = sci;
1103  }
1104  return sc;
1105 }
1106 
1107 bool ompl::base::CompoundStateSpace::equalStates(const State *state1, const State *state2) const
1108 {
1109  const auto *cstate1 = static_cast<const CompoundState *>(state1);
1110  const auto *cstate2 = static_cast<const CompoundState *>(state2);
1111  for (unsigned int i = 0; i < componentCount_; ++i)
1112  if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
1113  return false;
1114  return true;
1115 }
1116 
1117 void ompl::base::CompoundStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
1118 {
1119  const auto *cfrom = static_cast<const CompoundState *>(from);
1120  const auto *cto = static_cast<const CompoundState *>(to);
1121  auto *cstate = static_cast<CompoundState *>(state);
1122  for (unsigned int i = 0; i < componentCount_; ++i)
1123  components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
1124 }
1125 
1127 {
1128  auto ss(std::make_shared<CompoundStateSampler>(this));
1129  if (weightSum_ < std::numeric_limits<double>::epsilon())
1130  for (unsigned int i = 0; i < componentCount_; ++i)
1131  ss->addSampler(components_[i]->allocStateSampler(), 1.0);
1132  else
1133  for (unsigned int i = 0; i < componentCount_; ++i)
1134  ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / weightSum_);
1135  return ss;
1136 }
1137 
1139 {
1140  if (subspace->getName() == getName())
1141  return allocStateSampler();
1142  if (hasSubspace(subspace->getName()))
1143  return std::make_shared<SubspaceStateSampler>(this, subspace,
1144  getSubspaceWeight(subspace->getName()) / weightSum_);
1145  return StateSpace::allocSubspaceStateSampler(subspace);
1146 }
1147 
1149 {
1150  auto *state = new CompoundState();
1151  allocStateComponents(state);
1152  return static_cast<State *>(state);
1153 }
1154 
1156 {
1157  state->components = new State *[componentCount_];
1158  for (unsigned int i = 0; i < componentCount_; ++i)
1159  state->components[i] = components_[i]->allocState();
1160 }
1161 
1163 {
1164  auto *cstate = static_cast<CompoundState *>(state);
1165  for (unsigned int i = 0; i < componentCount_; ++i)
1166  components_[i]->freeState(cstate->components[i]);
1167  delete[] cstate->components;
1168  delete cstate;
1169 }
1170 
1172 {
1173  locked_ = true;
1174 }
1175 
1177 {
1178  return locked_;
1179 }
1180 
1181 double *ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
1182 {
1183  auto *cstate = static_cast<CompoundState *>(state);
1184  unsigned int idx = 0;
1185 
1186  for (unsigned int i = 0; i < componentCount_; ++i)
1187  for (unsigned int j = 0; j <= index; ++j)
1188  {
1189  double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
1190  if (va)
1191  {
1192  if (idx == index)
1193  return va;
1194  else
1195  idx++;
1196  }
1197  else
1198  break;
1199  }
1200  return nullptr;
1201 }
1202 
1203 void ompl::base::CompoundStateSpace::printState(const State *state, std::ostream &out) const
1204 {
1205  out << "Compound state [" << std::endl;
1206  const auto *cstate = static_cast<const CompoundState *>(state);
1207  for (unsigned int i = 0; i < componentCount_; ++i)
1208  components_[i]->printState(cstate->components[i], out);
1209  out << "]" << std::endl;
1210 }
1211 
1212 void ompl::base::CompoundStateSpace::printSettings(std::ostream &out) const
1213 {
1214  out << "Compound state space '" << getName() << "' of dimension " << getDimension()
1215  << (isLocked() ? " (locked)" : "") << " [" << std::endl;
1216  for (unsigned int i = 0; i < componentCount_; ++i)
1217  {
1218  components_[i]->printSettings(out);
1219  out << " of weight " << weights_[i] << std::endl;
1220  }
1221  out << "]" << std::endl;
1222  printProjections(out);
1223 }
1224 
1226 {
1227  for (unsigned int i = 0; i < componentCount_; ++i)
1228  components_[i]->setup();
1229 
1231 }
1232 
1234 {
1236  for (unsigned int i = 0; i < componentCount_; ++i)
1237  components_[i]->computeLocations();
1238 }
1239 
1240 namespace ompl
1241 {
1242  namespace base
1243  {
1244  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
1245  const State *source)
1246  {
1247  return copyStateData(destS.get(), dest, sourceS.get(), source);
1248  }
1249 
1250  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
1251  const State *source)
1252  {
1253  // if states correspond to the same space, simply do copy
1254  if (destS->getName() == sourceS->getName())
1255  {
1256  if (dest != source)
1257  destS->copyState(dest, source);
1258  return ALL_DATA_COPIED;
1259  }
1260 
1262 
1263  // if "to" state is compound
1264  if (destS->isCompound())
1265  {
1266  const auto *compoundDestS = destS->as<CompoundStateSpace>();
1267  auto *compoundDest = dest->as<CompoundState>();
1268 
1269  // if there is a subspace in "to" that corresponds to "from", set the data and return
1270  for (unsigned int i = 0; i < compoundDestS->getSubspaceCount(); ++i)
1271  if (compoundDestS->getSubspace(i)->getName() == sourceS->getName())
1272  {
1273  if (compoundDest->components[i] != source)
1274  compoundDestS->getSubspace(i)->copyState(compoundDest->components[i], source);
1275  return ALL_DATA_COPIED;
1276  }
1277 
1278  // it could be there are further levels of compound spaces where the data can be set
1279  // so we call this function recursively
1280  for (unsigned int i = 0; i < compoundDestS->getSubspaceCount(); ++i)
1281  {
1282  AdvancedStateCopyOperation res = copyStateData(compoundDestS->getSubspace(i).get(),
1283  compoundDest->components[i], sourceS, source);
1284 
1285  if (res != NO_DATA_COPIED)
1286  result = SOME_DATA_COPIED;
1287 
1288  // if all data was copied, we stop
1289  if (res == ALL_DATA_COPIED)
1290  return ALL_DATA_COPIED;
1291  }
1292  }
1293 
1294  // if we got to this point, it means that the data in "from" could not be copied as a chunk to "to"
1295  // it could be the case "from" is from a compound space as well, so we can copy parts of "from", as needed
1296  if (sourceS->isCompound())
1297  {
1298  const auto *compoundSourceS = sourceS->as<CompoundStateSpace>();
1299  const auto *compoundSource = source->as<CompoundState>();
1300 
1301  unsigned int copiedComponents = 0;
1302 
1303  // if there is a subspace in "to" that corresponds to "from", set the data and return
1304  for (unsigned int i = 0; i < compoundSourceS->getSubspaceCount(); ++i)
1305  {
1306  AdvancedStateCopyOperation res = copyStateData(destS, dest, compoundSourceS->getSubspace(i).get(),
1307  compoundSource->components[i]);
1308  if (res == ALL_DATA_COPIED)
1309  copiedComponents++;
1310  if (res != NO_DATA_COPIED)
1311  result = SOME_DATA_COPIED;
1312  }
1313 
1314  // if each individual component got copied, then the entire data in "from" got copied
1315  if (copiedComponents == compoundSourceS->getSubspaceCount())
1316  result = ALL_DATA_COPIED;
1317  }
1318 
1319  return result;
1320  }
1321 
1322  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
1323  const State *source, const std::vector<std::string> &subspaces)
1324  {
1325  return copyStateData(destS.get(), dest, sourceS.get(), source, subspaces);
1326  }
1327 
1328  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
1329  const State *source, const std::vector<std::string> &subspaces)
1330  {
1331  std::size_t copyCount = 0;
1332  const std::map<std::string, StateSpace::SubstateLocation> &destLoc = destS->getSubstateLocationsByName();
1333  const std::map<std::string, StateSpace::SubstateLocation> &sourceLoc =
1334  sourceS->getSubstateLocationsByName();
1335  for (const auto &subspace : subspaces)
1336  {
1337  auto dt = destLoc.find(subspace);
1338  if (dt != destLoc.end())
1339  {
1340  auto st = sourceLoc.find(subspace);
1341  if (st != sourceLoc.end())
1342  {
1343  dt->second.space->copyState(destS->getSubstateAtLocation(dest, dt->second),
1344  sourceS->getSubstateAtLocation(source, st->second));
1345  ++copyCount;
1346  }
1347  }
1348  }
1349  if (copyCount == subspaces.size())
1350  return ALL_DATA_COPIED;
1351  if (copyCount > 0)
1352  return SOME_DATA_COPIED;
1353  return NO_DATA_COPIED;
1354  }
1355 
1357  inline bool StateSpaceHasContent(const StateSpacePtr &m)
1358  {
1359  if (!m)
1360  return false;
1361  if (m->getDimension() == 0 && m->getType() == STATE_SPACE_UNKNOWN && m->isCompound())
1362  {
1363  const unsigned int nc = m->as<CompoundStateSpace>()->getSubspaceCount();
1364  for (unsigned int i = 0; i < nc; ++i)
1365  if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubspace(i)))
1366  return true;
1367  return false;
1368  }
1369  return true;
1370  }
1372 
1374  {
1375  if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
1376  return b;
1377 
1378  if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
1379  return a;
1380 
1381  std::vector<StateSpacePtr> components;
1382  std::vector<double> weights;
1383 
1384  bool change = false;
1385  if (a)
1386  {
1387  bool used = false;
1388  if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1389  if (!csm_a->isLocked())
1390  {
1391  used = true;
1392  for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1393  {
1394  components.push_back(csm_a->getSubspace(i));
1395  weights.push_back(csm_a->getSubspaceWeight(i));
1396  }
1397  }
1398 
1399  if (!used)
1400  {
1401  components.push_back(a);
1402  weights.push_back(1.0);
1403  }
1404  }
1405  if (b)
1406  {
1407  bool used = false;
1408  unsigned int size = components.size();
1409 
1410  if (auto *csm_b = dynamic_cast<CompoundStateSpace *>(b.get()))
1411  if (!csm_b->isLocked())
1412  {
1413  used = true;
1414  for (unsigned int i = 0; i < csm_b->getSubspaceCount(); ++i)
1415  {
1416  bool ok = true;
1417  for (unsigned int j = 0; j < size; ++j)
1418  if (components[j]->getName() == csm_b->getSubspace(i)->getName())
1419  {
1420  ok = false;
1421  break;
1422  }
1423  if (ok)
1424  {
1425  components.push_back(csm_b->getSubspace(i));
1426  weights.push_back(csm_b->getSubspaceWeight(i));
1427  change = true;
1428  }
1429  }
1430  if (components.size() == csm_b->getSubspaceCount())
1431  return b;
1432  }
1433 
1434  if (!used)
1435  {
1436  bool ok = true;
1437  for (unsigned int j = 0; j < size; ++j)
1438  if (components[j]->getName() == b->getName())
1439  {
1440  ok = false;
1441  break;
1442  }
1443  if (ok)
1444  {
1445  components.push_back(b);
1446  weights.push_back(1.0);
1447  change = true;
1448  }
1449  }
1450  }
1451 
1452  if (!change && a)
1453  return a;
1454 
1455  if (components.size() == 1)
1456  return components[0];
1457 
1458  return std::make_shared<CompoundStateSpace>(components, weights);
1459  }
1460 
1462  {
1463  std::vector<StateSpacePtr> components_a;
1464  std::vector<double> weights_a;
1465  std::vector<StateSpacePtr> components_b;
1466 
1467  if (a)
1468  {
1469  bool used = false;
1470  if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1471  if (!csm_a->isLocked())
1472  {
1473  used = true;
1474  for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1475  {
1476  components_a.push_back(csm_a->getSubspace(i));
1477  weights_a.push_back(csm_a->getSubspaceWeight(i));
1478  }
1479  }
1480 
1481  if (!used)
1482  {
1483  components_a.push_back(a);
1484  weights_a.push_back(1.0);
1485  }
1486  }
1487 
1488  if (b)
1489  {
1490  bool used = false;
1491  if (auto *csm_b = dynamic_cast<CompoundStateSpace *>(b.get()))
1492  if (!csm_b->isLocked())
1493  {
1494  used = true;
1495  for (unsigned int i = 0; i < csm_b->getSubspaceCount(); ++i)
1496  components_b.push_back(csm_b->getSubspace(i));
1497  }
1498  if (!used)
1499  components_b.push_back(b);
1500  }
1501 
1502  bool change = false;
1503  for (auto &i : components_b)
1504  for (unsigned int j = 0; j < components_a.size(); ++j)
1505  if (components_a[j]->getName() == i->getName())
1506  {
1507  components_a.erase(components_a.begin() + j);
1508  weights_a.erase(weights_a.begin() + j);
1509  change = true;
1510  break;
1511  }
1512 
1513  if (!change && a)
1514  return a;
1515 
1516  if (components_a.size() == 1)
1517  return components_a[0];
1518 
1519  return std::make_shared<CompoundStateSpace>(components_a, weights_a);
1520  }
1521 
1522  StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name)
1523  {
1524  std::vector<StateSpacePtr> components;
1525  std::vector<double> weights;
1526 
1527  bool change = false;
1528  if (a)
1529  {
1530  bool used = false;
1531  if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1532  if (!csm_a->isLocked())
1533  {
1534  used = true;
1535  for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1536  {
1537  if (csm_a->getSubspace(i)->getName() == name)
1538  {
1539  change = true;
1540  continue;
1541  }
1542  components.push_back(csm_a->getSubspace(i));
1543  weights.push_back(csm_a->getSubspaceWeight(i));
1544  }
1545  }
1546 
1547  if (!used)
1548  {
1549  if (a->getName() != name)
1550  {
1551  components.push_back(a);
1552  weights.push_back(1.0);
1553  }
1554  else
1555  change = true;
1556  }
1557  }
1558 
1559  if (!change && a)
1560  return a;
1561 
1562  if (components.size() == 1)
1563  return components[0];
1564 
1565  return std::make_shared<CompoundStateSpace>(components, weights);
1566  }
1567 
1569  {
1570  std::vector<StateSpacePtr> components_a;
1571  std::vector<double> weights_a;
1572  std::vector<StateSpacePtr> components_b;
1573  std::vector<double> weights_b;
1574 
1575  if (a)
1576  {
1577  bool used = false;
1578  if (auto *csm_a = dynamic_cast<CompoundStateSpace *>(a.get()))
1579  if (!csm_a->isLocked())
1580  {
1581  used = true;
1582  for (unsigned int i = 0; i < csm_a->getSubspaceCount(); ++i)
1583  {
1584  components_a.push_back(csm_a->getSubspace(i));
1585  weights_a.push_back(csm_a->getSubspaceWeight(i));
1586  }
1587  }
1588 
1589  if (!used)
1590  {
1591  components_a.push_back(a);
1592  weights_a.push_back(1.0);
1593  }
1594  }
1595 
1596  if (b)
1597  {
1598  bool used = false;
1599  if (auto *csm_b = dynamic_cast<CompoundStateSpace *>(b.get()))
1600  if (!csm_b->isLocked())
1601  {
1602  used = true;
1603  for (unsigned int i = 0; i < csm_b->getSubspaceCount(); ++i)
1604  {
1605  components_b.push_back(csm_b->getSubspace(i));
1606  weights_b.push_back(csm_b->getSubspaceWeight(i));
1607  }
1608  }
1609 
1610  if (!used)
1611  {
1612  components_b.push_back(b);
1613  weights_b.push_back(1.0);
1614  }
1615  }
1616 
1617  std::vector<StateSpacePtr> components;
1618  std::vector<double> weights;
1619 
1620  for (unsigned int i = 0; i < components_b.size(); ++i)
1621  {
1622  for (unsigned int j = 0; j < components_a.size(); ++j)
1623  if (components_a[j]->getName() == components_b[i]->getName())
1624  {
1625  components.push_back(components_b[i]);
1626  weights.push_back(std::max(weights_a[j], weights_b[i]));
1627  break;
1628  }
1629  }
1630 
1631  if (a && components.size() == components_a.size())
1632  return a;
1633 
1634  if (b && components.size() == components_b.size())
1635  return b;
1636 
1637  if (components.size() == 1)
1638  return components[0];
1639 
1640  return std::make_shared<CompoundStateSpace>(components, weights);
1641  }
1642  }
1643 }
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:370
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:212
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:237
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) ...
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:230
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:354
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 ...
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
std::vector< std::size_t > chain
In a complex state space there may be multiple compound state spaces that make up an even larger comp...
Definition: StateSpace.h:114
void list(std::ostream &out) const
Print the list of all contained state space instances.
Definition: StateSpace.cpp:542
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:387
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:485
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:225
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:244
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
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:207
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:335
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.
double getLongestValidSegmentLength() const
Get the longest valid segment at the time setup() was called.
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:480
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...
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:340
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:391
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:400
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:395
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
unsigned int getValidSegmentCountFactor() const
Get the value used to multiply the return value of validSegmentCount().
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:382
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
ProjectionEvaluatorPtr getProjection(const std::string &name) const
Get the projection registered under a specific name.
A state space representing Rn. The distance function is the L2 norm.
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:248
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:317
void setValidSegmentCountFactor(unsigned int factor)
Set factor to be the value to multiply the return value of validSegmentCount(). By default...
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
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:295
State * getSubstateAtLocation(State *state, const SubstateLocation &loc) const
Get the substate of state that is pointed to by loc.
Definition: StateSpace.cpp:300
void setStateSamplerAllocator(const StateSamplerAllocator &ssa)
Set the sampler allocator to use.
Some data was copied.
Definition: StateSpace.h:792
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
bool hasSubspace(const std::string &name) const
Check if a specific subspace is contained in this state space.
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...
int getType() const
Get the type of the state space. The type can be used to verify whether two space instances are of th...
Definition: StateSpace.h:205
void copyFromReals(State *destination, const std::vector< double > &reals) const
Copy the values from reals to the state destination using getValueAddressAtLocation() ...
Definition: StateSpace.cpp:347
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:329
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
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings() ...
Definition: StateSpace.cpp:406
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:534
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.
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
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
unsigned int getSubspaceIndex(const std::string &name) const
Get the index of a specific subspace from the compound state space.
virtual void copyState(State *destination, const State *source) const =0
Copy a state to another. The memory of source and destination should NOT overlap. ...
void diagram(std::ostream &out) const
Print a Graphviz digraph that represents the containment diagram for the state space.
Definition: StateSpace.cpp:560
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
virtual State * allocState() const =0
Allocate a state that can store a point in the described space.
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:500
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.