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  }
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 
133 {
134  BaseT::setup();
135  hasSolution_ = false;
136  firstRun_ = true;
137 }
138 
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 
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 
191 {
192  QuotientSpace::counter_ = 0;
193 }
194 
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  {
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  {
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  {
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  {
314  const std::vector<base::StateSpacePtr> Q1_decomposed = Q1_compound->getSubspaces();
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  {
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  // ##############################################################################/
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  // ##############################################################################/
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  {
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 
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  {
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  {
738 
739  sQ1->setXY(sQ0->values[0], sQ0->values[1]);
740  sQ1->setYaw(sX1->value);
741 
742  break;
743  }
744  case SE3_R3:
745  {
747  base::SO3StateSpace::StateType *sQ1_rotation = &sQ1->rotation();
748 
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  {
764  qQ1->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
765  base::SO3StateSpace::StateType *sQ1_SO3 = &sQ1_SE3->rotation();
767  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
768 
770  const base::SO3StateSpace::StateType *sX1_SO3 =
771  qX1->as<base::CompoundState>()->as<base::SO3StateSpace::StateType>(0);
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  {
791  qQ1->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
793  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
794 
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  {
811  qQ1->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
813  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
814 
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  {
829  qQ1->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
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);
836  qQ0->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
837 
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  {
859  qQ1->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
861  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
862 
864  const base::SO2StateSpace::StateType *sX1_SO2 =
865  qX1->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
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  {
882  qQ1->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
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);
889  qQ0->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
890 
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  {
915  qQ1->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
916  base::SO3StateSpace::StateType *sQ1_SE3_rotation = &sQ1_SE3->rotation();
918  qQ1->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
919 
921  const base::SO3StateSpace::StateType *sQ0_rotation = &sQ0->rotation();
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  {
940  qQ1->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
941  base::SO3StateSpace::StateType *sQ1_SE3_rotation = &sQ1_SE3->rotation();
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();
949  qQ0->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
950 
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 
983 {
984  switch (type_)
985  {
986  case RN_RM:
987  {
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  {
1001  sX1->value = sQ1->getYaw();
1002  break;
1003  }
1004  case SE3_R3:
1005  {
1007  const base::SO3StateSpace::StateType *sQ1_SO3 = &sQ1->rotation();
1008 
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();
1024  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1025 
1027  qX1->as<base::CompoundState>()->as<base::SO3StateSpace::StateType>(0);
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);
1047  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1048 
1050  qX1->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
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  {
1065  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1066 
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  {
1081  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
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  {
1094  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1095 
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 
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  {
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  {
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);
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);
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);
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);
1183  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1184 
1186  qQ0->as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
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);
1204  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1205 
1207  qQ0->as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
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  {
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);
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 
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();
1267  q->as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
1268 
1270  qQ0->as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
1271  base::SO3StateSpace::StateType *sQ0_rotation = &sQ0_SE3->rotation();
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 
1296 {
1297  return X1;
1298 }
1299 
1301 {
1302  return Q1;
1303 }
1304 
1306 {
1307  return Q0;
1308 }
1309 
1311 {
1312  return X1_dimension_;
1313 }
1314 
1316 {
1317  return Q1->getStateDimension();
1318 }
1319 
1321 {
1322  return getQ1Dimension();
1323 }
1324 
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 
1351 {
1352  return totalNumberOfSamples_;
1353 }
1354 
1356 {
1357  return totalNumberOfFeasibleSamples_;
1358 }
1359 
1361 {
1362  return parent_;
1363 }
1364 
1366 {
1367  return child_;
1368 }
1369 
1371 {
1372  child_ = child;
1373 }
1374 
1376 {
1377  parent_ = parent;
1378 }
1379 
1381 {
1382  return level_;
1383 }
1384 
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
Definition of a compound state.
Definition: State.h:151
const ompl::base::StateSpacePtr computeQuotientSpace(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Compute the quotient Q1 / Q0 between two given spaces.
unsigned int getQ0Dimension() const
Dimension of space Q0.
double getX() const
Get the X component of the state.
void setXYZ(double x, double y, double z)
Set the X, Y and Z components of the state.
A state space representing SO(2). The distance function and interpolation take into account angle wra...
A shared pointer wrapper for ompl::base::SpaceInformation.
double getY() const
Get the Y component of the state.
double getYaw() const
Get the yaw component of the state. This is the rotation in plane, with respect to the Z axis.
A space to allow the composition of state spaces.
Definition: StateSpace.h:638
void setYaw(double yaw)
Set the yaw component of the state. This is the rotation in plane, with respect to the Z axis.
void checkSpaceHasFiniteMeasure(const ompl::base::StateSpacePtr space) const
Check if quotient-space is unbounded.
void setChild(QuotientSpace *child_)
Set pointer to less simplified quotient-space.
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 of an abstract state.
Definition: State.h:114
void setX(double x)
Set the X component of the state.
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
std::vector< double > low
Lower bound.
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
unsigned int getLevel() const
Level in abstraction hierarchy of quotient-spaces.
QuotientSpace * getParent() const
Parent is a more simplified quotient-space (higher in abstraction hierarchy)
double * values
The value of the actual vector in Rn
unsigned int id_
Identity of space (to keep track of number of quotient-spaces created)
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
double value
The value of the angle in the interval (-Pi, Pi].
The definition of a state in SO(3) represented as a unit quaternion.
A state space representing SO(3). The internal representation is done with quaternions....
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
A shared pointer wrapper for ompl::base::OptimizationObjective.
void setLow(double value)
Set the lower bound in each dimension to a specific value.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
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.
const RealVectorBounds & getBounds() const
Get the bounds for this state space.
const ompl::base::SpaceInformationPtr & getX1() const
Get SpaceInformationPtr for X1 (Note: X1 is the second component of Q1 = Q0 x X1)
unsigned int getTotalNumberOfFeasibleSamples() const
Number of feasible samples drawn on space Q1.
QuotientSpace * getChild() const
Child is a less simplified quotient-space (lower in abstraction hierarchy)
A state space representing Rn. The distance function is the L2 norm.
void setLevel(unsigned int)
Change abstraction level.
@ STATE_SPACE_SE2
ompl::base::SE2StateSpace
void setY(double y)
Set the Y component of the state.
unsigned int getTotalNumberOfSamples() const
Number of samples drawn on space Q1.
static void resetCounter()
reset counter for number of levels
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
ompl::base::State * s_X1_tmp_
A temporary state on X1.
unsigned int getDimension() const
Dimension of space Q1.
const ompl::base::SpaceInformationPtr & getQ0() const
Get SpaceInformationPtr for Q0 (Note: Q0 is the first component of Q1 = Q0 x X1)
QuotientSpaceType identifyQuotientSpaceType(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Identify the type of the quotient Q1 / Q0.
const SO3StateSpace::StateType & rotation() const
Get the rotation component of the state.
std::vector< double > high
Upper bound.
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
A class to store the exit status of Planner::solve()
double getZ() const
Get the Z component of the state.
double z
Z component of quaternion vector.
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()....
double getY() const
Get the Y component of the state.
@ STATE_SPACE_SE3
ompl::base::SE3StateSpace
unsigned int getX1Dimension() const
Dimension of space X1.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
The definition of a state in SO(2)
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:151
void setXY(double x, double y)
Set the X and Y components of the state.
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override final
solve disabled (use MultiQuotient::solve) final prevents subclasses to override
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)
unsigned int getQ1Dimension() const
Dimension of space Q1.
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: StateSpace.h:654
A state in SE(2): (x, y, yaw)
const ompl::base::SpaceInformationPtr & getQ1() const
Get SpaceInformationPtr for Q1 (Note: Q1 is the product space Q1 = Q0 x X1)
A shared pointer wrapper for ompl::base::StateSpace.
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
Definition: StateSpace.cpp:978
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
double x
X component of quaternion vector.
A shared pointer wrapper for ompl::base::StateSampler.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
A single quotient-space.
double w
scalar component of quaternion
QuotientSpace(const ompl::base::SpaceInformationPtr &si, QuotientSpace *parent_=nullptr)
Quotient Space contains three OMPL spaces, which we call Q1, Q0 and X1.
double y
Y component of quaternion vector.
QuotientSpaceType getType() const
Type of quotient-space.
The exception type for ompl.
Definition: Exception.h:79
double getX() const
Get the X component of the state.
ompl::base::State * s_Q0_tmp_
A temporary state on Q0.
virtual void print(std::ostream &out) const
Internal function implementing actual printing to stream.
void setParent(QuotientSpace *parent_)
Set pointer to more simplified quotient-space.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
The lower and upper bounds for an Rn space.