I'm working with OMPL trying a version of this demo but changing it to 3 dimensions and using it for ROS2 (That last part about ROS2 is not relevant). Of course, I need the quaternion to make the propagator, but when I look at the values after using the functions:

state->as<ob::SO3StateSpace::StateType>()->x
state->as<ob::SO3StateSpace::StateType>()->y
state->as<ob::SO3StateSpace::StateType>()->z
state->as<ob::SO3StateSpace::StateType>()->w

What I get are extremely low values. For example: [w:6.81217e-310, x:6.81217e-310, y:0, z:1.82804e-322]. Because of this, once I convert these values to Euler Angles for example to Use them to calculate a position, I always get [0, 0, 0], which makes my vehicle unable to rotate, as you can see in tho next picture.

Am I missing something? Why aren't these quaternions normalized to 1?

Edit: When setting up the Start and Goal states, I've tried two ways.

First is the following:

(*start.get())[0] = X;
(*start.get())[1] = Y;
(*start.get())[2] = 1.0;
Quaternion quat = Euler2Quaternion(0.0, 0.0, YAW);
(*start.get())[3] = quat.w;
(*start.get())[4] = quat.x;
(*start.get())[5] = quat.y;
(*start.get())[6] = quat.z;

The other way is straight up making it random:

(*start.get()).random();

Both ways seem to correctly set up the start and goal states, as seen by using the function: planner->getProblemDefinition()->print(std::cout); which gets me the following text:

Start states:
Compound state [
RealVectorState [13.7817 7.57301 2.44067]
SO3State [-0.597399 -0.378351 0.425209 0.564944]

Goal state, threshold = 0.1, memory address = 0x7245b41b70e0, state = 
Compound state [
RealVectorState [-12.9914 16.6172 7.2046]
SO3State [0.719305 0.43711 -0.241913 -0.482715]
]

I then call planner->ob::Planner::solve(10.0) to solve the problem. Following RRT.cpp at line 97, which is the planner I'm using. It immediatly performs a validity check, which should call the validity check I've programmed. The traslation vector (X, Y, Z) seems perfectly fine. However, the quaternion changes completely to what I've shown you before. This tells me the problem is between these two points, as if it was impossible for the planner to correctly read that part of the Start and Goal states (both type ob::ScopedState)

Edit: I've tried manually normalizing it using the following function:

void normalizeQuaternion(Quaternion* quat)
{
  std::cout << "Q0: " << quat->w << ", " << quat->x << ", " << quat->y << ", " << quat->z << "]\n";
  double sqx = 0, sqy = 0, sqz = 0, sqw = 0;
  if (quat->x != 0)
    sqx = quat->x * quat->x;
  if (quat->y != 0)
    sqy = quat->y * quat->y;
  if (quat->z != 0)
    sqz = quat->z * quat->z;
  if (quat->w != 0)
    sqw = quat->w * quat->w;
  double N = sqrt(sqx + sqy + sqz + sqw);
  std::cout << "W " << sqw << " X " << sqx << " Y " << sqy << " Z " << sqz << " N " << N << std::endl;
  quat->x = quat->x / N;
  quat->y = quat->y / N;
  quat->z = quat->z / N;
  quat->w = quat->w / N;
  std::cout << "Q1: " << quat->w << ", " << quat->x << ", " << quat->y << ", " << quat->z << "]\n";
}

Doing that, I get the following results:

Q0: 6.13293e-310, 6.13293e-310, 0, 1.82804e-322]
W 0 X 0 Y 0 Z 0 N 0
Q1: inf, inf, -nan, inf]