00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "ompl/base/StateSpace.h"
00036 #include "ompl/util/Exception.h"
00037 #include "ompl/tools/config/MagicConstants.h"
00038 #include <boost/thread/mutex.hpp>
00039 #include <boost/lexical_cast.hpp>
00040 #include <numeric>
00041 #include <limits>
00042 #include <queue>
00043 #include <cmath>
00044 #include <list>
00045
00046 const std::string ompl::base::StateSpace::DEFAULT_PROJECTION_NAME = "";
00047
00049 namespace ompl
00050 {
00051 namespace base
00052 {
00053 static std::list<StateSpace*> STATE_SPACE_LIST;
00054 static boost::mutex STATE_SPACE_LIST_LOCK;
00055 }
00056 }
00058
00059 ompl::base::StateSpace::StateSpace(void)
00060 {
00061
00062 static boost::mutex lock;
00063 static unsigned int m = 0;
00064
00065 lock.lock();
00066 m++;
00067 lock.unlock();
00068
00069 name_ = "Space" + boost::lexical_cast<std::string>(m);
00070 msg_.setPrefix(name_);
00071
00072 boost::mutex::scoped_lock smLock(STATE_SPACE_LIST_LOCK);
00073 STATE_SPACE_LIST.push_back(this);
00074
00075 longestValidSegment_ = 0.0;
00076 longestValidSegmentFraction_ = 0.01;
00077 longestValidSegmentCountFactor_ = 1;
00078
00079 type_ = STATE_SPACE_UNKNOWN;
00080
00081 maxExtent_ = std::numeric_limits<double>::infinity();
00082 }
00083
00084 ompl::base::StateSpace::~StateSpace(void)
00085 {
00086 boost::mutex::scoped_lock smLock(STATE_SPACE_LIST_LOCK);
00087 STATE_SPACE_LIST.remove(this);
00088 }
00089
00090 const std::string& ompl::base::StateSpace::getName(void) const
00091 {
00092 return name_;
00093 }
00094
00095 void ompl::base::StateSpace::setName(const std::string &name)
00096 {
00097 name_ = name;
00098 msg_.setPrefix(name_);
00099 }
00100
00101 void ompl::base::StateSpace::registerProjections(void)
00102 {
00103 }
00104
00105 void ompl::base::StateSpace::setup(void)
00106 {
00107 maxExtent_ = getMaximumExtent();
00108 longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
00109
00110 if (longestValidSegment_ < std::numeric_limits<double>::epsilon())
00111 throw Exception("The longest valid segment for state space " + getName() + " must be positive");
00112
00113
00114 std::map<std::string, ProjectionEvaluatorPtr> oldProjections = projections_;
00115 registerProjections();
00116 for (std::map<std::string, ProjectionEvaluatorPtr>::iterator it = oldProjections.begin() ; it != oldProjections.end() ; ++it)
00117 if (it->second->userConfigured())
00118 {
00119 std::map<std::string, ProjectionEvaluatorPtr>::iterator o = projections_.find(it->first);
00120 if (o != projections_.end())
00121 if (!o->second->userConfigured())
00122 projections_[it->first] = it->second;
00123 }
00124
00125 for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00126 it->second->setup();
00127 }
00128
00129 double* ompl::base::StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00130 {
00131 return NULL;
00132 }
00133
00134 void ompl::base::StateSpace::printState(const State *state, std::ostream &out) const
00135 {
00136 out << "State instance [" << state << ']' << std::endl;
00137 }
00138
00139 void ompl::base::StateSpace::printSettings(std::ostream &out) const
00140 {
00141 out << "StateSpace '" << getName() << "' instance: " << this << std::endl;
00142 printProjections(out);
00143 }
00144
00145 void ompl::base::StateSpace::printProjections(std::ostream &out) const
00146 {
00147 if (projections_.empty())
00148 out << "No registered projections" << std::endl;
00149 else
00150 {
00151 out << "Registered projections:" << std::endl;
00152 for (std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.begin() ; it != projections_.end() ; ++it)
00153 {
00154 out << " - ";
00155 if (it->first == DEFAULT_PROJECTION_NAME)
00156 out << "<default>";
00157 else
00158 out << it->first;
00159 out << std::endl;
00160 it->second->printSettings(out);
00161 }
00162 }
00163 }
00164
00166 namespace ompl
00167 {
00168 namespace base
00169 {
00170 static bool StateSpaceIncludes(const StateSpace *self, const StateSpace *other)
00171 {
00172 std::queue<const StateSpace*> q;
00173 q.push(self);
00174 while (!q.empty())
00175 {
00176 const StateSpace *m = q.front();
00177 q.pop();
00178 if (m->getName() == other->getName())
00179 return true;
00180 if (m->isCompound())
00181 {
00182 unsigned int c = m->as<CompoundStateSpace>()->getSubSpaceCount();
00183 for (unsigned int i = 0 ; i < c ; ++i)
00184 q.push(m->as<CompoundStateSpace>()->getSubSpace(i).get());
00185 }
00186 }
00187 return false;
00188 }
00189
00190 static bool StateSpaceCovers(const StateSpace *self, const StateSpace *other)
00191 {
00192 if (StateSpaceIncludes(self, other))
00193 return true;
00194 else
00195 if (other->isCompound())
00196 {
00197 unsigned int c = other->as<CompoundStateSpace>()->getSubSpaceCount();
00198 for (unsigned int i = 0 ; i < c ; ++i)
00199 if (!StateSpaceCovers(self, other->as<CompoundStateSpace>()->getSubSpace(i).get()))
00200 return false;
00201 return true;
00202 }
00203 return false;
00204 }
00205 }
00206 }
00207
00209
00210 bool ompl::base::StateSpace::covers(const StateSpacePtr &other) const
00211 {
00212 return StateSpaceCovers(this, other.get());
00213 }
00214
00215 bool ompl::base::StateSpace::includes(const StateSpacePtr &other) const
00216 {
00217 return StateSpaceIncludes(this, other.get());
00218 }
00219
00220 void ompl::base::StateSpace::Diagram(std::ostream &out)
00221 {
00222 boost::mutex::scoped_lock smLock(STATE_SPACE_LIST_LOCK);
00223 out << "digraph StateSpaces {" << std::endl;
00224 for (std::list<StateSpace*>::iterator it = STATE_SPACE_LIST.begin() ; it != STATE_SPACE_LIST.end(); ++it)
00225 {
00226 out << '"' << (*it)->getName() << '"' << std::endl;
00227 for (std::list<StateSpace*>::iterator jt = STATE_SPACE_LIST.begin() ; jt != STATE_SPACE_LIST.end(); ++jt)
00228 if (it != jt)
00229 {
00230 if ((*it)->isCompound() && (*it)->as<CompoundStateSpace>()->hasSubSpace((*jt)->getName()))
00231 out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [label=\"" <<
00232 boost::lexical_cast<std::string>((*it)->as<CompoundStateSpace>()->getSubSpaceWeight((*jt)->getName())) <<
00233 "\"];" << std::endl;
00234 else
00235 if (!StateSpaceIncludes(*it, *jt) && StateSpaceCovers(*it, *jt))
00236 out << '"' << (*it)->getName() << "\" -> \"" << (*jt)->getName() << "\" [style=dashed];" << std::endl;
00237 }
00238 }
00239 out << '}' << std::endl;
00240 }
00241
00242 void ompl::base::StateSpace::sanityChecks(void) const
00243 {
00244 static const double EPS = std::numeric_limits<float>::epsilon();
00245 static const double ZERO = std::numeric_limits<double>::epsilon();
00246
00247
00248 {
00249 State *s1 = allocState();
00250 State *s2 = allocState();
00251 StateSamplerPtr ss = allocStateSampler();
00252
00253 for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
00254 {
00255 ss->sampleUniform(s1);
00256 if (distance(s1, s1) > EPS)
00257 throw Exception("Distance from a state to itself should be 0");
00258 ss->sampleUniform(s2);
00259 if (!equalStates(s1, s2))
00260 {
00261 double d12 = distance(s1, s2);
00262 if (d12 < ZERO)
00263 throw Exception("Distance between different states should be above 0");
00264 double d21 = distance(s2, s1);
00265 if (fabs(d12 - d21) > EPS)
00266 throw Exception("The distance function should be symmetric");
00267 }
00268 }
00269
00270 freeState(s1);
00271 freeState(s2);
00272 }
00273
00274
00275
00276 if (!isDiscrete() && !isHybrid())
00277 {
00278 State *s1 = allocState();
00279 State *s2 = allocState();
00280 State *s3 = allocState();
00281 StateSamplerPtr ss = allocStateSampler();
00282
00283 for (unsigned int i = 0 ; i < magic::TEST_STATE_COUNT ; ++i)
00284 {
00285 ss->sampleUniform(s1);
00286 ss->sampleUniform(s2);
00287 ss->sampleUniform(s3);
00288
00289 interpolate(s1, s2, 0.0, s3);
00290 if (distance(s1, s3) > EPS)
00291 throw Exception("Interpolation from a state at time 0 should be not change the original state");
00292
00293 interpolate(s1, s2, 1.0, s3);
00294 if (distance(s2, s3) > EPS)
00295 throw Exception("Interpolation to a state at time 1 should be the same as the final state");
00296
00297 interpolate(s1, s2, 0.5, s3);
00298 double diff = distance(s1, s3) + distance(s3, s2) - distance(s1, s2);
00299 if (fabs(diff) > EPS)
00300 throw Exception("Interpolation to midpoint state does not lead to distances that satisfy the triangle inequality (" +
00301 boost::lexical_cast<std::string>(diff) + " difference)");
00302
00303 interpolate(s3, s2, 0.5, s3);
00304 interpolate(s1, s2, 0.75, s2);
00305
00306 if (distance(s2, s3) > EPS)
00307 throw Exception("Continued interpolation does not work as expected");
00308 }
00309 freeState(s1);
00310 freeState(s2);
00311 freeState(s3);
00312 }
00313 }
00314
00315 bool ompl::base::StateSpace::hasDefaultProjection(void) const
00316 {
00317 return hasProjection(DEFAULT_PROJECTION_NAME);
00318 }
00319
00320 bool ompl::base::StateSpace::hasProjection(const std::string &name) const
00321 {
00322 return projections_.find(name) != projections_.end();
00323 }
00324
00325 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getDefaultProjection(void) const
00326 {
00327 if (hasDefaultProjection())
00328 return getProjection(DEFAULT_PROJECTION_NAME);
00329 else
00330 {
00331 msg_.error("No default projection is set. Perhaps setup() needs to be called");
00332 return ProjectionEvaluatorPtr();
00333 }
00334 }
00335
00336 ompl::base::ProjectionEvaluatorPtr ompl::base::StateSpace::getProjection(const std::string &name) const
00337 {
00338 std::map<std::string, ProjectionEvaluatorPtr>::const_iterator it = projections_.find(name);
00339 if (it != projections_.end())
00340 return it->second;
00341 else
00342 {
00343 msg_.error("Projection '" + name + "' is not defined");
00344 return ProjectionEvaluatorPtr();
00345 }
00346 }
00347
00348 const std::map<std::string, ompl::base::ProjectionEvaluatorPtr>& ompl::base::StateSpace::getRegisteredProjections(void) const
00349 {
00350 return projections_;
00351 }
00352
00353 void ompl::base::StateSpace::registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
00354 {
00355 registerProjection(DEFAULT_PROJECTION_NAME, projection);
00356 }
00357
00358 void ompl::base::StateSpace::registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
00359 {
00360 if (projection)
00361 projections_[name] = projection;
00362 else
00363 msg_.error("Attempting to register invalid projection under name '%s'. Ignoring.", name.c_str());
00364 }
00365
00366 bool ompl::base::StateSpace::isCompound(void) const
00367 {
00368 return false;
00369 }
00370
00371 bool ompl::base::StateSpace::isDiscrete(void) const
00372 {
00373 return false;
00374 }
00375
00376 bool ompl::base::StateSpace::isHybrid(void) const
00377 {
00378 return false;
00379 }
00380
00381 void ompl::base::StateSpace::setValidSegmentCountFactor(unsigned int factor)
00382 {
00383 if (factor < 1)
00384 throw Exception("The multiplicative factor for the valid segment count between two states must be strictly positive");
00385 longestValidSegmentCountFactor_ = factor;
00386 }
00387
00388 void ompl::base::StateSpace::setLongestValidSegmentFraction(double segmentFraction)
00389 {
00390 if (segmentFraction < std::numeric_limits<double>::epsilon() || segmentFraction > 1.0 - std::numeric_limits<double>::epsilon())
00391 throw Exception("The fraction of the extent must be larger than 0 and less than 1");
00392 longestValidSegmentFraction_ = segmentFraction;
00393 }
00394
00395 unsigned int ompl::base::StateSpace::getValidSegmentCountFactor(void) const
00396 {
00397 return longestValidSegmentCountFactor_;
00398 }
00399
00400 double ompl::base::StateSpace::getLongestValidSegmentFraction(void) const
00401 {
00402 return longestValidSegmentFraction_;
00403 }
00404
00405 unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
00406 {
00407 return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
00408 }
00409
00410 ompl::base::CompoundStateSpace::CompoundStateSpace(void) : StateSpace(), componentCount_(0), locked_(false)
00411 {
00412 setName("Compound" + getName());
00413 }
00414
00415 ompl::base::CompoundStateSpace::CompoundStateSpace(const std::vector<StateSpacePtr> &components,
00416 const std::vector<double> &weights) : StateSpace(), componentCount_(0), locked_(false)
00417 {
00418 if (components.size() != weights.size())
00419 throw Exception("Number of component spaces and weights are not the same");
00420 setName("Compound" + getName());
00421 for (unsigned int i = 0 ; i < components.size() ; ++i)
00422 addSubSpace(components[i], weights[i]);
00423 }
00424
00425 void ompl::base::CompoundStateSpace::addSubSpace(const StateSpacePtr &component, double weight)
00426 {
00427 if (locked_)
00428 throw Exception("This state space is locked. No further components can be added");
00429 if (weight < 0.0)
00430 throw Exception("Subspace weight cannot be negative");
00431 components_.push_back(component);
00432 weights_.push_back(weight);
00433 componentCount_ = components_.size();
00434 }
00435
00436 bool ompl::base::CompoundStateSpace::isCompound(void) const
00437 {
00438 return true;
00439 }
00440
00441 bool ompl::base::CompoundStateSpace::isHybrid(void) const
00442 {
00443 bool c = false;
00444 bool d = false;
00445 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00446 {
00447 if (components_[i]->isHybrid())
00448 return true;
00449 if (components_[i]->isDiscrete())
00450 d = true;
00451 else
00452 c = true;
00453 }
00454 return c && d;
00455 }
00456
00457 unsigned int ompl::base::CompoundStateSpace::getSubSpaceCount(void) const
00458 {
00459 return componentCount_;
00460 }
00461
00462 const ompl::base::StateSpacePtr& ompl::base::CompoundStateSpace::getSubSpace(const unsigned int index) const
00463 {
00464 if (componentCount_ > index)
00465 return components_[index];
00466 else
00467 throw Exception("Subspace index does not exist");
00468 }
00469
00470 bool ompl::base::CompoundStateSpace::hasSubSpace(const std::string &name) const
00471 {
00472 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00473 if (components_[i]->getName() == name)
00474 return true;
00475 return false;
00476 }
00477
00478 unsigned int ompl::base::CompoundStateSpace::getSubSpaceIndex(const std::string& name) const
00479 {
00480 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00481 if (components_[i]->getName() == name)
00482 return i;
00483 throw Exception("Subspace " + name + " does not exist");
00484 }
00485
00486 const ompl::base::StateSpacePtr& ompl::base::CompoundStateSpace::getSubSpace(const std::string& name) const
00487 {
00488 return components_[getSubSpaceIndex(name)];
00489 }
00490
00491 double ompl::base::CompoundStateSpace::getSubSpaceWeight(const unsigned int index) const
00492 {
00493 if (componentCount_ > index)
00494 return weights_[index];
00495 else
00496 throw Exception("Subspace index does not exist");
00497 }
00498
00499 double ompl::base::CompoundStateSpace::getSubSpaceWeight(const std::string &name) const
00500 {
00501 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00502 if (components_[i]->getName() == name)
00503 return weights_[i];
00504 throw Exception("Subspace " + name + " does not exist");
00505 }
00506
00507 void ompl::base::CompoundStateSpace::setSubSpaceWeight(const unsigned int index, double weight)
00508 {
00509 if (weight < 0.0)
00510 throw Exception("Subspace weight cannot be negative");
00511 if (componentCount_ > index)
00512 weights_[index] = weight;
00513 else
00514 throw Exception("Subspace index does not exist");
00515 }
00516
00517 void ompl::base::CompoundStateSpace::setSubSpaceWeight(const std::string &name, double weight)
00518 {
00519 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00520 if (components_[i]->getName() == name)
00521 {
00522 setSubSpaceWeight(i, weight);
00523 return;
00524 }
00525 throw Exception("Subspace " + name + " does not exist");
00526 }
00527
00528 const std::vector<ompl::base::StateSpacePtr>& ompl::base::CompoundStateSpace::getSubSpaces(void) const
00529 {
00530 return components_;
00531 }
00532
00533 const std::vector<double>& ompl::base::CompoundStateSpace::getSubSpaceWeights(void) const
00534 {
00535 return weights_;
00536 }
00537
00538 unsigned int ompl::base::CompoundStateSpace::getDimension(void) const
00539 {
00540 unsigned int dim = 0;
00541 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00542 dim += components_[i]->getDimension();
00543 return dim;
00544 }
00545
00546 double ompl::base::CompoundStateSpace::getMaximumExtent(void) const
00547 {
00548 double e = 0.0;
00549 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00550 e += weights_[i] * components_[i]->getMaximumExtent();
00551 return e;
00552 }
00553
00554 void ompl::base::CompoundStateSpace::enforceBounds(State *state) const
00555 {
00556 CompoundState *cstate = static_cast<CompoundState*>(state);
00557 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00558 components_[i]->enforceBounds(cstate->components[i]);
00559 }
00560
00561 bool ompl::base::CompoundStateSpace::satisfiesBounds(const State *state) const
00562 {
00563 const CompoundState *cstate = static_cast<const CompoundState*>(state);
00564 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00565 if (!components_[i]->satisfiesBounds(cstate->components[i]))
00566 return false;
00567 return true;
00568 }
00569
00570 void ompl::base::CompoundStateSpace::copyState(State *destination, const State *source) const
00571 {
00572 CompoundState *cdest = static_cast<CompoundState*>(destination);
00573 const CompoundState *csrc = static_cast<const CompoundState*>(source);
00574 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00575 components_[i]->copyState(cdest->components[i], csrc->components[i]);
00576 }
00577
00578 double ompl::base::CompoundStateSpace::distance(const State *state1, const State *state2) const
00579 {
00580 const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00581 const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00582 double dist = 0.0;
00583 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00584 dist += weights_[i] * components_[i]->distance(cstate1->components[i], cstate2->components[i]);
00585 return dist;
00586 }
00587
00588 void ompl::base::CompoundStateSpace::setLongestValidSegmentFraction(double segmentFraction)
00589 {
00590 StateSpace::setLongestValidSegmentFraction(segmentFraction);
00591 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00592 components_[i]->setLongestValidSegmentFraction(segmentFraction);
00593 }
00594
00595 unsigned int ompl::base::CompoundStateSpace::validSegmentCount(const State *state1, const State *state2) const
00596 {
00597 const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00598 const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00599 unsigned int sc = 0;
00600 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00601 {
00602 unsigned int sci = components_[i]->validSegmentCount(cstate1->components[i], cstate2->components[i]);
00603 if (sci > sc)
00604 sc = sci;
00605 }
00606 return sc;
00607 }
00608
00609 bool ompl::base::CompoundStateSpace::equalStates(const State *state1, const State *state2) const
00610 {
00611 const CompoundState *cstate1 = static_cast<const CompoundState*>(state1);
00612 const CompoundState *cstate2 = static_cast<const CompoundState*>(state2);
00613 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00614 if (!components_[i]->equalStates(cstate1->components[i], cstate2->components[i]))
00615 return false;
00616 return true;
00617 }
00618
00619 void ompl::base::CompoundStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00620 {
00621 const CompoundState *cfrom = static_cast<const CompoundState*>(from);
00622 const CompoundState *cto = static_cast<const CompoundState*>(to);
00623 CompoundState *cstate = static_cast<CompoundState*>(state);
00624 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00625 components_[i]->interpolate(cfrom->components[i], cto->components[i], t, cstate->components[i]);
00626 }
00627
00628 ompl::base::StateSamplerPtr ompl::base::CompoundStateSpace::allocStateSampler(void) const
00629 {
00630 double totalWeight = std::accumulate(weights_.begin(), weights_.end(), 0.0);
00631 if (totalWeight < std::numeric_limits<double>::epsilon())
00632 totalWeight = 1.0;
00633 CompoundStateSampler *ss = new CompoundStateSampler(this);
00634 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00635 ss->addSampler(components_[i]->allocStateSampler(), weights_[i] / totalWeight);
00636 return StateSamplerPtr(ss);
00637 }
00638
00639 ompl::base::State* ompl::base::CompoundStateSpace::allocState(void) const
00640 {
00641 CompoundState *state = new CompoundState();
00642 allocStateComponents(state);
00643 return static_cast<State*>(state);
00644 }
00645
00646 void ompl::base::CompoundStateSpace::allocStateComponents(CompoundState *state) const
00647 {
00648 state->components = new State*[componentCount_];
00649 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00650 state->components[i] = components_[i]->allocState();
00651 }
00652
00653 void ompl::base::CompoundStateSpace::freeState(State *state) const
00654 {
00655 CompoundState *cstate = static_cast<CompoundState*>(state);
00656 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00657 components_[i]->freeState(cstate->components[i]);
00658 delete[] cstate->components;
00659 delete cstate;
00660 }
00661
00662 void ompl::base::CompoundStateSpace::lock(void)
00663 {
00664 locked_ = true;
00665 }
00666
00667 bool ompl::base::CompoundStateSpace::isLocked(void) const
00668 {
00669 return locked_;
00670 }
00671
00672 double* ompl::base::CompoundStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00673 {
00674 CompoundState *cstate = static_cast<CompoundState*>(state);
00675 unsigned int idx = 0;
00676
00677 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00678 for (unsigned int j = 0 ; j <= index ; ++j)
00679 {
00680 double *va = components_[i]->getValueAddressAtIndex(cstate->components[i], j);
00681 if (va)
00682 {
00683 if (idx == index)
00684 return va;
00685 else
00686 idx++;
00687 }
00688 else
00689 break;
00690 }
00691 return NULL;
00692 }
00693
00694 void ompl::base::CompoundStateSpace::printState(const State *state, std::ostream &out) const
00695 {
00696 out << "Compound state [" << std::endl;
00697 const CompoundState *cstate = static_cast<const CompoundState*>(state);
00698 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00699 components_[i]->printState(cstate->components[i], out);
00700 out << "]" << std::endl;
00701 }
00702
00703 void ompl::base::CompoundStateSpace::printSettings(std::ostream &out) const
00704 {
00705 out << "Compound state space '" << getName() << "' of dimension " << getDimension() << (isLocked() ? " (locked)" : "") << " [" << std::endl;
00706 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00707 {
00708 components_[i]->printSettings(out);
00709 out << " of weight " << weights_[i] << std::endl;
00710 }
00711 out << "]" << std::endl;
00712 printProjections(out);
00713 }
00714
00715 void ompl::base::CompoundStateSpace::setup(void)
00716 {
00717 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00718 components_[i]->setup();
00719
00720 StateSpace::setup();
00721 }
00722
00723 namespace ompl
00724 {
00725 namespace base
00726 {
00727
00729
00730 static const int NO_DATA_COPIED = 0;
00731 static const int SOME_DATA_COPIED = 1;
00732 static const int ALL_DATA_COPIED = 2;
00733
00735
00736
00737 int copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
00738 {
00739
00740 if (destS->getName() == sourceS->getName())
00741 {
00742 if (dest != source)
00743 destS->copyState(dest, source);
00744 return ALL_DATA_COPIED;
00745 }
00746
00747 int result = NO_DATA_COPIED;
00748
00749
00750 if (destS->isCompound())
00751 {
00752 const CompoundStateSpace *compoundDestS = destS->as<CompoundStateSpace>();
00753 CompoundState *compoundDest = dest->as<CompoundState>();
00754
00755
00756 for (unsigned int i = 0 ; i < compoundDestS->getSubSpaceCount() ; ++i)
00757 if (compoundDestS->getSubSpace(i)->getName() == sourceS->getName())
00758 {
00759 if (compoundDest->components[i] != source)
00760 compoundDestS->getSubSpace(i)->copyState(compoundDest->components[i], source);
00761 return ALL_DATA_COPIED;
00762 }
00763
00764
00765
00766 for (unsigned int i = 0 ; i < compoundDestS->getSubSpaceCount() ; ++i)
00767 {
00768 int res = copyStateData(compoundDestS->getSubSpace(i), compoundDest->components[i], sourceS, source);
00769
00770 if (res != NO_DATA_COPIED)
00771 result = SOME_DATA_COPIED;
00772
00773
00774 if (res == ALL_DATA_COPIED)
00775 return ALL_DATA_COPIED;
00776 }
00777 }
00778
00779
00780
00781 if (sourceS->isCompound())
00782 {
00783 const CompoundStateSpace *compoundSourceS = sourceS->as<CompoundStateSpace>();
00784 const CompoundState *compoundSource = source->as<CompoundState>();
00785
00786 unsigned int copiedComponents = 0;
00787
00788
00789 for (unsigned int i = 0 ; i < compoundSourceS->getSubSpaceCount() ; ++i)
00790 {
00791 int res = copyStateData(destS, dest, compoundSourceS->getSubSpace(i), compoundSource->components[i]);
00792 if (res == ALL_DATA_COPIED)
00793 copiedComponents++;
00794 if (res)
00795 result = SOME_DATA_COPIED;
00796 }
00797
00798
00799 if (copiedComponents == compoundSourceS->getSubSpaceCount())
00800 result = ALL_DATA_COPIED;
00801 }
00802
00803 return result;
00804 }
00805
00807 inline bool StateSpaceHasContent(const StateSpacePtr &m)
00808 {
00809 if (!m)
00810 return false;
00811 if (m->getDimension() == 0 && m->getType() == STATE_SPACE_UNKNOWN && m->isCompound())
00812 {
00813 const unsigned int nc = m->as<CompoundStateSpace>()->getSubSpaceCount();
00814 for (unsigned int i = 0 ; i < nc ; ++i)
00815 if (StateSpaceHasContent(m->as<CompoundStateSpace>()->getSubSpace(i)))
00816 return true;
00817 return false;
00818 }
00819 return true;
00820 }
00822
00823 StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b)
00824 {
00825 if (!StateSpaceHasContent(a) && StateSpaceHasContent(b))
00826 return b;
00827
00828 if (!StateSpaceHasContent(b) && StateSpaceHasContent(a))
00829 return a;
00830
00831 std::vector<StateSpacePtr> components;
00832 std::vector<double> weights;
00833
00834 bool change = false;
00835 if (a)
00836 {
00837 bool used = false;
00838 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
00839 if (!csm_a->isLocked())
00840 {
00841 used = true;
00842 for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
00843 {
00844 components.push_back(csm_a->getSubSpace(i));
00845 weights.push_back(csm_a->getSubSpaceWeight(i));
00846 }
00847 }
00848
00849 if (!used)
00850 {
00851 components.push_back(a);
00852 weights.push_back(1.0);
00853 }
00854 }
00855 if (b)
00856 {
00857 bool used = false;
00858 unsigned int size = components.size();
00859
00860 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
00861 if (!csm_b->isLocked())
00862 {
00863 used = true;
00864 for (unsigned int i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
00865 {
00866 bool ok = true;
00867 for (unsigned int j = 0 ; j < size ; ++j)
00868 if (components[j]->getName() == csm_b->getSubSpace(i)->getName())
00869 {
00870 ok = false;
00871 break;
00872 }
00873 if (ok)
00874 {
00875 components.push_back(csm_b->getSubSpace(i));
00876 weights.push_back(csm_b->getSubSpaceWeight(i));
00877 change = true;
00878 }
00879 }
00880 if (components.size() == csm_b->getSubSpaceCount())
00881 return b;
00882 }
00883
00884 if (!used)
00885 {
00886 bool ok = true;
00887 for (unsigned int j = 0 ; j < size ; ++j)
00888 if (components[j]->getName() == b->getName())
00889 {
00890 ok = false;
00891 break;
00892 }
00893 if (ok)
00894 {
00895 components.push_back(b);
00896 weights.push_back(1.0);
00897 change = true;
00898 }
00899 }
00900 }
00901
00902 if (!change && a)
00903 return a;
00904
00905 if (components.size() == 1)
00906 return components[0];
00907
00908 return StateSpacePtr(new CompoundStateSpace(components, weights));
00909 }
00910
00911 StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
00912 {
00913 std::vector<StateSpacePtr> components_a;
00914 std::vector<double> weights_a;
00915 std::vector<StateSpacePtr> components_b;
00916
00917 if (a)
00918 {
00919 bool used = false;
00920 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
00921 if (!csm_a->isLocked())
00922 {
00923 used = true;
00924 for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
00925 {
00926 components_a.push_back(csm_a->getSubSpace(i));
00927 weights_a.push_back(csm_a->getSubSpaceWeight(i));
00928 }
00929 }
00930
00931 if (!used)
00932 {
00933 components_a.push_back(a);
00934 weights_a.push_back(1.0);
00935 }
00936 }
00937
00938 if (b)
00939 {
00940 bool used = false;
00941 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
00942 if (!csm_b->isLocked())
00943 {
00944 used = true;
00945 for (unsigned int i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
00946 components_b.push_back(csm_b->getSubSpace(i));
00947 }
00948 if (!used)
00949 components_b.push_back(b);
00950 }
00951
00952 bool change = false;
00953 for (unsigned int i = 0 ; i < components_b.size() ; ++i)
00954 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
00955 if (components_a[j]->getName() == components_b[i]->getName())
00956 {
00957 components_a.erase(components_a.begin() + j);
00958 weights_a.erase(weights_a.begin() + j);
00959 change = true;
00960 break;
00961 }
00962
00963 if (!change && a)
00964 return a;
00965
00966 if (components_a.size() == 1)
00967 return components_a[0];
00968
00969 return StateSpacePtr(new CompoundStateSpace(components_a, weights_a));
00970 }
00971
00972 StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name)
00973 {
00974 std::vector<StateSpacePtr> components;
00975 std::vector<double> weights;
00976
00977 bool change = false;
00978 if (a)
00979 {
00980 bool used = false;
00981 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
00982 if (!csm_a->isLocked())
00983 {
00984 used = true;
00985 for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
00986 {
00987 if (csm_a->getSubSpace(i)->getName() == name)
00988 {
00989 change = true;
00990 continue;
00991 }
00992 components.push_back(csm_a->getSubSpace(i));
00993 weights.push_back(csm_a->getSubSpaceWeight(i));
00994 }
00995 }
00996
00997 if (!used)
00998 {
00999 if (a->getName() != name)
01000 {
01001 components.push_back(a);
01002 weights.push_back(1.0);
01003 }
01004 else
01005 change = true;
01006 }
01007 }
01008
01009 if (!change && a)
01010 return a;
01011
01012 if (components.size() == 1)
01013 return components[0];
01014
01015 return StateSpacePtr(new CompoundStateSpace(components, weights));
01016 }
01017
01018 StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b)
01019 {
01020 std::vector<StateSpacePtr> components_a;
01021 std::vector<double> weights_a;
01022 std::vector<StateSpacePtr> components_b;
01023 std::vector<double> weights_b;
01024
01025 if (a)
01026 {
01027 bool used = false;
01028 if (CompoundStateSpace *csm_a = dynamic_cast<CompoundStateSpace*>(a.get()))
01029 if (!csm_a->isLocked())
01030 {
01031 used = true;
01032 for (unsigned int i = 0 ; i < csm_a->getSubSpaceCount() ; ++i)
01033 {
01034 components_a.push_back(csm_a->getSubSpace(i));
01035 weights_a.push_back(csm_a->getSubSpaceWeight(i));
01036 }
01037 }
01038
01039 if (!used)
01040 {
01041 components_a.push_back(a);
01042 weights_a.push_back(1.0);
01043 }
01044 }
01045
01046 if (b)
01047 {
01048 bool used = false;
01049 if (CompoundStateSpace *csm_b = dynamic_cast<CompoundStateSpace*>(b.get()))
01050 if (!csm_b->isLocked())
01051 {
01052 used = true;
01053 for (unsigned int i = 0 ; i < csm_b->getSubSpaceCount() ; ++i)
01054 {
01055 components_b.push_back(csm_b->getSubSpace(i));
01056 weights_b.push_back(csm_b->getSubSpaceWeight(i));
01057 }
01058 }
01059
01060 if (!used)
01061 {
01062 components_b.push_back(b);
01063 weights_b.push_back(1.0);
01064 }
01065 }
01066
01067 std::vector<StateSpacePtr> components;
01068 std::vector<double> weights;
01069
01070 for (unsigned int i = 0 ; i < components_b.size() ; ++i)
01071 {
01072 for (unsigned int j = 0 ; j < components_a.size() ; ++j)
01073 if (components_a[j]->getName() == components_b[i]->getName())
01074 {
01075 components.push_back(components_b[i]);
01076 weights.push_back(std::max(weights_a[j], weights_b[i]));
01077 break;
01078 }
01079 }
01080
01081 if (a && components.size() == components_a.size())
01082 return a;
01083
01084 if (b && components.size() == components_b.size())
01085 return b;
01086
01087 if (components.size() == 1)
01088 return components[0];
01089
01090 return StateSpacePtr(new CompoundStateSpace(components, weights));
01091 }
01092
01093 }
01094 }