diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp index 9c97eae5d1a64b3bb80c692f8be4d7e7f577616c..8c853736748ef963aac5b2004b54efec2d340833 100644 --- a/src/multibody/joint/joint-spherical.hpp +++ b/src/multibody/joint/joint-spherical.hpp @@ -357,10 +357,10 @@ namespace se3 if (update_I) { - I.template block<6,3>(0,Inertia::ANGULAR).setZero(); - I.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero(); I.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR) -= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR); + I.template block<6,3>(0,Inertia::ANGULAR).setZero(); + I.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero(); } } diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp index 9ca7f5d7d6a7875741e398acfa95088cde0d63f1..975050b6b11d334b84a4a905a76bd34bfaeefe1e 100644 --- a/src/multibody/joint/joint-translation.hpp +++ b/src/multibody/joint/joint-translation.hpp @@ -358,10 +358,10 @@ namespace se3 if (update_I) { - I.template middleCols<3>(Inertia::LINEAR).setZero(); - I.template block<3,3>(Inertia::LINEAR,Inertia::ANGULAR).setZero(); I.template block<3,3>(Inertia::ANGULAR,Inertia::ANGULAR) -= data.UDinv.template middleRows<3>(Inertia::ANGULAR) * I.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR); + I.template middleCols<3>(Inertia::LINEAR).setZero(); + I.template block<3,3>(Inertia::LINEAR,Inertia::ANGULAR).setZero(); } }