Using acceleration in the integration of joint position.

[aescande]

This PR basically corrects the EulerIntegration scheme. Previously, it was done like this:

x(k+1) = x(k) + v(k)*dt
v(k+1) = v(k) + a(k)*dt

or corresponding formulas for more complex joints. (x is the joint position, v its velocity and a its acceleration). This was corresponding to integrate position with constant velocity then velocity with constant acceleration, so that the acceleration a(k) at instant k was only influencing the position x(k+2). Now it is done like this:

x(k+1) = x(k) + v(k)*dt + 1/2*a(k)*dt^2
v(k+1) = v(k) + a(k)*dt

which corresponds to integrating both position and velocity for a constant acceleration.

There are still things to check/correct:

• for planar joints, I kept the old formula, because I don't quite understand (yet) what is done.
• the integration of the translation part of a free joint seems odd: integrating N time a constant velocity with increments dt/N or once with increment dt does not yield the same results. This was already the case in previous version.

[haudren]

Quick remark for the test:

warning: enumeration value ‘Fixed’ not handled in switch [-Wswitch]

[haudren]

So something is a bit fishy, the tests pass on Windows, but fail on Linux... I wonder where the difference lies.

[aescande]

Yes, and it does not seems to be small numeric differences. I'll investigate that

[haudren]

Might have something to do with this:

warning: conversion to ‘int’ from ‘double’ may alter its value [-Wconversion]
     if (abs(x) >= taylor_n_bound)

[haudren]

Ok so if you replace abs with fabs the tests still fail but with tiny errors:

Running 9 test cases...
/home/herve/dev/RBDyn/tests/AlgoTest.cpp(467): error in "EulerTest": check { q.begin(), q.end() } == { goalQ.begin(), goalQ.end() } failed. 
Mismatch in a position 1: 0.707107 != 0.707107

*** 1 failure detected in test suite "Algo"

And

Running 1 test case...
/home/herve/dev/RBDyn/tests/IntegrationTest.cpp(189): error in "JointIntegrationTest": difference{0.0010542} between mbc0.q[1][i]{0.44383554716773121} and mbc.q[1][i]{0.44336814831097465} exceeds 0.001

*** 1 failure detected in test suite "JointTest"

[gergondet]

I'm confirming Hervé remark on the test. Switching to std::abs (and std::sqrt although it has no effect) mostly fixes the tests. The error can be fixed by reducing the time step (actually, the test passes even with 1100 instead of 1000).

Edit: just saw you pushed a fix, so... cool :+1:

[gergondet]

The PR is fine imho but you totally broke the indentation as RBDyn uses tabs and not spaces (you also left a few trailing spaces which is not as big a deal).

Can you fix it easily with Visual Studio? (otherwise if you give me write access to your fork I'll fix the indentation for you)

Also, poke @jorisv concerning planar joints

[haudren]

BTW this breaks the Tasks Mimic joint test. You will need to increase the stiffness of the task to make them pass.