Loading...
Searching...
No Matches
ProjectionFactory.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2021,
5 * Max Planck Institute for Intelligent Systems (MPI-IS).
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the MPI-IS nor the names
19 * of its contributors may be used to endorse or promote products
20 * derived from this software without specific prior written
21 * permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36
37/* Author: Andreas Orthey */
38
39#include <ompl/multilevel/datastructures/ProjectionFactory.h>
40
41// XRN -> X
42#include <ompl/multilevel/datastructures/projections/XRN_X_SO2.h>
43#include <ompl/multilevel/datastructures/projections/XRN_X_SO3.h>
44#include <ompl/multilevel/datastructures/projections/XRN_X_SE2.h>
45#include <ompl/multilevel/datastructures/projections/XRN_X_SE3.h>
46
47// XRN -> XRM
48#include <ompl/multilevel/datastructures/projections/XRN_XRM_SO2.h>
49#include <ompl/multilevel/datastructures/projections/XRN_XRM_SO3.h>
50#include <ompl/multilevel/datastructures/projections/XRN_XRM_SE2.h>
51#include <ompl/multilevel/datastructures/projections/XRN_XRM_SE3.h>
52
53#include <ompl/multilevel/datastructures/projections/SE3_R3.h>
54#include <ompl/multilevel/datastructures/projections/SE3RN_R3.h>
55#include <ompl/multilevel/datastructures/projections/SE2_R2.h>
56#include <ompl/multilevel/datastructures/projections/SE2RN_R2.h>
57
58#include <ompl/multilevel/datastructures/projections/RN_RM.h>
59#include <ompl/multilevel/datastructures/projections/RNSO2_RN.h>
60#include <ompl/multilevel/datastructures/projections/SO2N_SO2M.h>
61
62#include <ompl/multilevel/datastructures/projections/None.h>
63#include <ompl/multilevel/datastructures/projections/EmptySet.h>
64#include <ompl/multilevel/datastructures/projections/Identity.h>
65#include <ompl/multilevel/datastructures/projections/Relaxation.h>
66
67#include <ompl/util/Exception.h>
68
69using namespace ompl::multilevel;
70using namespace ompl::base;
71
73{
74 const base::StateSpacePtr Bundle_space = Bundle->getStateSpace();
75 int nrProjections = GetNumberOfComponents(Bundle_space);
76
77 OMPL_DEBUG("Bundle components: %d", nrProjections);
78
79 if (nrProjections > 1)
80 {
81 std::vector<ProjectionPtr> components;
82 const base::CompoundStateSpace *Bundle_compound = Bundle_space->as<base::CompoundStateSpace>();
83 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->getSubspaces();
84
85 for (int m = 0; m < nrProjections; m++)
86 {
87 const base::StateSpacePtr BundleM = Bundle_decomposed.at(m);
88 ProjectionPtr componentM = makeProjection(BundleM);
89 components.push_back(componentM);
90 }
91 return std::make_shared<CompoundProjection>(Bundle_space, nullptr, components);
92 }
93 else
94 {
95 ProjectionPtr component = makeProjection(Bundle_space);
96 return component;
97 }
98}
99
100ProjectionPtr ProjectionFactory::makeProjection(const SpaceInformationPtr &Bundle, const SpaceInformationPtr &Base)
101{
102 if (Base == nullptr)
103 {
104 return makeProjection(Bundle);
105 }
106
107 const base::StateSpacePtr Bundle_space = Bundle->getStateSpace();
108 int nrProjections = GetNumberOfComponents(Bundle_space);
109 const base::StateSpacePtr Base_space = Base->getStateSpace();
110 int baseSpaceComponents = GetNumberOfComponents(Base_space);
111
112 OMPL_DEBUG("Bundle components: %d", nrProjections);
113
114 if (baseSpaceComponents != nrProjections)
115 {
116 Base->printSettings();
117 OMPL_ERROR("Base Space has %d, but Bundle Space has %d components.", baseSpaceComponents, nrProjections);
118 throw Exception("Different Number Of Components");
119 }
120
121 // Check if planning spaces are equivalent, i.e. if (X, \phi) == (Y, \phi)
122 bool areValidityCheckersEquivalent = false;
123 if (*(Base->getStateValidityChecker().get()) == *(Bundle->getStateValidityChecker().get()))
124 {
125 areValidityCheckersEquivalent = true;
126 }
127
128 if (nrProjections > 1)
129 {
130 std::vector<ProjectionPtr> components;
131
132 base::CompoundStateSpace *Bundle_compound = Bundle_space->as<base::CompoundStateSpace>();
133 base::CompoundStateSpace *Base_compound = Base_space->as<base::CompoundStateSpace>();
134
135 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->getSubspaces();
136 const std::vector<base::StateSpacePtr> Base_decomposed = Base_compound->getSubspaces();
137
138 for (int m = 0; m < nrProjections; m++)
139 {
140 base::StateSpacePtr BaseM = Base_decomposed.at(m);
141 base::StateSpacePtr BundleM = Bundle_decomposed.at(m);
142 ProjectionPtr componentM = makeProjection(BundleM, BaseM, areValidityCheckersEquivalent);
143 components.push_back(componentM);
144 }
145
146 return std::make_shared<CompoundProjection>(Bundle_space, Base_space, components);
147 }
148 else
149 {
150 ProjectionPtr component = makeProjection(Bundle_space, Base_space, areValidityCheckersEquivalent);
151 return component;
152 }
153}
154
155ProjectionPtr ProjectionFactory::makeProjection(const StateSpacePtr &Bundle)
156{
157 return makeProjection(Bundle, nullptr, false);
158}
159
160ProjectionPtr ProjectionFactory::makeProjection(const StateSpacePtr &Bundle, const StateSpacePtr &Base,
161 bool areValidityCheckersEquivalent)
162{
163 ProjectionType type = identifyProjectionType(Bundle, Base);
164 if (type == PROJECTION_IDENTITY && !areValidityCheckersEquivalent)
165 {
167 }
168
169 ProjectionPtr component;
170
171 if (type == PROJECTION_NONE)
172 {
173 component = std::make_shared<Projection_None>(Bundle, Base);
174 }
175 else if (type == PROJECTION_EMPTY_SET)
176 {
177 component = std::make_shared<Projection_EmptySet>(Bundle, Base);
178 }
179 else if (type == PROJECTION_IDENTITY)
180 {
181 component = std::make_shared<Projection_Identity>(Bundle, Base);
182 }
183 else if (type == PROJECTION_CONSTRAINED_RELAXATION)
184 {
185 component = std::make_shared<Projection_Relaxation>(Bundle, Base);
186 }
187 else if (type == PROJECTION_RN_RM)
188 {
189 component = std::make_shared<Projection_RN_RM>(Bundle, Base);
190 }
191 else if (type == PROJECTION_RNSO2_RN)
192 {
193 component = std::make_shared<Projection_RNSO2_RN>(Bundle, Base);
194 }
195 else if (type == PROJECTION_SE2_R2)
196 {
197 component = std::make_shared<Projection_SE2_R2>(Bundle, Base);
198 }
199 else if (type == PROJECTION_SE2RN_R2)
200 {
201 component = std::make_shared<Projection_SE2RN_R2>(Bundle, Base);
202 }
203 else if (type == PROJECTION_SE2RN_SE2)
204 {
205 component = std::make_shared<Projection_SE2RN_SE2>(Bundle, Base);
206 }
207 else if (type == PROJECTION_SE2RN_SE2RM)
208 {
209 component = std::make_shared<Projection_SE2RN_SE2RM>(Bundle, Base);
210 }
211 else if (type == PROJECTION_SO2RN_SO2)
212 {
213 component = std::make_shared<Projection_SO2RN_SO2>(Bundle, Base);
214 }
215 else if (type == PROJECTION_SO2RN_SO2RM)
216 {
217 component = std::make_shared<Projection_SO2RN_SO2RM>(Bundle, Base);
218 }
219 else if (type == PROJECTION_SO3RN_SO3)
220 {
221 component = std::make_shared<Projection_SO3RN_SO3>(Bundle, Base);
222 }
223 else if (type == PROJECTION_SO3RN_SO3RM)
224 {
225 component = std::make_shared<Projection_SO3RN_SO3RM>(Bundle, Base);
226 }
227 else if (type == PROJECTION_SE3_R3)
228 {
229 component = std::make_shared<Projection_SE3_R3>(Bundle, Base);
230 }
231 else if (type == PROJECTION_SE3RN_R3)
232 {
233 component = std::make_shared<Projection_SE3RN_R3>(Bundle, Base);
234 }
235 else if (type == PROJECTION_SE3RN_SE3)
236 {
237 component = std::make_shared<Projection_SE3RN_SE3>(Bundle, Base);
238 }
239 else if (type == PROJECTION_SE3RN_SE3RM)
240 {
241 component = std::make_shared<Projection_SE3RN_SE3RM>(Bundle, Base);
242 }
243 else if (type == PROJECTION_SO2N_SO2M)
244 {
245 component = std::make_shared<Projection_SO2N_SO2M>(Bundle, Base);
246 }
247 else
248 {
249 OMPL_ERROR("NYI: %d", type);
250 throw Exception("BundleSpaceType not yet implemented.");
251 }
252 auto componentFiber = std::dynamic_pointer_cast<FiberedProjection>(component);
253 if (componentFiber != nullptr)
254 {
255 componentFiber->makeFiberSpace();
256 }
257 return component;
258}
259
261{
262 if (Base == nullptr)
263 {
264 return PROJECTION_NONE;
265 }
266
267 if (isMapping_Identity(Bundle, Base))
268 {
269 return PROJECTION_IDENTITY;
270 }
271
272 if (isMapping_EmptyProjection(Bundle, Base))
273 {
275 }
276
277 // RN ->
278 if (isMapping_RN_to_RM(Bundle, Base))
279 {
280 return PROJECTION_RN_RM;
281 }
282 if (isMapping_RNSO2_to_RN(Bundle, Base))
283 {
284 return PROJECTION_RNSO2_RN;
285 }
286
287 // SE3 ->
288 if (isMapping_SE3_to_R3(Bundle, Base))
289 {
290 return PROJECTION_SE3_R3;
291 }
292 if (isMapping_SE3RN_to_SE3(Bundle, Base))
293 {
295 }
296 if (isMapping_SE3RN_to_R3(Bundle, Base))
297 {
298 return PROJECTION_SE3RN_R3;
299 }
300 if (isMapping_SE3RN_to_SE3RM(Bundle, Base))
301 {
303 }
304
305 // SE2 ->
306 if (isMapping_SE2_to_R2(Bundle, Base))
307 {
308 return PROJECTION_SE2_R2;
309 }
310 if (isMapping_SE2RN_to_SE2(Bundle, Base))
311 {
313 }
314 if (isMapping_SE2RN_to_R2(Bundle, Base))
315 {
316 return PROJECTION_SE2RN_R2;
317 }
318 if (isMapping_SE2RN_to_SE2RM(Bundle, Base))
319 {
321 }
322
323 // SO2 ->
324 if (isMapping_SO2RN_to_SO2(Bundle, Base))
325 {
327 }
328 if (isMapping_SO2RN_to_SO2RM(Bundle, Base))
329 {
331 }
332 if (isMapping_SO2N_to_SO2M(Bundle, Base))
333 {
335 }
336
337 // SO3 ->
338 if (isMapping_SO3RN_to_SO3(Bundle, Base))
339 {
341 }
342 if (isMapping_SO3RN_to_SO3RM(Bundle, Base))
343 {
345 }
346
347 OMPL_ERROR("Fiber Bundle unknown.");
348 return PROJECTION_UNKNOWN;
349}
350
352{
353 if (Bundle->isCompound())
354 {
355 if (Base->isCompound())
356 {
357 base::CompoundStateSpace *Bundle_compound = Bundle->as<base::CompoundStateSpace>();
358 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->getSubspaces();
359 base::CompoundStateSpace *Base_compound = Base->as<base::CompoundStateSpace>();
360 const std::vector<base::StateSpacePtr> Base_decomposed = Base_compound->getSubspaces();
361
362 if (Bundle_decomposed.size() == Base_decomposed.size())
363 {
364 for (unsigned int k = 0; k < Bundle_decomposed.size(); k++)
365 {
366 if (!isMapping_Identity(Bundle_decomposed.at(k), Base_decomposed.at(k)))
367 {
368 return false;
369 }
370 }
371 }
372 return true;
373 }
374 }
375 else
376 {
377 if ((Base->getType() == Bundle->getType()) && (Base->getDimension() == Bundle->getDimension()))
378 {
379 return true;
380 }
381 }
382 return false;
383}
384
386{
387 if (Bundle->isCompound())
388 return false;
389
390 if (Bundle->getType() == base::STATE_SPACE_REAL_VECTOR)
391 {
392 unsigned int n = Bundle->getDimension();
393 if (Base->getType() == base::STATE_SPACE_REAL_VECTOR)
394 {
395 unsigned int m = Base->getDimension();
396 if (n > m && m > 0)
397 {
398 return true;
399 }
400 }
401 }
402 return false;
403}
404
406{
407 if (!Bundle->isCompound())
408 return false;
409
410 if (Bundle->getType() == base::STATE_SPACE_SE3)
411 {
412 if (Base->getType() == base::STATE_SPACE_REAL_VECTOR)
413 {
414 if (Base->getDimension() == 3)
415 {
416 return true;
417 }
418 }
419 }
420 return false;
421}
423{
424 if (!Bundle->isCompound())
425 return false;
426
427 base::CompoundStateSpace *Bundle_compound = Bundle->as<base::CompoundStateSpace>();
428 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->getSubspaces();
429 unsigned int Bundle_subspaces = Bundle_decomposed.size();
430 if (Bundle_subspaces == 2)
431 {
432 if (Bundle_decomposed.at(0)->getType() == base::STATE_SPACE_SE3 &&
433 Bundle_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
434 {
435 if (Base->getType() == base::STATE_SPACE_REAL_VECTOR)
436 {
437 unsigned int m = Base->getDimension();
438 if (m == 3)
439 {
440 return true;
441 }
442 }
443 }
444 }
445 return false;
446}
447
449{
450 if (!Bundle->isCompound())
451 return false;
452
453 if (Bundle->getType() == base::STATE_SPACE_SE2)
454 {
455 if (Base->getType() == base::STATE_SPACE_REAL_VECTOR)
456 {
457 if (Base->getDimension() == 2)
458 {
459 return true;
460 }
461 }
462 }
463 return false;
464}
465
467{
468 if (!Bundle->isCompound())
469 return false;
470
471 base::CompoundStateSpace *Bundle_compound = Bundle->as<base::CompoundStateSpace>();
472 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->getSubspaces();
473 unsigned int Bundle_subspaces = Bundle_decomposed.size();
474 if (Bundle_subspaces == 2)
475 {
476 if (Bundle_decomposed.at(0)->getType() == base::STATE_SPACE_REAL_VECTOR &&
477 Bundle_decomposed.at(1)->getType() == base::STATE_SPACE_SO2)
478 {
479 if (Base->getType() == base::STATE_SPACE_REAL_VECTOR)
480 {
481 unsigned int n = Bundle_decomposed.at(0)->getDimension();
482 unsigned int m = Base->getDimension();
483 if (m == n)
484 {
485 return true;
486 }
487 }
488 }
489 }
490 return false;
491}
492
494{
495 if (!Bundle->isCompound())
496 return false;
497
498 base::CompoundStateSpace *Bundle_compound = Bundle->as<base::CompoundStateSpace>();
499 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->getSubspaces();
500 unsigned int Bundle_subspaces = Bundle_decomposed.size();
501 if (Bundle_subspaces == 2)
502 {
503 if (Bundle_decomposed.at(0)->getType() == base::STATE_SPACE_SE2 &&
504 Bundle_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
505 {
506 if (Base->getType() == base::STATE_SPACE_REAL_VECTOR)
507 {
508 unsigned int m = Base->getDimension();
509 if (m == 2)
510 {
511 return true;
512 }
513 }
514 }
515 }
516 return false;
517}
518
520{
521 return isMapping_XRN_to_X(Bundle, Base, base::STATE_SPACE_SE2);
522}
523
525{
526 return isMapping_XRN_to_X(Bundle, Base, base::STATE_SPACE_SE3);
527}
528
530{
531 return isMapping_XRN_to_X(Bundle, Base, base::STATE_SPACE_SO2);
532}
533
535{
536 return isMapping_XRN_to_X(Bundle, Base, base::STATE_SPACE_SO3);
537}
538
543
548
553
558
560{
561 if (!Bundle->isCompound())
562 return false;
563
564 base::CompoundStateSpace *Bundle_compound = Bundle->as<base::CompoundStateSpace>();
565 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->getSubspaces();
566 unsigned int Bundle_subspaces = Bundle_decomposed.size();
567
568 for (unsigned int k = 0; k < Bundle_subspaces; k++)
569 {
570 if (!(Bundle_decomposed.at(k)->getType() == base::STATE_SPACE_SO2))
571 {
572 return false;
573 }
574 }
575 if (!Base->isCompound())
576 {
577 if (!(Base->getType() == base::STATE_SPACE_SO2))
578 {
579 return false;
580 }
581 }
582 else
583 {
584 base::CompoundStateSpace *Base_compound = Base->as<base::CompoundStateSpace>();
585 const std::vector<base::StateSpacePtr> Base_decomposed = Base_compound->getSubspaces();
586 unsigned int Base_subspaces = Base_decomposed.size();
587
588 for (unsigned int k = 0; k < Base_subspaces; k++)
589 {
590 if (!(Base_decomposed.at(k)->getType() == base::STATE_SPACE_SO2))
591 {
592 return false;
593 }
594 }
595 }
596
597 return true;
598}
599
601 const StateSpaceType type)
602{
603 if (!Bundle->isCompound())
604 return false;
605
606 base::CompoundStateSpace *Bundle_compound = Bundle->as<base::CompoundStateSpace>();
607 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->getSubspaces();
608 unsigned int Bundle_subspaces = Bundle_decomposed.size();
609 if (Bundle_subspaces == 2)
610 {
611 if (Bundle_decomposed.at(0)->getType() == type &&
612 Bundle_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
613 {
614 if (Base->getType() == type)
615 {
616 return true;
617 }
618 }
619 }
620 return false;
621}
622
624 const StateSpaceType type)
625{
626 if (!Bundle->isCompound())
627 return false;
628
629 base::CompoundStateSpace *Bundle_compound = Bundle->as<base::CompoundStateSpace>();
630 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->getSubspaces();
631 if (Bundle_decomposed.size() == 2)
632 {
633 if (Bundle_decomposed.at(0)->getType() == type &&
634 Bundle_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
635 {
636 if (!Base->isCompound())
637 return false;
638 unsigned int n = Bundle_decomposed.at(1)->getDimension();
639
640 base::CompoundStateSpace *Base_compound = Base->as<base::CompoundStateSpace>();
641 const std::vector<base::StateSpacePtr> Base_decomposed = Base_compound->getSubspaces();
642 if (Base_decomposed.size() == 2)
643 {
644 if (Base_decomposed.at(0)->getType() == type &&
645 Base_decomposed.at(1)->getType() == base::STATE_SPACE_REAL_VECTOR)
646 {
647 unsigned int m = Base_decomposed.at(1)->getDimension();
648 if (n > m && m > 0)
649 {
650 return true;
651 }
652 }
653 }
654 }
655 }
656 return false;
657}
658
660{
661 if (Base == nullptr || Base->getDimension() <= 0)
662 {
663 return true;
664 }
665 return false;
666}
667
669{
670 int nrComponents = 0;
671
672 if (space->isCompound())
673 {
674 base::CompoundStateSpace *compound = space->as<base::CompoundStateSpace>();
675 nrComponents = compound->getSubspaceCount();
676 if (nrComponents == 2)
677 {
678 int type = space->getType();
679
680 if ((type == base::STATE_SPACE_SE2) || (type == base::STATE_SPACE_SE3) ||
681 (type == base::STATE_SPACE_DUBINS))
682 {
683 nrComponents = 1;
684 }
685 else
686 {
687 const std::vector<base::StateSpacePtr> decomposed = compound->getSubspaces();
688 int t0 = decomposed.at(0)->getType();
689 int t1 = decomposed.at(1)->getType();
695 {
696 if (decomposed.at(1)->getDimension() > 0)
697 {
698 nrComponents = 1;
699 }
700 }
701 }
702 }
703 }
704 else
705 {
706 nrComponents = 1;
707 }
708 return nrComponents;
709}
The exception type for ompl.
Definition Exception.h:47
A space to allow the composition of state spaces.
Definition StateSpace.h:574
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition StateSpace.h:590
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::base::StateSpace.
bool isMapping_SE2_to_R2(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_SO3RN_to_SO3RM(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_SE3RN_to_SE3RM(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_SE3_to_R3(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_SO2RN_to_SO2RM(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_EmptyProjection(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if the mapping is an empty projection.
bool isMapping_SE2RN_to_SE2(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
int GetNumberOfComponents(const base::StateSpacePtr &space)
Estimate number of components on state space.
bool isMapping_SE2RN_to_SE2RM(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_SO2RN_to_SO2(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
ProjectionType identifyProjectionType(const base::StateSpacePtr &BundleSpace, const base::StateSpacePtr &BaseSpace)
Guess the projection type from the list of projections in ompl::multilevel::ProjectionTypes.
bool isMapping_RNSO2_to_RN(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_XRN_to_X(const base::StateSpacePtr &, const base::StateSpacePtr &, const base::StateSpaceType)
Check if mapping is to whereby .
bool isMapping_Identity(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if the mapping is an identity mapping.
bool isMapping_SO3RN_to_SO3(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_RN_to_RM(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_XRN_to_XRM(const base::StateSpacePtr &, const base::StateSpacePtr &, const base::StateSpaceType)
Check if mapping is to whereby .
bool isMapping_SE3RN_to_R3(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
ProjectionPtr makeProjection(const base::SpaceInformationPtr &Bundle, const base::SpaceInformationPtr &Base)
Guess projection(s) between two SpaceInformationPtr Bundle and Base.
bool isMapping_SE3RN_to_SE3(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_SO2N_to_SO2M(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
bool isMapping_SE2RN_to_R2(const base::StateSpacePtr &, const base::StateSpacePtr &)
Check if mapping is to .
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition Console.h:70
This namespace contains sampling based planning routines shared by both planning under geometric cons...
StateSpaceType
The type of a state space.
@ STATE_SPACE_SE2
ompl::base::SE2StateSpace
@ STATE_SPACE_DUBINS
ompl::base::DubinsStateSpace
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
@ STATE_SPACE_SE3
ompl::base::SE3StateSpace
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_RNSO2_RN
RNSO2 \rightarrow RN, n > 0.
@ PROJECTION_SO3RN_SO3
SO3RN \rightarrow SO3.
@ PROJECTION_SO2N_SO2M
SO2N \rightarrow SO2M (N copies of SO2 onto M copies of SO2)
@ PROJECTION_SE2_R2
SE2 \rightarrow R2.
@ PROJECTION_EMPTY_SET
ompl::multilevel::Projection_EmptySet
@ PROJECTION_SE3RN_SE3RM
SE3RN \rightarrow SE3RM, m < n.
@ PROJECTION_SE2RN_SE2RM
SE2RN \rightarrow SE2RM, m < n.
@ PROJECTION_SO2RN_SO2
SO2RN \rightarrow SO2.
@ PROJECTION_SO3RN_SO3RM
SO3RN \rightarrow SO3RM, m < n.
@ PROJECTION_IDENTITY
ompl::multilevel::Projection_Identity
@ PROJECTION_SE2RN_SE2
SE2RN \rightarrow SE2.
@ PROJECTION_SE2RN_R2
SE2RN \rightarrow R2.
@ PROJECTION_NONE
ompl::multilevel::Projection_None
@ PROJECTION_SE3_R3
SE3 \rightarrow R3.
@ PROJECTION_CONSTRAINED_RELAXATION
ompl::multilevel::Projection_Relaxation
@ PROJECTION_SO2RN_SO2RM
SO2RN \rightarrow SO2RM, m < n.
@ PROJECTION_SE3RN_SE3
SE3RN \rightarrow SE3.
@ PROJECTION_UNKNOWN
Unknown projection.
@ PROJECTION_RN_RM
RN \rightarrow RM, m < n.
@ PROJECTION_SE3RN_R3
SE3RN \rightarrow R3.