Merge pull request #1161 from robotology/fixdynlin

Fix bug in DynamicsLinearization due to use of ; in place of +
robotology · Mar 11, 2024 · e35f305 · e35f305
2 parents 7848e8a + b691625
commit e35f305
Show file tree

Hide file tree

Showing 3 changed files with 1 addition and 4 deletions.
diff --git a/src/model/src/DynamicsLinearization.cpp b/src/model/src/DynamicsLinearization.cpp
@@ -305,7 +305,7 @@ void ForwardDynamicsLinearizationWrtJointPos(const Model& model,
                double invD = 1/(bufs.aba.D(dofIndex));
                double d_invD = - invD * bufs.dPos[dofDeriv].D(dofIndex) * invD;
                double dPos_ddq =
-                    (bufs.aba.u(dofIndex)-bufs.aba.U(dofIndex).dot(bufs.aba.linksAccelerations(visitedLinkIndex)))*d_invD;
+                    (bufs.aba.u(dofIndex)-bufs.aba.U(dofIndex).dot(bufs.aba.linksAccelerations(visitedLinkIndex)))*d_invD+
                     (bufs.dPos[dofDeriv].u(dofIndex)
                         -bufs.dPos[dofDeriv].U(dofIndex).dot(bufs.aba.linksAccelerations(visitedLinkIndex))
                         -bufs.aba.U(dofIndex).dot(bufs.dPos[dofDeriv].linksAccelerations(visitedLinkIndex)))*invD;

diff --git a/src/tests/integration/CMakeLists.txt b/src/tests/integration/CMakeLists.txt
@@ -43,7 +43,6 @@ add_integration_test(Dynamics)
 add_integration_test(DenavitHartenberg)
 add_integration_test(ReducedModelWithFT)
 
-
 # See issue https://github.com/robotology/idyntree/issues/367
 add_integration_test_no_valgrind(iCubTorqueEstimation)
 

diff --git a/src/tests/integration/DynamicsLinearizationIntegrationTest.cpp b/src/tests/integration/DynamicsLinearizationIntegrationTest.cpp
@@ -505,8 +505,6 @@ int main()
     std::cerr << "Checking DynamicsLinearization test on a point mass model" << std::endl;
     checkABAandABALinearizationAreConsistent(doubleBodyModel);
 
-    return 0;
-
     // Then test random generated chains
     for(unsigned int joints =0; joints < 20; joints++)
     {