[mex-wholebodymodel] Checks on the kinetic energy associated with the floating base system

[DanielePucci]

The floating-base, mechanical system

M(q) dot(v) + C(q,v) v + g(q) = S tau

must comply with:

i) The mass matrix M is symmetric, positive definite;

ii) dot(M) - 2C must be skew-symmetric.

The point i) has been debugged sufficiently in my opinion. The main problem for debugging ii) is that you would in general require the time derivative of the mass matrix, which is not trivial in general.

This time differentiation, however, may be avoided. In particular, ii) implies that the kinetic energy of the system is constant when

a) tau = 0

b) g(q) = 0

Then, I would launch simulations with random initial conditions (q,v)(0) and plot

Ek = v^t M(q) v

If Ek is not constant under a) and b), then there is a problem in the model itself.

[naveenoid]

Test included in 54c22da3b6f53a148ce33c1b9c8148526044690c still no confirmed constant kinetic energy. Testing integration result now.

[naveenoid]

CONFIRMED...It is consistent. Final bug was a unnecesary lag of one timestep in angular velocity re-orientation (to body frame). Fixed in f2e106044839298e7ba28aeea6f6df8548079685 Verify using forwardDynamicsConsistencyTest.m Very small fluctuation in Kinetic energy in order of 1e-3 J @iron76 @traversaro @francesco-romano

[DanielePucci]

Very well done. Thanks!

[traversaro]

[naveenoid]

Along the way, we also now have adjustable world-frame location, adjustable gravity, and of course, verified equations of forward dynamics. I will now proceed to check each of the models. Good luck with the demo, peeps!