Are quaternions automatically normalized?

[raisimtech]

Are quaternions automatically normalized?

[jhwangbo]

I would perturb the system using a small rotation such that you don't have to normalize. Anyway, the answer is yes. All quaternions are automatically normalized. Otherwise, we will get very weird visual artifacts.

Yes, you have to iterate over joints. That is the only way. You don't have to do that every loop. You can store indices of floating and ball joints.

[RobertoRoos]

Follow-up on this, I get the idea that the normalization is not done as I would expect. I am running a finite-difference approximation as part of a unit-test and the results are different based on whether I 'manually' normalize my quaternions. See the following excerpt:

        VectorXd q_perturbed = q;
        q_perturbed[i] += eps;

        // VectorXd quat_i = q_perturbed.segment(3, 4).normalized();
        // q_perturbed.segment(3, 4) = quat_i;

        model->SetCurrent(q_perturbed);

The computed end-effector positions are different if I enable my normalization. If the quaternions are normalized by RaiSim I would expect it to make no difference.

I apologize for the abstract example. I will work on an example to reproduce this.

And I realise that perturbing a quaternion by each element instead of a 3D angle makes little sense, but it has to be this way because the optimization problem I am running.
Thank you!

[RobertoRoos]

A test program:

#include <iostream>
#include <raisim/World.hpp>
#include <Eigen/Dense>

using namespace Eigen;

int main() {

    const double dt = 0.001;
    raisim::World world1;
    raisim::World world2;
    world1.setTimeStep(dt);
    world2.setTimeStep(dt);

    const char path[] = "resources/anymal/urdf/anymal.urdf";
    auto anymal1 = world1.addArticulatedSystem(path);
    auto anymal2 = world2.addArticulatedSystem(path);

    VectorXd q1(19), q2(19);
    q1 << 0, 0, 5.0, 1, 0, 0, 0, 0.03, 0.4, -0.8, -0.03, 0.4, -0.8, 0.03, -0.4, 0.8, -0.03, -0.4, 0.8;
    q2 << q1;
    VectorXd dq1(18), dq2(18);
    dq1.setRandom();
    dq2 << dq1;

    VectorXd quat = VectorXd::Random(4).normalized(); // Random but normalized quaternion
    quat[0] += 0.1; // Add perturbation
    VectorXd quat_normalized = quat.normalized(); // Normalized perturbed quaternion

    q1.segment(3, 4) = quat_normalized;
    q2.segment(3, 4) = quat;

    anymal1->setGeneralizedCoordinate(q1);
    anymal2->setGeneralizedCoordinate(q2);

    anymal1->setGeneralizedVelocity(dq1);
    anymal2->setGeneralizedVelocity(dq2);

    std::cout << "Pos 1: " << anymal1->getGeneralizedCoordinate() << std::endl;
    std::cout << "Pos 2: " << anymal2->getGeneralizedCoordinate() << std::endl;

    // Integrate both worlds a bunch of time to make difference bigger
    for (int i = 0; i < 500; i++) {

        world1.integrate();
        world2.integrate();
    }

    VectorXd q1_final = anymal1->getGeneralizedCoordinate().e();
    VectorXd q2_final = anymal2->getGeneralizedCoordinate().e();

    std::cout << "Pos 1: " << q1_final.transpose() << std::endl;
    std::cout << "Pos 2: " << q2_final.transpose() << std::endl;

    std::cout << "Difference: " << (q2_final - q1_final).norm() << std::endl;

    return 0;
}

But this actually shows very little difference between the models:

Pos 1: 0.339438 -0.109098 4.05936 -0.361593 -0.103475 -0.805635 -0.457707 0.0276391 0.42548 -0.854667 -0.21751
0.14748 -0.426108 0.065231 -0.264471 1.14129 0.273159 -0.297943 0.82097
Pos 2: 0.339439 -0.109097 4.05936 -0.361617 -0.103408 -0.805638 -0.457698 0.0276264 0.425251 -0.854524 -0.217635
0.147274 -0.425802 0.0653461 -0.264373 1.14106 0.273116 -0.297834 0.821033
Difference: 0.000568711

And this perturbation is relatively large. I guess I must be doing something else incorrectly in my program.

[jhwangbo]

The quaternions are normalized after integration. If you give it a wrong quaternion from the beginning, the simulation will be wrong.

[RobertoRoos]

Ah, then it makes sense. So starting an integration with non-normalized quaternions is fine, but the results can be unexpected. And having a singular quaternion will crash the program, which makes sense.