39#include <ompl/multilevel/datastructures/projections/SE3_R3.h>
40#include <ompl/base/spaces/SE3StateSpace.h>
41#include <ompl/base/spaces/SO3StateSpace.h>
42#include <ompl/base/spaces/RealVectorStateSpace.h>
46Projection_SE3_R3::Projection_SE3_R3(ompl::base::StateSpacePtr
BundleSpace, ompl::base::StateSpacePtr BaseSpace)
55 const auto *xBundle_SO3 = &xBundle_SE3->rotation();
58 xFiber_SO3->x = xBundle_SO3->x;
59 xFiber_SO3->y = xBundle_SO3->y;
60 xFiber_SO3->z = xBundle_SO3->z;
61 xFiber_SO3->w = xBundle_SO3->w;
64void Projection_SE3_R3::project(
const ompl::base::State *xBundle, ompl::base::State *xBase)
const
68 xBase_R3->values[0] = xBundle_SE3->
getX();
69 xBase_R3->values[1] = xBundle_SE3->
getY();
70 xBase_R3->values[2] = xBundle_SE3->
getZ();
73void Projection_SE3_R3::lift(
const ompl::base::State *xBase,
const ompl::base::State *xFiber,
74 ompl::base::State *xBundle)
const
77 auto *xBundle_SO3 = &xBundle_SE3->
rotation();
82 xBundle_SE3->setXYZ(xBase_R3->values[0], xBase_R3->values[1], xBase_R3->values[2]);
84 xBundle_SO3->x = xFiber_SO3->x;
85 xBundle_SO3->y = xFiber_SO3->y;
86 xBundle_SO3->z = xFiber_SO3->z;
87 xBundle_SO3->w = xFiber_SO3->w;
90ompl::base::StateSpacePtr Projection_SE3_R3::computeFiberSpace()
92 unsigned int N = getDimension();
93 unsigned int Y = getBaseDimension();
96 OMPL_ERROR(
"Assumed input is SE(3) -> R3, but got %d -> %d dimensions.", N, Y);
97 throw "Invalid Dimensionality";
99 return std::make_shared<base::SO3StateSpace>();
double getZ() const
Get the Z component of the state.
double getY() const
Get the Y component of the state.
double getX() const
Get the X component of the state.
const SO3StateSpace::StateType & rotation() const
Get the rotation 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_SE3_R3
SE3 \rightarrow R3.