GCC Code Coverage Report

Directory:	./
File:	unittest/contact-dynamics.cpp
Date:	2025-02-12 21:03:38

	Exec	Total	Coverage
Lines:	220	220	100.0%
Branches:	665	1322	50.3%

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