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  throw Exception("The longest valid segment for state space " + getName() + " must be positive");
227 
228  computeLocationsHelper(this, substateLocationsByName_, valueLocationsInOrder_, valueLocationsByName_);
229 
230  // make sure we don't overwrite projections that have been configured by the user
231  std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
232  registerProjections();
233  for (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
234  if (it->second->userConfigured())
235  {
236  std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
237  if (o != projections_.end())
238  if (!o->second->userConfigured())
239  projections_[it->first] = it->second;
240  }
241 
242  // remove previously set parameters for projections
243  std::vector<std::string> pnames;
244  params_.getParamNames(pnames);
245  for (std::vector<std::string>::const_iterator it = pnames.begin() ; it != pnames.end() ; ++it)
246  if (it->substr(0, 11) == "projection.")
247  params_.remove(*it);
248 
249  // setup projections and add their parameters
250  for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
251  {
252  it->second->setup();
253  if (it->first == DEFAULT_PROJECTION_NAME)
254  params_.include(it->second->params(), "projection");
255  else
256  params_.include(it->second->params(), "projection." + it->first);
257  }
258 }
259 
260 const std::map<std::string, ompl::base::StateSpace::SubstateLocation>& ompl::base::StateSpace::getSubstateLocationsByName(void) const
261 {
262  return substateLocationsByName_;
263 }
264 
266 {
267  std::size_t index = 0;
268  while (loc.chain.size() > index)
269  state = state->as<CompoundState>()->components[loc.chain[index++]];
270  return state;
271 }
272 
274 {
275  std::size_t index = 0;
276  while (loc.chain.size() > index)
277  state = state->as<CompoundState>()->components[loc.chain[index++]];
278  return state;
279 }
280 
281 double* ompl::base::StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
282 {
283  return NULL;
284 }
285 
286 const double* ompl::base::StateSpace::getValueAddressAtIndex(const State *state, const unsigned int index) const
287 {
288  double *val = getValueAddressAtIndex(const_cast<State*>(state), index); // this const-cast does not hurt, since the state is not modified
289  return val;
290 }
291 
292 const std::vector<ompl::base::StateSpace::ValueLocation>& ompl::base::StateSpace::getValueLocations(void) const
293 {
294  return valueLocationsInOrder_;
295 }
296 
297 const std::map<std::string, ompl::base::StateSpace::ValueLocation>& ompl::base::StateSpace::getValueLocationsByName(void) const
298 {
299  return valueLocationsByName_;
300 }
301 
302 void ompl::base::StateSpace::copyToReals(std::vector<double> &reals, const State *source) const
303 {
304  reals.resize(valueLocationsInOrder_.size());
305  for (std::size_t i = 0 ; i < valueLocationsInOrder_.size() ; ++i)
306  reals[i] = *getValueAddressAtLocation(source, valueLocationsInOrder_[i]);
307 }
308 
309 void ompl::base::StateSpace::copyFromReals(State *destination, const std::vector<double> &reals) const
310 {
311  assert(reals.size() == valueLocationsInOrder_.size());
312  for (std::size_t i = 0 ; i < reals.size() ; ++i)
313  *getValueAddressAtLocation(destination, valueLocationsInOrder_[i]) = reals[i];
314 }
315 
317 {
318  std::size_t index = 0;
319  while (loc.stateLocation.chain.size() > index)
320  state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
321  return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
322 }
323 
324 const double* ompl::base::StateSpace::getValueAddressAtLocation(const State *state, const ValueLocation &loc) const
325 {
326  std::size_t index = 0;
327  while (loc.stateLocation.chain.size() > index)
328  state = state->as<CompoundState>()->components[loc.stateLocation.chain[index++]];
329  return loc.stateLocation.space->getValueAddressAtIndex(state, loc.index);
330 }
331 
332 double* ompl::base::StateSpace::getValueAddressAtName(State *state, const std::string &name) const
333 {
334  std::map<std::string, ValueLocation>::const_iterator it = valueLocationsByName_.find(name);
335  return (it != valueLocationsByName_.end()) ? getValueAddressAtLocation(state, it->second) : NULL;
336 }
337 
338 const double* ompl::base::StateSpace::getValueAddressAtName(const 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 
345 {
346  return 0;
347 }
348 
349 void ompl::base::StateSpace::serialize(void *serialization, const State *state) const
350 {
351 }
352 
353 void ompl::base::StateSpace::deserialize(State *state, const void *serialization) const
354 {
355 }
356 
357 void ompl::base::StateSpace::printState(const State *state, std::ostream &out) const
358 {
359  out << "State instance [" << state << ']' << std::endl;
360 }
361 
362 void ompl::base::StateSpace::printSettings(std::ostream &out) const
363 {
364  out << "StateSpace '" << getName() << "' instance: " << this << std::endl;
365  printProjections(out);
366 }
367 
368 void ompl::base::StateSpace::printProjections(std::ostream &out) const
369 {
370  if (projections_.empty())
371  out << "No registered projections" << std::endl;
372  else
373  {
374  out << "Registered projections:" << std::endl;
375  for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
376  {
377  out << " - ";
378  if (it->first == DEFAULT_PROJECTION_NAME)
379  out << "<default>";
380  else
381  out << it->first;
382  out << std::endl;
383  it->second->printSettings(out);
384  }
385  }
386 }
387 
389 namespace ompl
390 {
391  namespace base
392  {
393  static bool StateSpaceIncludes(const StateSpace *self, const StateSpace *other)
394  {
395  std::queue<const StateSpace*> q;
396  q.push(self);
397  while (!q.empty())
398  {
399  const StateSpace *m = q.front();
400  q.pop();
401  if (m->getName() == other->getName())
402  return true;
403  if (m->isCompound())
404  {
405  unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
406  for (unsigned int i = 0 ; i < c ; ++i)
407  q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
408  }
409  }
410  return false;
411  }
412 
413  static bool StateSpaceCovers(const StateSpace *self, const StateSpace *other)
414  {
415  if (StateSpaceIncludes(self, other))
416  return true;
417  else
418  if (other->isCompound())
419  {
420  unsigned int c = other->as<CompoundStateSpace>()->getSubspaceCount();
421  for (unsigned int i = 0 ; i < c ; ++i)
422  if (!StateSpaceCovers(self, other->as<CompoundStateSpace>()->getSubspace(i).get()))
423  return false;
424  return true;
425  }
426  return false;
427  }
428 
429  struct CompareSubstateLocation
430  {
431  bool operator()(const StateSpace::SubstateLocation &a, const StateSpace::SubstateLocation &b) const
432  {
433  if (a.space->getDimension() != b.space->getDimension())
434  return a.space->getDimension() > b.space->getDimension();
435  return a.space->getName() > b.space->getName();
436  }
437  };
438 
439  }
440 }
441 
443 
445 {
446  return StateSpaceCovers(this, other.get());
447 }
448 
450 {
451  return StateSpaceIncludes(this, other.get());
452 }
453 
455 {
456  return StateSpaceCovers(this, other);
457 }
458 
460 {
461  return StateSpaceIncludes(this, other);
462 }
463 
464 void ompl::base::StateSpace::getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const
465 {
466  getCommonSubspaces(other.get(), subspaces);
467 }
468 
469 void ompl::base::StateSpace::getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const
470 {
471  std::set<StateSpace::SubstateLocation, CompareSubstateLocation> intersection;
472  const std::map<std::string, StateSpace::SubstateLocation> &S = other->getSubstateLocationsByName();
473  for (std::map<std::string, StateSpace::SubstateLocation>::const_iterator it = substateLocationsByName_.begin() ; it != substateLocationsByName_.end() ; ++it)
474  {
475  if (S.find(it->first) != S.end())
476  intersection.insert(it->second);
477  }
478 
479  bool found = true;
480  while (found)
481  {
482  found = false;
483  for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator it = intersection.begin() ; it != intersection.end() ; ++it)
484  for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator jt = intersection.begin() ; jt != intersection.end() ; ++jt)
485  if (it != jt)
486  if (StateSpaceCovers(it->space, jt->space))
487  {
488  intersection.erase(jt);
489  found = true;
490  break;
491  }
492  }
493  subspaces.clear();
494  for (std::set<StateSpace::SubstateLocation, CompareSubstateLocation>::iterator it = intersection.begin() ; it != intersection.end() ; ++it)
495  subspaces.push_back(it->space->getName());
496 }
497 
498 void ompl::base::StateSpace::List(std::ostream &out)
499 {
500  AllocatedSpaces &as = getAllocatedSpaces();
501  boost::mutex::scoped_lock smLock(as.lock_);
502  for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
503  out << "@ " << *it << ": " << (*it)->getName() << std::endl;
504 }
505 
506 void ompl::base::StateSpace::list(std::ostream &out) const
507 {
508  std::queue<const StateSpace*> q;
509  q.push(this);
510  while (!q.empty())
511  {
512  const StateSpace *m = q.front();
513  q.pop();
514  out << "@ " << m << ": " << m->getName() << std::endl;
515  if (m->isCompound())
516  {
517  unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
518  for (unsigned int i = 0 ; i < c ; ++i)
519  q.push(m->as<CompoundStateSpace>()->getSubspace(i).get());
520  }
521  }
522 }
523 
524 void ompl::base::StateSpace::diagram(std::ostream &out) const
525 {
526  out << "digraph StateSpace {" << std::endl;
527  out << '"' << getName() << '"' << std::endl;
528 
529  std::queue<const StateSpace*> q;
530  q.push(this);
531  while (!q.empty())
532  {
533  const StateSpace *m = q.front();
534  q.pop();
535  if (m->isCompound())
536  {
537  unsigned int c = m->as<CompoundStateSpace>()->getSubspaceCount();
538  for (unsigned int i = 0 ; i < c ; ++i)
539  {
540  const StateSpace *s = m->as<CompoundStateSpace>()->getSubspace(i).get();
541  q.push(s);
542  out << '"' << m->getName() << "\" -> \"" << s->getName() << "\" [label=\"" <<
543  boost::lexical_cast<std::string>(m->as<CompoundStateSpace>()->getSubspaceWeight(i)) << "\"];" << std::endl;
544  }
545  }
546  }
547 
548  out << '}' << std::endl;
549 }
550 
551 void ompl::base::StateSpace::Diagram(std::ostream &out)
552 {
553  AllocatedSpaces &as = getAllocatedSpaces();
554  boost::mutex::scoped_lock smLock(as.lock_);
555  out << "digraph StateSpaces {" << std::endl;
556  for (std::list<StateSpace*>::iterator it = as.list_.begin() ; it != as.list_.end(); ++it)
557  {
558  out << '"' << (*it)->getName() << '"' << std::endl;
559  for (std::list<StateSpace*>::iterator jt = as.list_.begin() ; jt != as.list_.end(); ++jt)
560  if (it != jt)
561  {
562  if ((*it)->isCompound() && (*it)->as<CompoundStateSpace>()->hasSubspace((*jt)->getName()))
563  out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [label=\"" <<
564  boost::lexical_cast<std::string>((*it)->as<CompoundStateSpace>()->getSubspaceWeight((*jt)->getName())) <<
565  "\"];" << std::endl;
566  else
567  if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
568  out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [style=dashed];" << std::endl;
569  }
570  }
571  out << '}' << std::endl;
572 }
573 
575 {
576  sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(), ~0);
577 }
578 
579 void ompl::base::StateSpace::sanityChecks(double zero, double eps, unsigned int flags) const
580 {
581  {
582  double maxExt = getMaximumExtent();
583 
584  State *s1 = allocState();
585  State *s2 = allocState();
586  StateSamplerPtr ss = allocStateSampler();
587  char *serialization = NULL;
588  if ((flags & STATESPACE_SERIALIZATION) && getSerializationLength() > 0)
589  serialization = new char[getSerializationLength()];
590  for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
591  {
592  ss->sampleUniform(s1);
593  if (distance(s1, s1) > eps)
594  throw Exception("Distance from a state to itself should be 0");
595  if (!equalStates(s1, s1))
596  throw Exception("A state should be equal to itself");
597  if ((flags & STATESPACE_RESPECT_BOUNDS) && !satisfiesBounds(s1))
598  throw Exception("Sampled states should be within bounds");
599  copyState(s2, s1);
600  if (!equalStates(s1, s2))
601  throw Exception("Copy of a state is not the same as the original state. copyState() may not work correctly.");
602  if (flags & STATESPACE_ENFORCE_BOUNDS_NO_OP)
603  {
604  enforceBounds(s1);
605  if (!equalStates(s1, s2))
606  throw Exception("enforceBounds() seems to modify states that are in fact within bounds.");
607  }
608  if (flags & STATESPACE_SERIALIZATION)
609  {
610  ss->sampleUniform(s2);
611  serialize(serialization, s1);
612  deserialize(s2, serialization);
613  if (!equalStates(s1, s2))
614  throw Exception("Serialization/deserialization operations do not seem to work as expected.");
615  }
616  ss->sampleUniform(s2);
617  if (!equalStates(s1, s2))
618  {
619  double d12 = distance(s1, s2);
620  if ((flags & STATESPACE_DISTANCE_DIFFERENT_STATES) && d12 < zero)
621  throw Exception("Distance between different states should be above 0");
622  double d21 = distance(s2, s1);
623  if ((flags & STATESPACE_DISTANCE_SYMMETRIC) && fabs(d12 - d21) > eps)
624  throw Exception("The distance function should be symmetric (A->B=" +
625  boost::lexical_cast<std::string>(d12) + ", B->A=" +
626  boost::lexical_cast<std::string>(d21) + ", difference is " +
627  boost::lexical_cast<std::string>(fabs(d12 - d21)) + ")");
628  if (flags & STATESPACE_DISTANCE_BOUND)
629  if (d12 > maxExt + zero)
630  throw Exception("The distance function should not report values larger than the maximum extent ("+
631  boost::lexical_cast<std::string>(d12) + " > " + boost::lexical_cast<std::string>(maxExt) + ")");
632  }
633  }
634  if (serialization)
635  delete[] serialization;
636  freeState(s1);
637  freeState(s2);
638  }
639 
640 
641  // Test that interpolation works as expected and also test triangle inequality
642  if (!isDiscrete() && !isHybrid())
643  {
644  State *s1 = allocState();
645  State *s2 = allocState();
646  State *s3 = allocState();
647  StateSamplerPtr ss = allocStateSampler();
648 
649  for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
650  {
651  ss->sampleUniform(s1);
652  ss->sampleUniform(s2);
653  ss->sampleUniform(s3);
654 
655  interpolate(s1, s2, 0.0, s3);
656  if ((flags & STATESPACE_INTERPOLATION) && distance(s1, s3) > eps)
657  throw Exception("Interpolation from a state at time 0 should be not change the original state");
658 
659  interpolate(s1, s2, 1.0, s3);
660  if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
661  throw Exception("Interpolation to a state at time 1 should be the same as the final state");
662 
663  interpolate(s1, s2, 0.5, s3);
664  double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
665  if ((flags & STATESPACE_TRIANGLE_INEQUALITY) && fabs(diff) > eps)
666  throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle inequality (" +
667  boost::lexical_cast<std::string>(diff) + " difference)");
668 
669  interpolate(s3, s2, 0.5, s3);
670  interpolate(s1, s2, 0.75, s2);
671 
672  if ((flags & STATESPACE_INTERPOLATION) && distance(s2, s3) > eps)
673  throw Exception("Continued interpolation does not work as expected. Please also check that interpolate() works with overlapping memory for its state arguments");
674  }
675  freeState(s1);
676  freeState(s2);
677  freeState(s3);
678  }
679 }
680 
682 {
683  return hasProjection(DEFAULT_PROJECTION_NAME);
684 }
685 
686 bool ompl::base::StateSpace::hasProjection(const std::string &name) const
687 {
688  return projections_.find(name) != projections_.end();
689 }
690 
692 {
693  if (hasDefaultProjection())
694  return getProjection(DEFAULT_PROJECTION_NAME);
695  else
696  {
697  OMPL_ERROR("No default projection is set. Perhaps setup() needs to be called");
698  return ProjectionEvaluatorPtr();
699  }
700 }
701 
703 {
704  std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
705  if (it != projections_.end())
706  return it->second;
707  else
708  {
709  OMPL_ERROR("Projection '%s' is not defined", name.c_str());
710  return ProjectionEvaluatorPtr();
711  }
712 }
713 
714 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr>& ompl::base::StateSpace::getRegisteredProjections(void) const
715 {
716  return projections_;
717 }
718 
720 {
721  registerProjection(DEFAULT_PROJECTION_NAME, projection);
722 }
723 
724 void ompl::base::StateSpace::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
725 {
726  if (projection)
727  projections_[name] = projection;
728  else
729  OMPL_ERROR("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
730 }
731 
733 {
734  return false;
735 }
736 
738 {
739  return false;
740 }
741 
743 {
744  return false;
745 }
746 
748 {
749  ssa_ = ssa;
750 }
751 
753 {
754  ssa_ = StateSamplerAllocator();
755 }
756 
758 {
759  if (ssa_)
760  return ssa_(this);
761  else
762  return allocDefaultStateSampler();
763 }
764 
766 {
767  return allocSubspaceStateSampler(subspace.get());
768 }
769 
771 {
772  if (subspace->getName() == getName())
773  return allocStateSampler();
774  return StateSamplerPtr(new SubspaceStateSampler(this, subspace, 1.0));
775 }
776 
778 {
779  if (factor < 1)
780  throw Exception("The multiplicative factor for the valid segment count between two states must be strictly positive");
781  longestValidSegmentCountFactor_ = factor;
782 }
783 
785 {
786  if (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
787  throw Exception("The fraction of the extent must be larger than 0 and less than 1");
788  longestValidSegmentFraction_ = segmentFraction;
789 }
790 
792 {
793  return longestValidSegmentCountFactor_;
794 }
795 
797 {
798  return longestValidSegmentFraction_;
799 }
800 
801 unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
802 {
803  return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
804 }
805 
806 ompl::base::CompoundStateSpace::CompoundStateSpace(void) : StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
807 {
808  setName("Compound" + getName());
809 }
810 
811 ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
812  const std::vector<double> &weights) :
813  StateSpace(), componentCount_(0), weightSum_(0.0), locked_(false)
814 {
815  if (components.size() != weights.size())
816  throw Exception("Number of component spaces and weights are not the same");
817  setName("Compound" + getName());
818  for (unsigned int i = 0 ; i < components.size() ; ++i)
819  addSubspace(components[i], weights[i]);
820 }
821 
822 void ompl::base::CompoundStateSpace::addSubspace(const StateSpacePtr &component, double weight)
823 {
824  if (locked_)
825  throw Exception("This state space is locked. No further components can be added");
826  if (weight < 0.0)
827  throw Exception("Subspace weight cannot be negative");
828  components_.push_back(component);
829  weights_.push_back(weight);
830  weightSum_ += weight;
831  componentCount_ = components_.size();
832 }
833 
835 {
836  return true;
837 }
838 
840 {
841  bool c = false;
842  bool d = false;
843  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
844  {
845  if (components_[i]->isHybrid())
846  return true;
847  if (components_[i]->isDiscrete())
848  d = true;
849  else
850  c = true;
851  }
852  return c && d;
853 }
854 
856 {
857  return componentCount_;
858 }
859 
861 {
862  if (componentCount_ > index)
863  return components_[index];
864  else
865  throw Exception("Subspace index does not exist");
866 }
867 
868 bool ompl::base::CompoundStateSpace::hasSubspace(const std::string &name) const
869 {
870  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
871  if (components_[i]->getName() == name)
872  return true;
873  return false;
874 }
875 
876 unsigned int ompl::base::CompoundStateSpace::getSubspaceIndex(const std::string& name) const
877 {
878  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
879  if (components_[i]->getName() == name)
880  return i;
881  throw Exception("Subspace " + name + " does not exist");
882 }
883 
885 {
886  return components_[getSubspaceIndex(name)];
887 }
888 
889 double ompl::base::CompoundStateSpace::getSubspaceWeight(const unsigned int index) const
890 {
891  if (componentCount_ > index)
892  return weights_[index];
893  else
894  throw Exception("Subspace index does not exist");
895 }
896 
897 double ompl::base::CompoundStateSpace::getSubspaceWeight(const std::string &name) const
898 {
899  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
900  if (components_[i]->getName() == name)
901  return weights_[i];
902  throw Exception("Subspace " + name + " does not exist");
903 }
904 
905 void ompl::base::CompoundStateSpace::setSubspaceWeight(const unsigned int index, double weight)
906 {
907  if (weight < 0.0)
908  throw Exception("Subspace weight cannot be negative");
909  if (componentCount_ > index)
910  {
911  weightSum_ += weight - weights_[index];
912  weights_[index] = weight;
913  }
914  else
915  throw Exception("Subspace index does not exist");
916 }
917 
918 void ompl::base::CompoundStateSpace::setSubspaceWeight(const std::string &name, double weight)
919 {
920  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
921  if (components_[i]->getName() == name)
922  {
923  setSubspaceWeight(i, weight);
924  return;
925  }
926  throw Exception("Subspace " + name + " does not exist");
927 }
928 
929 const std::vector<ompl::base::StateSpacePtr>& ompl::base::CompoundStateSpace::getSubspaces(void) const
930 {
931  return components_;
932 }
933 
934 const std::vector<double>& ompl::base::CompoundStateSpace::getSubspaceWeights(void) const
935 {
936  return weights_;
937 }
938 
940 {
941  unsigned int dim = 0;
942  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
943  dim += components_[i]->getDimension();
944  return dim;
945 }
946 
948 {
949  double e = 0.0;
950  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
951  if (weights_[i] >= std::numeric_limits<double>::epsilon()) // avoid possible multiplication of 0 times infinity
952  e += weights_[i] * components_[i]->getMaximumExtent();
953  return e;
954 }
955 
957 {
958  CompoundState *cstate = static_cast<CompoundState*>(state);
959  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
960  components_[i]->enforceBounds(cstate->components[i]);
961 }
962 
964 {
965  const CompoundState *cstate = static_cast<const CompoundState*>(state);
966  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
967  if (!components_[i]->satisfiesBounds(cstate->components[i]))
968  return false;
969  return true;
970 }
971 
972 void ompl::base::CompoundStateSpace::copyState(State *destination, const State *source) const
973 {
974  CompoundState *cdest = static_cast<CompoundState*>(destination);
975  const CompoundState *csrc = static_cast<const CompoundState*>(source);
976  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
977  components_[i]->copyState(cdest->components[i], csrc->components[i]);
978 }
979 
981 {
982  unsigned int l = 0;
983  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
984  l += components_[i]->getSerializationLength();
985  return l;
986 }
987 
988 void ompl::base::CompoundStateSpace::serialize(void *serialization, const State *state) const
989 {
990  const CompoundState *cstate = static_cast<const CompoundState*>(state);
991  unsigned int l = 0;
992  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
993  {
994  components_[i]->serialize(reinterpret_cast<char*>(serialization) + l, cstate->components[i]);
995  l += components_[i]->getSerializationLength();
996  }
997 }
998 
999 void ompl::base::CompoundStateSpace::deserialize(State *state, const void *serialization) const
1000 {
1001  CompoundState *cstate = static_cast<CompoundState*>(state);
1002  unsigned int l = 0;
1003  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1004  {
1005  components_[i]->deserialize(cstate->components[i], reinterpret_cast<const char*>(serialization) + l);
1006  l += components_[i]->getSerializationLength();
1007  }
1008 }
1009 
1010 double ompl::base::CompoundStateSpace::distance(const State *state1, const State *state2) const
1011 {
1012  const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
1013  const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
1014  double dist = 0.0;
1015  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1016  dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
1017  return dist;
1018 }
1019 
1021 {
1023  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1024  components_[i]->setLongestValidSegmentFraction(segmentFraction);
1025 }
1026 
1027 unsigned int ompl::base::CompoundStateSpace::validSegmentCount(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  unsigned int sc = 0;
1032  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1033  {
1034  unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
1035  if (sci > sc)
1036  sc = sci;
1037  }
1038  return sc;
1039 }
1040 
1041 bool ompl::base::CompoundStateSpace::equalStates(const State *state1, const State *state2) const
1042 {
1043  const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
1044  const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
1045  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1046  if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
1047  return false;
1048  return true;
1049 }
1050 
1051 void ompl::base::CompoundStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
1052 {
1053  const CompoundState *cfrom = static_cast<const CompoundState*>(from);
1054  const CompoundState *cto = static_cast<const CompoundState*>(to);
1055  CompoundState *cstate = static_cast<CompoundState*>(state);
1056  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1057  components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
1058 }
1059 
1061 {
1063  if (weightSum_ < std::numeric_limits<double>::epsilon())
1064  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1065  ss->addSampler(components_[i]->allocStateSampler(), 1.0);
1066  else
1067  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1068  ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / weightSum_);
1069  return StateSamplerPtr(ss);
1070 }
1071 
1073 {
1074  if (subspace->getName() == getName())
1075  return allocStateSampler();
1076  if (hasSubspace(subspace->getName()))
1077  return StateSamplerPtr(new SubspaceStateSampler(this, subspace, getSubspaceWeight(subspace->getName()) / weightSum_));
1078  return StateSpace::allocSubspaceStateSampler(subspace);
1079 }
1080 
1082 {
1083  CompoundState *state = new CompoundState();
1084  allocStateComponents(state);
1085  return static_cast<State*>(state);
1086 }
1087 
1089 {
1090  state->components = new State*[componentCount_];
1091  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1092  state->components[i] = components_[i]->allocState();
1093 }
1094 
1096 {
1097  CompoundState *cstate = static_cast<CompoundState*>(state);
1098  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1099  components_[i]->freeState(cstate->components[i]);
1100  delete[] cstate->components;
1101  delete cstate;
1102 }
1103 
1105 {
1106  locked_ = true;
1107 }
1108 
1110 {
1111  return locked_;
1112 }
1113 
1114 double* ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
1115 {
1116  CompoundState *cstate = static_cast<CompoundState*>(state);
1117  unsigned int idx = 0;
1118 
1119  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1120  for (unsigned int j = 0 ; j <= index ; ++j)
1121  {
1122  double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
1123  if (va)
1124  {
1125  if (idx == index)
1126  return va;
1127  else
1128  idx++;
1129  }
1130  else
1131  break;
1132  }
1133  return NULL;
1134 }
1135 
1136 void ompl::base::CompoundStateSpace::printState(const State *state, std::ostream &out) const
1137 {
1138  out << "Compound state [" << std::endl;
1139  const CompoundState *cstate = static_cast<const CompoundState*>(state);
1140  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1141  components_[i]->printState(cstate->components[i], out);
1142  out << "]" << std::endl;
1143 }
1144 
1146 {
1147  out << "Compound state space '" << getName() << "' of dimension " << getDimension() << (isLocked() ? " (locked)" : "") << " [" << std::endl;
1148  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1149  {
1150  components_[i]->printSettings(out);
1151  out << " of weight " << weights_[i] << std::endl;
1152  }
1153  out << "]" << std::endl;
1154  printProjections(out);
1155 }
1156 
1158 {
1159  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1160  components_[i]->setup();
1161 
1163 }
1164 
1166 {
1168  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
1169  components_[i]->computeLocations();
1170 }
1171 
1172 namespace ompl
1173 {
1174  namespace base
1175  {
1176 
1177  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
1178  {
1179  return copyStateData(destS.get(), dest, sourceS.get(), source);
1180  }
1181 
1182  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS, const State *source)
1183  {
1184  // if states correspond to the same space, simply do copy
1185  if (destS->getName() == sourceS->getName())
1186  {
1187  if (dest != source)
1188  destS->copyState(dest, source);
1189  return ALL_DATA_COPIED;
1190  }
1191 
1193 
1194  // if "to" state is compound
1195  if (destS->isCompound())
1196  {
1197  const CompoundStateSpace *compoundDestS = destS->as<CompoundStateSpace>();
1198  CompoundState *compoundDest = dest->as<CompoundState>();
1199 
1200  // if there is a subspace in "to" that corresponds to "from", set the data and return
1201  for (unsigned int i = 0 ; i < compoundDestS->getSubspaceCount() ; ++i)
1202  if (compoundDestS->getSubspace(i)->getName() == sourceS->getName())
1203  {
1204  if (compoundDest->components[i] != source)
1205  compoundDestS->getSubspace(i)->copyState(compoundDest->components[i], source);
1206  return ALL_DATA_COPIED;
1207  }
1208 
1209  // it could be there are further levels of compound spaces where the data can be set
1210  // so we call this function recursively
1211  for (unsigned int i = 0 ; i < compoundDestS->getSubspaceCount() ; ++i)
1212  {
1213  AdvancedStateCopyOperation res = copyStateData(compoundDestS->getSubspace(i).get(), compoundDest->components[i], sourceS, source);
1214 
1215  if (res != NO_DATA_COPIED)
1216  result = SOME_DATA_COPIED;
1217 
1218  // if all data was copied, we stop
1219  if (res == ALL_DATA_COPIED)
1220  return ALL_DATA_COPIED;
1221  }
1222  }
1223 
1224  // if we got to this point, it means that the data in "from" could not be copied as a chunk to "to"
1225  // it could be the case "from" is from a compound space as well, so we can copy parts of "from", as needed
1226  if (sourceS->isCompound())
1227  {
1228  const CompoundStateSpace *compoundSourceS = sourceS->as<CompoundStateSpace>();
1229  const CompoundState *compoundSource = source->as<CompoundState>();
1230 
1231  unsigned int copiedComponents = 0;
1232 
1233  // if there is a subspace in "to" that corresponds to "from", set the data and return
1234  for (unsigned int i = 0 ; i < compoundSourceS->getSubspaceCount() ; ++i)
1235  {
1236  AdvancedStateCopyOperation res = copyStateData(destS, dest, compoundSourceS->getSubspace(i).get(), compoundSource->components[i]);
1237  if (res == ALL_DATA_COPIED)
1238  copiedComponents++;
1239  if (res != NO_DATA_COPIED)
1240  result = SOME_DATA_COPIED;
1241  }
1242 
1243  // if each individual component got copied, then the entire data in "from" got copied
1244  if (copiedComponents == compoundSourceS->getSubspaceCount())
1245  result = ALL_DATA_COPIED;
1246  }
1247 
1248  return result;
1249  }
1250 
1252  const StateSpacePtr &sourceS, const State *source,
1253  const std::vector<std::string> &subspaces)
1254  {
1255  return copyStateData(destS.get(), dest, sourceS.get(), source, subspaces);
1256  }
1257 
1259  const StateSpace *sourceS, const State *source,
1260  const std::vector<std::string> &subspaces)
1261  {
1262  std::size_t copyCount = 0;
1263  const std::map<std::string, StateSpace::SubstateLocation> &destLoc = destS->getSubstateLocationsByName();
1264  const std::map<std::string, StateSpace::SubstateLocation> &sourceLoc = sourceS->getSubstateLocationsByName();
1265  for (std::size_t i = 0 ; i < subspaces.size() ; ++i)
1266  {
1267  std::map<std::string, StateSpace::SubstateLocation>::const_iterator dt = destLoc.find(subspaces[i]);
1268  if (dt != destLoc.end())
1269  {
1270  std::map<std::string, StateSpace::SubstateLocation>::const_iterator st = sourceLoc.find(subspaces[i]);
1271  if (st != sourceLoc.end())
1272  {
1273  dt->second.space->copyState(destS->getSubstateAtLocation(dest, dt->second), sourceS->getSubstateAtLocation(source, st->second));
1274  ++copyCount;
1275  }
1276  }
1277  }
1278  if (copyCount == subspaces.size())
1279  return ALL_DATA_COPIED;
1280  if (copyCount > 0)
1281  return SOME_DATA_COPIED;
1282  return NO_DATA_COPIED;
1283  }
1284 
1286  inline bool StateSpaceHasContent(const StateSpacePtr &m)
1287  {
1288  if (!m)
1289  return false;
1290  if (m->getDimension() == 0 && m->getType() == STATE_SPACE_UNKNOWN && m->isCompound())
1291  {
1292  const unsigned int nc = m->as<CompoundStateSpace>()->getSubspaceCount();
1293  for (unsigned int i = 0 ; i < nc ; ++i)
1294  if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubspace(i)))
1295  return true;
1296  return false;
1297  }
1298  return true;
1299  }
1301 
1303  {
1304  if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
1305  return b;
1306 
1307  if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
1308  return a;
1309 
1310  std::vector<StateSpacePtr> components;
1311  std::vector<double> weights;
1312 
1313  bool change = false;
1314  if (a)
1315  {
1316  bool used = false;
1317  if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
1318  if (!csm_a->isLocked())
1319  {
1320  used = true;
1321  for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1322  {
1323  components.push_back(csm_a->getSubspace(i));
1324  weights.push_back(csm_a->getSubspaceWeight(i));
1325  }
1326  }
1327 
1328  if (!used)
1329  {
1330  components.push_back(a);
1331  weights.push_back(1.0);
1332  }
1333  }
1334  if (b)
1335  {
1336  bool used = false;
1337  unsigned int size = components.size();
1338 
1339  if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
1340  if (!csm_b->isLocked())
1341  {
1342  used = true;
1343  for (unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
1344  {
1345  bool ok = true;
1346  for (unsigned int j = 0 ; j < size ; ++j)
1347  if (components[j]->getName() == csm_b->getSubspace(i)->getName())
1348  {
1349  ok = false;
1350  break;
1351  }
1352  if (ok)
1353  {
1354  components.push_back(csm_b->getSubspace(i));
1355  weights.push_back(csm_b->getSubspaceWeight(i));
1356  change = true;
1357  }
1358  }
1359  if (components.size() == csm_b->getSubspaceCount())
1360  return b;
1361  }
1362 
1363  if (!used)
1364  {
1365  bool ok = true;
1366  for (unsigned int j = 0 ; j < size ; ++j)
1367  if (components[j]->getName() == b->getName())
1368  {
1369  ok = false;
1370  break;
1371  }
1372  if (ok)
1373  {
1374  components.push_back(b);
1375  weights.push_back(1.0);
1376  change = true;
1377  }
1378  }
1379  }
1380 
1381  if (!change && a)
1382  return a;
1383 
1384  if (components.size() == 1)
1385  return components[0];
1386 
1387  return StateSpacePtr(new CompoundStateSpace(components, weights));
1388  }
1389 
1391  {
1392  std::vector<StateSpacePtr> components_a;
1393  std::vector<double> weights_a;
1394  std::vector<StateSpacePtr> components_b;
1395 
1396  if (a)
1397  {
1398  bool used = false;
1399  if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
1400  if (!csm_a->isLocked())
1401  {
1402  used = true;
1403  for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1404  {
1405  components_a.push_back(csm_a->getSubspace(i));
1406  weights_a.push_back(csm_a->getSubspaceWeight(i));
1407  }
1408  }
1409 
1410  if (!used)
1411  {
1412  components_a.push_back(a);
1413  weights_a.push_back(1.0);
1414  }
1415  }
1416 
1417  if (b)
1418  {
1419  bool used = false;
1420  if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
1421  if (!csm_b->isLocked())
1422  {
1423  used = true;
1424  for (unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
1425  components_b.push_back(csm_b->getSubspace(i));
1426  }
1427  if (!used)
1428  components_b.push_back(b);
1429  }
1430 
1431  bool change = false;
1432  for (unsigned int i = 0 ; i < components_b.size() ; ++i)
1433  for (unsigned int j = 0 ; j < components_a.size() ; ++j)
1434  if (components_a[j]->getName() == components_b[i]->getName())
1435  {
1436  components_a.erase(components_a.begin() + j);
1437  weights_a.erase(weights_a.begin() + j);
1438  change = true;
1439  break;
1440  }
1441 
1442  if (!change && a)
1443  return a;
1444 
1445  if (components_a.size() == 1)
1446  return components_a[0];
1447 
1448  return StateSpacePtr(new CompoundStateSpace(components_a, weights_a));
1449  }
1450 
1451  StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name)
1452  {
1453  std::vector<StateSpacePtr> components;
1454  std::vector<double> weights;
1455 
1456  bool change = false;
1457  if (a)
1458  {
1459  bool used = false;
1460  if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
1461  if (!csm_a->isLocked())
1462  {
1463  used = true;
1464  for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1465  {
1466  if (csm_a->getSubspace(i)->getName() == name)
1467  {
1468  change = true;
1469  continue;
1470  }
1471  components.push_back(csm_a->getSubspace(i));
1472  weights.push_back(csm_a->getSubspaceWeight(i));
1473  }
1474  }
1475 
1476  if (!used)
1477  {
1478  if (a->getName() != name)
1479  {
1480  components.push_back(a);
1481  weights.push_back(1.0);
1482  }
1483  else
1484  change = true;
1485  }
1486  }
1487 
1488  if (!change && a)
1489  return a;
1490 
1491  if (components.size() == 1)
1492  return components[0];
1493 
1494  return StateSpacePtr(new CompoundStateSpace(components, weights));
1495  }
1496 
1498  {
1499  std::vector<StateSpacePtr> components_a;
1500  std::vector<double> weights_a;
1501  std::vector<StateSpacePtr> components_b;
1502  std::vector<double> weights_b;
1503 
1504  if (a)
1505  {
1506  bool used = false;
1507  if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
1508  if (!csm_a->isLocked())
1509  {
1510  used = true;
1511  for (unsigned int i = 0 ; i < csm_a->getSubspaceCount() ; ++i)
1512  {
1513  components_a.push_back(csm_a->getSubspace(i));
1514  weights_a.push_back(csm_a->getSubspaceWeight(i));
1515  }
1516  }
1517 
1518  if (!used)
1519  {
1520  components_a.push_back(a);
1521  weights_a.push_back(1.0);
1522  }
1523  }
1524 
1525  if (b)
1526  {
1527  bool used = false;
1528  if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
1529  if (!csm_b->isLocked())
1530  {
1531  used = true;
1532  for (unsigned int i = 0 ; i < csm_b->getSubspaceCount() ; ++i)
1533  {
1534  components_b.push_back(csm_b->getSubspace(i));
1535  weights_b.push_back(csm_b->getSubspaceWeight(i));
1536  }
1537  }
1538 
1539  if (!used)
1540  {
1541  components_b.push_back(b);
1542  weights_b.push_back(1.0);
1543  }
1544  }
1545 
1546  std::vector<StateSpacePtr> components;
1547  std::vector<double> weights;
1548 
1549  for (unsigned int i = 0 ; i < components_b.size() ; ++i)
1550  {
1551  for (unsigned int j = 0 ; j < components_a.size() ; ++j)
1552  if (components_a[j]->getName() == components_b[i]->getName())
1553  {
1554  components.push_back(components_b[i]);
1555  weights.push_back(std::max(weights_a[j], weights_b[i]));
1556  break;
1557  }
1558  }
1559 
1560  if (a && components.size() == components_a.size())
1561  return a;
1562 
1563  if (b && components.size() == components_b.size())
1564  return b;
1565 
1566  if (components.size() == 1)
1567  return components[0];
1568 
1569  return StateSpacePtr(new CompoundStateSpace(components, weights));
1570  }
1571  }
1572 }