39#include <ompl/multilevel/datastructures/projections/XRN_XRM_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_SE3RM::Projection_SE3RN_SE3RM(ompl::base::StateSpacePtr
BundleSpace,
47 ompl::base::StateSpacePtr BaseSpace)
55 const auto *xBundle_SE3 = xBundle->
as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
56 const auto *xBundle_SO3 = &xBundle_SE3->rotation();
57 const auto *xBundle_RN = xBundle->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
59 auto *xBase_SE3 = xBase->
as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
60 auto *xBase_SO3 = &xBase_SE3->rotation();
61 auto *xBase_RM = xBase->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
63 xBase_SE3->setXYZ(xBundle_SE3->getX(), xBundle_SE3->getY(), xBundle_SE3->getZ());
64 xBase_SO3->x = xBundle_SO3->x;
65 xBase_SO3->y = xBundle_SO3->y;
66 xBase_SO3->z = xBundle_SO3->z;
67 xBase_SO3->w = xBundle_SO3->w;
69 for (
unsigned int k = 0; k < getBaseDimension() - 6; k++)
71 xBase_RM->values[k] = xBundle_RN->values[k];
75void Projection_SE3RN_SE3RM::lift(
const ompl::base::State *xBase,
const ompl::base::State *xFiber,
76 ompl::base::State *xBundle)
const
78 auto *xBundle_SE3 = xBundle->
as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
79 auto *xBundle_SO3 = &xBundle_SE3->rotation();
80 auto *xBundle_RN = xBundle->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
82 const auto *xBase_SE3 = xBase->
as<base::CompoundState>()->as<base::SE3StateSpace::StateType>(0);
83 const auto *xBase_SO3 = &xBase_SE3->rotation();
84 const auto *xBase_RM = xBase->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
88 xBundle_SE3->setXYZ(xBase_SE3->getX(), xBase_SE3->getY(), xBase_SE3->getZ());
89 xBundle_SO3->x = xBase_SO3->x;
90 xBundle_SO3->y = xBase_SO3->y;
91 xBundle_SO3->z = xBase_SO3->z;
92 xBundle_SO3->w = xBase_SO3->w;
99 for (
unsigned int k = 0; k < M; k++)
101 xBundle_RN->values[k] = xBase_RM->values[k];
103 for (
unsigned int k = M; k < M + N; k++)
105 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_SE3RN_SE3RM
SE3RN \rightarrow SE3RM, m < n.