39#include <ompl/multilevel/datastructures/projections/XRN_XRM.h>
40#include <ompl/base/spaces/RealVectorStateSpace.h>
42#include <ompl/util/Exception.h>
46Projection_XRN_XRM::Projection_XRN_XRM(ompl::base::StateSpacePtr
BundleSpace, ompl::base::StateSpacePtr BaseSpace)
52 const auto *xBundle_RN = xBundle->
as<base::CompoundState>()->as<base::RealVectorStateSpace::StateType>(1);
56 unsigned int Noffset = getBaseDimension() - dimensionBaseFirstSubspace;
59 xFiber_RJ->values[k] = xBundle_RN->values[k + Noffset];
63ompl::base::StateSpacePtr Projection_XRN_XRM::computeFiberSpace()
65 base::CompoundStateSpace *Bundle_compound = getBundle()->
as<base::CompoundStateSpace>();
66 const std::vector<base::StateSpacePtr> Bundle_decomposed = Bundle_compound->
getSubspaces();
68 base::CompoundStateSpace *Base_compound = getBase()->as<base::CompoundStateSpace>();
69 const std::vector<base::StateSpacePtr> Base_decomposed = Base_compound->
getSubspaces();
71 dimensionBaseFirstSubspace = Base_decomposed.at(0)->getDimension();
73 unsigned int Nbundle = Bundle_decomposed.at(1)->getDimension();
74 unsigned int Nbase = Base_decomposed.at(1)->getDimension();
75 unsigned int Nfiber = Nbundle - Nbase;
77 base::StateSpacePtr Fiber_RN(
new base::RealVectorStateSpace(Nfiber));
79 const auto *Bundle_RN = Bundle_decomposed.at(1)->as<base::RealVectorStateSpace>();
80 base::RealVectorBounds Bundle_bounds = Bundle_RN->getBounds();
81 std::vector<double> low;
83 std::vector<double> high;
85 base::RealVectorBounds Fiber_bounds(Nfiber);
86 for (
unsigned int k = 0; k < Nfiber; k++)
88 Fiber_bounds.setLow(k, Bundle_bounds.low.at(k + Nbase));
89 Fiber_bounds.setHigh(k, Bundle_bounds.high.at(k + Nbase));
91 std::static_pointer_cast<base::RealVectorStateSpace>(Fiber_RN)->setBounds(Fiber_bounds);
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
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,...