[Test] Increase test for centroidal dynamics with wellknown equalities

unittest/centroidal.cpp

@@ -196,6 +196,59 @@ BOOST_AUTO_TEST_CASE (test_dccrb)
     BOOST_CHECK(dAg.isApprox(dAg_ref,sqrt(alpha)));
     BOOST_CHECK(dAg.isApprox(dAg_ref_from_M,sqrt(alpha)));
   }
+  
+  // Compute tensor dAg/dq
+  {
+    std::vector<Data::Matrix6x> dAgdq((size_t)model.nv,Data::Matrix6x::Zero(6,model.nv));
+    Data data(model), data_fd(model);
+    Eigen::VectorXd v_fd(Eigen::VectorXd::Zero(model.nv));
+    ccrba(model,data_fd,q,v);
+    SE3 oMc_ref(SE3::Identity());
+    oMc_ref.translation() = data_fd.com[0];
+    
+    Data::Matrix6x Ag0 = oMc_ref.toDualActionMatrix() * data_fd.Ag;
+    const Force hg0 = oMc_ref.act(data_fd.hg);
+    
+    Data::Matrix6x Ag_fd(6,model.nv);
+    Force hg_fd;
+    const double alpha = 1e-8;
+    Eigen::VectorXd q_plus(model.nq);
+    Data::Matrix6x dhdq(6,model.nv);
+    for(int k = 0; k < model.nv; ++k)
+    {
+      v_fd[k] = alpha;
+      q_plus = integrate(model,q,v_fd);
+      ccrba(model,data_fd,q_plus,v);
+      SE3 oMc_fd(SE3::Identity());
+      oMc_fd.translation() = data_fd.com[0];
+      Ag_fd = oMc_fd.toDualActionMatrix() * data_fd.Ag;
+      hg_fd = oMc_fd.act(data_fd.hg);
+      dAgdq[(size_t)k] = (Ag_fd - Ag0)/alpha;
+      dhdq.col(k) = (hg_fd - hg0).toVector()/alpha;
+      v_fd[k] = 0.;
+    }
+    
+    Data::Matrix6x dAg_ref(6,model.nv); dAg_ref.setZero();
+    for(int k = 0; k < model.nv; ++k)
+    {
+      dAg_ref += dAgdq[(size_t)k] * v[k];
+    }
+    
+    Data::Matrix6x dAg_ref_bis(6,model.nv); dAg_ref_bis.setZero();
+    for(int k = 0; k < model.nv; ++k)
+    {
+      dAg_ref_bis.col(k) = dAgdq[(size_t)k] * v;
+    }
+    
+    dccrba(model, data, q, v);
+    SE3 oMc(SE3::Identity());
+    oMc.translation() = data.com[0];
+    Data::Matrix6x dAg = oMc.toDualActionMatrix() * data.dAg;
+    BOOST_CHECK(dAg.isApprox(dAg_ref,sqrt(alpha)));
+    BOOST_CHECK(dhdq.isApprox(dAg_ref_bis,sqrt(alpha)));
+    BOOST_CHECK((dAg*v).isApprox(dhdq*v,sqrt(alpha)));
+    
+  }
 }
 
 BOOST_AUTO_TEST_SUITE_END()