39#include <ompl/multilevel/datastructures/projections/SE2RN_R2.h>
40#include <ompl/base/spaces/SE2StateSpace.h>
41#include <ompl/base/spaces/SO2StateSpace.h>
42#include <ompl/base/spaces/RealVectorStateSpace.h>
44#include <ompl/util/Exception.h>
48Projection_SE2RN_R2::Projection_SE2RN_R2(ompl::base::StateSpacePtr
BundleSpace, 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 *xFiber_SO2 = xFiber->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
60 auto *xFiber_RN = xFiber->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
62 xFiber_SO2->value = xBundle_SE2->getYaw();
65 xFiber_RN->values[k] = xBundle_RN->values[k];
69void Projection_SE2RN_R2::project(
const ompl::base::State *xBundle, ompl::base::State *xBase)
const
71 const auto *xBundle_SE2 = xBundle->
as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
73 xBase_R2->values[0] = xBundle_SE2->getX();
74 xBase_R2->values[1] = xBundle_SE2->getY();
77void Projection_SE2RN_R2::lift(
const ompl::base::State *xBase,
const ompl::base::State *xFiber,
78 ompl::base::State *xBundle)
const
80 auto *xBundle_SE2 = xBundle->
as<base::CompoundState>()->as<base::SE2StateSpace::StateType>(0);
81 auto *xBundle_RN = xBundle->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
85 const auto *xFiber_SO2 = xFiber->
as<base::CompoundState>()->as<base::SO2StateSpace::StateType>(0);
86 const auto *xFiber_RN = xFiber->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
88 xBundle_SE2->setX(xBase_R2->values[0]);
89 xBundle_SE2->setY(xBase_R2->values[1]);
90 xBundle_SE2->setYaw(xFiber_SO2->value);
94 xBundle_RN->values[k] = xFiber_RN->values[k];
98ompl::base::StateSpacePtr Projection_SE2RN_R2::computeFiberSpace()
100 unsigned int N = getDimension();
101 unsigned int Y = getBaseDimension();
104 OMPL_ERROR(
"Assumed input is SE(2)xRN -> R2, but got %d -> %d dimensions.", N, Y);
105 throw Exception(
"Invalid Dimensionality");
108 base::CompoundStateSpace *Bundle_compound = getBundle()->as<base::CompoundStateSpace>();
109 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->
getSubspaces();
110 const std::vector<base::StateSpacePtr> Bundle_SE2_decomposed =
111 Bundle_decomposed.at(0)->as<base::CompoundStateSpace>()->getSubspaces();
113 const auto *Bundle_RN = Bundle_decomposed.at(1)->as<base::RealVectorStateSpace>();
114 unsigned int N_RN = Bundle_RN->getDimension();
116 base::StateSpacePtr SO2(
new base::SO2StateSpace());
117 base::StateSpacePtr RN(
new base::RealVectorStateSpace(N_RN));
118 RN->as<base::RealVectorStateSpace>()->setBounds(Bundle_RN->getBounds());
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
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.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_SE2RN_R2
SE2RN \rightarrow R2.