algo/centroidal: fix missing term for centroidal dynamics derivatives

stack-of-tasks · Nov 22, 2023 · 856f26e · 856f26e
1 parent 7f9be4c
commit 856f26e
Showing 1 changed file with 4 additions and 2 deletions.
diff --git 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;
   }