GCC Code Coverage Report

Directory: ./
File: unittest/cholesky.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 161 161 100.0%
Branches: 382 746 51.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 //
4
5 /*
6 * Validate the sparse Cholesky decomposition of the mass matrix. The code
7 * tests both the numerical value and the computation time. For a strong
8 * computation benchmark, see benchmark/timings.
9 *
10 */
11
12 #include "pinocchio/spatial/se3.hpp"
13 #include "pinocchio/multibody/model.hpp"
14 #include "pinocchio/multibody/data.hpp"
15 #include "pinocchio/algorithm/crba.hpp"
16 #include "pinocchio/algorithm/cholesky.hpp"
17 #include "pinocchio/multibody/sample-models.hpp"
18 #include "pinocchio/utils/timer.hpp"
19 #include "pinocchio/algorithm/joint-configuration.hpp"
20
21 #include <iostream>
22 #ifdef NDEBUG
23 #include <Eigen/Cholesky>
24 #endif
25
26 #include <boost/test/unit_test.hpp>
27 #include <boost/utility/binary.hpp>
28
29 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
30
31
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_cholesky)
32 {
33 using namespace Eigen;
34 using namespace pinocchio;
35
36
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
37
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model, true);
38
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data(model);
39
40
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.);
41
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.);
42
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
43
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data.M.fill(0); // Only nonzero coeff of M are initialized by CRBA.
44
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 crba(model, data, q, Convention::WORLD);
45
46
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::cholesky::decompose(model, data);
47
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data.M.triangularView<Eigen::StrictlyLower>() =
48
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();
49
50 2 const Eigen::MatrixXd & U = data.U;
51 2 const Eigen::VectorXd & D = data.D;
52 2 const Eigen::MatrixXd & M = data.M;
53
54 #ifndef NDEBUG
55
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 std::cout << "M = [\n" << M << "];" << std::endl;
56
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 std::cout << "U = [\n" << U << "];" << std::endl;
57
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 std::cout << "D = [\n" << D.transpose() << "];" << std::endl;
58 #endif
59
60
11/22
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 1 times.
 2 BOOST_CHECK(M.isApprox(U * D.asDiagonal() * U.transpose(), 1e-12));
61
62
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
63 // std::cout << "v = [" << v.transpose() << "]';" << std::endl;
64
65
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd Uv = v;
66
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::cholesky::Uv(model, data, Uv);
67
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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(Uv.isApprox(U * v, 1e-12));
68
69
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd Utv = v;
70
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::cholesky::Utv(model, data, Utv);
71
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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
 2 BOOST_CHECK(Utv.isApprox(U.transpose() * v, 1e-12));
72
73
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd Uiv = v;
74
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::cholesky::Uiv(model, data, Uiv);
75
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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
 2 BOOST_CHECK(Uiv.isApprox(U.inverse() * v, 1e-12));
76
77
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd Utiv = v;
78
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::cholesky::Utiv(model, data, Utiv);
79
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
 2 BOOST_CHECK(Utiv.isApprox(U.transpose().inverse() * v, 1e-12));
80
81
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd Miv = v;
82
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::cholesky::solve(model, data, Miv);
83
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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
 2 BOOST_CHECK(Miv.isApprox(M.inverse() * v, 1e-12));
