Merge pull request #1294 from cmastalli/topic/replace-selfadjointview

Replaced selfadjointView to enhace efficiency

CHANGELOG.md

@@ -6,6 +6,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+* Improved efficiency for computing impulse-dynamics derivatives in https://github.com/loco-3d/crocoddyl/pull/1294
 * Fixed bug of wrench cone fields not being updated with setters in https://github.com/loco-3d/crocoddyl/pull/1274
 * Replaced parent by parentJoint (which was introduced in Pinocchio 3) in https://github.com/loco-3d/crocoddyl/pull/1271
 * Introduced the notion of terminal dimension, residuals and constraints in https://github.com/loco-3d/crocoddyl/pull/1269

include/crocoddyl/multibody/actions/impulse-fwddyn.hxx

@@ -249,15 +249,17 @@ void ActionModelImpulseFwdDynamicsTpl<Scalar>::initCalcDiff(
   Eigen::Block<MatrixXs> f_partial_da = data->Kinv.bottomRightCorner(nc, nc);
 
   data->pinocchio.dtau_dq -= data->dgrav_dq;
+  data->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
+      data->pinocchio.M.transpose()
+          .template triangularView<Eigen::StrictlyLower>();
   data->Fx.topLeftCorner(nv, nv).setIdentity();
   data->Fx.topRightCorner(nv, nv).setZero();
   data->Fx.bottomLeftCorner(nv, nv).noalias() =
       -a_partial_dtau * data->pinocchio.dtau_dq;
   data->Fx.bottomLeftCorner(nv, nv).noalias() -=
       a_partial_da * data->multibody.impulses->dv0_dq.topRows(nc);
   data->Fx.bottomRightCorner(nv, nv).noalias() =
-      a_partial_dtau *
-      data->pinocchio.M.template selfadjointView<Eigen::Upper>();
+      a_partial_dtau * data->pinocchio.M;
 
   // Computing the cost derivatives
   if (enable_force_) {