GCC Code Coverage Report


Directory: ./
File: unittest/rnea-derivatives.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 325 325 100.0%
Branches: 1054 2068 51.0%

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/kinematics.hpp"
10 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
12 #include "pinocchio/algorithm/rnea-derivatives.hpp"
13 #include "pinocchio/algorithm/crba.hpp"
14 #include "pinocchio/multibody/sample-models.hpp"
15
16 #include <iostream>
17
18 #include <boost/test/unit_test.hpp>
19 #include <boost/utility/binary.hpp>
20
21 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
22
23
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_generalized_gravity_derivatives)
24 {
25 using namespace Eigen;
26 using namespace pinocchio;
27
28
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
29
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model);
30
31
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);
32
33
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.);
34
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.);
35
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
36
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd v(VectorXd::Zero(model.nv));
37
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd a(VectorXd::Zero(model.nv));
38
39 // Check againt non-derivative algo
40
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd g_partial_dq(model.nv, model.nv);
41
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 g_partial_dq.setZero();
42
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeGeneralizedGravityDerivatives(model, data, q, g_partial_dq);
43
44
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd g0 = computeGeneralizedGravity(model, data_fd, q);
45
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.g.isApprox(g0));
46
47
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd g_partial_dq_fd(model.nv, model.nv);
48
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 g_partial_dq_fd.setZero();
49
50
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
51
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q_plus(model.nq);
52
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd g_plus(model.nv);
53 2 const double alpha = 1e-8;
54
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
55 {
56
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] += alpha;
57
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 q_plus = integrate(model, q, v_eps);
58
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
64 g_plus = computeGeneralizedGravity(model, data_fd, q_plus);
59
60
4/8
✓ 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.
64 g_partial_dq_fd.col(k) = (g_plus - g0) / alpha;
61
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] -= alpha;
62 }
63
64
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(g_partial_dq.isApprox(g_partial_dq_fd, sqrt(alpha)));
65 2 }
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_generalized_gravity_derivatives_fext)
68 {
69 using namespace Eigen;
70 using namespace pinocchio;
71
72
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
73
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model);
74
75
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);
76
77
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.);
78
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.);
79
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
80
81 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
82
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 ForceVector fext((size_t)model.njoints);
83
2/2
✓ Branch 4 taken 28 times.
✓ Branch 5 taken 1 times.
58 for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
84
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
56 (*it).setRandom();
85
86 // Check againt non-derivative algo
87
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd static_vec_partial_dq(model.nv, model.nv);
88
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 static_vec_partial_dq.setZero();
89
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeStaticTorqueDerivatives(model, data, q, fext, static_vec_partial_dq);
90
91
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd tau0 = computeStaticTorque(model, data_fd, q, fext);
92
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.tau.isApprox(tau0));
93
94
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd static_vec_partial_dq_fd(model.nv, 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 v_eps(Eigen::VectorXd::Zero(model.nv));
97
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q_plus(model.nq);
98
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd tau_plus(model.nv);
99 2 const double alpha = 1e-8;
100
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
101 {
102
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] += alpha;
103
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 q_plus = integrate(model, q, v_eps);
104
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
64 tau_plus = computeStaticTorque(model, data_fd, q_plus, fext);
105
106
4/8
✓ 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.
64 static_vec_partial_dq_fd.col(k) = (tau_plus - tau0) / alpha;
107
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] = 0.;
108 }
109
110
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(static_vec_partial_dq.isApprox(static_vec_partial_dq_fd, sqrt(alpha)));
111 2 }
112
113
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)
114 {
115 using namespace Eigen;
116 using namespace pinocchio;
117
118
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
119
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model);
120
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);
121
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 Data data(model), data_fd(model), data_ref(model);
123
124
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.);
125
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.);
126
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
127
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));
128
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));
129
130 /// Check againt computeGeneralizedGravityDerivatives
131
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_dq(model.nv, model.nv);
132
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq.setZero();
133
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_dv(model.nv, model.nv);
134
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dv.setZero();
135
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_da(model.nv, model.nv);
136
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_da.setZero();
137
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeRNEADerivatives(
138
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,
139 rnea_partial_dv, rnea_partial_da);
140
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_ref, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv));
141
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
56 for (Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
142 {
143
8/16
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 27 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 27 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 27 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 27 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 27 times.
54 BOOST_CHECK(data.of[k].isApprox(data.oMi[k].act(data_ref.f[k])));
144 }
145
146
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd g_partial_dq(model.nv, model.nv);
147
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 g_partial_dq.setZero();
148
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeGeneralizedGravityDerivatives(model, data_ref, q, g_partial_dq);
149
150
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.dFdq.isApprox(data_ref.dFdq));
151
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(rnea_partial_dq.isApprox(g_partial_dq));
152
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.tau.isApprox(data_ref.g));
153
154
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 VectorXd tau0 = rnea(model, data_fd, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv));
155
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_dq_fd(model.nv, model.nv);
156
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq_fd.setZero();
157
158
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));
159
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q_plus(model.nq);
160
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd tau_plus(model.nv);
161 2 const double alpha = 1e-8;
162
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
163 {
164
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] += alpha;
165
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 q_plus = integrate(model, q, v_eps);
166
4/8
✓ 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.
64 tau_plus = rnea(model, data_fd, q_plus, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv));
167
168
4/8
✓ 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.
64 rnea_partial_dq_fd.col(k) = (tau_plus - tau0) / alpha;
169
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] -= alpha;
170 }
171
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(rnea_partial_dq.isApprox(rnea_partial_dq_fd, sqrt(alpha)));
172
173 // Check with q and a non zero
174
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 tau0 = rnea(model, data_fd, q, 0 * v, a);
175
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq_fd.setZero();
176
177
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
178 {
179
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] += alpha;
180
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 q_plus = integrate(model, q, v_eps);
181
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 tau_plus = rnea(model, data_fd, q_plus, VectorXd::Zero(model.nv), a);
182
183
4/8
✓ 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.
64 rnea_partial_dq_fd.col(k) = (tau_plus - tau0) / alpha;
184
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] -= alpha;
185 }
186
187
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq.setZero();
188
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeRNEADerivatives(
189
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);
190
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 forwardKinematics(model, data_ref, q, VectorXd::Zero(model.nv), a);
191
192
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
56 for (Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
193 {
194
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
195
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
196
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
197
8/16
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 27 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 27 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 27 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 27 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 27 times.
54 BOOST_CHECK(data.oh[k].isApprox(Force::Zero()));
198 }
199
200
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.tau.isApprox(tau0));
201
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(rnea_partial_dq.isApprox(rnea_partial_dq_fd, sqrt(alpha)));
202
203 // Check with q and v non zero
204
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 const Motion gravity(model.gravity);
205
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 model.gravity.setZero();
206
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 tau0 = rnea(model, data_fd, q, v, VectorXd::Zero(model.nv));
207
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq_fd.setZero();
208
209
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
210 {
211
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] += alpha;
212
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 q_plus = integrate(model, q, v_eps);
213
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 tau_plus = rnea(model, data_fd, q_plus, v, VectorXd::Zero(model.nv));
214
215
4/8
✓ 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.
64 rnea_partial_dq_fd.col(k) = (tau_plus - tau0) / alpha;
216
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] -= alpha;
217 }
218
219
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd v_plus(v);
220
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_dv_fd(model.nv, model.nv);
221
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dv_fd.setZero();
222
223
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
224 {
225
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_plus[k] += alpha;
226
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 tau_plus = rnea(model, data_fd, q, v_plus, VectorXd::Zero(model.nv));
227
228
4/8
✓ 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.
64 rnea_partial_dv_fd.col(k) = (tau_plus - tau0) / alpha;
229
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_plus[k] -= alpha;
230 }
231
232
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq.setZero();
233
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dv.setZero();
234
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeRNEADerivatives(
235
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 model, data, q, v, VectorXd::Zero(model.nv), rnea_partial_dq, rnea_partial_dv, rnea_partial_da);
236
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 forwardKinematics(model, data_ref, q, v, VectorXd::Zero(model.nv));
237
238
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
56 for (Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
239 {
240
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
241
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
242
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
243 }
244
245
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(data.tau.isApprox(tau0));
246
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(rnea_partial_dq.isApprox(rnea_partial_dq_fd, sqrt(alpha)));
247
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(rnea_partial_dv.isApprox(rnea_partial_dv_fd, sqrt(alpha)));
248
249 // std::cout << "rnea_partial_dv:\n" << rnea_partial_dv.block<10,10>(0,0) << std::endl;
250 // std::cout << "rnea_partial_dv ref:\n" << rnea_partial_dv_fd.block<10,10>(0,0) << std::endl;
251 // std::cout << "rnea_partial_dv:\n" << rnea_partial_dv.topRows<10>() << std::endl;
252 // std::cout << "rnea_partial_dv ref:\n" << rnea_partial_dv_fd.topRows<10>() << std::endl;
253 // Check with q, v and a non zero
254
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 model.gravity = gravity;
255
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 v_plus = v;
256
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 tau0 = rnea(model, data_fd, q, v, a);
257
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq_fd.setZero();
258
259
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
260 {
261
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] += alpha;
262
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 q_plus = integrate(model, q, v_eps);
263
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
64 tau_plus = rnea(model, data_fd, q_plus, v, a);
264
265
4/8
✓ 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.
64 rnea_partial_dq_fd.col(k) = (tau_plus - tau0) / alpha;
266
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] -= alpha;
267 }
268
269
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dv_fd.setZero();
270
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
271 {
272
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_plus[k] += alpha;
273
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
64 tau_plus = rnea(model, data_fd, q, v_plus, a);
274
275
4/8
✓ 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.
64 rnea_partial_dv_fd.col(k) = (tau_plus - tau0) / alpha;
276
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_plus[k] -= alpha;
277 }
278
279
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq.setZero();
280
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dv.setZero();
281
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);
282
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 forwardKinematics(model, data_ref, q, v, a);
283
284
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
56 for (Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
285 {
286
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
287
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
288
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
289 }
290
291
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeJointJacobiansTimeVariation(model, data_ref, q, v);
292
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));
293
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 crba(model, data_ref, q, Convention::WORLD);
294
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 data_ref.M.triangularView<Eigen::StrictlyLower>() =
295
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>();
296
297
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_da.triangularView<Eigen::StrictlyLower>() =
298
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 rnea_partial_da.transpose().triangularView<Eigen::StrictlyLower>();
299
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(rnea_partial_da.isApprox(data_ref.M));
300
301
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.tau.isApprox(tau0));
302
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(rnea_partial_dq.isApprox(rnea_partial_dq_fd, sqrt(alpha)));
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(rnea_partial_dv.isApprox(rnea_partial_dv_fd, sqrt(alpha)));
304
305
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data2(model);
306
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeRNEADerivatives(model, data2, q, v, a);
307
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 data2.M.triangularView<Eigen::StrictlyLower>() =
308
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 data2.M.transpose().triangularView<Eigen::StrictlyLower>();
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(rnea_partial_dq.isApprox(data2.dtau_dq));
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(rnea_partial_dv.isApprox(data2.dtau_dv));
312
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(rnea_partial_da.isApprox(data2.M));
313 2 }
314
315
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_fext)
316 {
317 using namespace Eigen;
318 using namespace pinocchio;
319
320
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
321
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model);
322 typedef Model::Force Force;
323
324
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
2 Data data(model), data_fd(model), data_ref(model);
325
326
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.);
327
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.);
328
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
329
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));
330
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));
331
332 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
333
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 ForceVector fext((size_t)model.njoints);
334
2/2
✓ Branch 4 taken 28 times.
✓ Branch 5 taken 1 times.
58 for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
335
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
56 (*it).setRandom();
336
337 /// Check againt computeGeneralizedGravityDerivatives
338
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_dq(model.nv, model.nv);
339
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq.setZero();
340
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_dv(model.nv, model.nv);
341
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dv.setZero();
342
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_da(model.nv, model.nv);
343
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_da.setZero();
344
345
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeRNEADerivatives(
346 model, data, q, v, a, fext, rnea_partial_dq, rnea_partial_dv, rnea_partial_da);
347
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea(model, data_ref, q, v, a, fext);
348
349
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.tau.isApprox(data_ref.tau));
350
351
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeRNEADerivatives(model, data_ref, q, v, a);
352
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(rnea_partial_dv.isApprox(data_ref.dtau_dv));
353
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(rnea_partial_da.isApprox(data_ref.M));
354
355
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_dq_fd(model.nv, model.nv);
356
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq_fd.setZero();
357
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_dv_fd(model.nv, model.nv);
358
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dv_fd.setZero();
359
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_da_fd(model.nv, model.nv);
360
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_da_fd.setZero();
361
362
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));
363
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q_plus(model.nq);
364
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd tau_plus(model.nv);
365 2 const double eps = 1e-8;
366
367
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 const VectorXd tau_ref = rnea(model, data_ref, q, v, a, fext);
368
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
369 {
370
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] = eps;
371
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 q_plus = integrate(model, q, v_eps);
372
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
64 tau_plus = rnea(model, data_fd, q_plus, v, a, fext);
373
374
4/8
✓ 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.
64 rnea_partial_dq_fd.col(k) = (tau_plus - tau_ref) / eps;
375
376
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_eps[k] = 0.;
377 }
378
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(rnea_partial_dq.isApprox(rnea_partial_dq_fd, sqrt(eps)));
379
380
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd v_plus(v);
381
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
382 {
383
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_plus[k] += eps;
384
385
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
64 tau_plus = rnea(model, data_fd, q, v_plus, a, fext);
386
387
4/8
✓ 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.
64 rnea_partial_dv_fd.col(k) = (tau_plus - tau_ref) / eps;
388
389
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 v_plus[k] -= eps;
390 }
391
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(rnea_partial_dv.isApprox(rnea_partial_dv_fd, sqrt(eps)));
392
393
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd a_plus(a);
394
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
66 for (int k = 0; k < model.nv; ++k)
395 {
396
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 a_plus[k] += eps;
397
398
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
64 tau_plus = rnea(model, data_fd, q, v, a_plus, fext);
399
400
4/8
✓ 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.
64 rnea_partial_da_fd.col(k) = (tau_plus - tau_ref) / eps;
401
402
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
64 a_plus[k] -= eps;
403 }
404
405
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_da.triangularView<Eigen::Lower>() =
406
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 rnea_partial_da.transpose().triangularView<Eigen::Lower>();
407
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(rnea_partial_da.isApprox(rnea_partial_da_fd, sqrt(eps)));
408
409 // test the shortcut
410
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data_shortcut(model);
411
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeRNEADerivatives(model, data_shortcut, q, v, a, fext);
412
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_shortcut.dtau_dq.isApprox(rnea_partial_dq));
413
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_shortcut.dtau_dv.isApprox(rnea_partial_dv));
414
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 data_shortcut.M.triangularView<Eigen::Lower>() =
415
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_shortcut.M.transpose().triangularView<Eigen::Lower>();
416
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_shortcut.M.isApprox(rnea_partial_da));
417 2 }
418
419
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_vs_kinematics_derivatives)
420 {
421 using namespace Eigen;
422 using namespace pinocchio;
423
424
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
425
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model);
426
427
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_ref(model);
428
429
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.);
430
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.);
431
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
432
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));
433
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));
434
435 /// Check againt computeGeneralizedGravityDerivatives
436
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_dq(model.nv, model.nv);
437
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dq.setZero();
438
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_dv(model.nv, model.nv);
439
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_dv.setZero();
440
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 MatrixXd rnea_partial_da(model.nv, model.nv);
441
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 rnea_partial_da.setZero();
442
443
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);
444
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeForwardKinematicsDerivatives(model, data_ref, q, v, a);
445
446
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));
447
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));
448
449
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
56 for (size_t k = 1; k < (size_t)model.njoints; ++k)
450 {
451
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
452
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
453
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.oa[k].isApprox(data_ref.oa[k]));
454 }
455 2 }
456
457
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_multiple_calls)
458 {
459 using namespace Eigen;
460 using namespace pinocchio;
461
462
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
463
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model);
464
465
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Data data1(model), data2(model);
466
467
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.);
468
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.);
469
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
470
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));
471
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));
472
473
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeRNEADerivatives(model, data1, q, v, a);
474
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 data2 = data1;
475
476
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 1 times.
42 for (int k = 0; k < 20; ++k)
477 {
478
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
40 computeRNEADerivatives(model, data1, q, v, a);
479 }
480
481
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(data1.J.isApprox(data2.J));
482
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(data1.dJ.isApprox(data2.dJ));
483
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(data1.dVdq.isApprox(data2.dVdq));
484
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(data1.dAdq.isApprox(data2.dAdq));
485
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(data1.dAdv.isApprox(data2.dAdv));
486
487
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq));
488
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(data1.dFdv.isApprox(data2.dFdv));
489
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(data1.dFda.isApprox(data2.dFda));
490
491
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(data1.dtau_dq.isApprox(data2.dtau_dq));
492
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(data1.dtau_dv.isApprox(data2.dtau_dv));
493
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(data1.M.isApprox(data2.M));
494 2 }
495
496
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_get_coriolis)
497 {
498 using namespace Eigen;
499 using namespace pinocchio;
500
501
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Model model;
502
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 buildModels::humanoidRandom(model);
503
504
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.);
505
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.);
506
507
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data_ref(model);
508
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Data data(model);
509
510
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 VectorXd q = randomConfiguration(model);
511
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);
512
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 VectorXd tau = VectorXd::Random(model.nv);
513
514
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeCoriolisMatrix(model, data_ref, q, v);
515
516
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 computeRNEADerivatives(model, data, q, v, tau);
517
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 getCoriolisMatrix(model, data);
518
519
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));
520
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));
521
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
522
2/2
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 1 times.
56 for (JointIndex k = 1; k < model.joints.size(); ++k)
523 {
524
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.B[k].isApprox(data_ref.B[k]));
525
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
54 BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
526 }
527
528
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(data.C.isApprox(data_ref.C));
529 2 }
530
531 BOOST_AUTO_TEST_SUITE_END()
532