84
85
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd Mv = v;
86
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Mv = pinocchio::cholesky::Mv(model, data, Mv);
87
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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(Mv.isApprox(M * v, 1e-12));
88
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Mv = v;
89
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::cholesky::UDUtv(model, data, Mv);
90
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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(Mv.isApprox(M * v, 1e-12));
91 2 }
92
93 /* The flag triger the following timers:
94 * 000001: sparse UDUt cholesky
95 * 000010: dense Eigen LDLt cholesky (with pivot)
96 * 000100: sparse resolution
97 * 001000: sparse U*v multiplication
98 * 010000: sparse U\v substitution
99 * 100000: sparse M*v multiplication without Cholesky
100 */
101
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_timings)
102 {
103 using namespace Eigen;
104 using namespace pinocchio;
105
106
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
107
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model, true);
108
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data(model);
109
110
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.);
111
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.);
112
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
113
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data.M.fill(0); // Only nonzero coeff of M are initialized by CRBA.
114
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 crba(model, data, q, Convention::WORLD);
115
116 2 long flag = BOOST_BINARY(1111111);
117
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 PinocchioTicToc timer(PinocchioTicToc::US);
118 #ifdef NDEBUG
119 #ifdef _INTENSE_TESTING_
120 const size_t NBT = 1000 * 1000;
121 #else
122 const size_t NBT = 10;
123 #endif
124 #else
125 2 const size_t NBT = 1;
126
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::cout << "(the time score in debug mode is not relevant) ";
127 #endif
128
129 2 bool verbose = flag & (flag - 1); // True is two or more binaries of the flag are 1.
130
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (verbose)
131
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 std::cout << "--" << std::endl;
132
133
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (flag >> 0 & 1)
134 {
135
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.tic();
136
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 4 SMOOTH(NBT)
137 {
138
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::cholesky::decompose(model, data);
139 }
140
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (verbose)
141
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::cout << "Decompose =\t";
142
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.toc(std::cout, NBT);
143 }
144
145
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (flag >> 1 & 1)
146 {
147
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.tic();
148
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
149
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd res(model.nv);
150
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 4 SMOOTH(NBT)
151 {
152
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::LDLT<Eigen::MatrixXd> Mchol(data.M);
153
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 res = Mchol.solve(v);
154 2 }
155
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (verbose)
156
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::cout << "Eigen::LDLt =\t";
157
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.toc(std::cout, NBT);
158 2 }
159
160
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (flag >> 2 & 31)
161 {
162
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<Eigen::VectorXd> randvec(NBT);
163
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 4 for (size_t i = 0; i < NBT; ++i)
164
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 randvec[i] = Eigen::VectorXd::Random(model.nv);
165
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd zero = Eigen::VectorXd::Zero(model.nv);
166
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd res(model.nv);
167
168
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (flag >> 2 & 1)
169 {
170
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.tic();
171
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 4 SMOOTH(NBT)
172 {
173
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 pinocchio::cholesky::solve(model, data, randvec[_smooth]);
174 }
175
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (verbose)
176
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::cout << "solve =\t\t";
177
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.toc(std::cout, NBT);
178 }
179
180
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (flag >> 3 & 1)
181 {
182
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.tic();
183
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 4 SMOOTH(NBT)
184 {
185
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 pinocchio::cholesky::Uv(model, data, randvec[_smooth]);
186 }
187
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (verbose)
188
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::cout << "Uv =\t\t";
189
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.toc(std::cout, NBT);
190 }
191
192
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (flag >> 4 & 1)
193 {
194
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.tic();
195
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 4 SMOOTH(NBT)
196 {
197
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 pinocchio::cholesky::Uiv(model, data, randvec[_smooth]);
198 }
199
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (verbose)
200
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::cout << "Uiv =\t\t";
201
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.toc(std::cout, NBT);
202 }
203
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (flag >> 5 & 1)
204 {
205
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.tic();
206
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd res;
207
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 4 SMOOTH(NBT)
208 {
209
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 res = pinocchio::cholesky::Mv(model, data, randvec[_smooth]);
210 }
211
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (verbose)
212
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::cout << "Mv =\t\t";
213
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.toc(std::cout, NBT);
214 2 }
215
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (flag >> 6 & 1)
216 {
217
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.tic();
218
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 4 SMOOTH(NBT)
219 {
220
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 pinocchio::cholesky::UDUtv(model, data, randvec[_smooth]);
221 }
222
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 if (verbose)
223
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::cout << "UDUtv =\t\t";
224
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.toc(std::cout, NBT);
225 }
226 2 }
227 2 }
228
229
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_Minv_from_cholesky)
230 {
231 using namespace Eigen;
232 using namespace pinocchio;
233
234
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
235
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model, true);
236
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data(model);
237
238
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.);
239
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.);
240
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
241
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 crba(model, data, q, Convention::WORLD);
242
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data.M.triangularView<Eigen::StrictlyLower>() =
243
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 4 data.M.triangularView<Eigen::StrictlyUpper>().transpose();
244
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd Minv_ref(data.M.inverse());
245
246
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cholesky::decompose(model, data);
247
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd v_unit(VectorXd::Unit(model.nv, 0));
248
249
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd Ui_v_unit(model.nv);
250
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd Ui_v_unit_ref(model.nv);
251
252
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
253 {
254
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 v_unit = VectorXd::Unit(model.nv, k);
255
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 Ui_v_unit.setZero();
256
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 cholesky::internal::Miunit(model, data, k, Ui_v_unit);
257
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 Ui_v_unit_ref = v_unit;
258
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 cholesky::Uiv(model, data, Ui_v_unit_ref);
259
3/6
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
 64 Ui_v_unit_ref.array() *= data.Dinv.array();
260
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 cholesky::Utiv(model, data, Ui_v_unit_ref);
261
9/18
✓ 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 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 32 times.
 64 BOOST_CHECK(Ui_v_unit.head(k + 1).isApprox(Ui_v_unit_ref.head(k + 1)));
262
263
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 Ui_v_unit_ref = v_unit;
264
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 cholesky::solve(model, data, Ui_v_unit_ref);
265
9/18
✓ 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 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 32 times.
 64 BOOST_CHECK(Ui_v_unit.head(k + 1).isApprox(Ui_v_unit_ref.head(k + 1)));
266 }
267
268
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 MatrixXd Minv(model.nv, model.nv);
269
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Minv.setZero();
270
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cholesky::computeMinv(model, data, Minv);
271
272
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.isApprox(Minv_ref));
273
274 // Check second call to cholesky::computeMinv
275
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cholesky::computeMinv(model, data, Minv);
276
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.isApprox(Minv_ref));
277
278 // Call the second signature of cholesky::computeMinv
279
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_bis(model);
280
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 crba(model, data_bis, q, Convention::WORLD);
281
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cholesky::decompose(model, data_bis);
282
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cholesky::computeMinv(model, data_bis);
283
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_bis.Minv.isApprox(Minv_ref));
284 2 }
285
286 BOOST_AUTO_TEST_SUITE_END()
287