Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/contact-dynamics.cpp	Lines:	214	214	100.0 %
Date:	2024-04-26 13:14:21	Branches:	665	1322	50.3 %


//
// Copyright (c) 2016-2020 CNRS, INRIA
//

#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/contact-dynamics.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_FD )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment <4> (3).normalize();

  pinocchio::computeJointJacobians(model, data, q);

  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);

  const std::string RF = "rleg6_joint";
  const std::string LF = "lleg6_joint";

  Data::Matrix6x J_RF (6, model.nv);
  J_RF.setZero();
  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
  Data::Matrix6x J_LF (6, model.nv);
  J_LF.setZero();
  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);

  Eigen::MatrixXd J (12, model.nv);
  J.setZero();
  J.topRows<6> () = J_RF;
  J.bottomRows<6> () = J_LF;

  Eigen::VectorXd gamma (VectorXd::Ones(12));

  Eigen::MatrixXd H(J.transpose());

  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  MatrixXd Minv (data.M.inverse());
  MatrixXd JMinvJt (J * Minv * J.transpose());

  Eigen::MatrixXd G_ref(J.transpose());
  cholesky::Uiv(model, data, G_ref);
  for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
    Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
    BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));

  VectorXd lambda_ref = -JMinvJt.inverse() * (J*Minv*(tau - data.nle) + gamma);
  BOOST_CHECK(data.lambda_c.isApprox(lambda_ref, 1e-12));

  VectorXd a_ref = Minv*(tau - data.nle + J.transpose()*lambda_ref);

  Eigen::VectorXd dynamics_residual_ref (data.M * a_ref + data.nle - tau - J.transpose()*lambda_ref);
  BOOST_CHECK(dynamics_residual_ref.norm() <= 1e-11); // previously 1e-12, may be due to numerical approximations, i obtain 2.03e-12

  Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
  BOOST_CHECK(constraint_residual.norm() <= 1e-12);

  Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);

}

BOOST_AUTO_TEST_CASE(test_computeKKTMatrix)
{
  using namespace Eigen;
  using namespace pinocchio;
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model), data_ref(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment<4>(3).normalize();

  pinocchio::computeJointJacobians(model, data_ref, q);

  const std::string RF = "rleg6_joint";
  const std::string LF = "lleg6_joint";

  Data::Matrix6x J_RF(6, model.nv);
  J_RF.setZero();
  getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF);
  Data::Matrix6x J_LF(6, model.nv);
  J_LF.setZero();
  getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);

  Eigen::MatrixXd J(12, model.nv);
  J.setZero();
  J.topRows<6>() = J_RF;
  J.bottomRows<6>() = J_LF;

  //Check Forward Dynamics
  pinocchio::crba(model,data_ref,q);
  data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();

  Eigen::MatrixXd MJtJ(model.nv+12, model.nv+12);
  MJtJ << data_ref.M, J.transpose(),
    J, Eigen::MatrixXd::Zero(12, 12);

  Eigen::MatrixXd KKTMatrix_inv(model.nv+12, model.nv+12);
  computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv);

  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
}

BOOST_AUTO_TEST_CASE(test_getKKTMatrix)
{
  using namespace Eigen;
  using namespace pinocchio;
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment <4> (3).normalize();

  pinocchio::computeJointJacobians(model, data, q);

  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);

  const std::string RF = "rleg6_joint";
  const std::string LF = "lleg6_joint";

  Data::Matrix6x J_RF (6, model.nv);
  J_RF.setZero();
  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
  Data::Matrix6x J_LF (6, model.nv);
  J_LF.setZero();
  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);

  Eigen::MatrixXd J (12, model.nv);
  J.setZero();
  J.topRows<6> () = J_RF;
  J.bottomRows<6> () = J_LF;

  Eigen::VectorXd gamma (VectorXd::Ones(12));

  Eigen::MatrixXd H(J.transpose());

  //Check Forward Dynamics
  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  Eigen::MatrixXd MJtJ(model.nv+12, model.nv+12);
  MJtJ << data.M, J.transpose(),
    J, Eigen::MatrixXd::Zero(12, 12);

  Eigen::MatrixXd KKTMatrix_inv(model.nv+12, model.nv+12);
  getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);

  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));

  //Check Impulse Dynamics
  const double r_coeff = 1.;
  VectorXd v_before = VectorXd::Ones(model.nv);
  pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
  MJtJ << data.M, J.transpose(),
    J, Eigen::MatrixXd::Zero(12, 12);

  getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);

  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
}

