diff --git a/unittest/contact-dynamics-derivatives.cpp b/unittest/contact-dynamics-derivatives.cpp
index 4c4773fc3a72a353d8ee051d4d5d58f52ea84741..6a35ebd66c35487f456c004cd171cef4835d45bb 100644
--- a/unittest/contact-dynamics-derivatives.cpp
+++ b/unittest/contact-dynamics-derivatives.cpp
@@ -17,11 +17,11 @@
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
-BOOST_AUTO_TEST_CASE ( test_FD )
+using namespace Eigen;
+using namespace pinocchio;
+
+BOOST_AUTO_TEST_CASE ( test_FD_with_contact_cst_gamma )
{
- using namespace Eigen;
- using namespace pinocchio;
-
pinocchio::Model model;
pinocchio::buildModels::humanoidRandom(model,true);
pinocchio::Data data(model), data_check(model);
@@ -153,7 +153,171 @@ BOOST_AUTO_TEST_CASE ( test_FD )
BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps)));
BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps)));
+}
+
+template<typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
+VectorXd contactDynamics(const Model & model, Data & data,
+ const Eigen::MatrixBase<ConfigVectorType> & q,
+ const Eigen::MatrixBase<TangentVectorType1> & v,
+ const Eigen::MatrixBase<TangentVectorType2> & tau,
+ const Model::JointIndex id)
+{
+ computeJointJacobians(model, data, q);
+ Data::Matrix6x J(6,model.nv); J.setZero();
+
+ getJointJacobian(model, data, id, LOCAL, J);
+ Motion::Vector6 gamma;
+ forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv));
+ gamma = data.a[id].toVector();
+
+ forwardDynamics(model, data, q, v, tau, J, gamma);
+ VectorXd res(VectorXd::Zero(model.nv+6));
+
+ res.head(model.nv) = data.ddq;
+ res.tail(6) = data.lambda_c;
+
+ return res;
+}
+
+BOOST_AUTO_TEST_CASE ( test_FD_with_contact_varying_gamma )
+{
+ pinocchio::Model model;
+ pinocchio::buildModels::humanoidRandom(model,true);
+ pinocchio::Data data(model), data_check(model);
+
+ VectorXd q = VectorXd::Ones(model.nq);
+ normalize(model,q);
+
+ VectorXd v = VectorXd::Random(model.nv);
+ VectorXd tau = VectorXd::Random(model.nv);
+
+ const std::string RF = "rleg6_joint";
+ const Model::JointIndex RF_id = model.getJointId(RF);
+
+ Data::Matrix6x J_RF(6,model.nv); J_RF.setZero();
+ computeJointJacobians(model, data, q);
+ getJointJacobian(model, data, RF_id, LOCAL, J_RF);
+ Motion::Vector6 gamma_RF; gamma_RF.setZero();
+
+ VectorXd x_ref = contactDynamics(model,data,q,v,tau,RF_id);
+ VectorXd ddq_ref = x_ref.head(model.nv);
+ Force::Vector6 contact_force_ref = x_ref.tail(6);
+
+ container::aligned_vector<Force> fext((size_t)model.njoints,Force::Zero());
+ fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);
+
+ // check call to RNEA
+ rnea(model,data_check,q,v,ddq_ref,fext);
+
+ BOOST_CHECK(data_check.tau.isApprox(tau));
+ BOOST_CHECK(data_check.a[RF_id].toVector().isApprox(-gamma_RF));
+
+ Data data_fd(model);
+ VectorXd q_plus(model.nq);
+ VectorXd v_eps(model.nv); v_eps.setZero();
+ VectorXd v_plus(v);
+ VectorXd tau_plus(tau);
+ VectorXd x_plus(model.nv + 6);
+ const double eps = 1e-8;
+
+ // check: dddq_dtau and dlambda_dtau
+ MatrixXd dddq_dtau(model.nv,model.nv);
+ Data::Matrix6x dlambda_dtau(6,model.nv);
+ for(int k = 0; k < model.nv; ++k)
+ {
+ tau_plus[k] += eps;
+ x_plus = contactDynamics(model,data,q,v,tau_plus,RF_id);
+
+ const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
+ Force::Vector6 contact_force_plus = x_plus.tail(6);
+
+ dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps;
+ dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps;
+
+ tau_plus[k] -= eps;
+ }
+
+ MatrixXd A(model.nv+6,model.nv+6);
+ data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
+ A.topLeftCorner(model.nv,model.nv) = data.M;
+ A.bottomLeftCorner(6, model.nv) = J_RF;
+ A.topRightCorner(model.nv, 6) = J_RF.transpose();
+ A.bottomRightCorner(6,6).setZero();
+
+ MatrixXd Ainv = A.inverse();
+ BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps)));
+ BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps)));
+
+ // check: dddq_dv and dlambda_dv
+ MatrixXd dddq_dv(model.nv,model.nv);
+ Data::Matrix6x dlambda_dv(6,model.nv);
+
+ for(int k = 0; k < model.nv; ++k)
+ {
+ v_plus[k] += eps;
+ x_plus = contactDynamics(model,data,q,v_plus,tau,RF_id);
+
+ const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
+ Force::Vector6 contact_force_plus = x_plus.tail(6);
+
+ dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps;
+ dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps;
+
+ v_plus[k] -= eps;
+ }
+
+
+ computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv));
+ Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv);
+ v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
+ Data data_kin(model);
+ computeForwardKinematicsDerivatives(model,data_kin,q,v,VectorXd::Zero(model.nv));
+ getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
+
+ MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
+ dddq_dv_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dv;
+ MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;
+ dlambda_dv_anal -= Ainv.bottomRows(6).rightCols(6) * a_partial_dv;
+
+ BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps)));
+ BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps)));
+
+
+ MatrixXd dddq_dq(model.nv,model.nv);
+ Data::Matrix6x dlambda_dq(6,model.nv);
+
+ for(int k = 0; k < model.nv; ++k)
+ {
+ v_eps[k] = eps;
+ q_plus = integrate(model,q,v_eps);
+
+ x_plus = contactDynamics(model,data,q_plus,v,tau,RF_id);
+
+ const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
+ Force::Vector6 contact_force_plus = x_plus.tail(6);
+
+ dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps;
+ dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps;
+
+ v_eps[k] = 0.;
+ }
+
+ computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext);
+ v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
+ computeForwardKinematicsDerivatives(model,data_kin,q,v,ddq_ref);
+ getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
+
+ MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
+ dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;
+
+ BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps)));
+
+ MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
+ dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;
+
+ BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps)));
+
}
BOOST_AUTO_TEST_SUITE_END ()