[EtienneAr feedback] topic/mimic: Rename MaxNVExtended into MaxNVMimi…

…cked and use it in crba patch
stack-of-tasks · Feb 17, 2025 · d5c4ead · d5c4ead
1 parent 334c76e
commit d5c4ead
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 7 deletions.
diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx
@@ -157,8 +157,10 @@ namespace pinocchio
         JointIndex secondary_id = jmodel.id();
         JointIndex primary_id = jmodel.derived().jmodel().id();
 
-        Eigen::Matrix<Scalar, 6, 1> J_cols = data.J.col(jmodel.idx_vExtended());
-        Eigen::Matrix<Scalar, 6, 1> Ag_sec;
+        Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options, 6, JointModel::MaxNVMimicked> J_cols =
+          jmodel.jointExtendedModelCols(data.J);
+        Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options, 6, JointModel::MaxNVMimicked> Ag_sec(
+          6, jmodel.nvExtended());
 
         motionSet::inertiaAction(data.oYcrb[secondary_id], J_cols, Ag_sec);
 
@@ -385,7 +387,7 @@ namespace pinocchio
       {
         typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointData;
         typedef
-          typename Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options, 6, JointModel::MaxNVExtended>
+          typename Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options, 6, JointModel::MaxNVMimicked>
             MotionSubspace;
         typedef GetMotionSubspaceTplNoMalloc<JointData, MotionSubspace> GetSNoMalloc;
         const JointIndex & secondary_id = jmodel.id();

diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp
@@ -307,14 +307,14 @@ namespace pinocchio
       NQ = Eigen::Dynamic,
       NV = Eigen::Dynamic,
       NVExtended = Eigen::Dynamic,
-      MaxNVExtended = 6
+      MaxNVMimicked = 6
     };
 
     typedef JointCollectionTpl<Scalar, Options> JointCollection;
     typedef JointDataMimicTpl<Scalar, Options, JointCollectionTpl> JointDataDerived;
     typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelDerived;
 
-    typedef ScaledJointMotionSubspaceTpl<Scalar, Options, MaxNVExtended> Constraint_t;
+    typedef ScaledJointMotionSubspaceTpl<Scalar, Options, MaxNVMimicked> Constraint_t;
     typedef SE3Tpl<Scalar, Options> Transformation_t;
     typedef MotionTpl<Scalar, Options> Motion_t;
     typedef MotionTpl<Scalar, Options> Bias_t;
@@ -558,7 +558,7 @@ namespace pinocchio
     PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
     enum
     {
-      MaxNVExtended = traits<JointDerived>::MaxNVExtended
+      MaxNVMimicked = traits<JointDerived>::MaxNVMimicked
     };
 
     typedef JointCollectionTpl<Scalar, Options> JointCollection;

diff --git a/unittest/crba.cpp b/unittest/crba.cpp
@@ -297,7 +297,7 @@ BOOST_AUTO_TEST_CASE(test_crba_malloc)
 
   const Eigen::VectorXd q = pinocchio::neutral(model);
   Eigen::internal::set_is_malloc_allowed(false);
-  // crba(model, data, q, pinocchio::Convention::WORLD);
+  crba(model, data, q, pinocchio::Convention::WORLD);
   crba(model, data, q, pinocchio::Convention::LOCAL);
   Eigen::internal::set_is_malloc_allowed(true);
 }