[DynamicsComputations] [DynTree] Bug in converting input gravity acceleration to proper base acceleration

traversaro commented 8 years ago

TL;DR:

There was a serious bug in some interfaces for setting gravity in iDynTree. If you are among the users affected (see below for more info), please updated as soon as possible, sorry for the trouble caused. FYI @robotology/codyco-developers @robotology/all-walkman-developers

Background

As you may know, recursive dynamics algorithms usually compute the effect of gravity using an additional fictious acceleration at the base, equal to the opposite of the gravitational acceleration. For more details, check your favorite computational Multi Body Dynamics book. For example, this is how this concept is explained at page 94 of Rigid Body Dynamics Algorithms by Roy Featherstone, Ed. 2008 : featherstone94

Consequently, all the link acceleration computed during the forward pass of the RNEA (for example) are not the actual link acceleration, but are instead the link acceleration minus the gravity acceleration, i.e. the so-called proper acceleration. [1] Note that the linear part of proper acceleration is also the acceleration that is measured by an accelerometer sensor (strictly speaking that one is the classical proper acceleration, while most Featherstone RNEA compute the spatial proper acceleration, but this is not relevant in this discussion).

Proper and actual acceleration in iDynTree interfaces

This difference needs to be properly accounted when exposing dynamics algorithms. When iDynTree was initially implemented, the interface of the DynTree class was modeled after the already existing Dynamics libraries in the iCub ecosystem, that where designed to get the base (proper) acceleration from the measurement provided by an IMU. For this reason originally the only interface available for setting the base acceleration of a model was SetInertialMeasure that was setting directly the proper acceleration.

However when we started using the library we discovered that some users, for example those coming from a control background, were not familiar with the proper acceleration concept.

For this reason we modified the wholeBodyModel interface (that was itself implemented using iDynTree) to optionally specify the gravity acceleration, separately from the actual base acceleration. At that time we did not modified the DynTree class interface, so the conversion from base actual acceleration and gravity acceleration was performed outside from the interface, directly in the wholeBodyModel class [2].

Noticing how this was convenient, we added similar interfaces to the DynTree class and the new DynamicsComputations class, a class similar to DynTree in spirit but YARP indipendent and that fully supported SWIG bindings in Matlab, Python, etc etc.

The Bug

Apparently the conversion from gravity acceleration to proper acceleration was wrong in three different points of the codebase:

the DynTree::setGravity method, in which the base proper acceleration was directly set to be equal to gravity acceleration, without properly changing the sign.
the DynamicsComputations::setRobotState method, in which the base proper acceleration was set to the sum of the actual and gravitational acceleration, instead of the difference.
the consistency test for this interfaces in which once again the proper acceleration was obtained by summing actual and gravitational acceleration, instead of subtracting them.
Users not affected
All the users of the wholeBodyInterface and all related projects (mex-wholebodymodel, WBI-Toolbox) are not affected, because the sign of the gravity is correctly handled by the yarpWholeBodyModel class.
Users in the Walkman project always used the iDynTree using the DynTree class, but using the old SetInertialMeasure, so they are not affected.
User of the DynamicsRegressorGenerator are not affected (the conversion was implemented correctly in that class: https://github.com/robotology/idyntree/blob/master/regressors/src/DynamicsRegressorGenerator.cpp#L437 ).
Users affected
Users of the new DynamicsComputions class and of its bindings are affected, among them:
- @andreadelprete & @gtournois , that just started to use it so they can just update to the latest version to solve any issue related to setRobotState()
- @francesco-romano & @claudia-lat that actually reported the issue in the first place, kudos to them. :)
- @prashanthr05 & @naveenoid : your case is the one I am more concerned of, can you check if this bug affected your computations?
Users of the DynTree::setGravity method:
- TeleRobot people, that I will contact by mail.
  References

[1] : https://en.wikipedia.org/wiki/Proper_acceleration [2] : https://github.com/robotology/yarp-wholebodyinterface/blob/3f367a/src/yarpWholeBodyModel.cpp#L597

traversaro commented 8 years ago

Test fixed in https://github.com/robotology/idyntree/commit/4ea5cbc22a95964e3d3531c3edf27b43fbc50099 . DynTree and DynamicsComputations class fixed in https://github.com/robotology/idyntree/commit/cfe17a7421141e9e574c4ba7aa7daad884cb7b2d .

Tagged versions v0.3.7 immediately before the fix (if someone is interested in the old behavior for back-compatibility) and v0.3.8 immediatly after.

DanielePucci commented 8 years ago

+1

iron76 commented 8 years ago

I think @traversaro did not sleep for a few days after spotting this bug. Great job anyway in spotting this out. @claudia-lat was also very close at spotting it out since we were experiencing bugs in comparing the outcomes of iDyntree with the results of ID in the Featherstone matlab toolbox.

naveenoid commented 8 years ago

:+1 Excellent job guys.

On Mon, Oct 19, 2015 at 2:00 AM, Francesco Nori notifications@github.com wrote:

I think @traversaro https://github.com/traversaro did not sleep for a few days after spotting this bug. Great job anyway in spotting this out. @claudia-lat https://github.com/claudia-lat was also very close at spotting it out since we were experiencing bugs in comparing the outcomes of iDyntree with the results of ID in the Featherstone matlab toolbox.

— Reply to this email directly or view it on GitHub https://github.com/robotology/idyntree/issues/81#issuecomment-149065577.

traversaro commented 8 years ago

@claudia-lat was indeed the one noticing the bug.

robotology / idyntree