GCC Code Coverage Report

Directory: ./
File: unittest/rnea-second-order-derivatives.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 147 147 100.0%
Branches: 334 644 51.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4
5 #include "pinocchio/multibody/model.hpp"
6 #include "pinocchio/multibody/data.hpp"
7 #include "pinocchio/algorithm/jacobian.hpp"
8 #include "pinocchio/algorithm/joint-configuration.hpp"
9 #include "pinocchio/algorithm/rnea-second-order-derivatives.hpp"
10 #include "pinocchio/algorithm/rnea-derivatives.hpp"
11 #include "pinocchio/multibody/sample-models.hpp"
12
13 #include <iostream>
14
15 #include <boost/test/unit_test.hpp>
16 #include <boost/utility/binary.hpp>
17
18 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
19
20
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_derivatives_SO)
21 {
22 using namespace Eigen;
23 using namespace pinocchio;
24
25
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Model model;
26
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 buildModels::humanoidRandom(model);
27
28
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Data data(model), data_fd(model);
29
30
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.);
31
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.);
32
33
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
34
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));
35
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));
36
37 // check with only q non-zero
38
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Tensor3x dtau2_dq(model.nv, model.nv, model.nv);
39
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Tensor3x dtau2_dv(model.nv, model.nv, model.nv);
40
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Tensor3x dtau2_dqdv(model.nv, model.nv, model.nv);
41
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Tensor3x dtau2_dadq(model.nv, model.nv, model.nv);
42
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dq.setZero();
43
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dv.setZero();
44
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dqdv.setZero();
45
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dadq.setZero();
46
47
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ComputeRNEASecondOrderDerivatives(
48
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 model, data, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), dtau2_dq, dtau2_dv,
49 dtau2_dqdv, dtau2_dadq);
50
51
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Tensor3x dtau2_dq_fd(model.nv, model.nv, model.nv);
52
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Tensor3x dtau2_dv_fd(model.nv, model.nv, model.nv);
53
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Tensor3x dtau2_dqdv_fd(model.nv, model.nv, model.nv);
54
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Tensor3x dtau2_dadq_fd(model.nv, model.nv, model.nv);
55
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dq_fd.setZero();
56
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dv_fd.setZero();
57
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dqdv_fd.setZero();
58
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dadq_fd.setZero();
59
60
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd drnea_dq_plus(MatrixXd::Zero(model.nv, model.nv));
61
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd drnea_dv_plus(MatrixXd::Zero(model.nv, model.nv));
62
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd drnea_da_plus(MatrixXd::Zero(model.nv, model.nv));
63
64
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd temp1(MatrixXd::Zero(model.nv, model.nv));
65
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd temp2(MatrixXd::Zero(model.nv, model.nv));
66
67
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd v_eps(VectorXd::Zero(model.nv));
68
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q_plus(model.nq);
69
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd v_plus(model.nv);
70
71
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd rnea_partial_dq(MatrixXd::Zero(model.nv, model.nv));
72
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd rnea_partial_dv(MatrixXd::Zero(model.nv, model.nv));
73
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd rnea_partial_da(MatrixXd::Zero(model.nv, model.nv));
74
75
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeRNEADerivatives(
76
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 model, data, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), rnea_partial_dq,
77 rnea_partial_dv, rnea_partial_da);
78
79 2 const double alpha = 1e-7;
80
81
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
82 {
83
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] += alpha;
84
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 q_plus = integrate(model, q, v_eps);
85
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 computeRNEADerivatives(
86
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 model, data_fd, q_plus, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), drnea_dq_plus,
87 drnea_dv_plus, drnea_da_plus);
88
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 temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
89
2/2
✓ Branch 0 taken 1024 times.
✓ Branch 1 taken 32 times.
 2112 for (int ii = 0; ii < model.nv; ii++)
