GCC Code Coverage Report

Directory: ./
File: unittest/com.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 123 123 100.0%
Branches: 419 830 50.5%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2019 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/crba.hpp"
10 #include "pinocchio/algorithm/compute-all-terms.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
12 #include "pinocchio/algorithm/center-of-mass.hpp"
13 #include "pinocchio/utils/timer.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_com)
24 {
25 using namespace Eigen;
26 using namespace pinocchio;
27
28
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
29
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model);
30
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data(model);
31
32
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = VectorXd::Ones(model.nq);
33
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 q.middleRows<4>(3).normalize();
34
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd v = VectorXd::Ones(model.nv);
35
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd a = VectorXd::Ones(model.nv);
36
37
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 crba(model, data, q, Convention::WORLD);
38
39 /* Test COM against CRBA*/
40
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Vector3d com = centerOfMass(model, data, q);
41
8/16
✓ 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 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(data.com[0].isApprox(getComFromCrba(model, data), 1e-12));
42
43 /* Test COM against Jcom (both use different way to compute the COM). */
44
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 com = centerOfMass(model, data, q);
45
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jacobianCenterOfMass(model, data, q);
46
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(com.isApprox(data.com[0], 1e-12));
47
48 /* Test COM against Jcom (both use different way to compute the COM). */
49
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 centerOfMass(model, data, q, v, a);
50
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(com.isApprox(data.com[0], 1e-12));
51
52 /* Test vCoM against nle algorithm without gravity field */
53
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a.setZero();
54
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 model.gravity.setZero();
55
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 centerOfMass(model, data, q, v, a);
56
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 nonLinearEffects(model, data, q, v);
57
58
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 pinocchio::SE3::Vector3 acom_from_nle(data.nle.head<3>() / data.mass[0]);
59
9/18
✓ 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 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 2 BOOST_CHECK((data.liMi[1].rotation() * acom_from_nle).isApprox(data.acom[0], 1e-12));
60
61 /* Test Jcom against CRBA */
62
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::MatrixXd Jcom = jacobianCenterOfMass(model, data, q);
63
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(data.Jcom.isApprox(getJacobianComFromCrba(model, data), 1e-12));
64
65 /* Test CoM velocity againt jacobianCenterOfMass */
66
8/16
✓ 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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));
67
68
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 centerOfMass(model, data, q, v);
69 /* Test CoM velocity againt jacobianCenterOfMass */
70
8/16
✓ 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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));
71
72 // std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl;
73 // std::cout << "mass = [ " << data.mass[0] << " ];" << std::endl;
74 // std::cout << "Jcom = [ " << data.Jcom << " ];" << std::endl;
75 // std::cout << "M3 = [ " << data.M.topRows<3>() << " ];" << std::endl;
76 2 }
77
78
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_mass)
79 {
80 using namespace Eigen;
81 using namespace pinocchio;
82
83
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
84
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model);
85
86
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 double mass = computeTotalMass(model);
87
88
6/12
✓ 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 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_CHECK(mass == mass); // checking it is not NaN
89
90 2 double mass_check = 0.0;
91
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 56 for (size_t i = 1; i < (size_t)(model.njoints); ++i)
92 54 mass_check += model.inertias[i].mass();
93
94
6/12
✓ 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 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
 2 BOOST_CHECK_CLOSE(mass, mass_check, 1e-12);
95
96
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data1(model);
97
98
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 double mass_data = computeTotalMass(model, data1);
99
100
6/12
✓ 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 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_CHECK(mass_data == mass_data); // checking it is not NaN
101
6/12
✓ 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 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 1 times.
 2 BOOST_CHECK_CLOSE(mass, mass_data, 1e-12);
102
6/12
✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_CLOSE(data1.mass[0], mass_data, 1e-12);
103
104
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data2(model);
105
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = VectorXd::Ones(model.nq);
106
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 q.middleRows<4>(3).normalize();
107
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 centerOfMass(model, data2, q);
108
109
6/12
✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_CLOSE(data2.mass[0], mass, 1e-12);
110 2 }
111
112
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_subtree_masses)
113 {
114 using namespace Eigen;
115 using namespace pinocchio;
116
117
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
118
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model);
119
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data1(model);
121
122
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeSubtreeMasses(model, data1);
123
124
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data2(model);
125
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = VectorXd::Ones(model.nq);
126
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 q.middleRows<4>(3).normalize();
127
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 centerOfMass(model, data2, q);
128
129
2/2
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 1 times.
 58 for (size_t i = 0; i < (size_t)(model.njoints); ++i)
