search

robotology-legacy / mex-wholebodymodel

Matlab MEX interface to the iWholeBodyModel interface.
11 stars 9 forks source link

[Simulation] Don't Repeat Yourself : Avoid copy pasting code, use functions #42

Closed traversaro closed 8 years ago

traversaro commented 8 years ago

Some logic that is currently copy-pasted across a lot of functions/scripts, and need to be consolidated in functions:

Constrained Dynamics with Torque as a input

This function is already available in the forwardDynamics function in https://github.com/robotology/mex-wholebodymodel/blob/master/matlab-src/forwardDynamics.m , but for some reason is not used in the balancing/controller simulation. Instead a forwardDynamics function with the same signature but with different semantics ( O_o ) is implemented in https://github.com/robotology/mex-wholebodymodel/blob/master/matlab-src/mex-wholebodymodel_balancing/forwardDynamics.m . We definitively need to consolidate this.

Quaternion to Rotation conversion

Duplicated in at least 6 files : https://github.com/robotology/mex-wholebodymodel/search?utf8=%E2%9C%93&q=2*qt_b_mod_s*skew%28qt_b_mod_r%29&type=Code

Simulation to mex-wholebodymodel (and reverse) conversion

The position of the robot is a (7+n_J) vector for the simulation code ( containing the quaternion of the base orientation) and a triple (RotationMatrix, BaseOriginPosition, JointPos ) (where RotationMatrix is a 3x3 matrix, BaseOriginPosition a 3 vector and JointPos a n_J vector). We hardcode how this two representation are mapped, we need to write two functions to encapsulate this mapping.

Duplicated in at least 8 files : https://github.com/robotology/mex-wholebodymodel/search?utf8=%E2%9C%93&q=x_b++%3D+chi%281%3A3%2C%3A%29%3B++qt_b+%3D+chi%284%3A7%2C%3A%29%3B

traversaro commented 8 years ago

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.

DanielePucci commented 8 years ago

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

traversaro commented 8 years ago

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

traversaro commented 8 years ago

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

DanielePucci commented 8 years ago

Ack.

naveenoid commented 8 years ago

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.

traversaro commented 8 years ago

Given that it was the source of the error in https://github.com/robotology/mex-wholebodymodel/issues/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".

traversaro commented 8 years ago

@gabrielenava Can you fix the last point in https://github.com/robotology/mex-wholebodymodel/issues/42#issue-112623327 ?