forwardDynamicsConsistencyTest failing

[traversaro]

I wanted to consolidate the tests such that they could be executed in a systematic way, but I tried to run the forwardDynamicsConsistencyTest and I get this output:

>> forwardDynamicsConsistencyTest
Robot name icubGazeboSim and World Reference Frame set about l_sole 
Random Initial configuration
  Columns 1 through 11

    1.0179    0.3310   -0.2220   -0.5140    0.4804    1.0425    0.1518   -0.3893   -1.5775    0.2726    1.3232

  Columns 12 through 22

    1.3148   -0.3192    1.5566   -0.2707   -0.1986   -1.2349    0.5591    0.3063   -0.1948    0.6448   -0.1784

  Columns 23 through 25

   -0.5732    0.4852    0.2465

Random Initial velocity
  Columns 1 through 11

    2.2397   -1.7970   -1.5510    3.3739    3.8100    0.0164   -4.5974    1.5961   -0.8527    2.7619   -2.5127

  Columns 12 through 22

    2.4490   -0.0596   -1.9908   -3.9090   -4.5929   -0.4722    3.6138    3.5071    2.4249   -3.4072    2.4572

  Columns 23 through 25

   -3.1428    2.5648   -4.2926

starting integration
Warning: Failure at t=6.837546e-01.  Unable to meet integration tolerances without reducing the step size below the
smallest value allowed (1.776357e-15) at time t. 
> In ode15s at 730
  In forwardDynamicsConsistencyTest at 72 

[DanielePucci]

I fear that

Robot name icubGazeboSim and World Reference Frame set about l_sole 

means that the position of the floating base pose is obtained through forward kinematics on the left foot, instead of integrating what comes from the floating base dynamics. In this case, the foot is stuck on the ground, and the dynamics should incorporate also the external wrench acting on the foot for passing the tests. But, I am just speculating. @naveenoid what do you think about it?

[traversaro]

It seems that there are two different files forwardDynamicsConsistencyTest.m in the repo... https://github.com/robotology/mex-wholebodymodel/blob/master/matlab-src/forwardDynamicsConsistencyTest.m seems the most update one, while https://github.com/robotology/mex-wholebodymodel/blob/master/matlab-src/tests/forwardDynamicsConsistencyTest.m seems to be just an outdated version.. @gabrielenava @naveenoid Am I right?

[naveenoid]

I will close this issue. It will be directly addressed in the cleanup referenced in the issue above.

[traversaro]

The test is still not working. I was able to get it to work for a simple two links floating base model, but it is still failing for full iCub model.

[naveenoid]

testing in a bit. This is with 0 input torques?

On Thu, Oct 22, 2015 at 5:31 PM, Silvio Traversaro <notifications@github.com

wrote:

Reopened #30 https://github.com/robotology/mex-wholebodymodel/issues/30.

— Reply to this email directly or view it on GitHub https://github.com/robotology/mex-wholebodymodel/issues/30#event-442783741 .

[traversaro]

Yes, cleanup test in https://github.com/robotology/mex-wholebodymodel/tree/fixKineticEnergyConservationTest .

To run together with the rest of the test, run ctest -VV. To run standalone (for debug) run the test/kinEnergyConservationTestAllModels script.

[traversaro]

I am experiencing strange matlab crashes, so I guess there is something going on the C++ part.

[traversaro]

7 hours later I solved the crash above mentioned. : D (see https://github.com/robotology/mex-wholebodymodel/issues/46), so I can concentrate on the real issue.

Apparently the test is still not passing:

The base translational acceleration in particular is quite fishy, because it should be 0 , but it is not zero, I will investigate in this direction. In particular a quite nice test (in a sense standalone and indipendent from this) is to check the M\g is equal to a n_J+6 vector, in which the first 3 elements are the elements of opposite of the gravity acceleration.

[traversaro]

I finally found the bug, it was due to a rotation matrix not inversed, you can find more info in the relative commit: https://github.com/robotology/mex-wholebodymodel/commit/0223cbc1d9b783ef1d5d6a4d1811717cc4583952 . To avoid problems like this, I also changed the notation in https://github.com/robotology/mex-wholebodymodel/commit/7a0c36257ec457f998f8bca2c3eddab989b03bb1 .

Again, this bug was fixed in other version of the forwardDynamics function copy and pasted around the repo, so this bug was actually caused by https://github.com/robotology/mex-wholebodymodel/issues/42 , let's try to avoid this kind of patters in the future @gabrielenava @naveenoid .

[DanielePucci]

:+1: :+1:

[traversaro]

To be sure everything was working I run the simulation for ~150 seconds, and I got this results (icubGazeboSim is the usual model with 25 dofs, while icub.urdf is a locally loaded model with 32dofs):

There is a very small drift in the twoLinks case, but I guess it is due to the integration process, not due to some bug in the dynamics equations.

[traversaro]

Ah, there was also a quaternion to rotation conversion performed with the minus formula, so I guess that also that was contributing to the issue.

[DanielePucci]

Great. Closing?

[traversaro]

When the necessary changes will be merged in master.

[DanielePucci]

Ack.

[naveenoid]

Sorry back on track after a day with no internet. This is a result of a mixup of older versions of the forwardDynamics. I think always this bug is traced down to quaternion ordering and frame in which angular velocity is being expressed for quaternion derivative. Good job anyhow.

On Fri, Oct 23, 2015 at 4:38 PM, Daniele notifications@github.com wrote:

Ack.

— Reply to this email directly or view it on GitHub https://github.com/robotology/mex-wholebodymodel/issues/30#issuecomment-150593294 .

[naveenoid]

I think this can be safely closed now, no?

[traversaro]

Yes.