diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f56b5c57d8d5885d9ac4831c73b2dd6a34b9ab7f..5d0bc4c340a3c304aa9e83c4f9f78d007085dbab 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -3,10 +3,6 @@
# Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
#
-MACRO(ADD_TARGET_CFLAGS target flag)
- SET_PROPERTY(TARGET ${target} APPEND_STRING PROPERTY COMPILE_FLAGS " ${flag}")
-ENDMACRO(ADD_TARGET_CFLAGS)
-
# ----------------------------------------------------
# --- C++ --------------------------------------------
# ----------------------------------------------------
diff --git a/src/algorithm/aba-derivatives.hxx b/src/algorithm/aba-derivatives.hxx
index b689ed3690b5afb1319d2303ac0525b3352b88fc..05c0544439ac1b637a551ed45e50270cae4e40d3 100644
--- a/src/algorithm/aba-derivatives.hxx
+++ b/src/algorithm/aba-derivatives.hxx
@@ -356,6 +356,7 @@ namespace pinocchio
data.u = tau;
MatrixType3 & Minv_ = EIGEN_CONST_CAST(MatrixType3,aba_partial_dtau);
+ Minv_.template triangularView<Eigen::Upper>().setZero();
/// First, compute Minv and a, the joint acceleration vector
typedef ComputeABADerivativesForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1> Pass1;
@@ -425,6 +426,7 @@ namespace pinocchio
data.u = tau;
MatrixType3 & Minv_ = EIGEN_CONST_CAST(MatrixType3,aba_partial_dtau);
+ Minv_.template triangularView<Eigen::Upper>().setZero();
/// First, compute Minv and a, the joint acceleration vector
typedef ComputeABADerivativesForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1> Pass1;
diff --git a/src/algorithm/aba.hxx b/src/algorithm/aba.hxx
index dd91c1cd552e5a0c107a0caf5b217a6767683a45..c2835edef8ecc59130bf345a950d32bf4273345c 100644
--- a/src/algorithm/aba.hxx
+++ b/src/algorithm/aba.hxx
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2016-2018 CNRS
+// Copyright (c) 2016-2018 CNRS, INRIA
//
#ifndef __pinocchio_aba_hxx__
@@ -457,6 +457,7 @@ namespace pinocchio
assert(q.size() == model.nq && "The joint configuration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
+ data.Minv.template triangularView<Eigen::Upper>().setZero();
typedef ComputeMinverseForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType> Pass1;
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
diff --git a/unittest/aba-derivatives.cpp b/unittest/aba-derivatives.cpp
index 0ea848324715300f8fbaa2c921de872c03058673..fbe03c1504271504296329bbec934f8122841fea 100644
--- a/unittest/aba-derivatives.cpp
+++ b/unittest/aba-derivatives.cpp
@@ -251,4 +251,46 @@ BOOST_AUTO_TEST_CASE(test_aba_derivatives_fext)
BOOST_CHECK(data_shortcut.Minv.isApprox(aba_partial_dtau));
}
+BOOST_AUTO_TEST_CASE(test_multiple_calls)
+{
+ using namespace Eigen;
+ using namespace pinocchio;
+
+ Model model;
+ buildModels::humanoidRandom(model);
+
+ Data data1(model), data2(model);
+
+ model.lowerPositionLimit.head<3>().fill(-1.);
+ model.upperPositionLimit.head<3>().fill(1.);
+ VectorXd q = randomConfiguration(model);
+ VectorXd v(VectorXd::Random(model.nv));
+ VectorXd tau(VectorXd::Random(model.nv));
+
+ computeABADerivatives(model,data1,q,v,tau);
+ data2 = data1;
+
+ for(int k = 0; k < 20; ++k)
+ {
+ computeABADerivatives(model,data1,q,v,tau);
+ }
+
+ BOOST_CHECK(data1.J.isApprox(data2.J));
+ BOOST_CHECK(data1.dJ.isApprox(data2.dJ));
+ BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq));
+ BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq));
+ BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv));
+
+ BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq));
+ BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv));
+ BOOST_CHECK(data1.dFda.isApprox(data2.dFda));
+
+ BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq));
+ BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv));
+
+ BOOST_CHECK(data1.ddq_dq.isApprox(data2.ddq_dq));
+ BOOST_CHECK(data1.ddq_dv.isApprox(data2.ddq_dv));
+ BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
+}
+
BOOST_AUTO_TEST_SUITE_END()
diff --git a/unittest/aba.cpp b/unittest/aba.cpp
index 72746a13ebcc1ba597a36bd1728cd74a27b8fb84..a4fbf42f41c9f6038026b8824dc245badda14b21 100644
--- a/unittest/aba.cpp
+++ b/unittest/aba.cpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2016-2018 CNRS
+// Copyright (c) 2016-2018 CNRS, INRIA
//
#include "pinocchio/spatial/fwd.hpp"
@@ -257,4 +257,30 @@ BOOST_AUTO_TEST_CASE ( test_computeMinverse )
// std::cout << "Minv_ref:\n" << Minv_ref.bottomRows<10>() << std::endl;
}
+
+BOOST_AUTO_TEST_CASE(test_multiple_calls)
+{
+ using namespace Eigen;
+ using namespace pinocchio;
+
+ Model model;
+ buildModels::humanoidRandom(model);
+
+ Data data1(model), data2(model);
+
+ model.lowerPositionLimit.head<3>().fill(-1.);
+ model.upperPositionLimit.head<3>().fill(1.);
+ VectorXd q = randomConfiguration(model);
+
+ computeMinverse(model,data1,q);
+ data2 = data1;
+
+ for(int k = 0; k < 20; ++k)
+ {
+ computeMinverse(model,data1,q);
+ }
+
+ BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
+}
+
BOOST_AUTO_TEST_SUITE_END ()
diff --git a/unittest/rnea-derivatives.cpp b/unittest/rnea-derivatives.cpp
index c4e80583bc5a46a23ba3ecb7c8c1852974934350..f7f20d8a116a016013a62f51f72a16913ca074c0 100644
--- a/unittest/rnea-derivatives.cpp
+++ b/unittest/rnea-derivatives.cpp
@@ -377,4 +377,43 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives_vs_kinematics_derivatives)
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
}
+BOOST_AUTO_TEST_CASE(test_multiple_calls)
+{
+ using namespace Eigen;
+ using namespace pinocchio;
+
+ Model model;
+ buildModels::humanoidRandom(model);
+
+ Data data1(model), data2(model);
+
+ model.lowerPositionLimit.head<3>().fill(-1.);
+ model.upperPositionLimit.head<3>().fill(1.);
+ VectorXd q = randomConfiguration(model);
+ VectorXd v(VectorXd::Random(model.nv));
+ VectorXd a(VectorXd::Random(model.nv));
+
+ computeRNEADerivatives(model,data1,q,v,a);
+ data2 = data1;
+
+ for(int k = 0; k < 20; ++k)
+ {
+ computeRNEADerivatives(model,data1,q,v,a);
+ }
+
+ BOOST_CHECK(data1.J.isApprox(data2.J));
+ BOOST_CHECK(data1.dJ.isApprox(data2.dJ));
+ BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq));
+ BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq));
+ BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv));
+
+ BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq));
+ BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv));
+ BOOST_CHECK(data1.dFda.isApprox(data2.dFda));
+
+ BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq));
+ BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv));
+ BOOST_CHECK(data1.M.isApprox(data2.M));
+}
+
BOOST_AUTO_TEST_SUITE_END()