QuotientSpace.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, University of Stuttgart
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 University of Stuttgart nor the names
18  * of its contributors may be used to endorse or promote products
19  * derived from this software without specific prior written
20  * permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Andreas Orthey */
37 #include <ompl/geometric/planners/quotientspace/datastructures/QuotientSpace.h>
38 
39 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
40 #include <ompl/base/goals/GoalSampleableRegion.h>
41 #include <ompl/base/spaces/SO2StateSpace.h>
42 #include <ompl/base/spaces/SO3StateSpace.h>
43 #include <ompl/base/spaces/SE2StateSpace.h>
44 #include <ompl/base/spaces/SE3StateSpace.h>
45 
46 #include <ompl/util/Exception.h>
47 
48 unsigned int ompl::geometric::QuotientSpace::counter_ = 0;
49 
50 ompl::geometric::QuotientSpace::QuotientSpace(const base::SpaceInformationPtr &si, QuotientSpace *parent_)
51  : base::Planner(si, "QuotientSpace"), Q1(si), parent_(parent_)
52 {
53  id_ = counter_++;
54 
55  OMPL_DEVMSG1("QuotientSpace %s", id_);
56 
57  if (parent_ != nullptr)
58  parent_->setChild(this); // need to be able to traverse down the tree
59 
60  const base::StateSpacePtr Q1_space = Q1->getStateSpace();
61 
62 
63  if (parent_ == nullptr)
64  {
65  OMPL_DEVMSG1("ATOMIC dimension: %d measure: %f", Q1_space->getDimension(), Q1_space->getMeasure());
66  type_ = ATOMIC;
67  }
68  else
69  {
70  Q0 = parent_->getSpaceInformation();
71  const base::StateSpacePtr Q0_space = Q0->getStateSpace();
72  // X1 = Q1 / Q0
73  const base::StateSpacePtr X1_space = computeQuotientSpace(Q1_space, Q0_space);
74 
75  if (X1_space != nullptr)
76  {
77  X1 = std::make_shared<base::SpaceInformation>(X1_space);
78  X1_sampler_ = X1->allocStateSampler();
79  if (Q0_space->getDimension() + X1_space->getDimension() != Q1_space->getDimension())
80  {
81  throw ompl::Exception("QuotientSpace Dimensions are wrong.");
82  }
83  OMPL_DEVMSG1("Q0 dimension: %d measure: %f", Q0_space->getDimension(), Q0_space->getMeasure());
84  OMPL_DEVMSG1("X1 dimension: %d measure: %f", X1_space->getDimension(), X1_space->getMeasure());
85  OMPL_DEVMSG1("Q1 dimension: %d measure: %f", Q1_space->getDimension(), Q1_space->getMeasure());
86  if ((Q0_space->getMeasure() <= 0) || (X1_space->getMeasure() <= 0) || (Q1_space->getMeasure() <= 0))
87  {
88  throw ompl::Exception("Zero-measure QuotientSpace detected.");
89  }
90  if (!X1_sampler_)
91  {
92  X1_sampler_ = X1->allocStateSampler();
93  }
94  checkSpaceHasFiniteMeasure(X1_space);
95  }
96  else
97  {
98  OMPL_DEVMSG1("Q0 dimension: %d measure: %f", Q0_space->getDimension(), Q0_space->getMeasure());
99  OMPL_DEVMSG1("Q1 dimension: %d measure: %f", Q1_space->getDimension(), Q1_space->getMeasure());
100  }
101  checkSpaceHasFiniteMeasure(Q0_space);
102  }
103  checkSpaceHasFiniteMeasure(Q1_space);
104 
105  if (!Q1_valid_sampler_)
106  {
107  Q1_valid_sampler_ = Q1->allocValidStateSampler();
108  }
109  if (!Q1_sampler_)
110  {
111  Q1_sampler_ = Q1->allocStateSampler();
112  }
113  if (parent_ != nullptr)
114  {
115  s_Q0_tmp_ = Q0->allocState();
116  if (X1_dimension_ > 0)
117  s_X1_tmp_ = X1->allocState();
118  }
119 }
120 
121 ompl::geometric::QuotientSpace::~QuotientSpace()
122 {
123  if (parent_ != nullptr)
124  {
125  if (s_Q0_tmp_)
126  Q0->freeState(s_Q0_tmp_);
127  if (X1 && s_X1_tmp_)
128  X1->freeState(s_X1_tmp_);
129  }
130 }
131 
132 void ompl::geometric::QuotientSpace::setup()
133 {
134  BaseT::setup();
135  hasSolution_ = false;
136  firstRun_ = true;
137 }
138 
139 void ompl::geometric::QuotientSpace::clear()
140 {
141  BaseT::clear();
142  totalNumberOfSamples_ = 0;
143  totalNumberOfFeasibleSamples_ = 0;
144 
145  hasSolution_ = false;
146  firstRun_ = true;
147  if (parent_ == nullptr && X1_dimension_ > 0)
148  X1_sampler_.reset();
149 
150  pdef_->clearSolutionPaths();
151 }
152 
153 void ompl::geometric::QuotientSpace::checkSpaceHasFiniteMeasure(const base::StateSpacePtr space) const
154 {
155  if (space->getMeasure() >= std::numeric_limits<double>::infinity())
156  {
157  const base::StateSpacePtr Q0_space = Q0->getStateSpace();
158  const base::StateSpacePtr Q1_space = Q1->getStateSpace();
159  OMPL_ERROR("Q0 dimension: %d measure: %f", Q0_space->getDimension(), Q0_space->getMeasure());
160  OMPL_ERROR("Q1 dimension: %d measure: %f", Q1_space->getDimension(), Q1_space->getMeasure());
161  if (X1 != nullptr)
162  {
163  const base::StateSpacePtr X1_space = X1->getStateSpace();
164  OMPL_ERROR("X1 dimension: %d measure: %f", X1_space->getDimension(), X1_space->getMeasure());
165  }
166  throw ompl::Exception("QuotientSpace has no bounds");
167  }
168 }
169 
170 ompl::base::PlannerStatus ompl::geometric::QuotientSpace::solve(const base::PlannerTerminationCondition &ptc)
171 {
172  (void)ptc;
173  throw ompl::Exception("A Quotient-Space cannot be solved alone. Use class MultiQuotient to solve Quotient-Spaces.");
174 }
175 
176 void ompl::geometric::QuotientSpace::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
177 {
178  BaseT::setProblemDefinition(pdef);
179 
180  if (pdef_->hasOptimizationObjective())
181  {
182  opt_ = pdef_->getOptimizationObjective();
183  }
184  else
185  {
186  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
187  }
188 }
189 
190 void ompl::geometric::QuotientSpace::resetCounter()
191 {
192  QuotientSpace::counter_ = 0;
193 }
194 
195 const ompl::base::StateSpacePtr ompl::geometric::QuotientSpace::computeQuotientSpace(const base::StateSpacePtr Q1,
196  const base::StateSpacePtr Q0)
197 {
198  type_ = identifyQuotientSpaceType(Q1, Q0);
199 
200  base::StateSpacePtr X1{nullptr};
201  Q1_dimension_ = Q1->getDimension();
202  Q0_dimension_ = Q0->getDimension();
203 
204  if (Q0_dimension_ == 0 || Q1_dimension_ == 0)
205  {
206  OMPL_ERROR("Q0 has dimension %d.", Q0_dimension_);
207  OMPL_ERROR("Q1 has dimension %d.", Q1_dimension_);
208  throw ompl::Exception("Detected Zero-dimensional QuotientSpace.");
209  }
210 
211  switch (type_)
212  {
213  case IDENTITY_SPACE_RN:
214  case IDENTITY_SPACE_SE2:
215  case IDENTITY_SPACE_SE2RN:
216  case IDENTITY_SPACE_SO2RN:
217  case IDENTITY_SPACE_SE3:
218  case IDENTITY_SPACE_SE3RN:
219  {
220  X1_dimension_ = 0;
221  break;
222  }
223  case RN_RM:
224  {
225  unsigned int N = Q1_dimension_ - Q0_dimension_;
226  X1 = std::make_shared<base::RealVectorStateSpace>(N);
227  X1_dimension_ = N;
228 
229  base::RealVectorBounds Q1_bounds = std::static_pointer_cast<base::RealVectorStateSpace>(Q1)->getBounds();
230  std::vector<double> low;
231  low.resize(N);
232  std::vector<double> high;
233  high.resize(N);
234  base::RealVectorBounds X1_bounds(N);
235  for (unsigned int k = 0; k < N; k++)
236  {
237  X1_bounds.setLow(k, Q1_bounds.low.at(k + Q0_dimension_));
238  X1_bounds.setHigh(k, Q1_bounds.high.at(k + Q0_dimension_));
239  }
240  std::static_pointer_cast<base::RealVectorStateSpace>(X1)->setBounds(X1_bounds);
241 
242  break;
243  }
244  case SE2_R2:
245  {
246  X1_dimension_ = 1;
247  X1 = std::make_shared<base::SO2StateSpace>();
248  break;
249  }
250  case SE3_R3:
251  {
252  X1_dimension_ = 3;
253  X1 = std::make_shared<base::SO3StateSpace>();
254  break;
255  }
256  case SE2RN_SE2:
257  case SE3RN_SE3:
258  case SO2RN_SO2:
259  {
260  base::CompoundStateSpace *Q1_compound = Q1->as<base::CompoundStateSpace>();
261  const std::vector<base::StateSpacePtr> Q1_decomposed = Q1_compound->getSubspaces();
262 
263  X1_dimension_ = Q1_decomposed.at(1)->getDimension();
264 
265  X1 = std::make_shared<base::RealVectorStateSpace>(X1_dimension_);
266  std::static_pointer_cast<base::RealVectorStateSpace>(X1)->setBounds(
267  std::static_pointer_cast<base::RealVectorStateSpace>(Q1_decomposed.at(1))->getBounds());
268 
269  break;
270  }
271  case SE2RN_R2:
272  {
273  base::CompoundStateSpace *Q1_compound = Q1->as<base::CompoundStateSpace>();
274  const std::vector<base::StateSpacePtr> Q1_decomposed = Q1_compound->getSubspaces();
275  const std::vector<base::StateSpacePtr> Q1_SE2_decomposed =
276  Q1_decomposed.at(0)->as<base::CompoundStateSpace>()->getSubspaces();
277 
278  const base::RealVectorStateSpace *Q1_RN = Q1_decomposed.at(1)->as<base::RealVectorStateSpace>();
279  unsigned int N = Q1_RN->getDimension();
280 
281  base::StateSpacePtr SO2(new base::SO2StateSpace());
282  base::StateSpacePtr RN(new base::RealVectorStateSpace(N));
283  RN->as<base::RealVectorStateSpace>()->setBounds(Q1_RN->getBounds());
284 
285  X1 = SO2 + RN;
286  X1_dimension_ = 1 + N;
287  break;
288  }
289  case SE3RN_R3:
290  {
291  base::CompoundStateSpace *Q1_compound = Q1->as<base::CompoundStateSpace>();
292  const std::vector<base::StateSpacePtr> Q1_decomposed = Q1_compound->getSubspaces();
293  const std::vector<base::StateSpacePtr> Q1_SE3_decomposed =
294  Q1_decomposed.at(0)->as<base::CompoundStateSpace>()->getSubspaces();
295 
296  // const base::SE3StateSpace *Q1_SE3 = Q1_SE3_decomposed.at(0)->as<base::SE3StateSpace>();
297  // const base::SO3StateSpace *Q1_SO3 = Q1_SE3_decomposed.at(1)->as<base::SO3StateSpace>();
298  const base::RealVectorStateSpace *Q1_RN = Q1_decomposed.at(1)->as<base::RealVectorStateSpace>();
299  unsigned int N = Q1_RN->getDimension();
300 
301  base::StateSpacePtr SO3(new base::SO3StateSpace());
302  base::StateSpacePtr RN(new base::RealVectorStateSpace(N));
303  RN->as<base::RealVectorStateSpace>()->setBounds(Q1_RN->getBounds());
304 
305  X1 = SO3 + RN;
306  X1_dimension_ = 3 + N;
307  break;
308  }
309  case SE2RN_SE2RM:
310  case SO2RN_SO2RM:
311  case SE3RN_SE3RM:
312  {
313  base::CompoundStateSpace *Q1_compound = Q1->as<base::CompoundStateSpace>();
314  const std::vector<base::StateSpacePtr> Q1_decomposed = Q1_compound->getSubspaces();
315  base::CompoundStateSpace *Q0_compound = Q0->as<base::CompoundStateSpace>();
316  const std::vector<base::StateSpacePtr> Q0_decomposed = Q0_compound->getSubspaces();
317 
318  unsigned int N = Q1_decomposed.at(1)->getDimension();
319  unsigned int M = Q0_decomposed.at(1)->getDimension();
320  X1_dimension_ = N - M;
321  X1 = std::make_shared<base::RealVectorStateSpace>(X1_dimension_);
322 
323  base::RealVectorBounds Q1_bounds =
324  std::static_pointer_cast<base::RealVectorStateSpace>(Q1_decomposed.at(1))->getBounds();
325  std::vector<double> low;
326  low.resize(X1_dimension_);
327  std::vector<double> high;
328  high.resize(X1_dimension_);
329  base::RealVectorBounds X1_bounds(X1_dimension_);
330  for (unsigned int k = 0; k < X1_dimension_; k++)
331  {
332  X1_bounds.setLow(k, Q1_bounds.low.at(k + M));
333  X1_bounds.setHigh(k, Q1_bounds.high.at(k + M));
334  }
335  std::static_pointer_cast<base::RealVectorStateSpace>(X1)->setBounds(X1_bounds);
336  break;
337  }
338  default:
339  {
340  OMPL_ERROR("Unknown QuotientSpace type: %d", type_);
341  throw ompl::Exception("Unknown type");
342  }
343  }
344  return X1;
345 }
346 
347 ompl::geometric::QuotientSpace::QuotientSpaceType
348 ompl::geometric::QuotientSpace::identifyQuotientSpaceType(const base::StateSpacePtr Q1, const base::StateSpacePtr Q0)
349 {
350  //
351  // We can currently handle 11 types of quotient-space mappings.
352  // Emptyset is used for constraint relaxations.
353  //
354  // (1) Q1 Rn , Q0 Rm [0<m<=n] => X1 = R(n-m) \union {\emptyset}
355  // (2a) Q1 SE2 , Q0 R2 => X1 = SO2
356  // (2b) Q1 SE2 , Q0 SE2 => X1 = \emptyset
357  // (3a) Q1 SE3 , Q0 R3 => X1 = SO3
358  // (3b) Q1 SE3 , Q0 SE3 => X1 = \emptyset
359  //
360  // (4) Q1 SE3xRn , Q0 SE3 => X1 = Rn
361  // (5) Q1 SE3xRn , Q0 R3 => X1 = SO3xRn
362  // (6) Q1 SE3xRn , Q0 SE3xRm [0<m<=n ] => X1 = R(n-m) \union {\emptyset}
363  //
364  // (7) Q1 SE2xRn , Q0 SE2 => X1 = Rn
365  // (8) Q1 SE2xRn , Q0 R2 => X1 = SO2xRN
366  // (9) Q1 SE2xRn , Q0 SE2xRm [0<m<=n ] => X1 = R(n-m) \union {\emptyset}
367  //
368  // (10) Q1 SO2xRn , Q0 SO2 => X1 = Rn
369  // (11) Q1 SO2xRn , Q0 SO2xRm [0<m<=n ] => X1 = R(n-m) \union {\emptyset}
370 
371  if (!Q1->isCompound())
372  {
373  // ##############################################################################/
374  //------------------ non-compound cases:
375  // ##############################################################################/
376  //
377  //------------------ (1) Q1 = Rn, Q0 = Rm, 0<m<n, X1 = R(n-m)
378  if (Q1->getType() == base::STATE_SPACE_REAL_VECTOR)
379  {
380  unsigned int n = Q1->getDimension();
381  if (Q0->getType() == base::STATE_SPACE_REAL_VECTOR)
382  {
383  unsigned int m = Q0->getDimension();
384  if (n > m && m > 0)
385  {
386  type_ = RN_RM;
387  }
388  else
389  {
390  if (n == m && m > 0)
391  {
392  type_ = IDENTITY_SPACE_RN;
393  }
394  else
395  {
396  OMPL_ERROR("Not allowed: dimensionality needs to be monotonically increasing.");
397  OMPL_ERROR("We require n >= m > 0 but have n=%d >= m=%d > 0", n, m);
398  throw ompl::Exception("Invalid dimensionality");
399  }
400  }
401  }
402  else
403  {
404  OMPL_ERROR("Q1 is R^%d but Q0 type %d is not handled.", n, Q0->getType());
405  throw ompl::Exception("INVALID_STATE_TYPE");
406  }
407  }
408  else
409  {
410  OMPL_ERROR("Q1 is non-compound state, but its type %d is not handled.", Q1->getType());
411  throw ompl::Exception("INVALID_STATE_TYPE");
412  }
413  }
414  else
415  {
416  // ##############################################################################/
417  //------------------ compound cases:
418  // ##############################################################################/
419  //
420  //------------------ (2) Q1 = SE2, Q0 = R2, X1 = SO2
421  // ##############################################################################/
422  if (Q1->getType() == base::STATE_SPACE_SE2)
423  {
424  if (Q0->getType() == base::STATE_SPACE_REAL_VECTOR)
425  {
426  if (Q0->getDimension() == 2)
427  {
428  type_ = SE2_R2;
429  }
430  else
431  {
432  OMPL_ERROR("Q1 is SE2 but Q0 type %d is of dimension %d", Q0->getType(), Q0->getDimension());
433  throw ompl::Exception("Invalid dimensions.");
434  }
435  }
436  else
437  {
438  if (Q0->getType() == base::STATE_SPACE_SE2)
439  {
440  type_ = IDENTITY_SPACE_SE2;
441  }
442  else
443  {
444  OMPL_ERROR("Q1 is SE2 but Q0 type %d is not handled.", Q0->getType());
445  throw ompl::Exception("INVALID_STATE_TYPE");
446  }
447  }
448  }
449  //------------------ (3) Q1 = SE3, Q0 = R3, X1 = SO3
450  // ##############################################################################/
451  else if (Q1->getType() == base::STATE_SPACE_SE3)
452  {
453  if (Q0->getType() == base::STATE_SPACE_REAL_VECTOR)
454  {
455  if (Q0->getDimension() == 3)
456  {
457  type_ = SE3_R3;
458  }
459  else
460  {
461  OMPL_ERROR("Q1 is SE3 but Q0 type %d is of dimension %d.", Q0->getType(), Q0->getDimension());
462  throw ompl::Exception("Invalid dimensions.");
463  }
464  }
465  else
466  {
467  if (Q0->getType() == base::STATE_SPACE_SE3)
468  {
469  type_ = IDENTITY_SPACE_SE3;
470  }
471  else
472  {
473  OMPL_ERROR("Q1 is SE2 but Q0 type %d is not handled.", Q0->getType());
474  throw ompl::Exception("Invalid QuotientSpace type");
475  }
476  OMPL_ERROR("Q1 is SE3 but Q0 type %d is not handled.", Q0->getType());
477  throw ompl::Exception("Invalid QuotientSpace type");
478  }
479  }
480  // ##############################################################################/
481  else
482  {
483  base::CompoundStateSpace *Q1_compound = Q1->as<base::CompoundStateSpace>();
484  const std::vector<base::StateSpacePtr> Q1_decomposed = Q1_compound->getSubspaces();
485  unsigned int Q1_subspaces = Q1_decomposed.size();
486  if (Q1_subspaces == 2)
487  {
488  if (Q1_decomposed.at(0)->getType() == base::STATE_SPACE_SE3 &&
489  Q1_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
490  {
491  unsigned int n = Q1_decomposed.at(1)->getDimension();
492  if (Q0->getType() == base::STATE_SPACE_SE3)
493  {
494  //------------------ (4) Q1 = SE3xRn, Q0 = SE3, X1 = Rn
495  // ##############################################################################/
496  type_ = SE3RN_SE3;
497  }
498  else if (Q0->getType() == base::STATE_SPACE_REAL_VECTOR)
499  {
500  //------------------ (5) Q1 = SE3xRn, Q0 = R3, X1 = SO3xRN
501  // ##############################################################################/
502  unsigned int m = Q0->getDimension();
503  if (m == 3)
504  {
505  type_ = SE3RN_R3;
506  }
507  else
508  {
509  OMPL_ERROR("Not allowed. Q0 needs to be 3-dimensional but is %d dimensional", m);
510  throw ompl::Exception("Invalid dimensions.");
511  }
512  }
513  else
514  {
515  //------------------ (6) Q1 = SE3xRn, Q0 = SE3xRm, X1 = R(n-m)
516  // ##############################################################################/
517  base::CompoundStateSpace *Q0_compound = Q0->as<base::CompoundStateSpace>();
518  const std::vector<base::StateSpacePtr> Q0_decomposed = Q0_compound->getSubspaces();
519  unsigned int Q0_subspaces = Q0_decomposed.size();
520  if (Q0_subspaces == 2)
521  {
522  if (Q1_decomposed.at(0)->getType() == base::STATE_SPACE_SE3 &&
523  Q1_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
524  {
525  unsigned int m = Q0_decomposed.at(1)->getDimension();
526  if (m < n && m > 0)
527  {
528  type_ = SE3RN_SE3RM;
529  }
530  else
531  {
532  if (m == n)
533  {
534  type_ = IDENTITY_SPACE_SE3RN;
535  }
536  else
537  {
538  OMPL_ERROR("We require n >= m > 0, but have n=%d >= m=%d > 0.", n, m);
539  throw ompl::Exception("Invalid dimensions.");
540  }
541  }
542  }
543  }
544  else
545  {
546  OMPL_ERROR("State compound with %d subspaces not handled.", Q0_subspaces);
547  throw ompl::Exception("Invalid QuotientSpace type");
548  }
549  }
550  }
551  else
552  {
553  if (Q1_decomposed.at(0)->getType() == base::STATE_SPACE_SE2 &&
554  Q1_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
555  {
556  unsigned int n = Q1_decomposed.at(1)->getDimension();
557  if (Q0->getType() == base::STATE_SPACE_SE2)
558  {
559  //------------------ (7) Q1 = SE2xRn, Q0 = SE2, X1 = Rn
560  // ##############################################################################/
561  type_ = SE2RN_SE2;
562  }
563  else if (Q0->getType() == base::STATE_SPACE_REAL_VECTOR)
564  {
565  //------------------ (8) Q1 = SE2xRn, Q0 = R2, X1 = SO2xRN
566  // ##############################################################################/
567  unsigned int m = Q0->getDimension();
568  if (m == 2)
569  {
570  type_ = SE2RN_R2;
571  }
572  else
573  {
574  OMPL_ERROR("Not allowed. Q0 needs to be 2-dimensional but is %d dimensional", m);
575  throw ompl::Exception("Invalid dimensions.");
576  }
577  }
578  else
579  {
580  //------------------ (9) Q1 = SE2xRn, Q0 = SE2xRm, X1 = R(n-m)
581  // ##############################################################################/
582  base::CompoundStateSpace *Q0_compound = Q0->as<base::CompoundStateSpace>();
583  const std::vector<base::StateSpacePtr> Q0_decomposed = Q0_compound->getSubspaces();
584  unsigned int Q0_subspaces = Q0_decomposed.size();
585  if (Q0_subspaces == 2)
586  {
587  if (Q1_decomposed.at(0)->getType() == base::STATE_SPACE_SE2 &&
588  Q1_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
589  {
590  unsigned int m = Q0_decomposed.at(1)->getDimension();
591  if (m < n && m > 0)
592  {
593  type_ = SE2RN_SE2RM;
594  }
595  else
596  {
597  if (m == n)
598  {
599  type_ = IDENTITY_SPACE_SE2RN;
600  }
601  else
602  {
603  OMPL_ERROR("We require n >= m > 0, but have n=%d >= m=%d > 0.", n, m);
604  throw ompl::Exception("Invalid dimensions.");
605  }
606  }
607  }
608  else
609  {
610  }
611  }
612  else
613  {
614  OMPL_ERROR("QO is compound with %d subspaces, but we only handle 2.", Q0_subspaces);
615  throw ompl::Exception("Invalid QuotientSpace type");
616  }
617  }
618  }
619  else if (Q1_decomposed.at(0)->getType() == base::STATE_SPACE_SO2 &&
620  Q1_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
621  {
622  if (Q0->getType() == base::STATE_SPACE_SO2)
623  {
624  //------------------ (10) Q1 = SO2xRn, Q0 = SO2, X1 = Rn
625  // ##############################################################################/
626  type_ = SO2RN_SO2;
627  }
628  else
629  {
630  //------------------ (11) Q1 = SO2xRn, Q0 = SO2xRm, X1 = R(n-m)
631  // ##############################################################################/
632  if (Q0->isCompound())
633  {
634  base::CompoundStateSpace *Q0_compound = Q0->as<base::CompoundStateSpace>();
635  const std::vector<base::StateSpacePtr> Q0_decomposed = Q0_compound->getSubspaces();
636  unsigned int Q0_subspaces = Q0_decomposed.size();
637  if (Q0_subspaces == 2)
638  {
639  if (Q1_decomposed.at(0)->getType() == base::STATE_SPACE_SO2 &&
640  Q1_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
641  {
642  unsigned int n = Q1_decomposed.at(1)->getDimension();
643  unsigned int m = Q0_decomposed.at(1)->getDimension();
644  if (m < n && m > 0)
645  {
646  type_ = SO2RN_SO2RM;
647  }
648  else
649  {
650  if (m == n)
651  {
652  type_ = IDENTITY_SPACE_SO2RN;
653  }
654  else
655  {
656  OMPL_ERROR("We require n >= m > 0 but have n=%d >= m=%d > 0.", n, m);
657  throw ompl::Exception("Invalid dimensions.");
658  }
659  }
660  }
661  else
662  {
663  OMPL_ERROR("Cannot project onto type %d.", Q1->getType());
664  throw ompl::Exception("Invalid QuotientSpace type.");
665  }
666  }
667  else
668  {
669  OMPL_ERROR("Q0 has %d subspaces. We can handle only 2.", Q0_subspaces);
670  throw ompl::Exception("Invalid QuotientSpace type.");
671  }
672  }
673  else
674  {
675  OMPL_ERROR("Cannot project onto type %d.", Q0->getType());
676  throw ompl::Exception("Invalid QuotientSpace type.");
677  }
678  }
679  }
680  else
681  {
682  OMPL_ERROR("State compound %d and %d not recognized.", Q1_decomposed.at(0)->getType(),
683  Q1_decomposed.at(1)->getType());
684  throw ompl::Exception("Invalid QuotientSpace type.");
685  }
686  }
687  }
688  else
689  {
690  OMPL_ERROR("Q1 has %d subspaces, but we only support 2.", Q1_subspaces);
691  throw ompl::Exception("Invalid QuotientSpace type.");
692  }
693  }
694  }
695  return type_;
696 }
697 
698 void ompl::geometric::QuotientSpace::mergeStates(const base::State *qQ0, const base::State *qX1, base::State *qQ1) const
699 {
700  // input : qQ0 \in Q0, qX1 \in X1
701  // output: qQ1 = qQ0 \circ qX1 \in Q1
702  const base::StateSpacePtr Q1_space = Q1->getStateSpace();
703  const base::StateSpacePtr X1_space = X1->getStateSpace();
704  const base::StateSpacePtr Q0_space = parent_->getSpaceInformation()->getStateSpace();
705 
706  switch (type_)
707  {
708  case IDENTITY_SPACE_RN:
709  case IDENTITY_SPACE_SE2:
710  case IDENTITY_SPACE_SE2RN:
711  case IDENTITY_SPACE_SO2RN:
712  case IDENTITY_SPACE_SE3:
713  case IDENTITY_SPACE_SE3RN:
714  {
715  throw ompl::Exception("Cannot merge states for Identity space");
716  }
717  case RN_RM:
718  {
719  base::RealVectorStateSpace::StateType *sQ1 = qQ1->as<base::RealVectorStateSpace::StateType>();
720  const base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
721  const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
722 
723  for (unsigned int k = 0; k < Q0_dimension_; k++)
724  {
725  sQ1->values[k] = sQ0->values[k];
726  }
727  for (unsigned int k = Q0_dimension_; k < Q1_dimension_; k++)
728  {
729  sQ1->values[k] = sX1->values[k - Q0_dimension_];
730  }
731  break;
732  }
733  case SE2_R2:
734  {
735  base::SE2StateSpace::StateType *sQ1 = qQ1->as<base::SE2StateSpace::StateType>();
736  const base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
737  const base::SO2StateSpace::StateType *sX1 = qX1->as<base::SO2StateSpace::StateType>();
738 
739  sQ1->setXY(sQ0->values[0], sQ0->values[1]);
740  sQ1->setYaw(sX1->value);
741 
742  break;
743  }
744  case SE3_R3:
745  {
746  base::SE3StateSpace::StateType *sQ1 = qQ1->as<base::SE3StateSpace::StateType>();
747  base::SO3StateSpace::StateType *sQ1_rotation = &sQ1->rotation();
748 
749  const base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
750  const base::SO3StateSpace::StateType *sX1 = qX1->as<base::SO3StateSpace::StateType>();
751 
752  sQ1->setXYZ(sQ0->values[0], sQ0->values[1], sQ0->values[2]);
753 
754  sQ1_rotation->x = sX1->x;
755  sQ1_rotation->y = sX1->y;
756  sQ1_rotation->z = sX1->z;
757  sQ1_rotation->w = sX1->w;
758 
759  break;
760  }
761  case SE3RN_R3:
762  {
763  base::SE3StateSpace::StateType *sQ1_SE3 =
764  qQ1->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
765  base::SO3StateSpace::StateType *sQ1_SO3 = &sQ1_SE3->rotation();
766  base::RealVectorStateSpace::StateType *sQ1_RN =
767  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
768 
769  const base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
770  const base::SO3StateSpace::StateType *sX1_SO3 =
771  qX1->as<base::CompoundState>()->as<base::SO3StateSpace::StateType>(0);
772  const base::RealVectorStateSpace::StateType *sX1_RN =
773  qX1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
774 
775  sQ1_SE3->setXYZ(sQ0->values[0], sQ0->values[1], sQ0->values[2]);
776  sQ1_SO3->x = sX1_SO3->x;
777  sQ1_SO3->y = sX1_SO3->y;
778  sQ1_SO3->z = sX1_SO3->z;
779  sQ1_SO3->w = sX1_SO3->w;
780 
781  for (unsigned int k = 0; k < X1_dimension_ - 3; k++)
782  {
783  sQ1_RN->values[k] = sX1_RN->values[k];
784  }
785 
786  break;
787  }
788  case SE2RN_SE2:
789  {
790  base::SE2StateSpace::StateType *sQ1_SE2 =
791  qQ1->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
792  base::RealVectorStateSpace::StateType *sQ1_RN =
793  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
794 
795  const base::SE2StateSpace::StateType *sQ0 = qQ0->as<base::SE2StateSpace::StateType>();
796  const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
797 
798  sQ1_SE2->setX(sQ0->getX());
799  sQ1_SE2->setY(sQ0->getY());
800  sQ1_SE2->setYaw(sQ0->getYaw());
801 
802  for (unsigned int k = 0; k < X1_dimension_; k++)
803  {
804  sQ1_RN->values[k] = sX1->values[k];
805  }
806  break;
807  }
808  case SO2RN_SO2:
809  {
810  base::SO2StateSpace::StateType *sQ1_SO2 =
811  qQ1->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
812  base::RealVectorStateSpace::StateType *sQ1_RN =
813  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
814 
815  const base::SO2StateSpace::StateType *sQ0 = qQ0->as<base::SO2StateSpace::StateType>();
816  const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
817 
818  sQ1_SO2->value = sQ0->value;
819 
820  for (unsigned int k = 0; k < X1_dimension_; k++)
821  {
822  sQ1_RN->values[k] = sX1->values[k];
823  }
824  break;
825  }
826  case SO2RN_SO2RM:
827  {
828  base::SO2StateSpace::StateType *sQ1_SO2 =
829  qQ1->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
830  base::RealVectorStateSpace::StateType *sQ1_RN =
831  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
832 
833  const base::SO2StateSpace::StateType *sQ0_SO2 =
834  qQ0->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
835  const base::RealVectorStateSpace::StateType *sQ0_RM =
836  qQ0->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
837 
838  const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
839 
840  sQ1_SO2->value = sQ0_SO2->value;
841 
842  unsigned int M = Q1_dimension_ - X1_dimension_ - 1;
843  unsigned int N = X1_dimension_;
844 
845  for (unsigned int k = 0; k < M; k++)
846  {
847  sQ1_RN->values[k] = sQ0_RM->values[k];
848  }
849  for (unsigned int k = M; k < M + N; k++)
850  {
851  sQ1_RN->values[k] = sX1->values[k - M];
852  }
853  break;
854  }
855 
856  case SE2RN_R2:
857  {
858  base::SE2StateSpace::StateType *sQ1_SE2 =
859  qQ1->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
860  base::RealVectorStateSpace::StateType *sQ1_RN =
861  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
862 
863  const base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
864  const base::SO2StateSpace::StateType *sX1_SO2 =
865  qX1->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
866  const base::RealVectorStateSpace::StateType *sX1_RN =
867  qX1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
868 
869  sQ1_SE2->setX(sQ0->values[0]);
870  sQ1_SE2->setY(sQ0->values[1]);
871  sQ1_SE2->setYaw(sX1_SO2->value);
872 
873  for (unsigned int k = 0; k < X1_dimension_ - 1; k++)
874  {
875  sQ1_RN->values[k] = sX1_RN->values[k];
876  }
877  break;
878  }
879  case SE2RN_SE2RM:
880  {
881  base::SE2StateSpace::StateType *sQ1_SE2 =
882  qQ1->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
883  base::RealVectorStateSpace::StateType *sQ1_RN =
884  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
885 
886  const base::SE2StateSpace::StateType *sQ0_SE2 =
887  qQ0->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
888  const base::RealVectorStateSpace::StateType *sQ0_RM =
889  qQ0->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
890 
891  const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
892 
893  sQ1_SE2->setX(sQ0_SE2->getX());
894  sQ1_SE2->setY(sQ0_SE2->getY());
895  sQ1_SE2->setYaw(sQ0_SE2->getYaw());
896 
897  //[X Y YAW] [1...M-1][M...N-1]
898  // SE2 RN
899  unsigned int M = Q1_dimension_ - X1_dimension_ - 3;
900  unsigned int N = X1_dimension_;
901 
902  for (unsigned int k = 0; k < M; k++)
903  {
904  sQ1_RN->values[k] = sQ0_RM->values[k];
905  }
906  for (unsigned int k = M; k < M + N; k++)
907  {
908  sQ1_RN->values[k] = sX1->values[k - M];
909  }
910  break;
911  }
912  case SE3RN_SE3:
913  {
914  base::SE3StateSpace::StateType *sQ1_SE3 =
915  qQ1->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
916  base::SO3StateSpace::StateType *sQ1_SE3_rotation = &sQ1_SE3->rotation();
917  base::RealVectorStateSpace::StateType *sQ1_RN =
918  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
919 
920  const base::SE3StateSpace::StateType *sQ0 = qQ0->as<base::SE3StateSpace::StateType>();
921  const base::SO3StateSpace::StateType *sQ0_rotation = &sQ0->rotation();
922  const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
923 
924  sQ1_SE3->setXYZ(sQ0->getX(), sQ0->getY(), sQ0->getZ());
925  sQ1_SE3_rotation->x = sQ0_rotation->x;
926  sQ1_SE3_rotation->y = sQ0_rotation->y;
927  sQ1_SE3_rotation->z = sQ0_rotation->z;
928  sQ1_SE3_rotation->w = sQ0_rotation->w;
929 
930  for (unsigned int k = 0; k < X1_dimension_; k++)
931  {
932  sQ1_RN->values[k] = sX1->values[k];
933  }
934 
935  break;
936  }
937  case SE3RN_SE3RM:
938  {
939  base::SE3StateSpace::StateType *sQ1_SE3 =
940  qQ1->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
941  base::SO3StateSpace::StateType *sQ1_SE3_rotation = &sQ1_SE3->rotation();
942  base::RealVectorStateSpace::StateType *sQ1_RN =
943  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
944 
945  const base::SE3StateSpace::StateType *sQ0_SE3 =
946  qQ0->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
947  const base::SO3StateSpace::StateType *sQ0_SE3_rotation = &sQ0_SE3->rotation();
948  const base::RealVectorStateSpace::StateType *sQ0_RM =
949  qQ0->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
950 
951  const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
952 
953  sQ1_SE3->setXYZ(sQ0_SE3->getX(), sQ0_SE3->getY(), sQ0_SE3->getZ());
954  sQ1_SE3_rotation->x = sQ0_SE3_rotation->x;
955  sQ1_SE3_rotation->y = sQ0_SE3_rotation->y;
956  sQ1_SE3_rotation->z = sQ0_SE3_rotation->z;
957  sQ1_SE3_rotation->w = sQ0_SE3_rotation->w;
958 
959  //[X Y Z YAW PITCH ROLL] [1...M-1][M...N-1]
960  // SE3 RN
961  unsigned int M = Q1_dimension_ - X1_dimension_ - 6;
962  unsigned int N = X1_dimension_;
963 
964  for (unsigned int k = 0; k < M; k++)
965  {
966  sQ1_RN->values[k] = sQ0_RM->values[k];
967  }
968  for (unsigned int k = M; k < M + N; k++)
969  {
970  sQ1_RN->values[k] = sX1->values[k - M];
971  }
972  break;
973  }
974  default:
975  {
976  OMPL_ERROR("Type %d not implemented.", type_);
977  throw ompl::Exception("Cannot merge states.");
978  }
979  }
980 }
981 
982 void ompl::geometric::QuotientSpace::projectX1(const base::State *q, base::State *qX1) const
983 {
984  switch (type_)
985  {
986  case RN_RM:
987  {
988  const base::RealVectorStateSpace::StateType *sQ1 = q->as<base::RealVectorStateSpace::StateType>();
989  base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
990 
991  for (unsigned int k = Q0_dimension_; k < Q1_dimension_; k++)
992  {
993  sX1->values[k - Q0_dimension_] = sQ1->values[k];
994  }
995  break;
996  }
997  case SE2_R2:
998  {
999  const base::SE2StateSpace::StateType *sQ1 = q->as<base::SE2StateSpace::StateType>();
1000  base::SO2StateSpace::StateType *sX1 = qX1->as<base::SO2StateSpace::StateType>();
1001  sX1->value = sQ1->getYaw();
1002  break;
1003  }
1004  case SE3_R3:
1005  {
1006  const base::SE3StateSpace::StateType *sQ1 = q->as<base::SE3StateSpace::StateType>();
1007  const base::SO3StateSpace::StateType *sQ1_SO3 = &sQ1->rotation();
1008 
1009  base::SO3StateSpace::StateType *sX1_SO3 = qX1->as<base::SO3StateSpace::StateType>();
1010 
1011  sX1_SO3->x = sQ1_SO3->x;
1012  sX1_SO3->y = sQ1_SO3->y;
1013  sX1_SO3->z = sQ1_SO3->z;
1014  sX1_SO3->w = sQ1_SO3->w;
1015 
1016  break;
1017  }
1018  case SE3RN_R3:
1019  {
1020  const base::SE3StateSpace::StateType *sQ1_SE3 =
1021  q->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
1022  const base::SO3StateSpace::StateType *sQ1_SO3 = &sQ1_SE3->rotation();
1023  const base::RealVectorStateSpace::StateType *sQ1_RN =
1024  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1025 
1026  base::SO3StateSpace::StateType *sX1_SO3 =
1027  qX1->as<base::CompoundState>()->as<base::SO3StateSpace::StateType>(0);
1028  base::RealVectorStateSpace::StateType *sX1_RN =
1029  qX1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1030 
1031  sX1_SO3->x = sQ1_SO3->x;
1032  sX1_SO3->y = sQ1_SO3->y;
1033  sX1_SO3->z = sQ1_SO3->z;
1034  sX1_SO3->w = sQ1_SO3->w;
1035  for (unsigned int k = 0; k < X1_dimension_ - 3; k++)
1036  {
1037  sX1_RN->values[k] = sQ1_RN->values[k];
1038  }
1039 
1040  break;
1041  }
1042  case SE2RN_R2:
1043  {
1044  const base::SE2StateSpace::StateType *sQ1_SE2 =
1045  q->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
1046  const base::RealVectorStateSpace::StateType *sQ1_RN =
1047  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1048 
1049  base::SO2StateSpace::StateType *sX1_SO2 =
1050  qX1->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
1051  base::RealVectorStateSpace::StateType *sX1_RN =
1052  qX1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1053 
1054  sX1_SO2->value = sQ1_SE2->getYaw();
1055  for (unsigned int k = 0; k < X1_dimension_ - 1; k++)
1056  {
1057  sX1_RN->values[k] = sQ1_RN->values[k];
1058  }
1059  break;
1060  }
1061  case SE2RN_SE2RM:
1062  case SO2RN_SO2RM:
1063  {
1064  const base::RealVectorStateSpace::StateType *sQ1_RN =
1065  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1066 
1067  const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
1068 
1069  unsigned int N = Q1_dimension_ - X1_dimension_ - 3;
1070  for (unsigned int k = N; k < Q1_dimension_ - 3; k++)
1071  {
1072  sX1->values[k - N] = sQ1_RN->values[k];
1073  }
1074  break;
1075  }
1076  case SE2RN_SE2:
1077  case SE3RN_SE3:
1078  case SO2RN_SO2:
1079  {
1080  const base::RealVectorStateSpace::StateType *sQ1_RN =
1081  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1082  base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
1083 
1084  for (unsigned int k = 0; k < X1_dimension_; k++)
1085  {
1086  sX1->values[k] = sQ1_RN->values[k];
1087  }
1088 
1089  break;
1090  }
1091  case SE3RN_SE3RM:
1092  {
1093  const base::RealVectorStateSpace::StateType *sQ1_RN =
1094  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1095 
1096  const base::RealVectorStateSpace::StateType *sX1 = qX1->as<base::RealVectorStateSpace::StateType>();
1097 
1098  unsigned int N = Q1_dimension_ - X1_dimension_ - 6;
1099  for (unsigned int k = N; k < Q1_dimension_ - 6; k++)
1100  {
1101  sX1->values[k - N] = sQ1_RN->values[k];
1102  }
1103  break;
1104  }
1105  default:
1106  {
1107  OMPL_ERROR("Type %d not implemented.", type_);
1108  throw ompl::Exception("Cannot project onto X1.");
1109  }
1110  }
1111 }
1112 
1113 void ompl::geometric::QuotientSpace::projectQ0(const base::State *q, base::State *qQ0) const
1114 {
1115  switch (type_)
1116  {
1117  case IDENTITY_SPACE_RN:
1118  case IDENTITY_SPACE_SE2:
1119  case IDENTITY_SPACE_SE2RN:
1120  case IDENTITY_SPACE_SO2RN:
1121  case IDENTITY_SPACE_SE3:
1122  case IDENTITY_SPACE_SE3RN:
1123  {
1124  // Identity function
1125  Q1->getStateSpace()->copyState(qQ0, q);
1126  break;
1127  }
1128  case RN_RM:
1129  {
1130  const base::RealVectorStateSpace::StateType *sQ1 = q->as<base::RealVectorStateSpace::StateType>();
1131  base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
1132 
1133  for (unsigned int k = 0; k < Q0_dimension_; k++)
1134  {
1135  sQ0->values[k] = sQ1->values[k];
1136  }
1137  break;
1138  }
1139  case SE2_R2:
1140  {
1141  const base::SE2StateSpace::StateType *sQ1 = q->as<base::SE2StateSpace::StateType>();
1142  base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
1143  sQ0->values[0] = sQ1->getX();
1144  sQ0->values[1] = sQ1->getY();
1145  break;
1146  }
1147  case SE2RN_R2:
1148  {
1149  const base::SE2StateSpace::StateType *sQ1 =
1150  q->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
1151  base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
1152  sQ0->values[0] = sQ1->getX();
1153  sQ0->values[1] = sQ1->getY();
1154  break;
1155  }
1156  case SE2RN_SE2:
1157  {
1158  const base::SE2StateSpace::StateType *sQ1_SE2 =
1159  q->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
1160  base::SE2StateSpace::StateType *sQ0_SE2 = qQ0->as<base::SE2StateSpace::StateType>();
1161 
1162  sQ0_SE2->setX(sQ1_SE2->getX());
1163  sQ0_SE2->setY(sQ1_SE2->getY());
1164  sQ0_SE2->setYaw(sQ1_SE2->getYaw());
1165 
1166  break;
1167  }
1168  case SO2RN_SO2:
1169  {
1170  const base::SO2StateSpace::StateType *sQ1_SO2 =
1171  q->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
1172  base::SO2StateSpace::StateType *sQ0_SO2 = qQ0->as<base::SO2StateSpace::StateType>();
1173 
1174  sQ0_SO2->value = sQ1_SO2->value;
1175 
1176  break;
1177  }
1178  case SO2RN_SO2RM:
1179  {
1180  const base::SO2StateSpace::StateType *sQ1_SO2 =
1181  q->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
1182  const base::RealVectorStateSpace::StateType *sQ1_RN =
1183  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1184 
1185  base::SO2StateSpace::StateType *sQ0_SO2 =
1186  qQ0->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
1187  base::RealVectorStateSpace::StateType *sQ0_RM =
1188  qQ0->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1189 
1190  sQ0_SO2->value = sQ1_SO2->value;
1191 
1192  for (unsigned int k = 0; k < Q0_dimension_ - 1; k++)
1193  {
1194  sQ0_RM->values[k] = sQ1_RN->values[k];
1195  }
1196  break;
1197  }
1198 
1199  case SE2RN_SE2RM:
1200  {
1201  const base::SE2StateSpace::StateType *sQ1_SE2 =
1202  q->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
1203  const base::RealVectorStateSpace::StateType *sQ1_RN =
1204  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1205 
1206  base::SE2StateSpace::StateType *sQ0_SE2 =
1207  qQ0->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
1208  base::RealVectorStateSpace::StateType *sQ0_RN =
1209  qQ0->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1210 
1211  sQ0_SE2->setX(sQ1_SE2->getX());
1212  sQ0_SE2->setY(sQ1_SE2->getY());
1213  sQ0_SE2->setYaw(sQ1_SE2->getYaw());
1214 
1215  for (unsigned int k = 0; k < Q0_dimension_ - 3; k++)
1216  {
1217  sQ0_RN->values[k] = sQ1_RN->values[k];
1218  }
1219  break;
1220  }
1221  case SE3_R3:
1222  {
1223  const base::SE3StateSpace::StateType *sQ1 = q->as<base::SE3StateSpace::StateType>();
1224  base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
1225 
1226  sQ0->values[0] = sQ1->getX();
1227  sQ0->values[1] = sQ1->getY();
1228  sQ0->values[2] = sQ1->getZ();
1229 
1230  break;
1231  }
1232  case SE3RN_R3:
1233  {
1234  const base::SE3StateSpace::StateType *sQ1_SE3 =
1235  q->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
1236  base::RealVectorStateSpace::StateType *sQ0 = qQ0->as<base::RealVectorStateSpace::StateType>();
1237 
1238  sQ0->values[0] = sQ1_SE3->getX();
1239  sQ0->values[1] = sQ1_SE3->getY();
1240  sQ0->values[2] = sQ1_SE3->getZ();
1241 
1242  break;
1243  }
1244  case SE3RN_SE3:
1245  {
1246  const base::SE3StateSpace::StateType *sQ1_SE3 =
1247  q->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
1248  const base::SO3StateSpace::StateType *sQ1_SE3_rotation = &sQ1_SE3->rotation();
1249 
1250  base::SE3StateSpace::StateType *sQ0 = qQ0->as<base::SE3StateSpace::StateType>();
1251  base::SO3StateSpace::StateType *sQ0_rotation = &sQ0->rotation();
1252 
1253  sQ0->setXYZ(sQ1_SE3->getX(), sQ1_SE3->getY(), sQ1_SE3->getZ());
1254  sQ0_rotation->x = sQ1_SE3_rotation->x;
1255  sQ0_rotation->y = sQ1_SE3_rotation->y;
1256  sQ0_rotation->z = sQ1_SE3_rotation->z;
1257  sQ0_rotation->w = sQ1_SE3_rotation->w;
1258 
1259  break;
1260  }
1261  case SE3RN_SE3RM:
1262  {
1263  const base::SE3StateSpace::StateType *sQ1_SE3 =
1264  q->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
1265  const base::SO3StateSpace::StateType *sQ1_SE3_rotation = &sQ1_SE3->rotation();
1266  const base::RealVectorStateSpace::StateType *sQ1_RN =
1267  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1268 
1269  base::SE3StateSpace::StateType *sQ0_SE3 =
1270  qQ0->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
1271  base::SO3StateSpace::StateType *sQ0_rotation = &sQ0_SE3->rotation();
1272  base::RealVectorStateSpace::StateType *sQ0_RN =
1273  qQ0->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1274 
1275  sQ0_SE3->setXYZ(sQ1_SE3->getX(), sQ1_SE3->getY(), sQ1_SE3->getZ());
1276  sQ0_rotation->x = sQ1_SE3_rotation->x;
1277  sQ0_rotation->y = sQ1_SE3_rotation->y;
1278  sQ0_rotation->z = sQ1_SE3_rotation->z;
1279  sQ0_rotation->w = sQ1_SE3_rotation->w;
1280 
1281  for (unsigned int k = 0; k < Q0_dimension_ - 6; k++)
1282  {
1283  sQ0_RN->values[k] = sQ1_RN->values[k];
1284  }
1285  break;
1286  }
1287  default:
1288  {
1289  OMPL_ERROR("Cannot project onto Q0. Type %d not implemented.", type_);
1290  throw ompl::Exception("Cannot project onto Q0.");
1291  }
1292  }
1293 }
1294 
1295 const ompl::base::SpaceInformationPtr &ompl::geometric::QuotientSpace::getX1() const
1296 {
1297  return X1;
1298 }
1299 
1300 const ompl::base::SpaceInformationPtr &ompl::geometric::QuotientSpace::getQ1() const
1301 {
1302  return Q1;
1303 }
1304 
1305 const ompl::base::SpaceInformationPtr &ompl::geometric::QuotientSpace::getQ0() const
1306 {
1307  return Q0;
1308 }
1309 
1310 unsigned int ompl::geometric::QuotientSpace::getX1Dimension() const
1311 {
1312  return X1_dimension_;
1313 }
1314 
1315 unsigned int ompl::geometric::QuotientSpace::getQ1Dimension() const
1316 {
1317  return Q1->getStateDimension();
1318 }
1319 
1320 unsigned int ompl::geometric::QuotientSpace::getDimension() const
1321 {
1322  return getQ1Dimension();
1323 }
1324 
1325 unsigned int ompl::geometric::QuotientSpace::getQ0Dimension() const
1326 {
1327  return Q0_dimension_;
1328 }
1329 
1330 const ompl::base::StateSamplerPtr &ompl::geometric::QuotientSpace::getX1SamplerPtr() const
1331 {
1332  return X1_sampler_;
1333 }
1334 
1335 const ompl::base::StateSamplerPtr &ompl::geometric::QuotientSpace::getQ1SamplerPtr() const
1336 {
1337  return Q1_sampler_;
1338 }
1339 
1340 bool ompl::geometric::QuotientSpace::hasSolution()
1341 {
1342  if (!hasSolution_)
1343  {
1344  base::PathPtr path;
1345  hasSolution_ = getSolution(path);
1346  }
1347  return hasSolution_;
1348 }
1349 
1350 unsigned int ompl::geometric::QuotientSpace::getTotalNumberOfSamples() const
1351 {
1352  return totalNumberOfSamples_;
1353 }
1354 
1355 unsigned int ompl::geometric::QuotientSpace::getTotalNumberOfFeasibleSamples() const
1356 {
1357  return totalNumberOfFeasibleSamples_;
1358 }
1359 
1360 ompl::geometric::QuotientSpace *ompl::geometric::QuotientSpace::getParent() const
1361 {
1362  return parent_;
1363 }
1364 
1365 ompl::geometric::QuotientSpace *ompl::geometric::QuotientSpace::getChild() const
1366 {
1367  return child_;
1368 }
1369 
1370 void ompl::geometric::QuotientSpace::setChild(ompl::geometric::QuotientSpace *child)
1371 {
1372  child_ = child;
1373 }
1374 
1375 void ompl::geometric::QuotientSpace::setParent(ompl::geometric::QuotientSpace *parent)
1376 {
1377  parent_ = parent;
1378 }
1379 
1380 unsigned int ompl::geometric::QuotientSpace::getLevel() const
1381 {
1382  return level_;
1383 }
1384 
1385 void ompl::geometric::QuotientSpace::setLevel(unsigned int level)
1386 {
1387  level_ = level;
1388 }
1389 
1390 ompl::geometric::QuotientSpace::QuotientSpaceType ompl::geometric::QuotientSpace::getType() const
1391 {
1392  return type_;
1393 }
1394 
1395 ompl::base::OptimizationObjectivePtr ompl::geometric::QuotientSpace::getOptimizationObjectivePtr() const
1396 {
1397  return opt_;
1398 }
1399 
1400 bool ompl::geometric::QuotientSpace::sampleQuotient(base::State *q_random)
1401 {
1402  Q1_sampler_->sampleUniform(q_random);
1403  return true;
1404 }
1405 
1406 bool ompl::geometric::QuotientSpace::sample(base::State *q_random)
1407 {
1408  bool valid = false;
1409  if (parent_ == nullptr)
1410  {
1411  // return Q1_valid_sampler->sample(q_random);
1412  Q1_sampler_->sampleUniform(q_random);
1413  valid = Q1->isValid(q_random);
1414  }
1415  else
1416  {
1417  if (X1_dimension_ > 0)
1418  {
1419  // Adjusted sampling function: Sampling in G0 x X1
1420  X1_sampler_->sampleUniform(s_X1_tmp_);
1421  parent_->sampleQuotient(s_Q0_tmp_);
1422  mergeStates(s_Q0_tmp_, s_X1_tmp_, q_random);
1423  }
1424  else
1425  {
1426  parent_->sampleQuotient(q_random);
1427  }
1428  valid = Q1->isValid(q_random);
1429  }
1430  totalNumberOfSamples_++;
1431  if (valid)
1432  {
1433  totalNumberOfFeasibleSamples_++;
1434  }
1435 
1436  return valid;
1437 }
1438 
1439 double ompl::geometric::QuotientSpace::getImportance() const
1440 {
1441  double N = (double)totalNumberOfSamples_;
1442  return 1.0 / (N + 1);
1443 }
1444 
1445 void ompl::geometric::QuotientSpace::print(std::ostream &out) const
1446 {
1447  out << "[QuotientSpace: id" << id_ << " |lvl" << level_ << "] ";
1448  unsigned int sublevel = std::max(1U, level_);
1449  if (parent_ == nullptr)
1450  {
1451  out << "X" << sublevel << "=Q" << sublevel << ": ";
1452  if (Q1->getStateSpace()->getType() == base::STATE_SPACE_SE2)
1453  {
1454  out << "SE(2)";
1455  }
1456  else if (Q1->getStateSpace()->getType() == base::STATE_SPACE_SE3)
1457  {
1458  out << "SE(3)";
1459  }
1460  else if (Q1->getStateSpace()->getType() == base::STATE_SPACE_REAL_VECTOR)
1461  {
1462  out << "R^" << Q1->getStateDimension();
1463  }
1464  else
1465  {
1466  out << "unknown";
1467  }
1468  }
1469  else
1470  {
1471  out << "X" << sublevel << "=Q" << sublevel << ": ";
1472  switch (type_)
1473  {
1474  case QuotientSpace::IDENTITY_SPACE_RN:
1475  {
1476  out << "R^" << Q0_dimension_ << " | Q" << level_ + 1 << ": R^" << Q1_dimension_;
1477  break;
1478  }
1479  case QuotientSpace::IDENTITY_SPACE_SE2:
1480  {
1481  out << "SE(2)"
1482  << " | Q" << level_ + 1 << ": SE(2)";
1483  break;
1484  }
1485  case QuotientSpace::IDENTITY_SPACE_SE2RN:
1486  {
1487  out << "SE(2)xR^" << Q0_dimension_ << " | Q" << level_ + 1 << ": SE(2)xR^" << Q1_dimension_;
1488  break;
1489  }
1490  case QuotientSpace::IDENTITY_SPACE_SO2RN:
1491  {
1492  out << "SO(2)xR^" << Q0_dimension_ << " | Q" << level_ + 1 << ": SO(2)xR^" << Q1_dimension_;
1493  break;
1494  }
1495  case QuotientSpace::IDENTITY_SPACE_SE3:
1496  {
1497  out << "SE(3)"
1498  << " | Q" << level_ + 1 << ": SE(3)";
1499  break;
1500  }
1501  case QuotientSpace::IDENTITY_SPACE_SE3RN:
1502  {
1503  out << "SE(3)xR^" << Q0_dimension_ << " | Q" << level_ + 1 << ": SE(3)xR^" << Q1_dimension_;
1504  break;
1505  }
1506  case QuotientSpace::RN_RM:
1507  {
1508  out << "R^" << Q0_dimension_ << " | Q" << level_ + 1 << ": R^" << Q1_dimension_ << " | X" << level_ + 1
1509  << ": R^" << Q1_dimension_ - Q0_dimension_;
1510  break;
1511  }
1512  case QuotientSpace::SE2_R2:
1513  {
1514  out << "R^2 | Q" << level_ + 1 << ": SE(2) | X" << level_ + 1 << ": SO(2)";
1515  break;
1516  }
1517  case QuotientSpace::SE3_R3:
1518  {
1519  out << "R^3 | Q" << level_ + 1 << ": SE(3) | X" << level_ + 1 << ": SO(3)";
1520  break;
1521  }
1522  case QuotientSpace::SE2RN_SE2:
1523  {
1524  out << "SE(2) | Q" << level_ + 1 << ": SE(2)xR^" << X1_dimension_ << " | X" << level_ + 1 << ": R^"
1525  << X1_dimension_;
1526  break;
1527  }
1528  case QuotientSpace::SO2RN_SO2:
1529  {
1530  out << "SO(2) | Q" << level_ + 1 << ": SO(2)xR^" << X1_dimension_ << " | X" << level_ + 1 << ": R^"
1531  << X1_dimension_;
1532  break;
1533  }
1534  case QuotientSpace::SE3RN_SE3:
1535  {
1536  out << "SE(3) | Q" << level_ + 1 << ": SE(3)xR^" << X1_dimension_ << " | X" << level_ + 1 << ": R^"
1537  << X1_dimension_;
1538  break;
1539  }
1540  case QuotientSpace::SE2RN_SE2RM:
1541  {
1542  out << "SE(2)xR^" << Q0_dimension_ - 3 << " | Q" << level_ + 1 << ": SE(2)xR^" << Q1_dimension_ - 3
1543  << " | X" << level_ + 1 << ": R^" << X1_dimension_;
1544  break;
1545  }
1546  case QuotientSpace::SO2RN_SO2RM:
1547  {
1548  out << "SO(2)xR^" << Q0_dimension_ - 1 << " | Q" << level_ + 1 << ": SO(2)xR^" << Q1_dimension_ - 1
1549  << " | X" << level_ + 1 << ": R^" << X1_dimension_;
1550  break;
1551  }
1552  case QuotientSpace::SE3RN_SE3RM:
1553  {
1554  out << "SE(3)xR^" << Q0_dimension_ - 6 << " | Q" << level_ + 1 << ": SE(3)xR^" << Q1_dimension_ - 6
1555  << " | X" << level_ + 1 << ": R^" << X1_dimension_;
1556  break;
1557  }
1558  default:
1559  {
1560  out << "unknown type_: " << type_;
1561  }
1562  }
1563  }
1564  out << " [Importance:" << getImportance() << "]";
1565 }
1566 
1567 namespace ompl
1568 {
1569  namespace geometric
1570  {
1571  std::ostream &operator<<(std::ostream &out, const QuotientSpace &quotient_)
1572  {
1573  quotient_.print(out);
1574  return out;
1575  }
1576  } // namespace geometric
1577 } // namespace ompl
ompl::Exception
The exception type for ompl.
Definition: Exception.h:47
ompl::base::CompoundStateSpace
A space to allow the composition of state spaces.
Definition: StateSpace.h:574
ompl::base::CompoundStateSpace::getSubspaces
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
Definition: StateSpace.cpp:978
ompl::base::CompoundStateSpace::as
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: StateSpace.h:590
ompl::base::CompoundState
Definition of a compound state.
Definition: State.h:87
OptimizationObjectivePtr
A shared pointer wrapper for ompl::base::OptimizationObjective.
ompl::base::PlannerTerminationCondition
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
ompl::base::Planner::getSpaceInformation
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
ompl::base::RealVectorBounds
The lower and upper bounds for an Rn space.
Definition: RealVectorBounds.h:48
ompl::base::RealVectorBounds::high
std::vector< double > high
Upper bound.
Definition: RealVectorBounds.h:87
ompl::base::RealVectorBounds::low
std::vector< double > low
Lower bound.
Definition: RealVectorBounds.h:84
ompl::base::RealVectorBounds::setLow
void setLow(double value)
Set the lower bound in each dimension to a specific value.
Definition: RealVectorBounds.cpp:42
ompl::base::RealVectorBounds::setHigh
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
Definition: RealVectorBounds.cpp:47
ompl::base::RealVectorBounds::resize
void resize(std::size_t size)
Change the number of dimensions for the bounds.
Definition: RealVectorBounds.cpp:83
ompl::base::RealVectorStateSpace::StateType::values
double * values
The value of the actual vector in Rn
Definition: RealVectorStateSpace.h:97
ompl::base::RealVectorStateSpace
A state space representing Rn. The distance function is the L2 norm.
Definition: RealVectorStateSpace.h:74
ompl::base::RealVectorStateSpace::getDimension
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
Definition: RealVectorStateSpace.cpp:138
ompl::base::RealVectorStateSpace::getBounds
const RealVectorBounds & getBounds() const
Get the bounds for this state space.
Definition: RealVectorStateSpace.h:130
ompl::base::SE2StateSpace::StateType
A state in SE(2): (x, y, yaw)
Definition: SE2StateSpace.h:54
ompl::base::SE2StateSpace::StateType::setXY
void setXY(double x, double y)
Set the X and Y components of the state.
Definition: SE2StateSpace.h:91
ompl::base::SE2StateSpace::StateType::setY
void setY(double y)
Set the Y component of the state.
Definition: SE2StateSpace.h:85
ompl::base::SE2StateSpace::StateType::getYaw
double getYaw() const
Get the yaw component of the state. This is the rotation in plane, with respect to the Z axis.
Definition: SE2StateSpace.h:73
ompl::base::SE2StateSpace::StateType::getY
double getY() const
Get the Y component of the state.
Definition: SE2StateSpace.h:65
ompl::base::SE2StateSpace::StateType::setX
void setX(double x)
Set the X component of the state.
Definition: SE2StateSpace.h:79
ompl::base::SE2StateSpace::StateType::setYaw
void setYaw(double yaw)
Set the yaw component of the state. This is the rotation in plane, with respect to the Z axis.
Definition: SE2StateSpace.h:100
ompl::base::SE2StateSpace::StateType::getX
double getX() const
Get the X component of the state.
Definition: SE2StateSpace.h:59
ompl::base::SE3StateSpace::StateType
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
Definition: SE3StateSpace.h:54
ompl::base::SE3StateSpace::StateType::getZ
double getZ() const
Get the Z component of the state.
Definition: SE3StateSpace.h:71
ompl::base::SE3StateSpace::StateType::setXYZ
void setXYZ(double x, double y, double z)
Set the X, Y and Z components of the state.
Definition: SE3StateSpace.h:107
ompl::base::SE3StateSpace::StateType::getY
double getY() const
Get the Y component of the state.
Definition: SE3StateSpace.h:65
ompl::base::SE3StateSpace::StateType::rotation
const SO3StateSpace::StateType & rotation() const
Get the rotation component of the state.
Definition: SE3StateSpace.h:77
ompl::base::SE3StateSpace::StateType::getX
double getX() const
Get the X component of the state.
Definition: SE3StateSpace.h:59
ompl::base::SO2StateSpace::StateType
The definition of a state in SO(2)
Definition: SO2StateSpace.h:68
ompl::base::SO2StateSpace::StateType::value
double value
The value of the angle in the interval (-Pi, Pi].
Definition: SO2StateSpace.h:77
ompl::base::SO2StateSpace
A state space representing SO(2). The distance function and interpolation take into account angle wra...
Definition: SO2StateSpace.h:64
ompl::base::SO3StateSpace::StateType
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:91
ompl::base::SO3StateSpace::StateType::x
double x
X component of quaternion vector.
Definition: SO3StateSpace.h:100
ompl::base::SO3StateSpace::StateType::w
double w
scalar component of quaternion
Definition: SO3StateSpace.h:109
ompl::base::SO3StateSpace::StateType::z
double z
Z component of quaternion vector.
Definition: SO3StateSpace.h:106
ompl::base::SO3StateSpace::StateType::y
double y
Y component of quaternion vector.
Definition: SO3StateSpace.h:103
ompl::base::SO3StateSpace
A state space representing SO(3). The internal representation is done with quaternions....
Definition: SO3StateSpace.h:83
SpaceInformationPtr
A shared pointer wrapper for ompl::base::SpaceInformation.
StateSamplerPtr
A shared pointer wrapper for ompl::base::StateSampler.
StateSpacePtr
A shared pointer wrapper for ompl::base::StateSpace.
ompl::base::StateSpace::as
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
ompl::base::State
Definition of an abstract state.
Definition: State.h:50
ompl::base::State::as
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::geometric::QuotientSpace
A single quotient-space.
Definition: QuotientSpace.h:49
ompl::geometric::QuotientSpace::getQ1Dimension
unsigned int getQ1Dimension() const
Dimension of space Q1.
Definition: QuotientSpace.cpp:1315
ompl::geometric::QuotientSpace::clear
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: QuotientSpace.cpp:139
ompl::geometric::QuotientSpace::mergeStates
void mergeStates(const ompl::base::State *qQ0, const ompl::base::State *qX1, ompl::base::State *qQ1) const
Merge a state from Q0 and X1 into a state on Q1 (concatenate)
Definition: QuotientSpace.cpp:698
ompl::geometric::QuotientSpace::setProblemDefinition
virtual void setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planner. The problem needs to be set before calling solve()....
Definition: QuotientSpace.cpp:176
ompl::geometric::QuotientSpace::resetCounter
static void resetCounter()
reset counter for number of levels
Definition: QuotientSpace.cpp:190
ompl::geometric::QuotientSpace::getQ0
const ompl::base::SpaceInformationPtr & getQ0() const
Get SpaceInformationPtr for Q0 (Note: Q0 is the first component of Q1 = Q0 x X1)
Definition: QuotientSpace.cpp:1305
ompl::geometric::QuotientSpace::id_
unsigned int id_
Identity of space (to keep track of number of quotient-spaces created)
Definition: QuotientSpace.h:204
ompl::geometric::QuotientSpace::s_Q0_tmp_
ompl::base::State * s_Q0_tmp_
A temporary state on Q0.
Definition: QuotientSpace.h:193
ompl::geometric::QuotientSpace::setup
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: QuotientSpace.cpp:132
ompl::geometric::QuotientSpace::getChild
QuotientSpace * getChild() const
Child is a less simplified quotient-space (lower in abstraction hierarchy)
Definition: QuotientSpace.cpp:1365
ompl::geometric::QuotientSpace::setChild
void setChild(QuotientSpace *child_)
Set pointer to less simplified quotient-space.
Definition: QuotientSpace.cpp:1370
ompl::geometric::QuotientSpace::solve
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override final
solve disabled (use MultiQuotient::solve) final prevents subclasses to override
Definition: QuotientSpace.cpp:170
ompl::geometric::QuotientSpace::getTotalNumberOfSamples
unsigned int getTotalNumberOfSamples() const
Number of samples drawn on space Q1.
Definition: QuotientSpace.cpp:1350
ompl::geometric::QuotientSpace::getLevel
unsigned int getLevel() const
Level in abstraction hierarchy of quotient-spaces.
Definition: QuotientSpace.cpp:1380
ompl::geometric::QuotientSpace::identifyQuotientSpaceType
QuotientSpaceType identifyQuotientSpaceType(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Identify the type of the quotient Q1 / Q0.
Definition: QuotientSpace.cpp:348
ompl::geometric::QuotientSpace::computeQuotientSpace
const ompl::base::StateSpacePtr computeQuotientSpace(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Compute the quotient Q1 / Q0 between two given spaces.
Definition: QuotientSpace.cpp:195
ompl::geometric::QuotientSpace::QuotientSpace
QuotientSpace(const ompl::base::SpaceInformationPtr &si, QuotientSpace *parent_=nullptr)
Quotient Space contains three OMPL spaces, which we call Q1, Q0 and X1.
Definition: QuotientSpace.cpp:50
ompl::geometric::QuotientSpace::getX1Dimension
unsigned int getX1Dimension() const
Dimension of space X1.
Definition: QuotientSpace.cpp:1310
ompl::geometric::QuotientSpace::print
virtual void print(std::ostream &out) const
Internal function implementing actual printing to stream.
Definition: QuotientSpace.cpp:1445
ompl::geometric::QuotientSpace::setParent
void setParent(QuotientSpace *parent_)
Set pointer to more simplified quotient-space.
Definition: QuotientSpace.cpp:1375
ompl::geometric::QuotientSpace::projectX1
void projectX1(const ompl::base::State *q, ompl::base::State *qX1) const
Quotient Space Projection Operator onto second component ProjectX1: Q0 \times X1 \rightarrow X1.
Definition: QuotientSpace.cpp:982
ompl::geometric::QuotientSpace::getParent
QuotientSpace * getParent() const
Parent is a more simplified quotient-space (higher in abstraction hierarchy)
Definition: QuotientSpace.cpp:1360
ompl::geometric::QuotientSpace::setLevel
void setLevel(unsigned int)
Change abstraction level.
Definition: QuotientSpace.cpp:1385
ompl::geometric::QuotientSpace::getTotalNumberOfFeasibleSamples
unsigned int getTotalNumberOfFeasibleSamples() const
Number of feasible samples drawn on space Q1.
Definition: QuotientSpace.cpp:1355
ompl::geometric::QuotientSpace::getQ0Dimension
unsigned int getQ0Dimension() const
Dimension of space Q0.
Definition: QuotientSpace.cpp:1325
ompl::geometric::QuotientSpace::getDimension
unsigned int getDimension() const
Dimension of space Q1.
Definition: QuotientSpace.cpp:1320
ompl::geometric::QuotientSpace::s_X1_tmp_
ompl::base::State * s_X1_tmp_
A temporary state on X1.
Definition: QuotientSpace.h:195
ompl::geometric::QuotientSpace::checkSpaceHasFiniteMeasure
void checkSpaceHasFiniteMeasure(const ompl::base::StateSpacePtr space) const
Check if quotient-space is unbounded.
Definition: QuotientSpace.cpp:153
ompl::geometric::QuotientSpace::getX1
const ompl::base::SpaceInformationPtr & getX1() const
Get SpaceInformationPtr for X1 (Note: X1 is the second component of Q1 = Q0 x X1)
Definition: QuotientSpace.cpp:1295
ompl::geometric::QuotientSpace::projectQ0
void projectQ0(const ompl::base::State *q, ompl::base::State *qQ0) const
Quotient Space Projection Operator onto first component ProjectQ0: Q0 \times X1 \rightarrow Q0.
Definition: QuotientSpace.cpp:1113
ompl::geometric::QuotientSpace::getType
QuotientSpaceType getType() const
Type of quotient-space.
Definition: QuotientSpace.cpp:1390
ompl::geometric::QuotientSpace::getQ1
const ompl::base::SpaceInformationPtr & getQ1() const
Get SpaceInformationPtr for Q1 (Note: Q1 is the product space Q1 = Q0 x X1)
Definition: QuotientSpace.cpp:1300
OMPL_ERROR
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
ompl::base::STATE_SPACE_SE2
@ STATE_SPACE_SE2
ompl::base::SE2StateSpace
Definition: StateSpaceTypes.h:61
ompl::base::STATE_SPACE_REAL_VECTOR
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
Definition: StateSpaceTypes.h:52
ompl::base::STATE_SPACE_SO2
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
Definition: StateSpaceTypes.h:55
ompl::base::STATE_SPACE_SE3
@ STATE_SPACE_SE3
ompl::base::SE3StateSpace
Definition: StateSpaceTypes.h:64
ompl
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
ompl::base::PlannerStatus
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49