GCC Code Coverage Report


Directory: ./
File: unittest/aba-derivatives.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 354 354 100.0%
Branches: 1405 2776 50.6%

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