39#include <ompl/multilevel/datastructures/projections/RN_RM.h>
40#include <ompl/base/spaces/RealVectorStateSpace.h>
56 for (
unsigned int k = getBaseDimension(); k < getDimension(); k++)
58 xFiber_RM->
values[k - getBaseDimension()] = xBundle_RN->values[k];
67 for (
unsigned int k = 0; k < getBaseDimension(); k++)
69 xBase_RM->
values[k] = xBundle_RN->values[k];
80 for (
unsigned int k = 0; k < getBaseDimension(); k++)
84 for (
unsigned int k = getBaseDimension(); k < getDimension(); k++)
86 xBundle_RN->values[k] = xFiber_RJ->
values[k - getBaseDimension()];
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);
97 std::static_pointer_cast<base::RealVectorStateSpace>(getBundle())->getBounds();
99 std::vector<double> low;
101 std::vector<double> high;
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);
The lower and upper bounds for an Rn space.
std::vector< double > high
Upper bound.
std::vector< double > low
Lower bound.
The definition of a state in Rn
double * values
The value of the actual vector in Rn
A shared pointer wrapper for ompl::base::StateSpace.
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.