GCC Code Coverage Report

Directory: ./
File: unittest/crba.cpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 108 108 100.0%
Branches: 355 704 50.4%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2023 CNRS INRIA
3 //
4
5 /*
6 * Test the CRBA algorithm. The code validates both the computation times and
7 * the value by comparing the results of the CRBA with the reconstruction of
8 * the mass matrix using the RNEA.
9 * For a strong timing benchmark, see benchmark/timings.
10 *
11 */
12
13 #ifndef EIGEN_RUNTIME_NO_MALLOC
14 #define EIGEN_RUNTIME_NO_MALLOC
15 #endif
16
17 #include "pinocchio/multibody/model.hpp"
18 #include "pinocchio/multibody/data.hpp"
19 #include "pinocchio/algorithm/model.hpp"
20 #include "pinocchio/algorithm/crba.hpp"
21 #include "pinocchio/algorithm/centroidal.hpp"
22 #include "pinocchio/algorithm/rnea.hpp"
23 #include "pinocchio/algorithm/jacobian.hpp"
24 #include "pinocchio/algorithm/center-of-mass.hpp"
25 #include "pinocchio/algorithm/joint-configuration.hpp"
26 #include "pinocchio/multibody/sample-models.hpp"
27 #include "pinocchio/utils/timer.hpp"
28
29 #include <iostream>
30
31 #include <boost/test/unit_test.hpp>
32 #include <boost/utility/binary.hpp>
33
34 #include "utils/model-generator.hpp"
35
36 template<typename JointModel>
37 static void addJointAndBody(
38 pinocchio::Model & model,
39 const pinocchio::JointModelBase<JointModel> & joint,
40 const std::string & parent_name,
41 const std::string & name,
42 const pinocchio::SE3 placement = pinocchio::SE3::Random(),
43 bool setRandomLimits = true)
44 {
45 using namespace pinocchio;
46 typedef typename JointModel::ConfigVector_t CV;
47 typedef typename JointModel::TangentVector_t TV;
48
49 Model::JointIndex idx;
50
51 if (setRandomLimits)
52 idx = model.addJoint(
53 model.getJointId(parent_name), joint, SE3::Random(), name + "_joint",
54 TV::Random() + TV::Constant(1), TV::Random() + TV::Constant(1),
55 CV::Random() - CV::Constant(1), CV::Random() + CV::Constant(1));
56 else
57 idx = model.addJoint(model.getJointId(parent_name), joint, placement, name + "_joint");
58
59 model.addJointFrame(idx);
60
61 model.appendBodyToJoint(idx, Inertia::Random(), SE3::Identity());
62 model.addBodyFrame(name + "_body", idx);
63 }
64
65 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
66
67
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_crba)
68 {
69
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
70
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model);
71
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data(model);
72
73 #ifdef NDEBUG
74 #ifdef _INTENSE_TESTING_
75 const size_t NBT = 1000 * 1000;
76 #else
77 const size_t NBT = 10;
78 #endif
79
80 Eigen::VectorXd q = pinocchio::neutral(model);
81
82 PinocchioTicToc timer(PinocchioTicToc::US);
83 timer.tic();
84 SMOOTH(NBT)
85 {
86 pinocchio::crba(model, data, q, pinocchio::Convention::WORLD);
87 }
88 timer.toc(std::cout, NBT);
89
90 #else
91 2 const size_t NBT = 1;
92 using namespace Eigen;
93 using namespace pinocchio;
94
95
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::MatrixXd M(model.nv, model.nv);
96
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
97
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 q.segment<4>(3).normalize();
98
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::Zero(model.nv);
99
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
100
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data.M.fill(0);
101
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::crba(model, data, q, Convention::WORLD);
102
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data.M.triangularView<Eigen::StrictlyLower>() =
103
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 4 data.M.transpose().triangularView<Eigen::StrictlyLower>();
104
105 /* Joint inertia from iterative crba. */
106
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 const Eigen::VectorXd bias = rnea(model, data, q, v, a);
107
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int i = 0; i < model.nv; ++i)
108 {
109
5/10
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 32 times.
✗ Branch 14 not taken.
 64 M.col(i) = rnea(model, data, q, v, Eigen::VectorXd::Unit(model.nv, i)) - bias;
110 }
111
112 // std::cout << "Mcrb = [ " << data.M << " ];" << std::endl;
113 // std::cout << "Mrne = [ " << M << " ]; " << std::endl;
114
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(M.isApprox(data.M, 1e-12));
115
116
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::cout << "(the time score in debug mode is not relevant) ";
117
118
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 q = pinocchio::neutral(model);
119
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 PinocchioTicToc timer(PinocchioTicToc::US);
121
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.tic();
122
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 4 SMOOTH(NBT)
123 {
124
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::crba(model, data, q, Convention::WORLD);
125 }
126
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 timer.toc(std::cout, NBT);
127
128 #endif // ifndef NDEBUG
129 2 }
130
131
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_crba_mimic)
132 {
133
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 14 for (int i = 0; i < pinocchio::MimicTestCases::N_CASES; i++)
134 {
135
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 const pinocchio::MimicTestCases mimic_test_case(i);
136 12 const pinocchio::Model & model_mimic = mimic_test_case.model_mimic;
137 12 const pinocchio::Model & model_full = mimic_test_case.model_full;
138 12 const Eigen::MatrixXd & G = mimic_test_case.G;
139
140
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 Eigen::VectorXd q_mimic = pinocchio::randomConfiguration(model_mimic);
141
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 Eigen::VectorXd v_mimic = Eigen::VectorXd::Random(model_mimic.nv);
142
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 Eigen::VectorXd as_mimic = Eigen::VectorXd::Random(model_mimic.nv);
143
144
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 Eigen::VectorXd q_full(model_full.nq);
145
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 Eigen::VectorXd v_full = G * v_mimic;
146
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 Eigen::VectorXd a_full = G * as_mimic;
147
148
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 mimic_test_case.toFull(q_mimic, q_full);
149
150
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 pinocchio::Data data_full(model_full);
151
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 pinocchio::Data data_mimic(model_mimic);
152
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 pinocchio::Data data_ref_mimic(model_mimic);
153
154
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 pinocchio::crba(model_full, data_full, q_full);
155 // World vs local
156
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 pinocchio::crba(model_mimic, data_ref_mimic, q_mimic, pinocchio::Convention::WORLD);
157
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 pinocchio::crba(model_mimic, data_mimic, q_mimic, pinocchio::Convention::LOCAL);
158
159
7/14
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 6 times.
 12 BOOST_CHECK(data_ref_mimic.M.isApprox(data_mimic.M));
160
161 // Compute other half of matrix
162
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 data_mimic.M.triangularView<Eigen::StrictlyLower>() =
163
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 24 data_mimic.M.transpose().triangularView<Eigen::StrictlyLower>();
164
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 data_full.M.triangularView<Eigen::StrictlyLower>() =
165
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 24 data_full.M.transpose().triangularView<Eigen::StrictlyLower>();
166
167 // Check full model against reduced
168
10/20
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 6 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 6 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 6 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 6 times.
 12 BOOST_CHECK(data_mimic.M.isApprox(G.transpose() * data_full.M * G, 1e-12));
169 12 }
170 2 }
171
172
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_minimal_crba)
173 {
174
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
175
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model);
176
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(model), data_ref(model);
177
178
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 model.lowerPositionLimit.head<7>().fill(-1.);
179
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 model.upperPositionLimit.head<7>().fill(1.);
180
181 Eigen::VectorXd q =
182
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit);
183
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));
184
185
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL);
186
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data_ref.M.triangularView<Eigen::StrictlyLower>() =
187
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 4 data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
188
189
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::crba(model, data, q, pinocchio::Convention::WORLD);
190
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data.M.triangularView<Eigen::StrictlyLower>() =
191
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 4 data.M.transpose().triangularView<Eigen::StrictlyLower>();
192
193
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));
194
195
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ccrba(model, data_ref, q, v);
196
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data_ref, q);
197
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
198
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK(data.J.isApprox(data_ref.J));
199
200 // Check double call
201 {
202
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data2(model);
203
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::crba(model, data2, q, pinocchio::Convention::LOCAL);
204
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::crba(model, data2, q, pinocchio::Convention::LOCAL);
205
206
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(data2.Ycrb[0] == data.Ycrb[0]);
207 2 }
208 2 }
209
210
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_roto_inertia_effects)
211 {
212
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Model model, model_ref;
213
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model);
214
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 model_ref = model;
215
216
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(model == model_ref);
217
218
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(model), data_ref(model_ref);
219
220
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 model.armature = Eigen::VectorXd::Random(model.nv) + Eigen::VectorXd::Constant(model.nv, 1.);
221
222
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd q = randomConfiguration(model);
223
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::crba(model_ref, data_ref, q, pinocchio::Convention::WORLD);
224
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data_ref.M.triangularView<Eigen::StrictlyLower>() =
225
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 4 data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
226
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 data_ref.M.diagonal() += model.armature;
227
228
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::crba(model, data, q, pinocchio::Convention::WORLD);
229
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data.M.triangularView<Eigen::StrictlyLower>() =
230
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 4 data.M.transpose().triangularView<Eigen::StrictlyLower>();
231
232
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));
233 2 }
234
235 #ifndef NDEBUG
236
237
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_crba_malloc)
238 {
239 using namespace pinocchio;
240
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
241
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model, true, true);
242
243
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 8 model.addJoint(
244
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 size_t(model.njoints - 1), pinocchio::JointModelRevoluteUnaligned(SE3::Vector3::UnitX()),
245
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 SE3::Random(), "revolute_unaligned");
246
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data(model);
247
248
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const Eigen::VectorXd q = pinocchio::neutral(model);
249 2 Eigen::internal::set_is_malloc_allowed(false);
250
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 crba(model, data, q, pinocchio::Convention::WORLD);
251
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 crba(model, data, q, pinocchio::Convention::LOCAL);
252 2 Eigen::internal::set_is_malloc_allowed(true);
253 2 }
254
255 #endif
256
257 BOOST_AUTO_TEST_SUITE_END()
258