GCC Code Coverage Report

Directory:	./
File:	unittest/impulse-dynamics.cpp
Date:	2024-08-27 18:20:05

	Exec	Total	Coverage
Lines:	266	280	95.0%
Branches:	842	1703	49.4%

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2020 CNRS INRIA
3			//
4
5			#include "pinocchio/algorithm/aba.hpp"
6			#include "pinocchio/algorithm/rnea.hpp"
7			#include "pinocchio/algorithm/frames.hpp"
8			#include "pinocchio/algorithm/jacobian.hpp"
9			#include "pinocchio/algorithm/centroidal.hpp"
10			#include "pinocchio/algorithm/kinematics.hpp"
11			#include "pinocchio/algorithm/contact-info.hpp"
12			#include "pinocchio/algorithm/compute-all-terms.hpp"
13			#include "pinocchio/algorithm/impulse-dynamics.hpp"
14			#include "pinocchio/algorithm/contact-dynamics.hpp"
15			#include "pinocchio/algorithm/joint-configuration.hpp"
16			#include "pinocchio/multibody/sample-models.hpp"
17			#include "pinocchio/utils/timer.hpp"
18
19			#include <iostream>
20
21			#include <boost/test/unit_test.hpp>
22			#include <boost/utility/binary.hpp>
23
24			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
25
26	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_sparse_impulse_dynamics_empty)
27			{
28			using namespace Eigen;
29			using namespace pinocchio;
30
31	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
32	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
33	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);
34
35	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.lowerPositionLimit.head<3>().fill(-1.);
36	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.upperPositionLimit.head<3>().fill(1.);
37	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
38
39	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd v = VectorXd::Random(model.nv);
40	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd tau = VectorXd::Random(model.nv);
41
42			// Contact models and data
43	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
44	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
45
46		2	const double mu0 = 0.;
47	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ProximalSettings prox_settings(1e-12, mu0, 1);
48		2	const double r_coeff = 0.5;
49	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(model, data_ref, q, v);
50	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
51	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>();
52
53	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	initConstraintDynamics(model, data, contact_models);
54	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
55
56	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
57	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>();
58
59	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.J.isApprox(data_ref.J));
60	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.M.isApprox(data_ref.M));
61	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.dq_after.isApprox(v));
62		2	}
63
64	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_sparse_impulse_dynamics_in_contact_6D_LOCAL)
65			{
66			using namespace Eigen;
67			using namespace pinocchio;
68
69	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
70	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
71	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);
72
73	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.lowerPositionLimit.head<3>().fill(-1.);
74	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.upperPositionLimit.head<3>().fill(1.);
75	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
76
77	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd v = VectorXd::Random(model.nv);
78	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd tau = VectorXd::Random(model.nv);
79
80	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
81			// const Model::JointIndex RF_id = model.getJointId(RF);
82	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
83			// const Model::JointIndex LF_id = model.getJointId(LF);
84
85			// Contact models and data
86	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
87	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
88	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL);
89	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RF);
90	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	contact_datas.push_back(RigidConstraintData(ci_RF));
91	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL);
92	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LF);
93	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	contact_datas.push_back(RigidConstraintData(ci_LF));
94
95		2	Eigen::DenseIndex constraint_dim = 0;
96	2/2 ✓ Branch 1 taken 2 times. ✓ Branch 2 taken 1 times.	6	for (size_t k = 0; k < contact_models.size(); ++k)
97		4	constraint_dim += contact_models[k].size();
98
99		2	const double mu0 = 0.;
100	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ProximalSettings prox_settings(1e-12, mu0, 1);
101		2	const double r_coeff = 0.5;
102	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::MatrixXd J_ref(constraint_dim, model.nv);
103	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_ref.setZero();
104
105	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(model, data_ref, q, v);
106	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	framesForwardKinematics(model, data_ref, q);
107	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
108	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>();
109
110	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	updateFramePlacements(model, data_ref);
111	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	getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_ref.middleRows<6>(0));
112	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	getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_ref.middleRows<6>(6));
113
114	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::VectorXd rhs_ref(constraint_dim);
115
116	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	rhs_ref.segment<6>(0) = getFrameVelocity(model, data_ref, model.getFrameId(RF), LOCAL).toVector();
117	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	rhs_ref.segment<6>(6) = getFrameVelocity(model, data_ref, model.getFrameId(LF), LOCAL).toVector();
118
119			Eigen::MatrixXd KKT_matrix_ref =
120	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd::Zero(model.nv + constraint_dim, model.nv + constraint_dim);
121	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	KKT_matrix_ref.bottomRightCorner(model.nv, model.nv) = data_ref.M;
122	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	KKT_matrix_ref.topRightCorner(constraint_dim, model.nv) = J_ref;
123	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	KKT_matrix_ref.bottomLeftCorner(model.nv, constraint_dim) = J_ref.transpose();
124
125			PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
126			PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
127	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	impulseDynamics(model, data_ref, q, v, J_ref, r_coeff, mu0);
128			PINOCCHIO_COMPILER_DIAGNOSTIC_POP
129
130	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
131	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>();
132	13/26 ✓ 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 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken. ✓ Branch 29 taken 1 times. ✗ Branch 30 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 39 taken 1 times. ✗ Branch 40 not taken. ✗ Branch 44 not taken. ✓ Branch 45 taken 1 times.	2	BOOST_CHECK(
133			(data_ref.M * data_ref.dq_after - data_ref.M * v - J_ref.transpose() * data_ref.impulse_c)
134			.isZero());
135	11/22 ✓ 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 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 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK((J_ref * data_ref.dq_after + r_coeff * J_ref * v).isZero());
136
137	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	initConstraintDynamics(model, data, contact_models);
138	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
139	11/22 ✓ 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 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 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK((J_ref * data.dq_after + r_coeff * J_ref * v).isZero());
140	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
141	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>();
142	13/26 ✓ 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 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken. ✓ Branch 29 taken 1 times. ✗ Branch 30 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 39 taken 1 times. ✗ Branch 40 not taken. ✗ Branch 44 not taken. ✓ Branch 45 taken 1 times.	2	BOOST_CHECK((data.M * data.dq_after - data.M * v - J_ref.transpose() * data.impulse_c).isZero());
143
144	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data_ag(model);
145	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ccrba(model, data_ag, q, v);
146	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.J.isApprox(data_ref.J));
147	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.M.isApprox(data_ref.M));
148	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.Ag.isApprox(data_ag.Ag));
149
150	2/2 ✓ Branch 1 taken 27 times. ✓ Branch 2 taken 1 times.	56	for (Model::JointIndex k = 1; k < model.joints.size(); ++k)
151			{
152	7/14 ✓ Branch 1 taken 27 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 27 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 27 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 27 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 27 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 27 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 27 times.	54	BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
153	7/14 ✓ Branch 1 taken 27 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 27 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 27 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 27 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 27 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 27 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 27 times.	54	BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k]));
154	8/16 ✓ Branch 1 taken 27 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 27 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 27 times. ✗ Branch 12 not taken. ✓ Branch 18 taken 27 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 27 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 27 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 27 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 27 times.	54	BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k])));
155			// BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k])));
156			}
157
158			// Check that the decomposition is correct
159		2	const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
160	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
161
162	9/18 ✓ 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 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 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv, model.nv)
163			.isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv)));
164	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
165
166			// Check solutions
167	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after));
168	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c));
169
170		2	Eigen::DenseIndex constraint_id = 0;
171	2/2 ✓ Branch 1 taken 2 times. ✓ Branch 2 taken 1 times.	6	for (size_t k = 0; k < contact_models.size(); ++k)
172			{
173		4	const RigidConstraintModel & cmodel = contact_models[k];
174		4	const RigidConstraintData & cdata = contact_datas[k];
175
176	1/3 ✗ Branch 0 not taken. ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	4	switch (cmodel.type)
177			{
178		✗	case pinocchio::CONTACT_3D: {
179		✗	BOOST_CHECK(cdata.contact_force.linear().isApprox(
180			data_ref.impulse_c.segment(constraint_id, cmodel.size())));
181		✗	break;
182			}
183
184		4	case pinocchio::CONTACT_6D: {
185			ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref(
186	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	4	data_ref.impulse_c.segment<6>(constraint_id));
187	7/14 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 2 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 2 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 2 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 2 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 2 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 2 times.	4	BOOST_CHECK(cdata.contact_force.isApprox(f_ref));
188		4	break;
189			}
190
191		✗	default:
192		✗	break;
193			}
194
195		4	constraint_id += cmodel.size();
196			}
197		2	}
198
199	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_sparse_impulse_dynamics_in_contact_6D_LOCAL_WORLD_ALIGNED)
200			{
201			using namespace Eigen;
202			using namespace pinocchio;
203
204	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
205	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
206	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);
207
208	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.lowerPositionLimit.head<3>().fill(-1.);
209	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.upperPositionLimit.head<3>().fill(1.);
210	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
211
212	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd v = VectorXd::Random(model.nv);
213	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd tau = VectorXd::Random(model.nv);
214
215	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
216			// const Model::JointIndex RF_id = model.getJointId(RF);
217	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
218			// const Model::JointIndex LF_id = model.getJointId(LF);
219
220			// Contact models and data
221	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
222	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
223	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL_WORLD_ALIGNED);
224	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RF);
225	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	contact_datas.push_back(RigidConstraintData(ci_RF));
226	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL_WORLD_ALIGNED);
227	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LF);
228	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	contact_datas.push_back(RigidConstraintData(ci_LF));
229
230		2	Eigen::DenseIndex constraint_dim = 0;
231	2/2 ✓ Branch 1 taken 2 times. ✓ Branch 2 taken 1 times.	6	for (size_t k = 0; k < contact_models.size(); ++k)
232		4	constraint_dim += contact_models[k].size();
233
234		2	const double mu0 = 0.;
235	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ProximalSettings prox_settings(1e-12, mu0, 1);
236		2	const double r_coeff = 0.5;
237	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::MatrixXd J_ref(constraint_dim, model.nv);
238	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_ref.setZero();
239
240	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(model, data_ref, q, v);
241	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	framesForwardKinematics(model, data_ref, q);
242	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
243	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>();
244
245	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	updateFramePlacements(model, data_ref);
246	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	getJointJacobian(
247	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	model, data_ref, model.getJointId(RF), LOCAL_WORLD_ALIGNED, J_ref.middleRows<6>(0));
248	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	getJointJacobian(
249	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	model, data_ref, model.getJointId(LF), LOCAL_WORLD_ALIGNED, J_ref.middleRows<6>(6));
250
251	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::VectorXd rhs_ref(constraint_dim);
252
253		✗	rhs_ref.segment<6>(0) =
254	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	getFrameVelocity(model, data_ref, model.getFrameId(RF), LOCAL_WORLD_ALIGNED).toVector();
255		✗	rhs_ref.segment<6>(6) =
256	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	getFrameVelocity(model, data_ref, model.getFrameId(LF), LOCAL_WORLD_ALIGNED).toVector();
257
258			Eigen::MatrixXd KKT_matrix_ref =
259	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd::Zero(model.nv + constraint_dim, model.nv + constraint_dim);
260	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	KKT_matrix_ref.bottomRightCorner(model.nv, model.nv) = data_ref.M;
261	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	KKT_matrix_ref.topRightCorner(constraint_dim, model.nv) = J_ref;
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	KKT_matrix_ref.bottomLeftCorner(model.nv, constraint_dim) = J_ref.transpose();
263
264			PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
265			PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
266	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	impulseDynamics(model, data_ref, q, v, J_ref, r_coeff, mu0);
267			PINOCCHIO_COMPILER_DIAGNOSTIC_POP
268
269	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
270	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>();
271	13/26 ✓ 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 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken. ✓ Branch 29 taken 1 times. ✗ Branch 30 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 39 taken 1 times. ✗ Branch 40 not taken. ✗ Branch 44 not taken. ✓ Branch 45 taken 1 times.	2	BOOST_CHECK(
272			(data_ref.M * data_ref.dq_after - data_ref.M * v - J_ref.transpose() * data_ref.impulse_c)
273			.isZero());
274	11/22 ✓ 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 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 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK((J_ref * data_ref.dq_after + r_coeff * J_ref * v).isZero());
275
276	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	initConstraintDynamics(model, data, contact_models);
277	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
278	11/22 ✓ 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 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 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK((J_ref * data.dq_after + r_coeff * J_ref * v).isZero());
279	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
280	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>();
281	13/26 ✓ 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 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken. ✓ Branch 29 taken 1 times. ✗ Branch 30 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 39 taken 1 times. ✗ Branch 40 not taken. ✗ Branch 44 not taken. ✓ Branch 45 taken 1 times.	2	BOOST_CHECK((data.M * data.dq_after - data.M * v - J_ref.transpose() * data.impulse_c).isZero());
282
283	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data_ag(model);
284	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ccrba(model, data_ag, q, v);
285	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.J.isApprox(data_ref.J));
286	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.M.isApprox(data_ref.M));
287	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.Ag.isApprox(data_ag.Ag));
288
289	2/2 ✓ Branch 1 taken 27 times. ✓ Branch 2 taken 1 times.	56	for (Model::JointIndex k = 1; k < model.joints.size(); ++k)
290			{
291	7/14 ✓ Branch 1 taken 27 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 27 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 27 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 27 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 27 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 27 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 27 times.	54	BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
292	7/14 ✓ Branch 1 taken 27 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 27 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 27 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 27 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 27 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 27 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 27 times.	54	BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k]));
293	8/16 ✓ Branch 1 taken 27 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 27 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 27 times. ✗ Branch 12 not taken. ✓ Branch 18 taken 27 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 27 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 27 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 27 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 27 times.	54	BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k])));
294			// BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k])));
295			}
296
297			// Check that the decomposition is correct
298		2	const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
299	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
300
301	9/18 ✓ 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 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 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv, model.nv)
302			.isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv)));
303	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
304
305			// Check solutions
306	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after));
307	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c));
308
309		2	Eigen::DenseIndex constraint_id = 0;
310	2/2 ✓ Branch 1 taken 2 times. ✓ Branch 2 taken 1 times.	6	for (size_t k = 0; k < contact_models.size(); ++k)
311			{
312		4	const RigidConstraintModel & cmodel = contact_models[k];
313		4	const RigidConstraintData & cdata = contact_datas[k];
314
315	1/3 ✗ Branch 0 not taken. ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	4	switch (cmodel.type)
316			{
317		✗	case pinocchio::CONTACT_3D: {
318		✗	BOOST_CHECK(cdata.contact_force.linear().isApprox(
319			data_ref.impulse_c.segment(constraint_id, cmodel.size())));
320		✗	break;
321			}
322
323		4	case pinocchio::CONTACT_6D: {
324			ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref(
325	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	4	data_ref.impulse_c.segment<6>(constraint_id));
326	7/14 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 2 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 2 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 2 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 2 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 2 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 2 times.	4	BOOST_CHECK(cdata.contact_force.isApprox(f_ref));
327		4	break;
328			}
329
330		✗	default:
331		✗	break;
332			}
333
334		4	constraint_id += cmodel.size();
335			}
336		2	}
337
338	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_sparse_impulse_dynamics_in_contact_6D_3D)
339			{
340			using namespace Eigen;
341			using namespace pinocchio;
342
343	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
344	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
345	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);
346
347	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.lowerPositionLimit.head<3>().fill(-1.);
348	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.upperPositionLimit.head<3>().fill(1.);
349	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
350
351	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd v = VectorXd::Random(model.nv);
352	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd tau = VectorXd::Random(model.nv);
353
354	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
355			// const Model::JointIndex RF_id = model.getJointId(RF);
356	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
357			// const Model::JointIndex LF_id = model.getJointId(LF);
358
359			// Contact models and data
360	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
361	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
362	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL);
363	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RF);
364	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	contact_datas.push_back(RigidConstraintData(ci_RF));
365	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	RigidConstraintModel ci_LF(CONTACT_3D, model, model.getJointId(LF), LOCAL);
366	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LF);
367	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	contact_datas.push_back(RigidConstraintData(ci_LF));
368
369		2	Eigen::DenseIndex constraint_dim = 0;
370	2/2 ✓ Branch 1 taken 2 times. ✓ Branch 2 taken 1 times.	6	for (size_t k = 0; k < contact_models.size(); ++k)
371		4	constraint_dim += contact_models[k].size();
372
373		2	const double mu0 = 0.;
374	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ProximalSettings prox_settings(1e-12, mu0, 1);
375		2	const double r_coeff = 0.5;
376	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd J_ref(constraint_dim, model.nv), Jtmp(6, model.nv);
377	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_ref.setZero();
378	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Jtmp.setZero();
379
380	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(model, data_ref, q, v);
381	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	framesForwardKinematics(model, data_ref, q);
382	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
383	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>();
384
385	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	updateFramePlacements(model, data_ref);
386	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	getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_ref.middleRows<6>(0));
387	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, Jtmp);
388	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	J_ref.middleRows<3>(6) = Jtmp.middleRows<3>(Motion::LINEAR);
389
390	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::VectorXd rhs_ref(constraint_dim);
391
392	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	rhs_ref.segment<6>(0) = getFrameVelocity(model, data_ref, model.getFrameId(RF), LOCAL).toVector();
393	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	rhs_ref.segment<3>(6) = getFrameVelocity(model, data_ref, model.getFrameId(LF), LOCAL).linear();
394
395			Eigen::MatrixXd KKT_matrix_ref =
396	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd::Zero(model.nv + constraint_dim, model.nv + constraint_dim);
397	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	KKT_matrix_ref.bottomRightCorner(model.nv, model.nv) = data_ref.M;
398	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	KKT_matrix_ref.topRightCorner(constraint_dim, model.nv) = J_ref;
399	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	KKT_matrix_ref.bottomLeftCorner(model.nv, constraint_dim) = J_ref.transpose();
400
401			PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
402			PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
403	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	impulseDynamics(model, data_ref, q, v, J_ref, r_coeff, mu0);
404			PINOCCHIO_COMPILER_DIAGNOSTIC_POP
405
406	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
407	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>();
408	13/26 ✓ 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 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken. ✓ Branch 29 taken 1 times. ✗ Branch 30 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 39 taken 1 times. ✗ Branch 40 not taken. ✗ Branch 44 not taken. ✓ Branch 45 taken 1 times.	2	BOOST_CHECK(
409			(data_ref.M * data_ref.dq_after - data_ref.M * v - J_ref.transpose() * data_ref.impulse_c)
410			.isZero());
411	11/22 ✓ 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 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 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK((J_ref * data_ref.dq_after + r_coeff * J_ref * v).isZero());
412
413	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	initConstraintDynamics(model, data, contact_models);
414	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
415	11/22 ✓ 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 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 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK((J_ref * data.dq_after + r_coeff * J_ref * v).isZero());
416	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
417	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>();
418	13/26 ✓ 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 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken. ✓ Branch 29 taken 1 times. ✗ Branch 30 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 39 taken 1 times. ✗ Branch 40 not taken. ✗ Branch 44 not taken. ✓ Branch 45 taken 1 times.	2	BOOST_CHECK((data.M * data.dq_after - data.M * v - J_ref.transpose() * data.impulse_c).isZero());
419
420	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data_ag(model);
421	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ccrba(model, data_ag, q, v);
422	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.J.isApprox(data_ref.J));
423	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.M.isApprox(data_ref.M));
424	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.Ag.isApprox(data_ag.Ag));
425
426	2/2 ✓ Branch 1 taken 27 times. ✓ Branch 2 taken 1 times.	56	for (Model::JointIndex k = 1; k < model.joints.size(); ++k)
427			{
428	7/14 ✓ Branch 1 taken 27 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 27 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 27 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 27 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 27 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 27 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 27 times.	54	BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
429	7/14 ✓ Branch 1 taken 27 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 27 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 27 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 27 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 27 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 27 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 27 times.	54	BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k]));
430	8/16 ✓ Branch 1 taken 27 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 27 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 27 times. ✗ Branch 12 not taken. ✓ Branch 18 taken 27 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 27 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 27 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 27 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 27 times.	54	BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k])));
431			// BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k])));
432			}
433
434			// Check that the decomposition is correct
435		2	const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
436	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
437
438	9/18 ✓ 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 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 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv, model.nv)
439			.isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv)));
440	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
441
442			// Check solutions
443	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after));
444	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c));
445
446		2	Eigen::DenseIndex constraint_id = 0;
447	2/2 ✓ Branch 1 taken 2 times. ✓ Branch 2 taken 1 times.	6	for (size_t k = 0; k < contact_models.size(); ++k)
448			{
449		4	const RigidConstraintModel & cmodel = contact_models[k];
450		4	const RigidConstraintData & cdata = contact_datas[k];
451
452	2/3 ✓ Branch 0 taken 1 times. ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	4	switch (cmodel.type)
453			{
454		2	case pinocchio::CONTACT_3D: {
455	9/18 ✓ 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 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 1 times. ✗ Branch 23 not taken. ✓ Branch 25 taken 1 times. ✗ Branch 26 not taken. ✓ Branch 28 taken 1 times. ✗ Branch 29 not taken. ✗ Branch 33 not taken. ✓ Branch 34 taken 1 times.	2	BOOST_CHECK(cdata.contact_force.linear().isApprox(
456			data_ref.impulse_c.segment(constraint_id, cmodel.size())));
457		2	break;
458			}
459
460		2	case pinocchio::CONTACT_6D: {
461			ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref(
462	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	data_ref.impulse_c.segment<6>(constraint_id));
463	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 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 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(cdata.contact_force.isApprox(f_ref));
464		2	break;
465			}
466
467		✗	default:
468		✗	break;
469			}
470
471		4	constraint_id += cmodel.size();
472			}
473		2	}
474
475			BOOST_AUTO_TEST_SUITE_END()
476