Почему кватернионы OMPL почти 0? ⇐ C++

[Anonymous]

Я работаю с OMPL, пробуя версию этой демонстрации, но изменяя ее на 3 измерения и использую ее для ROS2 (эта последняя часть о ROS2 не имеет значения). Конечно, мне нужен кватернион, чтобы сделать пропагатор, но когда я смотрю на значения после использования функций: < /p>
state->as()->x
state->as()->y
state->as()->z
state->as()->w
< /code>
То, что я получаю, является чрезвычайно низкими значениями. Например: [W: 6.81217e-310, x: 6.81217e-310, y: 0, z: 1.82804e-322].
Из-за этого, как только я преобразую эти значения в Euler, например, использовать Их для расчета позиции я всегда получаю [0, 0, 0], что делает мой автомобиль неспособным вращаться, как вы можете видеть на следующей картинке.

Я что -то упускаю? Почему эти кватернионы не нормализованы до 1? >
Сначала это следующее: < /p>
(*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;
< /code>
Другой путь прямо встает случайным: < /p>
(*start.get()).random();

Оба способа, кажется, правильно настроили состояния начала и цели, как видно с помощью функции: Planner-> getProblemDefinition ()-> print (std :: cout); Что дает мне следующий текст: < /p>
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]
]

Я затем позвоню в планировщик-> ob :: planner :: solve (10.0) , чтобы решить проблему. Следуя RRT.CPP в строке 97, которая является планировщиком, который я использую. Он сразу же выполняет проверку достоверности, которая должна вызвать проверку достоверности, которую я запрограммировал. Тразлирующий вектор (x, y, z) кажется совершенно хорошим. Однако кватернион полностью меняется на то, что я показывал вам раньше. Это говорит мне, что проблема заключается в том, что между этими двумя точками, как будто планировщик не мог правильно прочитать эту часть начала и состояния цели (оба типа OB :: ScopedState) < /p>

Изменить: я пробовал вручную нормализовать его, используя следующую функцию: < /p>
void normalizeQuaternion(Quaternion* quat)
{
std::cout 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

Подробнее здесь: https://stackoverflow.com/questions/794 ... s-almost-0