From 856f26eaf36a2ea530dccc9c7c075ca222960320 Mon Sep 17 00:00:00 2001
From: Justin Carpentier <justin.carpentier@inria.fr>
Date: Wed, 22 Nov 2023 17:05:11 +0100
Subject: [PATCH] algo/centroidal: fix missing term for centroidal dynamics
 derivatives

---
 include/pinocchio/algorithm/centroidal.hxx | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/include/pinocchio/algorithm/centroidal.hxx b/include/pinocchio/algorithm/centroidal.hxx
index 9bd4b95f4c..332fb4ce5c 100644
--- a/include/pinocchio/algorithm/centroidal.hxx
+++ b/include/pinocchio/algorithm/centroidal.hxx
@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2015-2019 CNRS INRIA
+// Copyright (c) 2015-2023 CNRS INRIA
 //
 
 #ifndef __pinocchio_algorithm_centroidal_hxx__
@@ -371,7 +371,9 @@ namespace pinocchio
     const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR);
     Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR);
     for(Eigen::DenseIndex i = 0; i<model.nv; ++i)
-      dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]);
+    {
+      dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]) + Ag_lin.col(i).cross(data.vcom[0]);
+    }
     
     return data.dAg;
   }