BOOST_AUTO_TEST_CASE ( test_FD_with_damping )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment <4> (3).normalize();

  pinocchio::computeJointJacobians(model, data, q);

  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);

  const std::string RF = "rleg6_joint";

  Data::Matrix6x J_RF (6, model.nv);
  J_RF.setZero();
  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);

  Eigen::MatrixXd J (12, model.nv);
  J.setZero();
  J.topRows<6> () = J_RF;
  J.bottomRows<6> () = J_RF;

  Eigen::VectorXd gamma (VectorXd::Ones(12));

  // Forward Dynamics with damping
  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12);

  // Matrix Definitions
  Eigen::MatrixXd H(J.transpose());
  data.M.triangularView<Eigen::StrictlyLower>() =
    data.M.transpose().triangularView<Eigen::StrictlyLower>();

  MatrixXd Minv (data.M.inverse());
  MatrixXd JMinvJt (J * Minv * J.transpose());

  // Check that JMinvJt is correctly formed
  Eigen::MatrixXd G_ref(J.transpose());
  cholesky::Uiv(model, data, G_ref);
  for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
  Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
  BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));

  // Actual Residuals
  Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
  Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
  BOOST_CHECK(constraint_residual.norm() <= 1e-9);
  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
}

BOOST_AUTO_TEST_CASE ( test_ID )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment <4> (3).normalize();

  pinocchio::computeJointJacobians(model, data, q);

  VectorXd v_before = VectorXd::Ones(model.nv);

  const std::string RF = "rleg6_joint";
  const std::string LF = "lleg6_joint";

  Data::Matrix6x J_RF (6, model.nv);
  J_RF.setZero();
  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
  Data::Matrix6x J_LF (6, model.nv);
  J_LF.setZero();
  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);

  Eigen::MatrixXd J (12, model.nv);
  J.setZero();
  J.topRows<6> () = J_RF;
  J.bottomRows<6> () = J_LF;

  const double r_coeff = 1.;

  Eigen::MatrixXd H(J.transpose());

  pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  MatrixXd Minv (data.M.inverse());
  MatrixXd JMinvJt (J * Minv * J.transpose());

  Eigen::MatrixXd G_ref(J.transpose());
  cholesky::Uiv(model, data, G_ref);
  for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
  Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
  BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));

  VectorXd lambda_ref = JMinvJt.inverse() * (-r_coeff * J * v_before - J * v_before);
  BOOST_CHECK(data.impulse_c.isApprox(lambda_ref, 1e-12));

  VectorXd v_after_ref = Minv*(data.M * v_before + J.transpose()*lambda_ref);

  Eigen::VectorXd constraint_residual (J * data.dq_after + r_coeff * J * v_before);
  BOOST_CHECK(constraint_residual.norm() <= 1e-12);

  Eigen::VectorXd dynamics_residual (data.M * data.dq_after - data.M * v_before - J.transpose()*data.impulse_c);
  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);

}

BOOST_AUTO_TEST_CASE (timings_fd_llt)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

#ifdef NDEBUG
#ifdef _INTENSE_TESTING_
  const size_t NBT = 1000*1000;
#else
  const size_t NBT = 100;
#endif

#else
  const size_t NBT = 1;
  std::cout << "(the time score in debug mode is not relevant)  " ;
#endif // ifndef NDEBUG

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment <4> (3).normalize();

  pinocchio::computeJointJacobians(model, data, q);

  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);

  const std::string RF = "rleg6_joint";
  const std::string LF = "lleg6_joint";

  Data::Matrix6x J_RF (6, model.nv);
  J_RF.setZero();
  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
  Data::Matrix6x J_LF (6, model.nv);
  J_LF.setZero();
  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);

  Eigen::MatrixXd J (12, model.nv);
  J.topRows<6> () = J_RF;
  J.bottomRows<6> () = J_LF;

  Eigen::VectorXd gamma (VectorXd::Ones(12));

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill( 1.);

  q = pinocchio::randomConfiguration(model);

  PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
  SMOOTH(NBT)
  {
    pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
  }
  timer.toc(std::cout,NBT);

}

