39#include <ompl/multilevel/datastructures/projections/XRN_XRM_SE2.h>
40#include <ompl/base/spaces/SE2StateSpace.h>
41#include <ompl/base/spaces/RealVectorStateSpace.h>
43#include <ompl/util/Exception.h>
47Projection_SE2RN_SE2RM::Projection_SE2RN_SE2RM(ompl::base::StateSpacePtr
BundleSpace,
48 ompl::base::StateSpacePtr BaseSpace)
56 const auto *xBundle_SE2 = xBundle->
as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
57 const auto *xBundle_RN = xBundle->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
59 auto *xBase_SE2 = xBase->
as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
60 auto *xBase_RN = xBase->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
62 xBase_SE2->setX(xBundle_SE2->getX());
63 xBase_SE2->setY(xBundle_SE2->getY());
64 xBase_SE2->setYaw(xBundle_SE2->getYaw());
66 for (
unsigned int k = 0; k < getBaseDimension() - 3; k++)
68 xBase_RN->values[k] = xBundle_RN->values[k];
72void Projection_SE2RN_SE2RM::lift(
const ompl::base::State *xBase,
const ompl::base::State *xFiber,
73 ompl::base::State *xBundle)
const
75 auto *xBundle_SE2 = xBundle->
as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
76 auto *xBundle_RN = xBundle->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
78 const auto *xBase_SE2 = xBase->
as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
79 const auto *xBase_RM = xBase->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
83 xBundle_SE2->setX(xBase_SE2->getX());
84 xBundle_SE2->setY(xBase_SE2->getY());
85 xBundle_SE2->setYaw(xBase_SE2->getYaw());
92 for (
unsigned int k = 0; k < M; k++)
94 xBundle_RN->values[k] = xBase_RM->values[k];
96 for (
unsigned int k = M; k < M + N; k++)
98 xBundle_RN->values[k] = xFiber_RJ->values[k - M];
ompl::base::State StateType
Define the type of state allocated by this space.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
unsigned int getFiberDimension() const
Dimension of Fiber Space.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_SE2RN_SE2RM
SE2RN \rightarrow SE2RM, m < n.