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
48unsigned int ompl::geometric::QuotientSpace::counter_ = 0;
49
50ompl::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 }
102 }
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
121ompl::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
153void 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
176void 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
347ompl::geometric::QuotientSpace::QuotientSpaceType
348ompl::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 {
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
1330const ompl::base::StateSamplerPtr &ompl::geometric::QuotientSpace::getX1SamplerPtr() const
1331{
1332 return X1_sampler_;
1333}
1334
1335const ompl::base::StateSamplerPtr &ompl::geometric::QuotientSpace::getQ1SamplerPtr() const
1336{
1337 return Q1_sampler_;
1338}
1339
1340bool 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
1390ompl::geometric::QuotientSpace::QuotientSpaceType ompl::geometric::QuotientSpace::getType() const
1391{
1392 return type_;
1393}
1394
1395ompl::base::OptimizationObjectivePtr ompl::geometric::QuotientSpace::getOptimizationObjectivePtr() const
1396{
1397 return opt_;
1398}
1399
1400bool ompl::geometric::QuotientSpace::sampleQuotient(base::State *q_random)
1401{
1402 Q1_sampler_->sampleUniform(q_random);
1403 return true;
1404}
1405
1406bool 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
1439double ompl::geometric::QuotientSpace::getImportance() const
1440{
1441 double N = (double)totalNumberOfSamples_;
1442 return 1.0 / (N + 1);
1443}
1444
1445void 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
1567namespace 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
The exception type for ompl.
Definition: Exception.h:47
A space to allow the composition of state spaces.
Definition: StateSpace.h:574
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
Definition: StateSpace.cpp:978
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: StateSpace.h:590
Definition of a compound state.
Definition: State.h:87
A shared pointer wrapper for ompl::base::OptimizationObjective.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
The lower and upper bounds for an Rn space.
std::vector< double > high
Upper bound.
std::vector< double > low
Lower bound.
void setLow(double value)
Set the lower bound in each dimension to a specific value.
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
void resize(std::size_t size)
Change the number of dimensions for the bounds.
double * values
The value of the actual vector in Rn
A state space representing Rn. The distance function is the L2 norm.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
const RealVectorBounds & getBounds() const
Get the bounds for this state space.
A state in SE(2): (x, y, yaw)
Definition: SE2StateSpace.h:54
void setXY(double x, double y)
Set the X and Y components of the state.
Definition: SE2StateSpace.h:91
void setY(double y)
Set the Y component of the state.
Definition: SE2StateSpace.h:85
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
double getY() const
Get the Y component of the state.
Definition: SE2StateSpace.h:65
void setX(double x)
Set the X component of the state.
Definition: SE2StateSpace.h:79
void setYaw(double yaw)
Set the yaw component of the state. This is the rotation in plane, with respect to the Z axis.
double getX() const
Get the X component of the state.
Definition: SE2StateSpace.h:59
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
Definition: SE3StateSpace.h:54
double getZ() const
Get the Z component of the state.
Definition: SE3StateSpace.h:71
void setXYZ(double x, double y, double z)
Set the X, Y and Z components of the state.
double getY() const
Get the Y component of the state.
Definition: SE3StateSpace.h:65
double getX() const
Get the X component of the state.
Definition: SE3StateSpace.h:59
const SO3StateSpace::StateType & rotation() const
Get the rotation component of the state.
Definition: SE3StateSpace.h:77
The definition of a state in SO(2)
Definition: SO2StateSpace.h:68
double value
The value of the angle in the interval (-Pi, Pi].
Definition: SO2StateSpace.h:77
A state space representing SO(2). The distance function and interpolation take into account angle wra...
Definition: SO2StateSpace.h:64
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:91
double x
X component of quaternion vector.
double w
scalar component of quaternion
double z
Z component of quaternion vector.
double y
Y component of quaternion vector.
A state space representing SO(3). The internal representation is done with quaternions....
Definition: SO3StateSpace.h:83
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
A single quotient-space.
Definition: QuotientSpace.h:49
unsigned int getQ1Dimension() const
Dimension of space Q1.
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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)
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()....
static void resetCounter()
reset counter for number of levels
const ompl::base::SpaceInformationPtr & getQ0() const
Get SpaceInformationPtr for Q0 (Note: Q0 is the first component of Q1 = Q0 x X1)
unsigned int id_
Identity of space (to keep track of number of quotient-spaces created)
ompl::base::State * s_Q0_tmp_
A temporary state on Q0.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
QuotientSpace * getChild() const
Child is a less simplified quotient-space (lower in abstraction hierarchy)
void setChild(QuotientSpace *child_)
Set pointer to less simplified quotient-space.
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override final
solve disabled (use MultiQuotient::solve) final prevents subclasses to override
unsigned int getTotalNumberOfSamples() const
Number of samples drawn on space Q1.
unsigned int getLevel() const
Level in abstraction hierarchy of quotient-spaces.
QuotientSpaceType identifyQuotientSpaceType(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Identify the type of the quotient Q1 / Q0.
const ompl::base::StateSpacePtr computeQuotientSpace(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Compute the quotient Q1 / Q0 between two given spaces.
QuotientSpace(const ompl::base::SpaceInformationPtr &si, QuotientSpace *parent_=nullptr)
Quotient Space contains three OMPL spaces, which we call Q1, Q0 and X1.
unsigned int getX1Dimension() const
Dimension of space X1.
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.
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.
QuotientSpace * getParent() const
Parent is a more simplified quotient-space (higher in abstraction hierarchy)
void setLevel(unsigned int)
Change abstraction level.
unsigned int getTotalNumberOfFeasibleSamples() const
Number of feasible samples drawn on space Q1.
unsigned int getQ0Dimension() const
Dimension of space Q0.
unsigned int getDimension() const
Dimension of space Q1.
ompl::base::State * s_X1_tmp_
A temporary state on X1.
void checkSpaceHasFiniteMeasure(const ompl::base::StateSpacePtr space) const
Check if quotient-space is unbounded.
const ompl::base::SpaceInformationPtr & getX1() const
Get SpaceInformationPtr for X1 (Note: X1 is the second component of Q1 = Q0 x X1)
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.
QuotientSpaceType getType() const
Type of quotient-space.
const ompl::base::SpaceInformationPtr & getQ1() const
Get SpaceInformationPtr for Q1 (Note: Q1 is the product space Q1 = Q0 x X1)
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
@ STATE_SPACE_SE2
ompl::base::SE2StateSpace
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
@ STATE_SPACE_SE3
ompl::base::SE3StateSpace
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49