130 {
131
6/12
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 28 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 28 times.
✗ Branch 9 not taken.
✓ Branch 15 taken 28 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 28 times.
✗ Branch 19 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 28 times.
 56 BOOST_CHECK_CLOSE(data1.mass[i], data2.mass[i], 1e-12);
132 }
133 2 }
134
135 // BOOST_AUTO_TEST_CASE ( test_timings )
136 //{
137 // using namespace Eigen;
138 // using namespace pinocchio;
139 //
140 // pinocchio::Model model;
141 // pinocchio::buildModels::humanoidRandom(model);
142 // pinocchio::Data data(model);
143 //
144 // long flag = BOOST_BINARY(1111);
145 // PinocchioTicToc timer(PinocchioTicToc::US);
146 // #ifdef NDEBUG
147 // #ifdef _INTENSE_TESTING_
148 // const size_t NBT = 1000*1000;
149 // #else
150 // const size_t NBT = 10;
151 // #endif
152 // #else
153 // const size_t NBT = 1;
154 // std::cout << "(the time score in debug mode is not relevant) " ;
155 // #endif
156 //
157 // bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
158 // if(verbose) std::cout <<"--" << std::endl;
159 // Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
160 //
161 // if( flag >> 0 & 1 )
162 // {
163 // timer.tic();
164 // SMOOTH(NBT)
165 // {
166 // centerOfMass(model,data,q);
167 // }
168 // if(verbose) std::cout << "COM =\t";
169 // timer.toc(std::cout,NBT);
170 // }
171 //
172 // if( flag >> 1 & 1 )
173 // {
174 // timer.tic();
175 // SMOOTH(NBT)
176 // {
177 // centerOfMass(model,data,q,false);
178 // }
179 // if(verbose) std::cout << "Without sub-tree =\t";
180 // timer.toc(std::cout,NBT);
181 // }
182 //
183 // if( flag >> 2 & 1 )
184 // {
185 // timer.tic();
186 // SMOOTH(NBT)
187 // {
188 // jacobianCenterOfMass(model,data,q);
189 // }
190 // if(verbose) std::cout << "Jcom =\t";
191 // timer.toc(std::cout,NBT);
192 // }
193 // }
194
195
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_subtree_com_jacobian)
196 {
197 using namespace Eigen;
198 using namespace pinocchio;
199
200
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Model model;
201
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 buildModels::humanoidRandom(model);
202
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data(model);
203
204
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(1000);
205
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.lowerPositionLimit.head<3>() = -model.upperPositionLimit.head<3>();
206
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = pinocchio::randomConfiguration(model);
207
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);
208
209
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_ref(model);
210
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jacobianCenterOfMass(model, data_ref, q, true);
211
212
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 centerOfMass(model, data, q, v);
213
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x Jcom1(3, model.nv);
214
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Jcom1.setZero();
215
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jacobianSubtreeCenterOfMass(model, data, 0, Jcom1);
216
217
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 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(Jcom1.isApprox(data_ref.Jcom));
218
219
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 centerOfMass(model, data_ref, q, v, true);
220
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data_ref, q);
221
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Data::Matrix3x Jcom_extracted(3, model.nv), Jcom_fd(3, model.nv);
222
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Data data_extracted(model), data_fd(model);
223 2 const double eps = 1e-8;
224
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jacobianCenterOfMass(model, data_extracted, q);
225
226 // Get subtree jacobian and check that it is consistent with the com velocity
227
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x Jcom(3, model.nv);
228
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Jcom.setZero();
229
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 56 for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; joint_id++)
230 {
231 SE3::Vector3 subtreeComVelocityInWorld_ref =
232
3/6
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
 54 data_ref.oMi[joint_id].rotation() * data_ref.vcom[joint_id];
233
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 Jcom.setZero();
234
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 data.J.setZero();
235
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 jacobianSubtreeCenterOfMass(model, data, joint_id, Jcom);
236
237
11/22
✓ 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 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 42 not taken.
✓ Branch 43 taken 27 times.
 54 BOOST_CHECK(
238 data.J.middleCols(model.joints[joint_id].idx_v(), data.nvSubtree[joint_id])
239 .isApprox(data_ref.J.middleCols(model.joints[joint_id].idx_v(), data.nvSubtree[joint_id])));
240
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 54 SE3::Vector3 subtreeComVelocityInWorld = Jcom * v;
241
242
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 Jcom_extracted.setZero();
243
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 getJacobianSubtreeCenterOfMass(model, data_extracted, joint_id, Jcom_extracted);
244
245 // Check with finite differences
246
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 Eigen::VectorXd v_plus(model.nv);
247
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 v_plus.setZero();
248
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 centerOfMass(model, data_fd, q);
249
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 54 const SE3::Vector3 com = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
250
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 Jcom_fd.setZero();
251
2/2
✓ Branch 0 taken 864 times.
✓ Branch 1 taken 27 times.
 1782 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
252 {
253
1/2
✓ Branch 1 taken 864 times.
✗ Branch 2 not taken.
 1728 v_plus[k] = eps;
254
1/2
✓ Branch 1 taken 864 times.
✗ Branch 2 not taken.
 1728 Eigen::VectorXd q_plus = integrate(model, q, v_plus);
255
1/2
✓ Branch 1 taken 864 times.
✗ Branch 2 not taken.
 1728 centerOfMass(model, data_fd, q_plus);
256
1/2
✓ Branch 3 taken 864 times.
✗ Branch 4 not taken.
 1728 const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
257
4/8
✓ Branch 1 taken 864 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 864 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 864 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 864 times.
✗ Branch 11 not taken.
 1728 Jcom_fd.col(k) = (com_plus - com) / eps;
258
1/2
✓ Branch 1 taken 864 times.
✗ Branch 2 not taken.
 1728 v_plus[k] = 0.;
259 1728 }
260
261 // Eigen::VectorXd q_plus = integrate(model,q,v*eps);
262 // centerOfMass(model,data_fd,q_plus);
263 // const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
264 //
265 // const SE3::Vector3 vcom_subtree_fd = (com_plus - com)/eps;
266
267
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 14 taken 27 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 27 times.
 54 BOOST_CHECK(Jcom.isApprox(Jcom_fd, sqrt(eps)));
268
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 14 taken 27 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 27 times.
 54 BOOST_CHECK(Jcom_extracted.isApprox(Jcom_fd, sqrt(eps)));
269
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 15 taken 27 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 27 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 27 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 27 times.
 54 BOOST_CHECK(Jcom_extracted.isApprox(Jcom));
270
271
6/12
✓ 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 24 not taken.
✓ Branch 25 taken 27 times.
 54 BOOST_CHECK(std::fabs(data.mass[joint_id] - data_ref.mass[joint_id]) <= 1e-12);
272
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.com[joint_id].isApprox(data_ref.oMi[joint_id].act(data_ref.com[joint_id])));
273
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 15 taken 27 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 27 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 27 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 27 times.
 54 BOOST_CHECK(subtreeComVelocityInWorld.isApprox(subtreeComVelocityInWorld_ref));
274 54 }
275
276 // Other signatures
277
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_other_signature(model);
278
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x Jcom2(3, model.nv);
279
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Jcom2.setZero();
280
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jacobianSubtreeCenterOfMass(model, data_other_signature, q, 0, Jcom2);
281
282
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(Jcom2.isApprox(Jcom1));
283
284
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jacobianCenterOfMass(model, data, q);
285
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x Jcom3(3, model.nv);
286
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Jcom3.setZero();
287
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJacobianSubtreeCenterOfMass(model, data, 0, Jcom3);
288
289
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(Jcom3.isApprox(Jcom1));
290 2 }
291
292 BOOST_AUTO_TEST_SUITE_END()
293