Fix use of pinocchio::make_symmetric()

Author: ManifoldFR

include/simple-mpc/math-util.hpp

@@ -0,0 +1,25 @@
+#include <Eigen/Core>
+
+namespace simple_mpc
+{
+  namespace math
+  {
+
+    /// @note Backport from <pinocchio/math/matrix.hpp> in topic/simulation branch.
+    template<typename Matrix>
+    void make_symmetric(const Eigen::MatrixBase<Matrix> & mat, const int mode = Eigen::Upper)
+    {
+      if (mode == Eigen::Upper)
+      {
+        mat.const_cast_derived().template triangularView<Eigen::StrictlyLower>() =
+          mat.transpose().template triangularView<Eigen::StrictlyLower>();
+      }
+      else if (mode == Eigen::Lower)
+      {
+        mat.const_cast_derived().template triangularView<Eigen::StrictlyUpper>() =
+          mat.transpose().template triangularView<Eigen::StrictlyUpper>();
+      }
+    }
+
+  } // namespace math
+} // namespace simple_mpc

tests/robot_handler.cpp

@@ -2,6 +2,7 @@
 #include <boost/test/unit_test.hpp>
 
 #include "simple-mpc/fwd.hpp"
+#include "simple-mpc/math-util.hpp"
 #include "simple-mpc/robot-handler.hpp"
 #include "test_utils.cpp"
 #include <pinocchio/algorithm/center-of-mass.hpp>
@@ -227,7 +228,7 @@ BOOST_AUTO_TEST_CASE(data_handler)
     pinocchio::computeJointJacobians(model, data);
     pinocchio::computeJointJacobiansTimeVariation(model, data, q, v);
     pinocchio::crba(model, data, q);
-    pinocchio::make_symmetric(data.M);
+    simple_mpc::math::make_symmetric(data.M);
     pinocchio::nonLinearEffects(model, data, q, v);
     pinocchio::dccrba(model, data, q, v);