90 {
91
2/2
✓ Branch 0 taken 32768 times.
✓ Branch 1 taken 1024 times.
 67584 for (int jj = 0; jj < model.nv; jj++)
92 {
93
2/4
✓ Branch 1 taken 32768 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32768 times.
✗ Branch 5 not taken.
 65536 dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
94 }
95 }
96
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] -= alpha;
97 }
98
99
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 Map<VectorXd> mq(dtau2_dq.data(), dtau2_dq.size());
100
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 Map<VectorXd> mq_fd(dtau2_dq_fd.data(), dtau2_dq_fd.size());
101
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(mq.isApprox(mq_fd, sqrt(alpha)));
102
103 // Check with q and a non zero
104
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dq.setZero();
105
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dv.setZero();
106
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dqdv.setZero();
107
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dadq.setZero();
108
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ComputeRNEASecondOrderDerivatives(
109
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 model, data, q, VectorXd::Zero(model.nv), a, dtau2_dq, dtau2_dv, dtau2_dqdv, dtau2_dadq);
110
111
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 rnea_partial_dq.setZero();
112
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 rnea_partial_dv.setZero();
113
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 rnea_partial_da.setZero();
114
115
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dq_fd.setZero();
116
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dadq_fd.setZero();
117
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 drnea_dq_plus.setZero();
118
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 drnea_dv_plus.setZero();
119
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 drnea_da_plus.setZero();
120
121
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeRNEADerivatives(
122
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 model, data, q, VectorXd::Zero(model.nv), a, rnea_partial_dq, rnea_partial_dv, rnea_partial_da);
123
124
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
125 {
126
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] += alpha;
127
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 q_plus = integrate(model, q, v_eps);
128
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 computeRNEADerivatives(
129
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 model, data_fd, q_plus, VectorXd::Zero(model.nv), a, drnea_dq_plus, drnea_dv_plus,
130 drnea_da_plus);
131
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 temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
132
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 temp2 = (drnea_da_plus - rnea_partial_da) / alpha;
133
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 temp2.triangularView<Eigen::StrictlyLower>() =
134
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.
 128 temp2.transpose().triangularView<Eigen::StrictlyLower>();
135
2/2
✓ Branch 0 taken 1024 times.
✓ Branch 1 taken 32 times.
 2112 for (int ii = 0; ii < model.nv; ii++)
136 {
137
2/2
✓ Branch 0 taken 32768 times.
✓ Branch 1 taken 1024 times.
 67584 for (int jj = 0; jj < model.nv; jj++)
138 {
139
2/4
✓ Branch 1 taken 32768 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32768 times.
✗ Branch 5 not taken.
 65536 dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
140
2/4
✓ Branch 1 taken 32768 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32768 times.
✗ Branch 5 not taken.
 65536 dtau2_dadq_fd(jj, ii, k) = temp2(jj, ii);
141 }
142 }
143
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] -= alpha;
144 }
145
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 Map<VectorXd> maq(dtau2_dadq.data(), dtau2_dadq.size());
146
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 Map<VectorXd> maq_fd(dtau2_dadq_fd.data(), dtau2_dadq_fd.size());
147
148
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 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(mq.isApprox(mq_fd, sqrt(alpha)));
149
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(maq.isApprox(maq_fd, sqrt(alpha)));
150
151 // Check with q,v and a non zero
152
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dq.setZero();
153
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dv.setZero();
154
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dqdv.setZero();
155
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dadq.setZero();
156
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ComputeRNEASecondOrderDerivatives(
157 model, data, q, v, a, dtau2_dq, dtau2_dv, dtau2_dqdv, dtau2_dadq);
158
159
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 rnea_partial_dq.setZero();
160
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 rnea_partial_dv.setZero();
161
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 rnea_partial_da.setZero();
162
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeRNEADerivatives(model, data, q, v, a, rnea_partial_dq, rnea_partial_dv, rnea_partial_da);
163
164
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dq_fd.setZero();
165
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dadq_fd.setZero();
166
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 drnea_dq_plus.setZero();
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 drnea_dv_plus.setZero();
168
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 drnea_da_plus.setZero();
169
170
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
171 {
172
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] += alpha;
173
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 q_plus = integrate(model, q, v_eps);
174
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 computeRNEADerivatives(
175 model, data_fd, q_plus, v, a, drnea_dq_plus, drnea_dv_plus, drnea_da_plus);
176
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 temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
177
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 temp2 = (drnea_da_plus - rnea_partial_da) / alpha;
178
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 temp2.triangularView<Eigen::StrictlyLower>() =
179
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.
 128 temp2.transpose().triangularView<Eigen::StrictlyLower>();
