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