39#include <ompl/multilevel/datastructures/projections/RN_RM.h>
40#include <ompl/base/spaces/RealVectorStateSpace.h>
44Projection_RN_RM::Projection_RN_RM(ompl::base::StateSpacePtr
BundleSpace, ompl::base::StateSpacePtr BaseSpace)
56 for (
unsigned int k = getBaseDimension(); k < getDimension(); k++)
58 xFiber_RM->
values[k - getBaseDimension()] = xBundle_RN->values[k];
62void Projection_RN_RM::project(
const ompl::base::State *xBundle, ompl::base::State *xBase)
const
67 for (
unsigned int k = 0; k < getBaseDimension(); k++)
69 xBase_RM->
values[k] = xBundle_RN->values[k];
73void Projection_RN_RM::lift(
const ompl::base::State *xBase,
const ompl::base::State *xFiber,
74 ompl::base::State *xBundle)
const
80 for (
unsigned int k = 0; k < getBaseDimension(); k++)
82 xBundle_RN->values[k] = xBase_RM->
values[k];
84 for (
unsigned int k = getBaseDimension(); k < getDimension(); k++)
86 xBundle_RN->values[k] = xFiber_RJ->
values[k - getBaseDimension()];
90ompl::base::StateSpacePtr Projection_RN_RM::computeFiberSpace()
92 unsigned int N1 = getDimension();
93 unsigned int N0 = getBaseDimension();
94 unsigned int NX = N1 - N0;
95 base::StateSpacePtr FiberSpace = std::make_shared<base::RealVectorStateSpace>(NX);
96 base::RealVectorBounds Bundle_bounds =
97 std::static_pointer_cast<base::RealVectorStateSpace>(getBundle())->getBounds();
99 std::vector<double> low;
101 std::vector<double> high;
103 base::RealVectorBounds Fiber_bounds(NX);
104 for (
unsigned int k = 0; k < NX; k++)
106 Fiber_bounds.setLow(k, Bundle_bounds.
low.at(k + N0));
107 Fiber_bounds.setHigh(k, Bundle_bounds.
high.at(k + N0));
109 std::static_pointer_cast<base::RealVectorStateSpace>(FiberSpace)->setBounds(Fiber_bounds);
std::vector< double > high
Upper bound.
std::vector< double > low
Lower bound.
double * values
The value of the actual vector in Rn
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.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_RN_RM
RN \rightarrow RM, m < n.