BOOST_AUTO_TEST_SUITE_END ()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2020 CNRS, INRIA
3			//
4
5			#include "pinocchio/spatial/se3.hpp"
6			#include "pinocchio/multibody/model.hpp"
7			#include "pinocchio/multibody/data.hpp"
8			#include "pinocchio/algorithm/jacobian.hpp"
9			#include "pinocchio/algorithm/contact-dynamics.hpp"
10			#include "pinocchio/algorithm/joint-configuration.hpp"
11			#include "pinocchio/parsers/sample-models.hpp"
12			#include "pinocchio/utils/timer.hpp"
13
14			#include <iostream>
15
16			#include <boost/test/unit_test.hpp>
17			#include <boost/utility/binary.hpp>
18
19			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
20
21	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_FD )
22			{
23			using namespace Eigen;
24			using namespace pinocchio;
25
26	✓✗	4	pinocchio::Model model;
27	✓✗	2	pinocchio::buildModels::humanoidRandom(model,true);
28	✓✗	4	pinocchio::Data data(model);
29
30	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
31	✓✗✓✗	2	q.segment <4> (3).normalize();
32
33	✓✗	2	pinocchio::computeJointJacobians(model, data, q);
34
35	✓✗✓✗	4	VectorXd v = VectorXd::Ones(model.nv);
36	✓✗✓✗	4	VectorXd tau = VectorXd::Zero(model.nv);
37
38	✓✗	4	const std::string RF = "rleg6_joint";
39	✓✗	4	const std::string LF = "lleg6_joint";
40
41	✓✗	4	Data::Matrix6x J_RF (6, model.nv);
42	✓✗	2	J_RF.setZero();
43	✓✗✓✗	2	getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
44	✓✗	4	Data::Matrix6x J_LF (6, model.nv);
45	✓✗	2	J_LF.setZero();
46	✓✗✓✗	2	getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
47
48	✓✗	4	Eigen::MatrixXd J (12, model.nv);
49	✓✗	2	J.setZero();
50	✓✗✓✗	2	J.topRows<6> () = J_RF;
51	✓✗✓✗	2	J.bottomRows<6> () = J_LF;
52
53	✓✗✓✗	4	Eigen::VectorXd gamma (VectorXd::Ones(12));
54
55	✓✗✓✗	4	Eigen::MatrixXd H(J.transpose());
56
57	✓✗	2	pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
58	✓✗✓✗ ✓✗✓✗	2	data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
59
60	✓✗✓✗	4	MatrixXd Minv (data.M.inverse());
61	✓✗✓✗ ✓✗✓✗	4	MatrixXd JMinvJt (J * Minv * J.transpose());
62
63	✓✗✓✗	4	Eigen::MatrixXd G_ref(J.transpose());
64	✓✗	2	cholesky::Uiv(model, data, G_ref);
65	✓✓✓✗ ✓✗✓✗	66	for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
66	✓✗✓✗ ✓✗	4	Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
67	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));
68
69	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	4	VectorXd lambda_ref = -JMinvJt.inverse() * (JMinv(tau - data.nle) + gamma);
70	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.lambda_c.isApprox(lambda_ref, 1e-12));
71
72	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	4	VectorXd a_ref = Minv(tau - data.nle + J.transpose()lambda_ref);
73
74	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	Eigen::VectorXd dynamics_residual_ref (data.M * a_ref + data.nle - tau - J.transpose()*lambda_ref);
75	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dynamics_residual_ref.norm() <= 1e-11); // previously 1e-12, may be due to numerical approximations, i obtain 2.03e-12
76
77	✓✗✓✗ ✓✗	4	Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
78	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(constraint_residual.norm() <= 1e-12);
79
80	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
81	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
82
83		2	}
84
85	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_computeKKTMatrix)
86			{
87			using namespace Eigen;
88			using namespace pinocchio;
89	✓✗	4	pinocchio::Model model;
90	✓✗	2	pinocchio::buildModels::humanoidRandom(model,true);
91	✓✗✓✗	4	pinocchio::Data data(model), data_ref(model);
92
93	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
94	✓✗✓✗	2	q.segment<4>(3).normalize();
95
96	✓✗	2	pinocchio::computeJointJacobians(model, data_ref, q);
97
98	✓✗	4	const std::string RF = "rleg6_joint";
99	✓✗	4	const std::string LF = "lleg6_joint";
100
101	✓✗	4	Data::Matrix6x J_RF(6, model.nv);
102	✓✗	2	J_RF.setZero();
103	✓✗✓✗	2	getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF);
104	✓✗	4	Data::Matrix6x J_LF(6, model.nv);
105	✓✗	2	J_LF.setZero();
106	✓✗✓✗	2	getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);
107
108	✓✗	4	Eigen::MatrixXd J(12, model.nv);
109	✓✗	2	J.setZero();
110	✓✗✓✗	2	J.topRows<6>() = J_RF;
111	✓✗✓✗	2	J.bottomRows<6>() = J_LF;
112
113			//Check Forward Dynamics
114	✓✗	2	pinocchio::crba(model,data_ref,q);
115	✓✗✓✗ ✓✗✓✗	2	data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
116
117	✓✗	4	Eigen::MatrixXd MJtJ(model.nv+12, model.nv+12);
118	✓✗✓✗ ✓✗	2	MJtJ << data_ref.M, J.transpose(),
119	✓✗✓✗ ✓✗	2	J, Eigen::MatrixXd::Zero(12, 12);
120
121	✓✗	4	Eigen::MatrixXd KKTMatrix_inv(model.nv+12, model.nv+12);
122	✓✗	2	computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv);
123
124	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
125		2	}
126
127	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_getKKTMatrix)
128			{
129			using namespace Eigen;
130			using namespace pinocchio;
131	✓✗	4	pinocchio::Model model;
132	✓✗	2	pinocchio::buildModels::humanoidRandom(model,true);
133	✓✗	4	pinocchio::Data data(model);
134
135	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
136	✓✗✓✗	2	q.segment <4> (3).normalize();
137
138	✓✗	2	pinocchio::computeJointJacobians(model, data, q);
139
140	✓✗✓✗	4	VectorXd v = VectorXd::Ones(model.nv);
141	✓✗✓✗	4	VectorXd tau = VectorXd::Zero(model.nv);
142
143	✓✗	4	const std::string RF = "rleg6_joint";
144	✓✗	4	const std::string LF = "lleg6_joint";
145
146	✓✗	4	Data::Matrix6x J_RF (6, model.nv);
147	✓✗	2	J_RF.setZero();
148	✓✗✓✗	2	getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
149	✓✗	4	Data::Matrix6x J_LF (6, model.nv);
150	✓✗	2	J_LF.setZero();
151	✓✗✓✗	2	getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
152
153	✓✗	4	Eigen::MatrixXd J (12, model.nv);
154	✓✗	2	J.setZero();
155	✓✗✓✗	2	J.topRows<6> () = J_RF;
156	✓✗✓✗	2	J.bottomRows<6> () = J_LF;
157
158	✓✗✓✗	4	Eigen::VectorXd gamma (VectorXd::Ones(12));
159
160	✓✗✓✗	4	Eigen::MatrixXd H(J.transpose());
161
162			//Check Forward Dynamics
163	✓✗	2	pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
164	✓✗✓✗ ✓✗✓✗	2	data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
165
166	✓✗	4	Eigen::MatrixXd MJtJ(model.nv+12, model.nv+12);
167	✓✗✓✗ ✓✗	2	MJtJ << data.M, J.transpose(),
168	✓✗✓✗ ✓✗	2	J, Eigen::MatrixXd::Zero(12, 12);
169
170	✓✗	4	Eigen::MatrixXd KKTMatrix_inv(model.nv+12, model.nv+12);
171	✓✗	2	getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
172
173	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
174
175			//Check Impulse Dynamics
176		2	const double r_coeff = 1.;
177	✓✗✓✗	4	VectorXd v_before = VectorXd::Ones(model.nv);
178	✓✗	2	pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
179	✓✗✓✗ ✓✗✓✗	2	data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
180	✓✗✓✗ ✓✗	2	MJtJ << data.M, J.transpose(),
181	✓✗✓✗ ✓✗	2	J, Eigen::MatrixXd::Zero(12, 12);
182
183	✓✗	2	getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
184
185	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
186		2	}
187
188	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_FD_with_damping )
189			{
190			using namespace Eigen;
191			using namespace pinocchio;
192
193	✓✗	4	pinocchio::Model model;
194	✓✗	2	pinocchio::buildModels::humanoidRandom(model,true);
195	✓✗	4	pinocchio::Data data(model);
196
197	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
198	✓✗✓✗	2	q.segment <4> (3).normalize();
199
200	✓✗	2	pinocchio::computeJointJacobians(model, data, q);
201
202	✓✗✓✗	4	VectorXd v = VectorXd::Ones(model.nv);
203	✓✗✓✗	4	VectorXd tau = VectorXd::Zero(model.nv);
204
205	✓✗	4	const std::string RF = "rleg6_joint";
206
207	✓✗	4	Data::Matrix6x J_RF (6, model.nv);
208	✓✗	2	J_RF.setZero();
209	✓✗✓✗	2	getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
210
211	✓✗	4	Eigen::MatrixXd J (12, model.nv);
212	✓✗	2	J.setZero();
213	✓✗✓✗	2	J.topRows<6> () = J_RF;
214	✓✗✓✗	2	J.bottomRows<6> () = J_RF;
215
216	✓✗✓✗	4	Eigen::VectorXd gamma (VectorXd::Ones(12));
217
218			// Forward Dynamics with damping
219	✓✗	2	pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12);
220
221			// Matrix Definitions
222	✓✗✓✗	4	Eigen::MatrixXd H(J.transpose());
223	✓✗	2	data.M.triangularView<Eigen::StrictlyLower>() =
224	✓✗✓✗ ✓✗	4	data.M.transpose().triangularView<Eigen::StrictlyLower>();
225
226	✓✗✓✗	4	MatrixXd Minv (data.M.inverse());
227	✓✗✓✗ ✓✗✓✗	4	MatrixXd JMinvJt (J * Minv * J.transpose());
228
229			// Check that JMinvJt is correctly formed
230	✓✗✓✗	4	Eigen::MatrixXd G_ref(J.transpose());
231	✓✗	2	cholesky::Uiv(model, data, G_ref);
232	✓✓✓✗ ✓✗✓✗	66	for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
233	✓✗✓✗ ✓✗	4	Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
234	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));
235
236			// Actual Residuals
237	✓✗✓✗ ✓✗	4	Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
238	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
239	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(constraint_residual.norm() <= 1e-9);
240	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
241		2	}
242
243	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_ID )
244			{
245			using namespace Eigen;
246			using namespace pinocchio;
247
248	✓✗	4	pinocchio::Model model;
249	✓✗	2	pinocchio::buildModels::humanoidRandom(model,true);
250	✓✗	4	pinocchio::Data data(model);
251
252	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
253	✓✗✓✗	2	q.segment <4> (3).normalize();
254
255	✓✗	2	pinocchio::computeJointJacobians(model, data, q);
256
257	✓✗✓✗	4	VectorXd v_before = VectorXd::Ones(model.nv);
258
259	✓✗	4	const std::string RF = "rleg6_joint";
260	✓✗	4	const std::string LF = "lleg6_joint";
261
262	✓✗	4	Data::Matrix6x J_RF (6, model.nv);
263	✓✗	2	J_RF.setZero();
264	✓✗✓✗	2	getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
265	✓✗	4	Data::Matrix6x J_LF (6, model.nv);
266	✓✗	2	J_LF.setZero();
267	✓✗✓✗	2	getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
268
269	✓✗	4	Eigen::MatrixXd J (12, model.nv);
270	✓✗	2	J.setZero();
271	✓✗✓✗	2	J.topRows<6> () = J_RF;
272	✓✗✓✗	2	J.bottomRows<6> () = J_LF;
273
274		2	const double r_coeff = 1.;
275
276	✓✗✓✗	4	Eigen::MatrixXd H(J.transpose());
277
278	✓✗	2	pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
279	✓✗✓✗ ✓✗✓✗	2	data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
280
281	✓✗✓✗	4	MatrixXd Minv (data.M.inverse());
282	✓✗✓✗ ✓✗✓✗	4	MatrixXd JMinvJt (J * Minv * J.transpose());
283
284	✓✗✓✗	4	Eigen::MatrixXd G_ref(J.transpose());
285	✓✗	2	cholesky::Uiv(model, data, G_ref);
286	✓✓✓✗ ✓✗✓✗	66	for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
287	✓✗✓✗ ✓✗	4	Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
288	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));
289
290	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	VectorXd lambda_ref = JMinvJt.inverse() * (-r_coeff * J * v_before - J * v_before);
291	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.impulse_c.isApprox(lambda_ref, 1e-12));
292
293	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	4	VectorXd v_after_ref = Minv(data.M v_before + J.transpose()*lambda_ref);
294
295	✓✗✓✗ ✓✗✓✗ ✓✗	4	Eigen::VectorXd constraint_residual (J * data.dq_after + r_coeff * J * v_before);
296	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(constraint_residual.norm() <= 1e-12);
297
298	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	Eigen::VectorXd dynamics_residual (data.M * data.dq_after - data.M * v_before - J.transpose()*data.impulse_c);
299	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
300
301		2	}
302
303	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE (timings_fd_llt)
304			{
305			using namespace Eigen;
306			using namespace pinocchio;
307
308	✓✗	4	pinocchio::Model model;
309	✓✗	2	pinocchio::buildModels::humanoidRandom(model,true);
310	✓✗	4	pinocchio::Data data(model);
311
312			#ifdef NDEBUG
313			#ifdef _INTENSE_TESTING_
314			const size_t NBT = 1000*1000;
315			#else
316			const size_t NBT = 100;
317			#endif
318
319			#else
320		2	const size_t NBT = 1;
321	✓✗	2	std::cout << "(the time score in debug mode is not relevant) " ;
322			#endif // ifndef NDEBUG
323
324	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
325	✓✗✓✗	2	q.segment <4> (3).normalize();
326
327	✓✗	2	pinocchio::computeJointJacobians(model, data, q);
328
329	✓✗✓✗	4	VectorXd v = VectorXd::Ones(model.nv);
330	✓✗✓✗	4	VectorXd tau = VectorXd::Zero(model.nv);
331
332	✓✗	4	const std::string RF = "rleg6_joint";
333	✓✗	4	const std::string LF = "lleg6_joint";
334
335	✓✗	4	Data::Matrix6x J_RF (6, model.nv);
336	✓✗	2	J_RF.setZero();
337	✓✗✓✗	2	getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
338	✓✗	4	Data::Matrix6x J_LF (6, model.nv);
339	✓✗	2	J_LF.setZero();
340	✓✗✓✗	2	getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
341
342	✓✗	4	Eigen::MatrixXd J (12, model.nv);
343	✓✗✓✗	2	J.topRows<6> () = J_RF;
344	✓✗✓✗	2	J.bottomRows<6> () = J_LF;
345
346	✓✗✓✗	4	Eigen::VectorXd gamma (VectorXd::Ones(12));
347
348	✓✗✓✗	2	model.lowerPositionLimit.head<7>().fill(-1.);
349	✓✗✓✗	2	model.upperPositionLimit.head<7>().fill( 1.);
350
351	✓✗	2	q = pinocchio::randomConfiguration(model);
352
353	✓✗✓✗	4	PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
354	✓✓	4	SMOOTH(NBT)
355			{
356	✓✗	2	pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
357			}
358	✓✗	2	timer.toc(std::cout,NBT);
359
360		2	}
361
362			BOOST_AUTO_TEST_SUITE_END ()