37 #include "ompl/extensions/morse/MorseProjection.h"
38 #include "ompl/base/spaces/RealVectorStateSpace.h"
39 #include "ompl/util/Exception.h"
45 throw Exception(
"MORSE State Space needed for Morse Projection");
56 return 2*space_->getEnvironment()->rigidBodies_;
61 cellSizes_.resize(getDimension());
62 for (
unsigned int i = 0; i < getDimension(); i++)
72 projection.resize(getDimension());
73 for (
unsigned int i = 0; i < space_->getEnvironment()->rigidBodies_; i++)
77 projection[2*i] = values[0];
78 projection[2*i+1] = values[1];
MORSE State. This is a compound state that allows accessing the properties of the bodies the state sp...
MorseProjection(const StateSpacePtr &space)
Construct a projection evaluator for a specific state space.
State space representing MORSE states.
A boost shared pointer wrapper for ompl::base::StateSpace.
virtual void setup(void)
Perform configuration steps, if needed.
void setup(void)
Perform configuration steps, if needed.
const T * as(void) const
Cast this instance to a desired type.
MorseStateSpace * space_
The state space this projection operates on.
virtual void project(const State *state, EuclideanProjection &projection) const
Compute the projection as an array of double values.
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
virtual void defaultCellSizes(void)
Set the default cell dimensions for this projection. The default implementation of this function sets...
The exception type for ompl.
The definition of a state in Rn
virtual unsigned int getDimension(void) const
Return the dimension of the projection defined by this evaluator.
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...