Merge pull request #2126 from jcarpent/topic/iit

Fix malloc issue in CRBA
stack-of-tasks · Dec 22, 2023 · b4c4dcd · b4c4dcd
2 parents 2a15772 + 27b0db5
commit b4c4dcd
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 1 deletion.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -15,6 +15,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Fix missing include for Boost >= 1.83 ([#2103](https://github.com/stack-of-tasks/pinocchio/pull/2103))
 - Remove f-strings to fix install with python 2 ([#2110](https://github.com/stack-of-tasks/pinocchio/pull/2110))
 - CMake: stop exporting CppAd/cppadcodegen & fetch submodule if not available ([#2112](https://github.com/stack-of-tasks/pinocchio/pull/2112))
+- Fix malloc issue in CRBA algo ([#2126](https://github.com/stack-of-tasks/pinocchio/pull/2126))
 
 ## [2.6.21] - 2023-11-27
 

diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx
@@ -77,7 +77,7 @@ namespace pinocchio
       jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
 
       /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
-      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]) 
+      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
       = jdata.S().transpose()*data.Fcrb[i].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
 
       const JointIndex & parent = model.parents[i];

diff --git a/unittest/crba.cpp b/unittest/crba.cpp
@@ -10,6 +10,10 @@
  *
  */
 
+#ifndef EIGEN_RUNTIME_NO_MALLOC 
+  #define EIGEN_RUNTIME_NO_MALLOC
+#endif
+
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/multibody/data.hpp"
 #include "pinocchio/algorithm/crba.hpp"
@@ -158,5 +162,24 @@ BOOST_AUTO_TEST_CASE(test_minimal_crba)
 
 }
 
+#ifndef NDEBUG
+
+BOOST_AUTO_TEST_CASE(test_crba_malloc)
+{
+  using namespace pinocchio;
+  pinocchio::Model model;
+  pinocchio::buildModels::humanoidRandom(model);
+
+  model.addJoint(size_t(model.njoints-1), pinocchio::JointModelRevoluteUnaligned(SE3::Vector3::UnitX()), SE3::Random(), "revolute_unaligned");
+  pinocchio::Data data(model);
+
+  const Eigen::VectorXd q = pinocchio::neutral(model);
+  Eigen::internal::set_is_malloc_allowed(false);
+  crba(model, data, q);
+  Eigen::internal::set_is_malloc_allowed(true);
+}
+
+#endif
+
 BOOST_AUTO_TEST_SUITE_END ()