[Simulation] Don't Repeat Yourself : Avoid copy pasting code, use functions about mex-wholebodymodel HOT 8 CLOSED

Regarding the quaternion to rotation, two formulas are used in the code:

dcm = eye(3) + 2*qt_b_mod_s*skew(qt_b_mod_r) + 2 * skew(qt_b_mod_r)^2;

in the quite used frame2pos function and

dcm = eye(3) - 2*qt_b_mod_s*skew(qt_b_mod_r) + 2 * skew(qt_b_mod_r)^2;

in the rest of the code. As far as I remember the one with the minus is the correct one, but I think it is good if we all agree on which fix is the one most correct.

from mex-wholebodymodel.

Guys, I'm really frustrated, since I remember that I personally worked on that, and I tried to fix this stuff with @gabrielenava. I don't know why these obscure formulae keep going around. I remember that the first one should be right, instead [1, p. 101, Eq. 3.8]. Anyways, please try to write these functions once for all in appropriate separate files, so we can debug them only once.

@naveenoid

[1] https://www-sop.inria.fr/act_recherche/formulaire/uploads/phd-425.pdf

from mex-wholebodymodel.

I checked and apparently the formula with a plus is the one consistent with Rodriguez Formula.

from mex-wholebodymodel.

I also checked and apparently mex-wholebodymodel_balancing code is not using the minus version anywhere.

from mex-wholebodymodel.

Ack.

from mex-wholebodymodel.

I have no idea why the incorrect version crept in. I remember fixing this a while ago also with @gabrielenava. I guess with automated tests this will stop being a problem.

from mex-wholebodymodel.

Given that it was the source of the error in #30 and an important piece of computation in general, I will now investigate how to consolidated all the different functions implementing the "Constrained Dynamics with Torque as a input".

from mex-wholebodymodel.

@gabrielenava Can you fix the last point in #42 (comment) ?

from mex-wholebodymodel.