37 #ifndef OMPL_EXTENSION_OPENDE_STATE_SPACE_
38 #define OMPL_EXTENSION_OPENDE_STATE_SPACE_
40 #include "ompl/base/StateSpace.h"
41 #include "ompl/base/spaces/RealVectorStateSpace.h"
42 #include "ompl/base/spaces/SO3StateSpace.h"
43 #include "ompl/extensions/opende/OpenDEEnvironment.h"
78 return as<base::RealVectorStateSpace::StateType>(body * 4)->values;
84 return as<base::RealVectorStateSpace::StateType>(body * 4)->values;
90 return *as<base::SO3StateSpace::StateType>(body * 4 + 3);
96 return *as<base::SO3StateSpace::StateType>(body * 4 + 3);
102 return as<base::RealVectorStateSpace::StateType>(body * 4 + 1)->values;
108 return as<base::RealVectorStateSpace::StateType>(body * 4 + 1)->values;
114 return as<base::RealVectorStateSpace::StateType>(body * 4 + 2)->values;
120 return as<base::RealVectorStateSpace::StateType>(body * 4 + 2)->values;
149 double positionWeight = 1.0,
double linVelWeight = 0.5,
150 double angVelWeight = 0.5,
double orientationWeight = 1.0);
165 return env_->stateBodies_.size();