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