GCC Code Coverage Report


Directory: ./
File: unittest/rnea.cpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 231 231 100.0%
Branches: 717 1424 50.4%

Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4
5 /*
6 * Unittest of the RNE algorithm. The code simply test that the algorithm does
7 * not cause any serious errors. The numerical values are not cross validated
8 * in any way.
9 *
10 */
11
12 #include "pinocchio/multibody/model.hpp"
13 #include "pinocchio/multibody/data.hpp"
14 #include "pinocchio/algorithm/model.hpp"
15 #include "pinocchio/spatial/fwd.hpp"
16 #include "pinocchio/algorithm/rnea.hpp"
17 #include "pinocchio/algorithm/jacobian.hpp"
18 #include "pinocchio/algorithm/center-of-mass.hpp"
19 #include "pinocchio/algorithm/joint-configuration.hpp"
20 #include "pinocchio/algorithm/crba.hpp"
21 #include "pinocchio/algorithm/centroidal.hpp"
22 #include "pinocchio/multibody/sample-models.hpp"
23 #include "pinocchio/utils/timer.hpp"
24
25 #include <iostream>
26
27 // #define __SSE3__
28 #include <fenv.h>
29
30 #ifdef __SSE3__
31 #include <pmmintrin.h>
32 #endif
33
34 #include <boost/test/unit_test.hpp>
35 #include <boost/utility/binary.hpp>
36
37 #include "utils/model-generator.hpp"
38
39 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
40
41
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_rnea)
42 {
43 #ifdef __SSE3__
44 _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
45 #endif
46 using namespace Eigen;
47 using namespace pinocchio;
48
49
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 pinocchio::Model model;
50
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model, true);
51
52
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.);
53
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.);
54
55
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 pinocchio::Data data(model);
56
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 data.v[0] = Motion::Zero();
57
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 data.a[0] = -model.gravity;
58
59
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
60
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd v = VectorXd::Random(model.nv);
61
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd a = VectorXd::Random(model.nv);
62
63 #ifdef NDEBUG
64 const size_t NBT = 10000;
65 #else
66 2 const size_t NBT = 1;
67
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 std::cout << "(the time score in debug mode is not relevant) ";
68 #endif
69
70
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 PinocchioTicToc timer(PinocchioTicToc::US);
71
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 timer.tic();
72
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
4 SMOOTH(NBT)
73 {
74
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea(model, data, q, v, a);
75 }
76
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 timer.toc(std::cout, NBT);
77 2 }
78
79
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_nle_vs_rnea)
80 {
81 using namespace Eigen;
82 using namespace pinocchio;
83
84
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 pinocchio::Model model;
85
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model, true);
86
87
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.);
88
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.);
89
90
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 pinocchio::Data data_nle(model);
91
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 pinocchio::Data data_rnea(model);
92
93
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
94
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd v = VectorXd::Random(model.nv);
95
96
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd tau_nle(VectorXd::Zero(model.nv));
97
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd tau_rnea(VectorXd::Zero(model.nv));
98
99 // -------
100
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 q.tail(model.nq - 7).setZero();
101
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 v.setZero();
102
103
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 tau_nle = nonLinearEffects(model, data_nle, q, v);
104
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 tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv));
105
106
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(tau_nle.isApprox(tau_rnea, 1e-12));
107
108 // -------
109
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 q.tail(model.nq - 7).setZero();
110
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 v.setOnes();
111
112
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 tau_nle = nonLinearEffects(model, data_nle, q, v);
113
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 tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv));
114
115
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(tau_nle.isApprox(tau_rnea, 1e-12));
116
117 // -------
118
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 q.tail(model.nq - 7).setOnes();
119
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 v.setOnes();
120
121
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 tau_nle = nonLinearEffects(model, data_nle, q, v);
122
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 tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv));
123
124
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(tau_nle.isApprox(tau_rnea, 1e-12));
125
126 // -------
127
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q = randomConfiguration(model);
128
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 v.setRandom();
129
130
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 tau_nle = nonLinearEffects(model, data_nle, q, v);
131
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
2 tau_rnea = rnea(model, data_rnea, q, v, VectorXd::Zero(model.nv));
132
133
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(tau_nle.isApprox(tau_rnea, 1e-12));
134 2 }
135
136
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(test_rnea_with_fext)
137 {
138 using namespace Eigen;
139 using namespace pinocchio;
140
141
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
142
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model, true);
143
144
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.);
145
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.);
146
147
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data_rnea_fext(model);
148
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data_rnea(model);
149
150
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
151
152
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd v(VectorXd::Random(model.nv));
153
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd a(VectorXd::Random(model.nv));
154
155
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Zero());
156
157
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 JointIndex rf = model.getJointId("rleg6_joint");
158
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Force Frf = Force::Random();
159
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 fext[rf] = Frf;
160
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 JointIndex lf = model.getJointId("lleg6_joint");
161
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Force Flf = Force::Random();
162
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 fext[lf] = Flf;
163
164
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea(model, data_rnea, q, v, a);
165
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd tau_ref(data_rnea.tau);
166
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Data::Matrix6x Jrf(Data::Matrix6x::Zero(6, model.nv));
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeJointJacobian(model, data_rnea, q, rf, Jrf);
168
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 tau_ref -= Jrf.transpose() * Frf.toVector();
169
170
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Data::Matrix6x Jlf(Data::Matrix6x::Zero(6, model.nv));
171
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeJointJacobian(model, data_rnea, q, lf, Jlf);
172
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 tau_ref -= Jlf.transpose() * Flf.toVector();
173
174
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea(model, data_rnea_fext, q, v, a, fext);
175
176
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(tau_ref.isApprox(data_rnea_fext.tau));
177 2 }
178
179
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_rnea_with_armature)
180 {
181 using namespace Eigen;
182 using namespace pinocchio;
183
184
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
185
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model, true);
186
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 = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv);
187
188
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.);
189
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.);
190
191
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data(model);
192
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data_ref(model);
193
194
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
195
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd v(VectorXd::Random(model.nv));
196
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd a(VectorXd::Random(model.nv));
197
198
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 crba(model, data_ref, q, Convention::WORLD);
199
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 data_ref.M.triangularView<StrictlyLower>() =
200
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<StrictlyLower>();
201
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 const VectorXd nle = nonLinearEffects(model, data_ref, q, v);
202
203
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 tau_ref = data_ref.M * a + nle;
204
205
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea(model, data, q, v, a);
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 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(tau_ref.isApprox(data.tau));
207 2 }
208
209
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_compute_gravity)
210 {
211 using namespace Eigen;
212 using namespace pinocchio;
213
214
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
215
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model, true);
216
217
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.);
218
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.);
219
220
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data_rnea(model);
221
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data(model);
222
223
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
224
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.
2 rnea(model, data_rnea, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv));
226
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeGeneralizedGravity(model, data, q);
227
228
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_rnea.tau.isApprox(data.g));
229
230 // Compare with Jcom
231
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 crba(model, data_rnea, q, Convention::WORLD);
232
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Data::Matrix3x Jcom = getJacobianComFromCrba(model, data_rnea);
233
234
4/8
✓ 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.
2 VectorXd g_ref(-data_rnea.mass[0] * Jcom.transpose() * Model::gravity981);
235
236
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(g_ref.isApprox(data.g));
237 2 }
238
239
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_compute_static_torque)
240 {
241 using namespace Eigen;
242 using namespace pinocchio;
243
244
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
245
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model, true);
246
247
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.);
248
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.);
249
250
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data_rnea(model);
251
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data(model);
252
253
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
254
255 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
256
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 ForceVector fext((size_t)model.njoints);
257
2/2
✓ Branch 4 taken 28 times.
✓ Branch 5 taken 1 times.
58 for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
258
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
56 (*it).setRandom();
259
260
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 rnea(model, data_rnea, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), fext);
261
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeStaticTorque(model, data, q, fext);
262
263
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 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_rnea.tau.isApprox(data.tau));
264
265 // Compare with Jcom + Jacobian of joint
266
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 crba(model, data_rnea, q, Convention::WORLD);
267
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Data::Matrix3x Jcom = getJacobianComFromCrba(model, data_rnea);
268
269
4/8
✓ 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.
2 VectorXd static_torque_ref = -data_rnea.mass[0] * Jcom.transpose() * Model::gravity981;
270
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeJointJacobians(model, data_rnea, q);
271
272
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data::Matrix6x J_local(6, model.nv);
273
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
56 for (JointIndex joint_id = 1; joint_id < (JointIndex)(model.njoints); ++joint_id)
274 {
275
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
54 J_local.setZero();
276
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
54 getJointJacobian(model, data_rnea, joint_id, LOCAL, J_local);
277
4/8
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
54 static_torque_ref -= J_local.transpose() * fext[joint_id].toVector();
278 }
279
280
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(static_torque_ref.isApprox(data.tau));
281 2 }
282
283
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_compute_coriolis)
284 {
285 using namespace Eigen;
286 using namespace pinocchio;
287
288 2 const double prec = Eigen::NumTraits<double>::dummy_precision();
289
290
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
291
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model, true);
292
293
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.);
294
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.);
295
296
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data_ref(model);
297
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data(model);
298
299
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
300
301
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd v(VectorXd::Random(model.nv));
302
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 computeCoriolisMatrix(model, data, q, Eigen::VectorXd::Zero(model.nv));
303
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK(data.C.isZero(prec));
304
305
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 model.gravity.setZero();
306
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 rnea(model, data_ref, q, v, VectorXd::Zero(model.nv));
307
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeJointJacobiansTimeVariation(model, data_ref, q, v);
308
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeCoriolisMatrix(model, data, q, v);
309
310
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.dJ.isApprox(data_ref.dJ));
311
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));
312
313
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd tau = data.C * v;
314
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(tau.isApprox(data_ref.tau, prec));
315
316
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 dccrba(model, data_ref, q, v);
317
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 crba(model, data_ref, q, Convention::WORLD);
318
319 2 const Data::Vector3 & com = data_ref.com[0];
320
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 Motion vcom(data_ref.vcom[0], Data::Vector3::Zero());
321
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 SE3 cM1(data.oMi[1]);
322
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 cM1.translation() -= com;
323
324
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((cM1.toDualActionMatrix() * data_ref.M.topRows<6>()).isApprox(data_ref.Ag, prec));
325
326
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 Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
327
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Force dh(data_ref.dAg * v);
328
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(dh.isApprox(dh_ref, prec));
329
330 {
331
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Data data_ref(model), data_ref_plus(model);
332
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
2 Eigen::MatrixXd dM(data.C + data.C.transpose());
333
334 2 const double alpha = 1e-8;
335
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Eigen::VectorXd q_plus(model.nq);
336
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 q_plus = integrate(model, q, alpha * v);
337
338
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 crba(model, data_ref, q, Convention::WORLD);
339
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 data_ref.M.triangularView<Eigen::StrictlyLower>() =
340
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>();
341
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 crba(model, data_ref_plus, q_plus, Convention::WORLD);
342
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 data_ref_plus.M.triangularView<Eigen::StrictlyLower>() =
343
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_plus.M.transpose().triangularView<Eigen::StrictlyLower>();
344
345
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
2 Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M) / alpha;
346
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(dM.isApprox(dM_ref, sqrt(alpha)));
347 2 }
348 2 }
349
350
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_rnea_mimic)
351 {
352
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
14 for (int i = 0; i < pinocchio::MimicTestCases::N_CASES; i++)
353 {
354
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 const pinocchio::MimicTestCases mimic_test_case(i);
355 12 const pinocchio::Model & model_mimic = mimic_test_case.model_mimic;
356 12 const pinocchio::Model & model_full = mimic_test_case.model_full;
357 12 const Eigen::MatrixXd & G = mimic_test_case.G;
358
359
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::Data data_nle_mimic(model_mimic);
360
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::Data data_rnea_mimic(model_mimic);
361
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::Data data_full(model_full);
362
363
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 Eigen::VectorXd q = pinocchio::randomConfiguration(model_mimic);
364
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
12 Eigen::VectorXd v = Eigen::VectorXd::Random(model_mimic.nv);
365
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
12 Eigen::VectorXd a = Eigen::VectorXd::Random(model_mimic.nv);
366
367
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 Eigen::VectorXd q_full(model_full.nq);
368
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;
369
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 * v;
370
371
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 mimic_test_case.toFull(q, q_full);
372
373
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::crba(model_full, data_full, q_full, pinocchio::Convention::WORLD);
374
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 data_full.M.triangularView<Eigen::StrictlyLower>() =
375
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>();
376
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
12 const Eigen::VectorXd nle = pinocchio::nonLinearEffects(model_full, data_full, q_full, v_full);
377
378 // // Use equation of motion to compute tau from a_reduced
379
8/16
✓ 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.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
12 Eigen::VectorXd tau_ref = G.transpose() * data_full.M * G * a + (G.transpose() * nle);
380
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::rnea(model_mimic, data_rnea_mimic, q, v, a);
381
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(tau_ref.isApprox(data_rnea_mimic.tau));
382
383 // NLE
384
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::Data data_nle(model_mimic);
385
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::Data data_ref_nle(model_mimic);
386 Eigen::VectorXd tau_ref_nle =
387
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.
12 pinocchio::rnea(model_mimic, data_ref_nle, q, v, Eigen::VectorXd::Zero(model_mimic.nv));
388
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
12 Eigen::VectorXd tau_nle = pinocchio::nonLinearEffects(model_mimic, data_nle, q, v);
389
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(tau_nle.isApprox(tau_ref_nle));
390
391 // Generalized Gravity
392
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::Data data_ref_gg(model_mimic);
393
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::Data data_gg(model_mimic);
394
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 Eigen::VectorXd tau_ref_gg = pinocchio::rnea(
395
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 model_mimic, data_ref_gg, q, Eigen::VectorXd::Zero(model_mimic.nv),
396
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
24 Eigen::VectorXd::Zero(model_mimic.nv));
397
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
12 Eigen::VectorXd tau_gg = pinocchio::computeGeneralizedGravity(model_mimic, data_gg, q);
398
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(tau_gg.isApprox(tau_ref_gg));
399
400 // Static Torque
401 typedef PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::Force) ForceVector;
402
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
12 ForceVector fext((size_t)model_mimic.njoints);
403
2/2
✓ Branch 4 taken 168 times.
✓ Branch 5 taken 6 times.
348 for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
404
1/2
✓ Branch 2 taken 168 times.
✗ Branch 3 not taken.
336 (*it).setRandom();
405
406
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::Data data_ref_st(model_mimic);
407
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::Data data_st(model_mimic);
408
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 pinocchio::rnea(
409
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 model_mimic, data_ref_st, q, Eigen::VectorXd::Zero(model_mimic.nv),
410
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
12 Eigen::VectorXd::Zero(model_mimic.nv), fext);
411
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
12 Eigen::VectorXd tau_st = pinocchio::computeStaticTorque(model_mimic, data_gg, q, fext);
412
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(tau_st.isApprox(data_ref_st.tau));
413 12 }
414 2 }
415
416 BOOST_AUTO_TEST_SUITE_END()
417