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