Fix bug in DynamicsLinearization due to use of ; in place of +

robotology · Mar 8, 2024 · 3fb5e06 · 3fb5e06
1 parent 759fd71
commit 3fb5e06
Showing 1 changed file with 1 addition and 1 deletion.
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;