OMPL中四元数数值趋近于0的原因排查求助

我正在修改OMPL的RigidBodyPlanningWithControls示例为3D版本（无关ROS2内容），需要基于四元数构建状态传播器，但通过以下代码读取四元数值时：

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

得到的数值极小，例如：[w:6.81217e-310, x:6.81217e-310, y:0, z:1.82804e-322]。将其转换为欧拉角后全为[0,0,0]，导致车辆无法旋转。

为何这些四元数未归一化为1？我是否遗漏了关键步骤？

补充信息：状态设置与规划流程 #

设置起始和目标状态时，我尝试了两种方式：
第一种手动赋值：

(*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;

第二种随机生成：

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

通过planner->getProblemDefinition()->print(std::cout);输出可见，起始和目标状态均设置正确：

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]
]

随后调用planner->ob::Planner::solve(10.0)求解问题，使用的是OMPL的RRT规划器（对应源码第97行）。程序立即执行自定义的有效性检查，此时平移向量(X,Y,Z)正常，但四元数变为之前所示的极小值。这说明问题出在状态设置与有效性检查之间，规划器似乎无法正确读取ob::ScopedState类型起始/目标状态的四元数部分。

补充：手动归一化尝试的结果 #

我尝试用以下函数手动归一化四元数：

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";
}

得到如下输出：

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]

内容的提问来源于stack exchange，提问作者Daniel Bajo Collados