180
2/2
✓ Branch 0 taken 1024 times.
✓ Branch 1 taken 32 times.
 2112 for (int ii = 0; ii < model.nv; ii++)
181 {
182
2/2
✓ Branch 0 taken 32768 times.
✓ Branch 1 taken 1024 times.
 67584 for (int jj = 0; jj < model.nv; jj++)
183 {
184
2/4
✓ Branch 1 taken 32768 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32768 times.
✗ Branch 5 not taken.
 65536 dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
185
2/4
✓ Branch 1 taken 32768 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32768 times.
✗ Branch 5 not taken.
 65536 dtau2_dadq_fd(jj, ii, k) = temp2(jj, ii);
186 }
187 }
188
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] -= alpha;
189 }
190
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dv_fd.setZero();
191
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dtau2_dqdv_fd.setZero();
192
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
193 {
194
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] += alpha;
195
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 v_plus = v + v_eps;
196
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 computeRNEADerivatives(
197 model, data_fd, q, v_plus, a, drnea_dq_plus, drnea_dv_plus, drnea_da_plus);
198
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 temp1 = (drnea_dv_plus - rnea_partial_dv) / alpha;
199
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 temp2 = (drnea_dq_plus - rnea_partial_dq) / alpha;
200
2/2
✓ Branch 0 taken 1024 times.
✓ Branch 1 taken 32 times.
 2112 for (int ii = 0; ii < model.nv; ii++)
201 {
202
2/2
✓ Branch 0 taken 32768 times.
✓ Branch 1 taken 1024 times.
 67584 for (int jj = 0; jj < model.nv; jj++)
203 {
204
2/4
✓ Branch 1 taken 32768 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32768 times.
✗ Branch 5 not taken.
 65536 dtau2_dv_fd(jj, ii, k) = temp1(jj, ii);
205
2/4
✓ Branch 1 taken 32768 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32768 times.
✗ Branch 5 not taken.
 65536 dtau2_dqdv_fd(jj, ii, k) = temp2(jj, ii);
206 }
207 }
208
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] -= alpha;
209 }
210
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 Map<VectorXd> mv(dtau2_dv.data(), dtau2_dv.size());
211
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 Map<VectorXd> mv_fd(dtau2_dv_fd.data(), dtau2_dv_fd.size());
212
213
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 Map<VectorXd> mqv(dtau2_dqdv.data(), dtau2_dqdv.size());
214
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 Map<VectorXd> mqv_fd(dtau2_dqdv_fd.data(), dtau2_dqdv_fd.size());
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(mq.isApprox(mq_fd, sqrt(alpha)));
217
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(maq.isApprox(maq_fd, sqrt(alpha)));
218
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(mv.isApprox(mv_fd, sqrt(alpha)));
219
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 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(mqv.isApprox(mqv_fd, sqrt(alpha)));
220
221
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data2(model);
222
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ComputeRNEASecondOrderDerivatives(model, data2, q, v, a);
223
224
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 Map<VectorXd> mq2(data2.d2tau_dqdq.data(), (data2.d2tau_dqdq).size());
225
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 Map<VectorXd> mv2(data2.d2tau_dvdv.data(), (data2.d2tau_dvdv).size());
226
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 Map<VectorXd> mqv2(data2.d2tau_dqdv.data(), (data2.d2tau_dqdv).size());
227
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 Map<VectorXd> maq2(data2.d2tau_dadq.data(), (data2.d2tau_dadq).size());
228
229
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(mq.isApprox(mq2, sqrt(alpha)));
230
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(mv.isApprox(mv2, sqrt(alpha)));
231
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(mqv.isApprox(mqv2, sqrt(alpha)));
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 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(maq.isApprox(maq2, sqrt(alpha)));
233 2 }
234
235 BOOST_AUTO_TEST_SUITE_END()
236