39#include <ompl/multilevel/datastructures/projections/XRN_X_SE3.h>
40#include <ompl/base/spaces/SE3StateSpace.h>
41#include <ompl/base/spaces/SO3StateSpace.h>
42#include <ompl/base/spaces/RealVectorStateSpace.h>
46Projection_SE3RN_SE3::Projection_SE3RN_SE3(ompl::base::StateSpacePtr
BundleSpace, ompl::base::StateSpacePtr BaseSpace)
54 const auto *xBundle_SE3 = xBundle->
as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
55 const auto *xBundle_SO3 = &xBundle_SE3->rotation();
58 auto *xBase_SO3 = &xBase_SE3->rotation();
60 xBase_SE3->setXYZ(xBundle_SE3->getX(), xBundle_SE3->getY(), xBundle_SE3->getZ());
61 xBase_SO3->x = xBundle_SO3->x;
62 xBase_SO3->y = xBundle_SO3->y;
63 xBase_SO3->z = xBundle_SO3->z;
64 xBase_SO3->w = xBundle_SO3->w;
67void Projection_SE3RN_SE3::lift(
const ompl::base::State *xBase,
const ompl::base::State *xFiber,
68 ompl::base::State *xBundle)
const
70 auto *xBundle_SE3 = xBundle->
as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
71 auto *xBundle_SO3 = &xBundle_SE3->rotation();
72 auto *xBundle_RN = xBundle->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
75 const auto *xBase_SO3 = &xBase_SE3->
rotation();
79 xBundle_SE3->setXYZ(xBase_SE3->getX(), xBase_SE3->getY(), xBase_SE3->getZ());
80 xBundle_SO3->x = xBase_SO3->x;
81 xBundle_SO3->y = xBase_SO3->y;
82 xBundle_SO3->z = xBase_SO3->z;
83 xBundle_SO3->w = xBase_SO3->w;
87 xBundle_RN->values[k] = xFiber_RN->values[k];
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.
unsigned int getFiberDimension() const
Dimension of Fiber Space.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_SE3RN_SE3
SE3RN \rightarrow SE3.