diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 05af7c160b..01ae1b3c9d 100644 --- 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 J_cols = data.J.col(jmodel.idx_vExtended()); - Eigen::Matrix Ag_sec; + Eigen::Matrix J_cols = + jmodel.jointExtendedModelCols(data.J); + Eigen::Matrix Ag_sec( + 6, jmodel.nvExtended()); motionSet::inertiaAction(data.oYcrb[secondary_id], J_cols, Ag_sec); @@ -385,7 +387,7 @@ namespace pinocchio { typedef JointDataTpl JointData; typedef - typename Eigen::Matrix + typename Eigen::Matrix MotionSubspace; typedef GetMotionSubspaceTplNoMalloc 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 index 2216634b21..2bce4003fd 100644 --- 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 JointCollection; typedef JointDataMimicTpl JointDataDerived; typedef JointModelMimicTpl JointModelDerived; - typedef ScaledJointMotionSubspaceTpl Constraint_t; + typedef ScaledJointMotionSubspaceTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionTpl Motion_t; typedef MotionTpl Bias_t; @@ -558,7 +558,7 @@ namespace pinocchio PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); enum { - MaxNVExtended = traits::MaxNVExtended + MaxNVMimicked = traits::MaxNVMimicked }; typedef JointCollectionTpl JointCollection; diff --git a/unittest/crba.cpp b/unittest/crba.cpp index d6998f90f3..91c90f3282 100644 --- 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); }