39#include <ompl/multilevel/datastructures/projections/SE2_R2.h>
40#include <ompl/base/spaces/SE2StateSpace.h>
41#include <ompl/base/spaces/SO2StateSpace.h>
42#include <ompl/base/spaces/RealVectorStateSpace.h>
46Projection_SE2_R2::Projection_SE2_R2(ompl::base::StateSpacePtr
BundleSpace, ompl::base::StateSpacePtr BaseSpace)
56 xFiber_SO2->value = xBundle_SE2->
getYaw();
59void Projection_SE2_R2::project(
const ompl::base::State *xBundle, ompl::base::State *xBase)
const
63 xBase_R2->values[0] = xBundle_SE2->
getX();
64 xBase_R2->values[1] = xBundle_SE2->
getY();
67void Projection_SE2_R2::lift(
const ompl::base::State *xBase,
const ompl::base::State *xFiber,
68 ompl::base::State *xBundle)
const
74 xBundle_SE2->setXY(xBase_R2->values[0], xBase_R2->values[1]);
75 xBundle_SE2->setYaw(xFiber_SO2->value);
78ompl::base::StateSpacePtr Projection_SE2_R2::computeFiberSpace()
80 unsigned int N = getDimension();
81 unsigned int Y = getBaseDimension();
84 OMPL_ERROR(
"Assumed input is SE(2) -> R2, but got %d -> %d dimensions.", N, Y);
85 throw "Invalid Dimensionality";
87 return std::make_shared<base::SO2StateSpace>();
double getYaw() const
Get the yaw component of the state. This is the rotation in plane, with respect to the Z axis.
double getY() const
Get the Y component of the state.
double getX() const
Get the X component of the state.
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.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_SE2_R2
SE2 \rightarrow R2.