GCC Code Coverage Report

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

	Exec	Total	Coverage
Lines:	956	956	100.0%
Branches:	2901	5788	50.1%

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2019-2022 INRIA
3			//
4
5			#include <iostream>
6
7			#include "pinocchio/algorithm/jacobian.hpp"
8			#include "pinocchio/algorithm/cholesky.hpp"
9			#include "pinocchio/algorithm/contact-info.hpp"
10			#include "pinocchio/algorithm/crba.hpp"
11			#include "pinocchio/algorithm/contact-cholesky.hxx"
12			#include "pinocchio/algorithm/joint-configuration.hpp"
13			#include "pinocchio/multibody/sample-models.hpp"
14
15			#include <boost/test/unit_test.hpp>
16			#include <boost/utility/binary.hpp>
17
18			namespace pinocchio
19			{
20			namespace cholesky
21			{
22			template<typename Scalar, int Options>
23			struct ContactCholeskyDecompositionAccessorTpl
24			: public ContactCholeskyDecompositionTpl<Scalar, Options>
25			{
26			typedef ContactCholeskyDecompositionTpl<Scalar, Options> Base;
27			typedef typename Base::IndexVector IndexVector;
28			typedef typename Base::BooleanVector BooleanVector;
29
30		3	ContactCholeskyDecompositionAccessorTpl(const Base & other)
31		3	: Base(other)
32			{
33		3	}
34
35		96	const IndexVector & getParents_fromRow() const
36			{
37		96	return this->parents_fromRow;
38			}
39		28	const IndexVector & getLastChild() const
40			{
41		28	return this->last_child;
42			}
43		32	const IndexVector & getNvSubtree_fromRow() const
44			{
45		32	return this->nv_subtree_fromRow;
46			}
47			const std::vector<BooleanVector> & getJoint1_indexes() const
48			{
49			return this->joint1_indexes;
50			}
51			const std::vector<BooleanVector> & getJoint2_indexes() const
52			{
53			return this->joint2_indexes;
54			}
55			};
56
57			typedef ContactCholeskyDecompositionAccessorTpl<double, 0> ContactCholeskyDecompositionAccessor;
58			} // namespace cholesky
59			} // namespace pinocchio
60
61			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
62
63	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(contact_operator_equal)
64			{
65
66			using namespace Eigen;
67			using namespace pinocchio;
68			using namespace cholesky;
69
70	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model humanoid_model;
71	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(humanoid_model);
72	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data humanoid_data(humanoid_model);
73
74	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model manipulator_model;
75	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::manipulator(manipulator_model);
76	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data manipulator_data(manipulator_model);
77
78	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
79	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty;
80
81	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	humanoid_model.lowerPositionLimit.head<3>().fill(-1.);
82	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	humanoid_model.upperPositionLimit.head<3>().fill(1.);
83	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd humanoid_q = randomConfiguration(humanoid_model);
84	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(humanoid_model, humanoid_data, humanoid_q, Convention::WORLD);
85
86	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd manipulator_q = randomConfiguration(manipulator_model);
87	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(manipulator_model, manipulator_data, manipulator_q, Convention::WORLD);
88
89	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	ContactCholeskyDecomposition humanoid_chol(humanoid_model), manipulator_chol(manipulator_model);
90	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	humanoid_chol.compute(humanoid_model, humanoid_data, contact_models_empty, contact_datas_empty);
91	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	manipulator_chol.compute(
92			manipulator_model, manipulator_data, contact_models_empty, contact_datas_empty);
93
94	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(humanoid_chol == humanoid_chol);
95	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(humanoid_chol != manipulator_chol);
96		2	}
97
98	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(contact_cholesky_simple)
99			{
100			using namespace Eigen;
101			using namespace pinocchio;
102			using namespace pinocchio::cholesky;
103
104	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
105	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
106	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Data data_ref(model);
107
108	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.);
109	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.);
110	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
111	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data_ref, q, Convention::WORLD);
112
113	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::decompose(model, data_ref);
114	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
115	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.triangularView<Eigen::StrictlyUpper>().transpose();
116
117	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition;
118	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
119	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty;
120	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.allocate(model, contact_models_empty);
121
122	6/12 ✓ 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 23 not taken. ✓ Branch 24 taken 1 times.	2	BOOST_CHECK(contact_chol_decomposition.D.size() == model.nv);
123	6/12 ✓ 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 23 not taken. ✓ Branch 24 taken 1 times.	2	BOOST_CHECK(contact_chol_decomposition.Dinv.size() == model.nv);
124	6/12 ✓ 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 23 not taken. ✓ Branch 24 taken 1 times.	2	BOOST_CHECK(contact_chol_decomposition.U.rows() == model.nv);
125	6/12 ✓ 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 23 not taken. ✓ Branch 24 taken 1 times.	2	BOOST_CHECK(contact_chol_decomposition.U.cols() == model.nv);
126	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(contact_chol_decomposition.size() == model.nv);
127	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 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 29 not taken. ✓ Branch 30 taken 1 times.	2	BOOST_CHECK(contact_chol_decomposition.U.diagonal().isOnes());
128
129	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
130	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data, q, Convention::WORLD);
131	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
132	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.triangularView<Eigen::StrictlyUpper>().transpose();
133
134	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.compute(model, data, contact_models_empty, contact_datas_empty);
135
136	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	data_ref.Minv = data_ref.M.inverse();
137	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
138	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getInverseMassMatrix(Minv_test);
139
140	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));
141	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(Minv_test.isApprox(data_ref.Minv));
142
143	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(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv)));
144	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(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv)));
145	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(
146			data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)));
147
148	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecompositionAccessor access(contact_chol_decomposition);
149	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	66	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
150			{
151	7/14 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 32 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 32 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 32 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 32 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 32 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 32 times.	64	BOOST_CHECK(access.getParents_fromRow()[k] == data.parents_fromRow[(size_t)k]);
152			}
153
154	2/2 ✓ Branch 0 taken 28 times. ✓ Branch 1 taken 1 times.	58	for (Eigen::DenseIndex k = 0; k < model.njoints; ++k)
155			{
156	7/14 ✓ Branch 1 taken 28 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 28 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 28 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 28 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 28 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 28 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 28 times.	56	BOOST_CHECK(access.getLastChild()[k] == data.lastChild[(size_t)k]);
157			}
158
159	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	66	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
160			{
161	7/14 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 32 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 32 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 32 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 32 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 32 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 32 times.	64	BOOST_CHECK(access.getNvSubtree_fromRow()[k] == data.nvSubtree_fromRow[(size_t)k]);
162			}
163
164			// Test basic operation
165	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd v_in(VectorXd::Random(model.nv));
166	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	MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20));
167
168			// Test Uv
169	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Uv_op_res(v_in), Uv_op_ref(v_in);
170
171	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uv(Uv_op_res);
172	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::Uv(model, data_ref, Uv_op_ref);
173
174	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(Uv_op_res.isApprox(Uv_op_ref));
175
176	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in);
177
178	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uv(Uv_mat_op_res);
179	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::Uv(model, data_ref, Uv_mat_op_ref);
180
181	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(Uv_mat_op_res.isApprox(Uv_mat_op_ref));
182
183			// Test Utv
184	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Utv_op_res(v_in), Utv_op_ref(v_in);
185
186	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utv(Utv_op_res);
187	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::Utv(model, data_ref, Utv_op_ref);
188
189	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(Utv_op_res.isApprox(Utv_op_ref));
190
191	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in);
192
193	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utv(Utv_mat_op_res);
194	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::Utv(model, data_ref, Utv_mat_op_ref);
195
196	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(Utv_mat_op_res.isApprox(Utv_mat_op_ref));
197
198			// Test Uiv
199	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in);
200
201	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uiv(Uiv_op_res);
202	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::Uiv(model, data_ref, Uiv_op_ref);
203
204	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(Uiv_op_res.isApprox(Uiv_op_ref));
205
206	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in);
207
208	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uiv(Uiv_mat_op_res);
209	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::Uiv(model, data_ref, Uiv_mat_op_ref);
210
211	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(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref));
212
213			// Test Utiv
214	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in);
215
216	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utiv(Utiv_op_res);
217	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::Utiv(model, data_ref, Utiv_op_ref);
218
219	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(Utiv_op_res.isApprox(Utiv_op_ref));
220
221	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in);
222
223	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utiv(Utiv_mat_op_res);
224	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::Utiv(model, data_ref, Utiv_mat_op_ref);
225
226	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(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref));
227
228			// SolveInPlace
229	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd sol(v_in);
230	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.solveInPlace(sol);
231
232	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	VectorXd sol_ref(data.M.inverse() * v_in);
233
234	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(sol.isApprox(sol_ref));
235
236	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd sol_mat(mat_in);
237	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.solveInPlace(sol_mat);
238
239	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	MatrixXd sol_mat_ref(data.M.inverse() * mat_in);
240
241	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(sol_mat.isApprox(sol_mat_ref));
242
243			// solve
244	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in);
245	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(sol_copy_mat.isApprox(sol_mat));
246
247			// inverse
248	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd M_inv(model.nv, model.nv);
249	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.inverse(M_inv);
250
251	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd M_inv_ref = data.M.inverse();
252	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(M_inv.isApprox(M_inv_ref));
253
254			// test retrieve Mass Matrix Cholesky Decomposition
255			ContactCholeskyDecomposition mass_matrix_chol =
256	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getMassMatrixChoeslkyDecomposition(model);
257
258			// test Operational Space Inertia Matrix
259	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
260	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
261	6/12 ✓ 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 23 not taken. ✓ Branch 24 taken 1 times.	2	BOOST_CHECK(iosim.size() == 0);
262	6/12 ✓ 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 23 not taken. ✓ Branch 24 taken 1 times.	2	BOOST_CHECK(osim.size() == 0);
263
264	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(mass_matrix_chol == contact_chol_decomposition);
265	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(mass_matrix_chol.U.isApprox(data_ref.U));
266	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(mass_matrix_chol.D.isApprox(data_ref.D));
267	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(mass_matrix_chol.Dinv.isApprox(data_ref.Dinv));
268		2	}
269
270	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(contact_cholesky_contact6D_LOCAL)
271			{
272			using namespace Eigen;
273			using namespace pinocchio;
274			using namespace pinocchio::cholesky;
275
276	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
277	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
278	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Data data_ref(model);
279
280	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.);
281	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.);
282	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
283
284	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
285	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
286
287	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
288	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
289	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);
290	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RF);
291	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));
292	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);
293	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LF);
294	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));
295
296			// Compute Mass Matrix
297	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data_ref, q, Convention::WORLD);
298	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
299	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.triangularView<Eigen::StrictlyUpper>().transpose();
300
301			// Compute Cholesky decomposition
302	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::decompose(model, data_ref);
303
304			// Compute Jacobians
305	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv);
306	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RF.setZero();
307	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);
308	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LF.setZero();
309	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);
310
311		2	const int constraint_dim = 12;
312		2	const int total_dim = model.nv + constraint_dim;
313
314	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H(total_dim, total_dim);
315	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	H.setZero();
316	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
317	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	H.middleRows<6>(0).rightCols(model.nv) = J_RF;
318	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	H.middleRows<6>(6).rightCols(model.nv) = J_LF;
319
320	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	H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
321
322	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
323	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data, q, Convention::WORLD);
324	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition;
325	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.allocate(model, contact_models);
326
327	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecompositionAccessor access(contact_chol_decomposition);
328	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	66	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
329			{
330	2/2 ✓ Branch 1 taken 1 times. ✓ Branch 2 taken 31 times.	64	if (data.parents_fromRow[(size_t)k] == -1)
331	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(access.getParents_fromRow()[k + constraint_dim] == -1);
332			else
333	7/14 ✓ Branch 1 taken 31 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 31 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 31 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 31 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 31 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 31 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 31 times.	62	BOOST_CHECK(
334			access.getParents_fromRow()[k + constraint_dim]
335			== data.parents_fromRow[(size_t)k] + constraint_dim);
336			}
337
338	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.compute(model, data, contact_models, contact_datas);
339
340	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
341	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.triangularView<Eigen::StrictlyUpper>().transpose();
342
343	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(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv)));
344	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(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv)));
345	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(
346			data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)));
347
348			Data::MatrixXs M_recomposed =
349	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)
350	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	* contact_chol_decomposition.D.tail(model.nv).asDiagonal()
351	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.	4	* contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv).transpose();
352	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(M_recomposed.isApprox(data.M));
353
354			Data::MatrixXs H_recomposed = contact_chol_decomposition.U
355	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	* contact_chol_decomposition.D.asDiagonal()
356	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	* contact_chol_decomposition.U.transpose();
357
358	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 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 29 not taken. ✓ Branch 30 taken 1 times.	2	BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
359	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(H_recomposed.topRightCorner(constraint_dim, model.nv)
360			.isApprox(H.topRightCorner(constraint_dim, model.nv)));
361	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(H_recomposed.isApprox(H));
362
363			// test Operational Space Inertia Matrix
364			{
365	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 = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse()
366	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.	4	* H.middleRows<12>(0).rightCols(model.nv).transpose();
367	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
368	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
369	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(constraint_dim, model.nv));
370	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getJMinv(JMinv_test);
371	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	MatrixXd JMinv_ref = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse();
372	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(JMinv_ref.isApprox(JMinv_test));
373
374	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(iosim.isApprox(JMinvJt));
375	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(osim.isApprox(JMinvJt.inverse()));
376
377	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const MatrixXd rhs = MatrixXd::Random(12, 12);
378	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const MatrixXd res_delassus = contact_chol_decomposition.getDelassusCholeskyExpression() * rhs;
379	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const MatrixXd res_delassus_ref = iosim * rhs;
380
381	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(res_delassus_ref.isApprox(res_delassus));
382
383			const MatrixXd res_delassus_inverse =
384	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	contact_chol_decomposition.getDelassusCholeskyExpression().solve(rhs);
385	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const MatrixXd res_delassus_inverse_ref = osim * rhs;
386
387	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(res_delassus_inverse_ref.isApprox(res_delassus_inverse));
388		2	}
389
390			// test Mass matrix cholesky
391	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	data_ref.Minv = data_ref.M.inverse();
392	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
393	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getInverseMassMatrix(Minv_test);
394
395	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(Minv_test.isApprox(data_ref.Minv));
396
397	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition_mu;
398	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition_mu.allocate(model, contact_models);
399	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition_mu.compute(model, data, contact_models, contact_datas, 0.);
400
401	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(contact_chol_decomposition_mu.D.isApprox(contact_chol_decomposition.D));
402	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(contact_chol_decomposition_mu.Dinv.isApprox(contact_chol_decomposition.Dinv));
403	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(contact_chol_decomposition_mu.U.isApprox(contact_chol_decomposition.U));
404
405		2	const double mu = 0.1;
406	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition_mu.compute(model, data, contact_models, contact_datas, mu);
407	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H_mu(H);
408	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	H_mu.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu);
409
410			// test damped Operational Space Inertia Matrix
411			{
412	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_mu = H_mu.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse()
413	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.	4	* H_mu.middleRows<12>(0).rightCols(model.nv).transpose()
414	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.	4	+ mu * MatrixXd::Identity(12, 12);
415	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd iosim_mu = contact_chol_decomposition_mu.getInverseOperationalSpaceInertiaMatrix();
416	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd osim_mu = contact_chol_decomposition_mu.getOperationalSpaceInertiaMatrix();
417
418	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(iosim_mu.isApprox(JMinvJt_mu));
419	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(osim_mu.isApprox(JMinvJt_mu.inverse()));
420
421	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const MatrixXd rhs = MatrixXd::Random(12, 1);
422	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const MatrixXd res = contact_chol_decomposition_mu.getDelassusCholeskyExpression() * rhs;
423	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const MatrixXd res_ref = iosim_mu * rhs;
424
425	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(res_ref.isApprox(res));
426
427			const MatrixXd res_no_mu =
428	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	contact_chol_decomposition_mu.getDelassusCholeskyExpression() * rhs - mu * rhs;
429	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	4	const MatrixXd res_no_mu_ref = contact_chol_decomposition.getDelassusCholeskyExpression() * rhs;
430
431	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(res_no_mu.isApprox(res_no_mu_ref));
432		2	}
433
434			Data::MatrixXs H_recomposed_mu = contact_chol_decomposition_mu.U
435	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	* contact_chol_decomposition_mu.D.asDiagonal()
436	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	* contact_chol_decomposition_mu.U.transpose();
437
438	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(H_recomposed_mu.isApprox(H_mu));
439
440			// Test basic operation
441	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	VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size()));
442	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	MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20));
443
444			// Test Uv
445	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Uv_op_res(v_in), Uv_op_ref(v_in);
446
447	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uv(Uv_op_res);
448	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	Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in;
449
450	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(Uv_op_res.isApprox(Uv_op_ref));
451
452	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in);
453
454	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uv(Uv_mat_op_res);
455	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	Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in;
456
457	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(Uv_mat_op_res.isApprox(Uv_mat_op_ref));
458
459			// Test Utv
460	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Utv_op_res(v_in), Utv_op_ref(v_in);
461
462	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utv(Utv_op_res);
463	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	Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in;
464
465	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(Utv_op_res.isApprox(Utv_op_ref));
466
467	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in);
468
469	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utv(Utv_mat_op_res);
470	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	Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in;
471
472	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(Utv_mat_op_res.isApprox(Utv_mat_op_ref));
473
474			// Test Uiv
475	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in);
476
477	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uiv(Uiv_op_res);
478	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	Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in;
479
480	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(Uiv_op_res.isApprox(Uiv_op_ref));
481
482	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in);
483
484	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uiv(Uiv_mat_op_res);
485	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	Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in;
486
487	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(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref));
488
489			// Test Utiv
490	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in);
491
492	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utiv(Utiv_op_res);
493	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	Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in;
494
495	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(Utiv_op_res.isApprox(Utiv_op_ref));
496
497	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in);
498
499	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utiv(Utiv_mat_op_res);
500	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	Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in;
501
502	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(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref));
503
504			// SolveInPlace
505	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd sol(v_in);
506	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.solveInPlace(sol);
507
508	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	VectorXd sol_ref(H.inverse() * v_in);
509
510	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(sol.isApprox(sol_ref));
511
512	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd sol_mat(mat_in), sol_mat_ref(mat_in);
513
514	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.solveInPlace(sol_mat);
515	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	sol_mat_ref.noalias() = H.inverse() * mat_in;
516
517	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(sol_mat.isApprox(sol_mat_ref));
518
519			// solve
520	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in);
521	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(sol_copy_mat.isApprox(sol_mat));
522
523			// inverse
524	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	MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
525	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.inverse(H_inv);
526
527	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd H_inv_ref = H.inverse();
528	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(H_inv.isApprox(H_inv_ref));
529
530			// Check matrix
531	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd mat1;
532	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.matrix(mat1);
533	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(mat1.isApprox(H));
534
535	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd mat2(constraint_dim + model.nv, constraint_dim + model.nv);
536	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	contact_chol_decomposition.matrix(mat2.middleCols(0, constraint_dim + model.nv));
537	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(mat2.isApprox(H));
538
539	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd mat3 = contact_chol_decomposition.matrix();
540	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(mat3.isApprox(H));
541		2	}
542
543	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(contact_cholesky_contact3D_6D_LOCAL)
544			{
545			using namespace Eigen;
546			using namespace pinocchio;
547			using namespace pinocchio::cholesky;
548
549	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
550	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
551	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Data data_ref(model);
552
553	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.);
554	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.);
555	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
556
557	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
558	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
559	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RA = "rarm6_joint";
560	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LA = "larm6_joint";
561
562	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
563	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
564	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);
565	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RF);
566	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));
567	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);
568	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LF);
569	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));
570			// RigidConstraintModel ci_RA(CONTACT_3D,model.getJointId(RA),LOCAL);
571			// contact_models.push_back(ci_RA);
572			// contact_datas.push_back(RigidConstraintData(ci_RA));
573			// RigidConstraintModel ci_LA(CONTACT_3D,model.getJointId(LA),LOCAL_WORLD_ALIGNED);
574			// contact_models.push_back(ci_LA);
575			// contact_datas.push_back(RigidConstraintData(ci_LA));
576
577			// Compute Mass Matrix
578	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data_ref, q, Convention::WORLD);
579	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
580	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.triangularView<Eigen::StrictlyUpper>().transpose();
581
582			// Compute Cholesky decomposition
583	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::decompose(model, data_ref);
584
585			// Compute Jacobians
586	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	Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv), J_LA(6, model.nv);
587	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RF.setZero();
588	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);
589	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LF.setZero();
590	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);
591	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RA.setZero();
592	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(RA), LOCAL, J_RA);
593	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LA.setZero();
594	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(LA), LOCAL_WORLD_ALIGNED, J_LA);
595
596		2	const int constraint_dim = 9;
597		2	const int total_dim = model.nv + constraint_dim;
598
599	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H(total_dim, total_dim);
600	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	H.setZero();
601	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
602	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	H.middleRows<6>(0).rightCols(model.nv) = J_RF;
603	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	H.middleRows<3>(6).rightCols(model.nv) = J_LF.middleRows<3>(Motion::LINEAR);
604			// H.middleRows<3>(9).rightCols(model.nv) = J_RA.middleRows<3>(Motion::LINEAR);
605			// H.middleRows<3>(12).rightCols(model.nv) = J_LA.middleRows<3>(Motion::LINEAR);
606
607	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	H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
608
609	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
610	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data, q, Convention::WORLD);
611	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition;
612	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.allocate(model, contact_models);
613
614	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecompositionAccessor access(contact_chol_decomposition);
615	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	66	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
616			{
617	2/2 ✓ Branch 1 taken 1 times. ✓ Branch 2 taken 31 times.	64	if (data.parents_fromRow[(size_t)k] == -1)
618	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(access.getParents_fromRow()[k + constraint_dim] == -1);
619			else
620	7/14 ✓ Branch 1 taken 31 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 31 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 31 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 31 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 31 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 31 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 31 times.	62	BOOST_CHECK(
621			access.getParents_fromRow()[k + constraint_dim]
622			== data.parents_fromRow[(size_t)k] + constraint_dim);
623			}
624
625	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.compute(model, data, contact_models, contact_datas);
626
627	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
628	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.triangularView<Eigen::StrictlyUpper>().transpose();
629
630	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(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv)));
631	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(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv)));
632	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(
633			data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)));
634
635			Data::MatrixXs M_recomposed =
636	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)
637	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	* contact_chol_decomposition.D.tail(model.nv).asDiagonal()
638	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.	4	* contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv).transpose();
639	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(M_recomposed.isApprox(data.M));
640
641			Data::MatrixXs H_recomposed = contact_chol_decomposition.U
642	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	* contact_chol_decomposition.D.asDiagonal()
643	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	* contact_chol_decomposition.U.transpose();
644
645	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 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 29 not taken. ✓ Branch 30 taken 1 times.	2	BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
646	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(H_recomposed.topRightCorner(constraint_dim, model.nv)
647			.isApprox(H.topRightCorner(constraint_dim, model.nv)));
648	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(H_recomposed.isApprox(H));
649
650			// Test basic operation
651	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	VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size()));
652	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	MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20));
653
654			// Test Uv
655	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Uv_op_res(v_in), Uv_op_ref(v_in);
656
657	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uv(Uv_op_res);
658	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	Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in;
659
660	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(Uv_op_res.isApprox(Uv_op_ref));
661
662	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in);
663
664	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uv(Uv_mat_op_res);
665	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	Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in;
666
667	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(Uv_mat_op_res.isApprox(Uv_mat_op_ref));
668
669			// Test Utv
670	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Utv_op_res(v_in), Utv_op_ref(v_in);
671
672	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utv(Utv_op_res);
673	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	Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in;
674
675	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(Utv_op_res.isApprox(Utv_op_ref));
676
677	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in);
678
679	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utv(Utv_mat_op_res);
680	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	Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in;
681
682	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(Utv_mat_op_res.isApprox(Utv_mat_op_ref));
683
684			// Test Uiv
685	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in);
686
687	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uiv(Uiv_op_res);
688	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	Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in;
689
690	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(Uiv_op_res.isApprox(Uiv_op_ref));
691
692	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in);
693
694	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uiv(Uiv_mat_op_res);
695	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	Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in;
696
697	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(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref));
698
699			// Test Utiv
700	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in);
701
702	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utiv(Utiv_op_res);
703	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	Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in;
704
705	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(Utiv_op_res.isApprox(Utiv_op_ref));
706
707	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in);
708
709	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utiv(Utiv_mat_op_res);
710	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	Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in;
711
712	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(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref));
713
714			// SolveInPlace
715	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd sol(v_in);
716	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.solveInPlace(sol);
717
718	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	VectorXd sol_ref(H.inverse() * v_in);
719
720	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(sol.isApprox(sol_ref));
721
722	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd sol_mat(mat_in), sol_mat_ref(mat_in);
723
724	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.solveInPlace(sol_mat);
725	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	sol_mat_ref.noalias() = H.inverse() * mat_in;
726
727	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(sol_mat.isApprox(sol_mat_ref));
728
729			// solve
730	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in);
731	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(sol_copy_mat.isApprox(sol_mat));
732
733			// inverse
734	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	MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
735	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.inverse(H_inv);
736
737	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd H_inv_ref = H.inverse();
738	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(H_inv.isApprox(H_inv_ref));
739
740			// Check matrix
741	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd mat1;
742	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.matrix(mat1);
743	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(mat1.isApprox(H));
744
745	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd mat2(constraint_dim + model.nv, constraint_dim + model.nv);
746	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	contact_chol_decomposition.matrix(mat2.middleCols(0, constraint_dim + model.nv));
747	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(mat2.isApprox(H));
748
749	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd mat3 = contact_chol_decomposition.matrix();
750	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(mat3.isApprox(H));
751
752			// test Operational Space Inertia Matrix
753	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 = H.middleRows<9>(0).rightCols(model.nv) * data_ref.M.inverse()
754	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.	4	* H.middleRows<9>(0).rightCols(model.nv).transpose();
755	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
756	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
757
758	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(iosim.isApprox(JMinvJt));
759	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(osim.isApprox(JMinvJt.inverse()));
760
761			// test Mass matrix cholesky
762	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	data_ref.Minv = data_ref.M.inverse();
763	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
764	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getInverseMassMatrix(Minv_test);
765
766	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(9, model.nv));
767	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getJMinv(JMinv_test);
768	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	MatrixXd JMinv_ref = H.middleRows<9>(0).rightCols(model.nv) * data_ref.M.inverse();
769	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(JMinv_ref.isApprox(JMinv_test));
770
771	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(Minv_test.isApprox(data_ref.Minv));
772		2	}
773
774	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(contact_cholesky_contact6D_LOCAL_WORLD_ALIGNED)
775			{
776			using namespace Eigen;
777			using namespace pinocchio;
778			using namespace pinocchio::cholesky;
779
780	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
781	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
782	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Data data_ref(model);
783
784	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.);
785	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.);
786	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
787
788	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
789	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
790
791	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
792	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
793	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);
794	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RF);
795	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));
796	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);
797	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LF);
798	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));
799
800			// Compute Mass Matrix
801	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data_ref, q, Convention::WORLD);
802	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
803	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.triangularView<Eigen::StrictlyUpper>().transpose();
804
805			// Compute Cholesky decomposition
806	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::decompose(model, data_ref);
807
808			// Compute Jacobians
809	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv);
810	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RF.setZero();
811	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), ci_RF.reference_frame, J_RF);
812	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LF.setZero();
813	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), ci_LF.reference_frame, J_LF);
814
815		2	const int constraint_dim = 12;
816		2	const int total_dim = model.nv + constraint_dim;
817
818	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H(total_dim, total_dim);
819	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	H.setZero();
820	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
821	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	H.middleRows<6>(0).rightCols(model.nv) = J_RF;
822	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	H.middleRows<6>(6).rightCols(model.nv) = J_LF;
823
824	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	H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
825
826	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
827	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data, q, Convention::WORLD);
828	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition;
829	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.allocate(model, contact_models);
830	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.compute(model, data, contact_models, contact_datas);
831
832	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
833	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.triangularView<Eigen::StrictlyUpper>().transpose();
834
835	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix();
836
837	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 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 29 not taken. ✓ Branch 30 taken 1 times.	2	BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
838	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(H_recomposed.topRightCorner(constraint_dim, model.nv)
839			.isApprox(H.topRightCorner(constraint_dim, model.nv)));
840	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(H_recomposed.isApprox(H));
841
842			// inverse
843	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	MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
844	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.inverse(H_inv);
845
846	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd H_inv_ref = H_recomposed.inverse();
847	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(H_inv.isApprox(H_inv_ref));
848
849			// test Operational Space Inertia Matrix
850	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 = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse()
851	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.	4	* H.middleRows<12>(0).rightCols(model.nv).transpose();
852	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
853	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
854
855	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(iosim.isApprox(JMinvJt));
856	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(osim.isApprox(JMinvJt.inverse()));
857
858			// test Mass matrix cholesky
859	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	data_ref.Minv = data_ref.M.inverse();
860	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
861	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getInverseMassMatrix(Minv_test);
862
863	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(12, model.nv));
864	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getJMinv(JMinv_test);
865	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	MatrixXd JMinv_ref = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse();
866	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(JMinv_ref.isApprox(JMinv_test));
867
868	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(Minv_test.isApprox(data_ref.Minv));
869		2	}
870
871	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(contact_cholesky_contact6D_by_joint_2)
872			{
873			using namespace Eigen;
874			using namespace pinocchio;
875			using namespace pinocchio::cholesky;
876
877	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
878	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
879	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Data data_ref(model);
880
881	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.);
882	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.);
883	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
884
885	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
886	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
887	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RA = "rarm6_joint";
888	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LA = "larm6_joint";
889
890	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
891	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
892	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, 0, model.getJointId(RF), LOCAL);
893	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RF);
894	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));
895	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, 0, model.getJointId(LF), LOCAL);
896	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LF);
897	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));
898	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	RigidConstraintModel ci_RA(CONTACT_6D, model, 0, model.getJointId(RA), LOCAL_WORLD_ALIGNED);
899	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RA);
900	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_RA));
901	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	RigidConstraintModel ci_LA(CONTACT_6D, model, 0, model.getJointId(LA), LOCAL);
902	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LA);
903	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_LA));
904
905			// Compute Mass Matrix
906	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data_ref, q, Convention::WORLD);
907	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
908	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.triangularView<Eigen::StrictlyUpper>().transpose();
909
910			// Compute Cholesky decomposition
911	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::decompose(model, data_ref);
912
913			// Compute Jacobians
914	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	Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv), J_LA(6, model.nv);
915	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RF.setZero();
916	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), WORLD, J_RF);
917	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LF.setZero();
918	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), WORLD, J_LF);
919	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RA.setZero();
920	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(RA), WORLD, J_RA);
921	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LA.setZero();
922	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(LA), WORLD, J_LA);
923
924		2	const int constraint_dim = 24;
925		2	const int total_dim = model.nv + constraint_dim;
926
927	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	const SE3 oMRA_wla = SE3(SE3::Matrix3::Identity(), ci_RA.joint1_placement.translation());
928
929		2	const double mu = 0.1;
930	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H(total_dim, total_dim);
931	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	H.setZero();
932	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	H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu);
933	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
934	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	H.middleRows<6>(0).rightCols(model.nv) = -ci_RF.joint1_placement.toActionMatrixInverse() * J_RF;
935	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	H.middleRows<6>(6).rightCols(model.nv) = -ci_LF.joint1_placement.toActionMatrixInverse() * J_LF;
936	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	H.middleRows<6>(12).rightCols(model.nv) = -oMRA_wla.toActionMatrixInverse() * J_RA;
937	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	H.middleRows<6>(18).rightCols(model.nv) = -ci_LA.joint1_placement.toActionMatrixInverse() * J_LA;
938
939	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	H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
940
941	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
942	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data, q, Convention::WORLD);
943	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition;
944	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.allocate(model, contact_models);
945	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu);
946
947	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
948	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.triangularView<Eigen::StrictlyUpper>().transpose();
949
950	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix();
951
952	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 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 29 not taken. ✓ Branch 30 taken 1 times.	2	BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
953	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(H_recomposed.topRightCorner(constraint_dim, model.nv)
954			.isApprox(H.topRightCorner(constraint_dim, model.nv)));
955	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(H_recomposed.isApprox(H));
956
957	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	VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size()));
958	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	MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20));
959
960			// Test Uv
961	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Uv_op_res(v_in), Uv_op_ref(v_in);
962
963	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uv(Uv_op_res);
964	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	Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in;
965
966	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(Uv_op_res.isApprox(Uv_op_ref));
967
968	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in);
969
970	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uv(Uv_mat_op_res);
971	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	Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in;
972
973	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(Uv_mat_op_res.isApprox(Uv_mat_op_ref));
974
975			// Test Utv
976	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Utv_op_res(v_in), Utv_op_ref(v_in);
977
978	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utv(Utv_op_res);
979	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	Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in;
980
981	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(Utv_op_res.isApprox(Utv_op_ref));
982
983	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in);
984
985	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utv(Utv_mat_op_res);
986	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	Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in;
987
988	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(Utv_mat_op_res.isApprox(Utv_mat_op_ref));
989
990			// Test Uiv
991	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in);
992
993	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uiv(Uiv_op_res);
994	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	Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in;
995
996	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(Uiv_op_res.isApprox(Uiv_op_ref));
997
998	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in);
999
1000	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uiv(Uiv_mat_op_res);
1001	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	Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in;
1002
1003	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(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref));
1004
1005			// Test Utiv
1006	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in);
1007
1008	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utiv(Utiv_op_res);
1009	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	Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in;
1010
1011	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(Utiv_op_res.isApprox(Utiv_op_ref));
1012
1013	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in);
1014
1015	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utiv(Utiv_mat_op_res);
1016	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	Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in;
1017
1018	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(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref));
1019
1020			// inverse
1021	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	MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
1022	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.inverse(H_inv);
1023
1024	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd H_inv2 = contact_chol_decomposition.U.transpose().inverse()
1025	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	4	* contact_chol_decomposition.Dinv.asDiagonal()
1026	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	* contact_chol_decomposition.U.inverse();
1027
1028			MatrixXd H_inv3 =
1029	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::Identity(contact_chol_decomposition.size(), contact_chol_decomposition.size());
1030	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.solveInPlace(H_inv3);
1031
1032	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd H_inv_ref = H_recomposed.inverse();
1033	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(H_inv.topLeftCorner(constraint_dim, constraint_dim)
1034			.isApprox(H_inv_ref.topLeftCorner(constraint_dim, constraint_dim)));
1035	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(H_inv.bottomRightCorner(model.nv, model.nv)
1036			.isApprox(H_inv_ref.bottomRightCorner(model.nv, model.nv)));
1037	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(H_inv.topRightCorner(constraint_dim, model.nv)
1038			.isApprox(H_inv_ref.topRightCorner(constraint_dim, model.nv)));
1039
1040	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(H_inv_ref.isApprox(H_inv));
1041	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(H_inv_ref.isApprox(H_inv2));
1042	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(H_inv_ref.isApprox(H_inv3));
1043	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	const VectorXd ei = VectorXd::Unit(contact_chol_decomposition.size(), constraint_dim);
1044	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd ei_inv = ei;
1045	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.solveInPlace(ei_inv);
1046	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd ei_inv2 = ei;
1047	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Uiv(ei_inv2);
1048	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	ei_inv2 = contact_chol_decomposition.Dinv.asDiagonal() * ei_inv2;
1049	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.Utiv(ei_inv2);
1050
1051	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(ei_inv.isApprox(H_inv_ref * ei));
1052	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(ei_inv2.isApprox(H_inv_ref * ei));
1053	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(ei_inv.isApprox(H_inv * ei));
1054	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(ei_inv2.isApprox(H_inv * ei));
1055
1056			// test Operational Space Inertia Matrix
1057	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 = H.middleRows<24>(0).rightCols(model.nv) * data_ref.M.inverse()
1058	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.	4	* H.middleRows<24>(0).rightCols(model.nv).transpose()
1059	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.	4	+ mu * Eigen::MatrixXd::Identity(24, 24);
1060	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
1061	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
1062
1063	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(iosim.isApprox(JMinvJt));
1064	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(osim.isApprox(JMinvJt.inverse()));
1065
1066			// test Mass matrix cholesky
1067	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	data_ref.Minv = data_ref.M.inverse();
1068	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
1069	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getInverseMassMatrix(Minv_test);
1070
1071	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(Minv_test.isApprox(data_ref.Minv));
1072	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(24, model.nv));
1073	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getJMinv(JMinv_test);
1074	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	MatrixXd JMinv_ref = H.middleRows<24>(0).rightCols(model.nv) * data_ref.M.inverse();
1075	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(JMinv_ref.isApprox(JMinv_test));
1076		2	}
1077
1078	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(contact_cholesky_contact3D_6D_WORLD_by_joint_2)
1079			{
1080			using namespace Eigen;
1081			using namespace pinocchio;
1082			using namespace pinocchio::cholesky;
1083
1084	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
1085	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
1086	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Data data_ref(model);
1087
1088	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.);
1089	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.);
1090	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
1091
1092	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
1093	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
1094	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RA = "rarm6_joint";
1095	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LA = "larm6_joint";
1096
1097	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
1098	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
1099	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, 0, model.getJointId(RF), LOCAL);
1100	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RF);
1101	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));
1102	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, 0, model.getJointId(LF), LOCAL);
1103	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LF);
1104	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));
1105	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	RigidConstraintModel ci_RA(CONTACT_3D, model, 0, model.getJointId(RA), LOCAL);
1106	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RA);
1107	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_RA));
1108	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	RigidConstraintModel ci_LA(CONTACT_3D, model, 0, model.getJointId(LA), LOCAL_WORLD_ALIGNED);
1109	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LA);
1110	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_LA));
1111
1112			// Compute Mass Matrix
1113	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data_ref, q, Convention::WORLD);
1114	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
1115	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.triangularView<Eigen::StrictlyUpper>().transpose();
1116
1117			// Compute Cholesky decomposition
1118	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::decompose(model, data_ref);
1119
1120			// Compute Jacobians
1121	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	Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv), J_LA(6, model.nv);
1122	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RF.setZero();
1123	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), WORLD, J_RF);
1124	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LF.setZero();
1125	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);
1126	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RA.setZero();
1127	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(RA), LOCAL, J_RA);
1128	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LA.setZero();
1129	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(LA), LOCAL_WORLD_ALIGNED, J_LA);
1130
1131		2	const int constraint_dim = 15;
1132		2	const int total_dim = model.nv + constraint_dim;
1133
1134	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H(total_dim, total_dim);
1135	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	H.setZero();
1136	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
1137	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	H.middleRows<6>(0).rightCols(model.nv) = -ci_RF.joint1_placement.toActionMatrix() * J_RF;
1138	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.middleRows<3>(6).rightCols(model.nv) =
1139	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 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 17 taken 1 times. ✗ Branch 18 not taken.	4	-data_ref.oMi[model.getJointId(LF)].rotation() * J_LF.middleRows<3>(Motion::LINEAR);
1140	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.middleRows<3>(9).rightCols(model.nv) =
1141	6/12 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 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 17 taken 1 times. ✗ Branch 18 not taken.	4	-data_ref.oMi[model.getJointId(RA)].rotation() * J_RA.middleRows<3>(Motion::LINEAR);
1142	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	H.middleRows<3>(12).rightCols(model.nv) = -J_LA.middleRows<3>(Motion::LINEAR);
1143
1144	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	H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
1145
1146	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
1147	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data, q, Convention::WORLD);
1148	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition;
1149	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.allocate(model, contact_models);
1150
1151	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.compute(model, data, contact_models, contact_datas);
1152
1153	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
1154	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.triangularView<Eigen::StrictlyUpper>().transpose();
1155
1156	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(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv)));
1157	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(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv)));
1158	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(
1159			data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)));
1160
1161			Data::MatrixXs M_recomposed =
1162	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)
1163	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	* contact_chol_decomposition.D.tail(model.nv).asDiagonal()
1164	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.	4	* contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv).transpose();
1165	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(M_recomposed.isApprox(data.M));
1166
1167			Data::MatrixXs H_recomposed = contact_chol_decomposition.U
1168	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	* contact_chol_decomposition.D.asDiagonal()
1169	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	* contact_chol_decomposition.U.transpose();
1170
1171	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 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 29 not taken. ✓ Branch 30 taken 1 times.	2	BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
1172	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(H_recomposed.topRightCorner(constraint_dim, model.nv)
1173			.isApprox(H.topRightCorner(constraint_dim, model.nv)));
1174	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(H_recomposed.isApprox(H));
1175
1176			// test Operational Space Inertia Matrix
1177	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 = H.middleRows<15>(0).rightCols(model.nv) * data_ref.M.inverse()
1178	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.	4	* H.middleRows<15>(0).rightCols(model.nv).transpose();
1179	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
1180	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
1181
1182	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(iosim.isApprox(JMinvJt));
1183	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(osim.isApprox(JMinvJt.inverse()));
1184
1185			// test Mass matrix cholesky
1186	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	data_ref.Minv = data_ref.M.inverse();
1187	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
1188	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getInverseMassMatrix(Minv_test);
1189
1190	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(Minv_test.isApprox(data_ref.Minv));
1191	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(15, model.nv));
1192	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getJMinv(JMinv_test);
1193	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	MatrixXd JMinv_ref = H.middleRows<15>(0).rightCols(model.nv) * data_ref.M.inverse();
1194	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(JMinv_ref.isApprox(JMinv_test));
1195		2	}
1196
1197	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(loop_contact_cholesky_contact6D)
1198			{
1199			using namespace Eigen;
1200			using namespace pinocchio;
1201			using namespace pinocchio::cholesky;
1202
1203	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
1204	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
1205	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Data data_ref(model);
1206
1207	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.);
1208	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.);
1209	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
1210
1211	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
1212	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
1213	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RA = "rarm6_joint";
1214	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LA = "larm6_joint";
1215
1216	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
1217	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
1218			RigidConstraintModel loop_RF_LF_local(
1219	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	CONTACT_6D, model, model.getJointId(RF), model.getJointId(LF), LOCAL);
1220			RigidConstraintModel loop_RA_LA_lwa(
1221	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	CONTACT_6D, model, model.getJointId(RA), model.getJointId(LA), LOCAL_WORLD_ALIGNED);
1222
1223	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	loop_RF_LF_local.joint1_placement.setRandom();
1224	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	loop_RF_LF_local.joint2_placement.setRandom();
1225
1226	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	loop_RA_LA_lwa.joint1_placement.setRandom();
1227	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	loop_RA_LA_lwa.joint2_placement.setRandom();
1228
1229	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(loop_RF_LF_local);
1230	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(loop_RF_LF_local));
1231
1232	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(loop_RA_LA_lwa);
1233	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(loop_RA_LA_lwa));
1234
1235			// Compute Mass Matrix
1236	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data_ref, q, Convention::WORLD);
1237	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
1238	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.triangularView<Eigen::StrictlyUpper>().transpose();
1239
1240			// Compute Cholesky decomposition
1241		2	const double mu = 0.1;
1242	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::decompose(model, data_ref);
1243
1244			// Compute Jacobians
1245	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	Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA_local(6, model.nv),
1246	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LA_local(6, model.nv);
1247	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RF.setZero();
1248	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);
1249	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	Data::Matrix6x J_RF_local = loop_RF_LF_local.joint1_placement.toActionMatrixInverse() * J_RF;
1250	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LF.setZero();
1251	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);
1252	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	Data::Matrix6x J_LF_local = loop_RF_LF_local.joint2_placement.toActionMatrixInverse() * J_LF;
1253	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RA_local.setZero();
1254	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(RA), LOCAL, J_RA_local);
1255	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_RA_local = loop_RA_LA_lwa.joint1_placement.toActionMatrixInverse() * J_RA_local;
1256	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LA_local.setZero();
1257	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(LA), LOCAL, J_LA_local);
1258	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_LA_local = loop_RA_LA_lwa.joint2_placement.toActionMatrixInverse() * J_LA_local;
1259
1260	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data::Matrix6x J_RF_world(6, model.nv), J_LF_world(6, model.nv);
1261	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RF_world.setZero();
1262	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), WORLD, J_RF_world);
1263	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LF_world.setZero();
1264	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), WORLD, J_LF_world);
1265
1266	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(model, data_ref, q);
1267			const SE3 oM1_loop1 =
1268	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	data_ref.oMi[loop_RF_LF_local.joint1_id] * loop_RF_LF_local.joint1_placement;
1269			const SE3 oM2_loop1 =
1270	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	data_ref.oMi[loop_RF_LF_local.joint2_id] * loop_RF_LF_local.joint2_placement;
1271	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const SE3 _1M2_loop1 = oM1_loop1.inverse() * oM2_loop1;
1272	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const SE3 oM1_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint1_id] * loop_RA_LA_lwa.joint1_placement;
1273	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const SE3 oM2_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint2_id] * loop_RA_LA_lwa.joint2_placement;
1274	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const SE3 _1M2_loop2 = oM1_loop2.inverse() * oM2_loop2;
1275
1276		2	const int constraint_dim = 12;
1277		2	const int total_dim = model.nv + constraint_dim;
1278
1279	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H(total_dim, total_dim);
1280	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	H.setZero();
1281	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	H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu);
1282	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
1283	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	H.middleRows<6>(0).rightCols(model.nv) = J_RF_local - _1M2_loop1.toActionMatrix() * J_LF_local;
1284	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	const SE3 oM1_loop2_lwa = SE3(oM1_loop2.rotation(), SE3::Vector3::Zero());
1285	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.middleRows<6>(6).rightCols(model.nv) =
1286	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	oM1_loop2_lwa.toActionMatrix() * J_RA_local
1287	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.	6	- (oM1_loop2_lwa.toActionMatrix() * _1M2_loop2.toActionMatrix()) * J_LA_local;
1288
1289	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	H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
1290
1291	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
1292	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data, q, Convention::WORLD);
1293	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition;
1294	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.allocate(model, contact_models);
1295	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu);
1296
1297	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix();
1298
1299	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
1300	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.triangularView<Eigen::StrictlyUpper>().transpose();
1301	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 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 29 not taken. ✓ Branch 30 taken 1 times.	2	BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
1302	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(H_recomposed.topRightCorner(constraint_dim, model.nv)
1303			.isApprox(H.topRightCorner(constraint_dim, model.nv)));
1304	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(H_recomposed.isApprox(H));
1305
1306			// inverse
1307	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	MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
1308	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.inverse(H_inv);
1309
1310	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd H_inv_ref = H_recomposed.inverse();
1311
1312	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(H_inv_ref.isApprox(H_inv));
1313
1314			// test Operational Space Inertia Matrix
1315	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 = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse()
1316	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.	4	* H.middleRows<12>(0).rightCols(model.nv).transpose()
1317	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.	4	+ mu * Eigen::MatrixXd::Identity(12, 12);
1318	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
1319	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
1320
1321	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(iosim.isApprox(JMinvJt));
1322	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(osim.isApprox(JMinvJt.inverse()));
1323
1324			// test Mass matrix cholesky
1325	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	data_ref.Minv = data_ref.M.inverse();
1326	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
1327	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getInverseMassMatrix(Minv_test);
1328
1329	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(Minv_test.isApprox(data_ref.Minv));
1330	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(12, model.nv));
1331	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getJMinv(JMinv_test);
1332	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	MatrixXd JMinv_ref = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse();
1333	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(JMinv_ref.isApprox(JMinv_test));
1334		2	}
1335
1336	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(loop_contact_cholesky_contact_3d)
1337			{
1338			using namespace Eigen;
1339			using namespace pinocchio;
1340			using namespace pinocchio::cholesky;
1341
1342	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
1343	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
1344	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Data data_ref(model);
1345
1346	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.);
1347	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.);
1348	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
1349
1350	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
1351	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
1352	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RA = "rarm6_joint";
1353	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LA = "larm6_joint";
1354
1355	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
1356	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
1357			RigidConstraintModel loop_RF_LF_local(
1358	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	CONTACT_3D, model, model.getJointId(RF), model.getJointId(LF), LOCAL);
1359			RigidConstraintModel loop_RA_LA_lwa(
1360	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	CONTACT_3D, model, model.getJointId(RA), model.getJointId(LA), LOCAL_WORLD_ALIGNED);
1361
1362	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	loop_RF_LF_local.joint1_placement.setRandom();
1363	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	loop_RF_LF_local.joint2_placement.setRandom();
1364
1365	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	loop_RA_LA_lwa.joint1_placement.setRandom();
1366	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	loop_RA_LA_lwa.joint2_placement.setRandom();
1367
1368	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(loop_RF_LF_local);
1369	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(loop_RF_LF_local));
1370
1371	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(loop_RA_LA_lwa);
1372	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(loop_RA_LA_lwa));
1373
1374			// Compute Mass Matrix
1375	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data_ref, q, Convention::WORLD);
1376	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
1377	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.triangularView<Eigen::StrictlyUpper>().transpose();
1378
1379			// Compute Cholesky decomposition
1380		2	const double mu = 0.1;
1381	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::cholesky::decompose(model, data_ref);
1382
1383			// Compute Jacobians
1384	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	Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA_local(6, model.nv),
1385	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LA_local(6, model.nv);
1386	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RF.setZero();
1387	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);
1388			const Data::Matrix6x J_RF_local =
1389	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	loop_RF_LF_local.joint1_placement.toActionMatrixInverse() * J_RF;
1390	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LF.setZero();
1391	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);
1392			const Data::Matrix6x J_LF_local =
1393	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	loop_RF_LF_local.joint2_placement.toActionMatrixInverse() * J_LF;
1394	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RA_local.setZero();
1395	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(RA), LOCAL, J_RA_local);
1396	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_RA_local = loop_RA_LA_lwa.joint1_placement.toActionMatrixInverse() * J_RA_local;
1397	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LA_local.setZero();
1398	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(LA), LOCAL, J_LA_local);
1399	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_LA_local = loop_RA_LA_lwa.joint2_placement.toActionMatrixInverse() * J_LA_local;
1400
1401	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data::Matrix6x J_RF_world(6, model.nv), J_LF_world(6, model.nv);
1402	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RF_world.setZero();
1403	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), WORLD, J_RF_world);
1404	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_LF_world.setZero();
1405	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), WORLD, J_LF_world);
1406
1407	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(model, data_ref, q);
1408			const SE3 oM1_loop1 =
1409	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	data_ref.oMi[loop_RF_LF_local.joint1_id] * loop_RF_LF_local.joint1_placement;
1410			const SE3 oM2_loop1 =
1411	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	data_ref.oMi[loop_RF_LF_local.joint2_id] * loop_RF_LF_local.joint2_placement;
1412	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const SE3 _1M2_loop1 = oM1_loop1.inverse() * oM2_loop1;
1413	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const SE3 oM1_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint1_id] * loop_RA_LA_lwa.joint1_placement;
1414	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const SE3 oM2_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint2_id] * loop_RA_LA_lwa.joint2_placement;
1415	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const SE3 _1M2_loop2 = oM1_loop2.inverse() * oM2_loop2;
1416
1417		2	const int constraint_dim = 6;
1418		2	const int total_dim = model.nv + constraint_dim;
1419
1420	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H(total_dim, total_dim);
1421	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	H.setZero();
1422	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	H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu);
1423	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
1424	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.middleRows<3>(0).rightCols(model.nv) =
1425	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	J_RF_local.middleRows<3>(Motion::LINEAR)
1426	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.	6	- _1M2_loop1.rotation() * J_LF_local.middleRows<3>(Motion::LINEAR);
1427	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	const SE3 oM1_loop2_lwa = SE3(oM1_loop2.rotation(), SE3::Vector3::Zero());
1428	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	const SE3 oM2_loop2_lwa = SE3(oM2_loop2.rotation(), SE3::Vector3::Zero());
1429	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	H.middleRows<3>(3).rightCols(model.nv) =
1430	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	(oM1_loop2_lwa.toActionMatrix() * J_RA_local - (oM2_loop2_lwa.toActionMatrix()) * J_LA_local)
1431	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	4	.middleRows<3>(Motion::LINEAR);
1432
1433	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	H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
1434
1435	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
1436	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data, q, Convention::WORLD);
1437	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition;
1438	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.allocate(model, contact_models);
1439	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu);
1440	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 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. ✗ Branch 27 not taken. ✓ Branch 28 taken 1 times.	2	BOOST_CHECK(contact_datas[0].c1Mc2.isApprox(_1M2_loop1));
1441	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 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. ✗ Branch 27 not taken. ✓ Branch 28 taken 1 times.	2	BOOST_CHECK(contact_datas[1].c1Mc2.isApprox(_1M2_loop2));
1442
1443	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix();
1444
1445	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
1446	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.triangularView<Eigen::StrictlyUpper>().transpose();
1447	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 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 29 not taken. ✓ Branch 30 taken 1 times.	2	BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
1448	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(H_recomposed.topRightCorner(constraint_dim, model.nv)
1449			.isApprox(H.topRightCorner(constraint_dim, model.nv)));
1450	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(H_recomposed.isApprox(H));
1451
1452			// std::cout << "H_recomposed.topRightCorner(constraint_dim,model.nv):\n" <<
1453			// H_recomposed.topRightCorner(constraint_dim,model.nv) << std::endl; std::cout <<
1454			// "H.topRightCorner(constraint_dim,model.nv):\n" << H .topRightCorner(constraint_dim,model.nv)
1455			// << std::endl;
1456
1457			// inverse
1458	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	MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
1459	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.inverse(H_inv);
1460
1461	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MatrixXd H_inv_ref = H_recomposed.inverse();
1462
1463	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(H_inv_ref.isApprox(H_inv));
1464
1465			// test Operational Space Inertia Matrix
1466	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 = H.middleRows<6>(0).rightCols(model.nv) * data_ref.M.inverse()
1467	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.	4	* H.middleRows<6>(0).rightCols(model.nv).transpose()
1468	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.	4	+ mu * Eigen::MatrixXd::Identity(6, 6);
1469	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
1470	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
1471
1472	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(iosim.isApprox(JMinvJt));
1473	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(osim.isApprox(JMinvJt.inverse()));
1474
1475			// test Mass matrix cholesky
1476	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	data_ref.Minv = data_ref.M.inverse();
1477	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
1478	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getInverseMassMatrix(Minv_test);
1479
1480	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(6, model.nv));
1481	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.getJMinv(JMinv_test);
1482	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	MatrixXd JMinv_ref = H.middleRows<6>(0).rightCols(model.nv) * data_ref.M.inverse();
1483	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(JMinv_ref.isApprox(JMinv_test));
1484	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(Minv_test.isApprox(data_ref.Minv));
1485		2	}
1486
1487	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(contact_cholesky_updateDamping)
1488			{
1489			using namespace Eigen;
1490			using namespace pinocchio;
1491			using namespace pinocchio::cholesky;
1492
1493	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Model model;
1494	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
1495	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::Data data_ref(model);
1496
1497	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.);
1498	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.);
1499	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
1500
1501	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF = "rleg6_joint";
1502	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string LF = "lleg6_joint";
1503
1504	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
1505	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
1506	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);
1507	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_RF);
1508	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));
1509	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);
1510	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_models.push_back(ci_LF);
1511	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));
1512
1513	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
1514	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data, q, Convention::WORLD);
1515
1516		2	const double mu1 = 1e-2, mu2 = 1e-10;
1517
1518			{
1519	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition;
1520	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.allocate(model, contact_models);
1521	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu1);
1522	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.updateDamping(mu2);
1523
1524	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition_ref;
1525	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition_ref.allocate(model, contact_models);
1526	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition_ref.compute(model, data, contact_models, contact_datas, mu2);
1527
1528	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(contact_chol_decomposition.D.isApprox(contact_chol_decomposition_ref.D));
1529	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(contact_chol_decomposition.Dinv.isApprox(contact_chol_decomposition_ref.Dinv));
1530	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(contact_chol_decomposition.U.isApprox(contact_chol_decomposition_ref.U));
1531		2	}
1532
1533			{
1534	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition;
1535	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.allocate(model, contact_models);
1536	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu1);
1537	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	contact_chol_decomposition.getDelassusCholeskyExpression().updateDamping(mu2);
1538
1539	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ContactCholeskyDecomposition contact_chol_decomposition_ref;
1540	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition_ref.allocate(model, contact_models);
1541	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	contact_chol_decomposition_ref.compute(model, data, contact_models, contact_datas, mu2);
1542
1543	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(contact_chol_decomposition.D.isApprox(contact_chol_decomposition_ref.D));
1544	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(contact_chol_decomposition.Dinv.isApprox(contact_chol_decomposition_ref.Dinv));
1545	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(contact_chol_decomposition.U.isApprox(contact_chol_decomposition_ref.U));
1546		2	}
1547		2	}
1548
1549			BOOST_AUTO_TEST_SUITE_END()
1550