From 71e20a9ba20c6eb4758ca339475262ffc4de7d14 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Mon, 2 Sep 2024 14:31:26 +0100 Subject: [PATCH 1/2] [core] Replaced selfadjointView to enhace efficiency --- include/crocoddyl/multibody/actions/impulse-fwddyn.hxx | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/include/crocoddyl/multibody/actions/impulse-fwddyn.hxx b/include/crocoddyl/multibody/actions/impulse-fwddyn.hxx index b3efc46b5..3c0f45be0 100644 --- a/include/crocoddyl/multibody/actions/impulse-fwddyn.hxx +++ b/include/crocoddyl/multibody/actions/impulse-fwddyn.hxx @@ -249,6 +249,9 @@ void ActionModelImpulseFwdDynamicsTpl::initCalcDiff( Eigen::Block f_partial_da = data->Kinv.bottomRightCorner(nc, nc); data->pinocchio.dtau_dq -= data->dgrav_dq; + data->pinocchio.M.template triangularView() = + data->pinocchio.M.transpose() + .template triangularView(); data->Fx.topLeftCorner(nv, nv).setIdentity(); data->Fx.topRightCorner(nv, nv).setZero(); data->Fx.bottomLeftCorner(nv, nv).noalias() = @@ -256,8 +259,7 @@ void ActionModelImpulseFwdDynamicsTpl::initCalcDiff( 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(); + a_partial_dtau * data->pinocchio.M; // Computing the cost derivatives if (enable_force_) { From eee395b00ef317cf583138ed05a838da69b75d4f Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Mon, 2 Sep 2024 14:34:21 +0100 Subject: [PATCH 2/2] [changelog] updated --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5b69e322c..b5b821997 100644 --- a/CHANGELOG.md +++ b/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