39#include <ompl/multilevel/datastructures/projections/SE3_R3.h>
40#include <ompl/base/spaces/SE3StateSpace.h>
41#include <ompl/base/spaces/SO3StateSpace.h>
42#include <ompl/base/spaces/RealVectorStateSpace.h>
55 const auto *xBundle_SO3 = &xBundle_SE3->
rotation();
58 xFiber_SO3->
x = xBundle_SO3->x;
59 xFiber_SO3->y = xBundle_SO3->y;
60 xFiber_SO3->z = xBundle_SO3->z;
61 xFiber_SO3->w = xBundle_SO3->w;
68 xBase_R3->values[0] = xBundle_SE3->
getX();
69 xBase_R3->values[1] = xBundle_SE3->
getY();
70 xBase_R3->values[2] = xBundle_SE3->
getZ();
77 auto *xBundle_SO3 = &xBundle_SE3->
rotation();
82 xBundle_SE3->setXYZ(xBase_R3->values[0], xBase_R3->values[1], xBase_R3->values[2]);
84 xBundle_SO3->x = xFiber_SO3->x;
85 xBundle_SO3->y = xFiber_SO3->y;
86 xBundle_SO3->z = xFiber_SO3->z;
87 xBundle_SO3->w = xFiber_SO3->w;
92 unsigned int N = getDimension();
93 unsigned int Y = getBaseDimension();
96 OMPL_ERROR(
"Assumed input is SE(3) -> R3, but got %d -> %d dimensions.", N, Y);
97 throw "Invalid Dimensionality";
99 return std::make_shared<base::SO3StateSpace>();
The definition of a state in Rn
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
double getZ() const
Get the Z component of the state.
double getY() const
Get the Y component of the state.
double getX() const
Get the X component of the state.
const SO3StateSpace::StateType & rotation() const
Get the rotation component of the state.
The definition of a state in SO(3) represented as a unit quaternion.
double x
X component of quaternion vector.
A shared pointer wrapper for ompl::base::StateSpace.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_SE3_R3
SE3 \rightarrow R3.