GCC Code Coverage Report

Directory: ./
File: unittest/value.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 113 0.0%
Branches: 0 990 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2018 CNRS
3 //
4
5 /*
6 * Compare the value obtained with the RNEA with the values obtained from
7 * RBDL. The test is not complete. It only validates the RNEA for the revolute
8 * joints. The free-flyer is not tested. It should be extended to account for
9 * the free flyer and for the other algorithms.
10 *
11 * Additionnal notes: the RNEA is an algorithm that can be used to validate
12 * many others (in particular, the mass matrix (CRBA) can be numerically
13 * validated from the RNEA, then the center-of-mass jacobian can be validated
14 * from the mass matrix, etc.
15 *
16 */
17
18 #include <iostream>
19 #include <iomanip>
20
21 #include "pinocchio/multibody/model.hpp"
22 #include "pinocchio/multibody/data.hpp"
23 #include "pinocchio/parsers/urdf.hpp"
24 #include "pinocchio/algorithm/rnea.hpp"
25 #include "pinocchio/algorithm/kinematics.hpp"
26
27 #include <boost/test/unit_test.hpp>
28
29 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
30
31 ✗ BOOST_AUTO_TEST_CASE(test_000)
32 {
33 ✗ std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
34
35 ✗ pinocchio::Model model;
36 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
37 ✗ model.gravity.linear(Eigen::Vector3d(0, 0, -9.8));
38 ✗ pinocchio::Data data(model);
39
40 ✗ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
41 ✗ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
42 ✗ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
43
44 ✗ std::cout << std::setprecision(10);
45
46 ✗ Eigen::VectorXd expected(model.nv);
47 ✗ expected << 0, 0, 1281.84, 0, -40.5132, 0, 4.4492, -1.5386, 0, -1.5386, -1.5386, 0, -4.4492,
48 ✗ -1.5386, 0, -1.5386, -1.5386, 0, -37.436, 0, 0, -2.548, 0, 0, 0.392, 0, 0.392, 0, -2.548, 0, 0,
49 ✗ 0.392, 0, 0.392, 0;
50 ✗ q = Eigen::VectorXd::Zero(model.nq);
51 ✗ v = Eigen::VectorXd::Zero(model.nv);
52 ✗ a = Eigen::VectorXd::Zero(model.nv);
53 ✗ rnea(model, data, q, v, a);
54 ✗ BOOST_CHECK(expected.isApprox(data.tau, 1e-12));
55 }
56
57 ✗ BOOST_AUTO_TEST_CASE(test_0V0)
58 {
59 ✗ std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
60
61 ✗ pinocchio::Model model;
62 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
63 ✗ model.gravity.linear(Eigen::Vector3d(0, 0, -9.8));
64 ✗ pinocchio::Data data(model);
65
66 ✗ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
67 ✗ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
68 ✗ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
69
70 ✗ std::cout << std::setprecision(10);
71
72 ✗ Eigen::VectorXd expected(model.nv);
73 ✗ expected << -48.10636, -73.218816, 1384.901025, -7.2292939, -186.342371, -20.6685852, -0.78887946,
74 ✗ -1.869651075, 1.8889752, -2.036768175, -2.105948175, -1.023232, -18.3738505, -4.133954895,
75 ✗ 9.0456456, -4.276615035, -4.143427035, -2.534896, -180.338765, 15.71570676, -29.8639164,
76 ✗ -59.917862, -2.3307916, -2.7648728, -45.76782776, 3.4151272, -18.4320456, -4.5768072,
77 ✗ -76.60945104, -0.5897908, -2.640844, -63.93417064, 5.359156, -25.8309196, -6.976116;
78 ✗ q = Eigen::VectorXd::Zero(model.nq);
79 ✗ for (int i = 6; i < model.nv; ++i)
80 ✗ v[i] = i / 10.;
81 ✗ a = Eigen::VectorXd::Zero(model.nv);
82 ✗ rnea(model, data, q, v, a);
83 // std::cout << (expected-data.tau).norm() << std::endl;
84 ✗ BOOST_CHECK(expected.isApprox(data.tau, 1e-7));
85 }
86
87 ✗ BOOST_AUTO_TEST_CASE(test_0VA)
88 {
89 ✗ std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
90
91 ✗ pinocchio::Model model;
92 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
93 ✗ model.gravity.linear(Eigen::Vector3d(0, 0, -9.8));
94 ✗ pinocchio::Data data(model);
95
96 ✗ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
97 ✗ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
98 ✗ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
99
100 ✗ std::cout << std::setprecision(10);
101 ✗ Eigen::VectorXd expected(model.nv);
102 ✗ expected << -15.3331636, -0.67891816, 1273.80521, 102.9435113, 110.3509945, 81.52296995,
103 ✗ 13.31476408, 14.26606068, 3.682505252, 9.048274318, 4.663303518, 2.05308568, 12.54347834,
104 ✗ 25.92680911, 6.327105656, 16.71123385, 8.96650473, 3.54200704, 70.15812475, 77.02410963,
105 ✗ 73.81994844, 41.73185754, 28.75786872, 28.94251127, 31.65847724, 20.40431127, 18.18579154,
106 ✗ 6.838471928, 50.44193173, 34.07362801, 34.53507156, 38.33983417, 24.61507156, 22.2842788,
107 ✗ 8.23435884;
108 ✗ q = Eigen::VectorXd::Zero(model.nq);
109 ✗ for (int i = 6; i < model.nv; ++i)
110 ✗ v[i] = i / 100.;
111 ✗ for (int i = 6; i < model.nv; ++i)
112 ✗ a[i] = i / 10.;
113 ✗ rnea(model, data, q, v, a);
114 ✗ BOOST_CHECK(expected.isApprox(data.tau, 1e-6));
115 }
116
117 ✗ BOOST_AUTO_TEST_CASE(test_Q00)
118 {
119 ✗ std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
120
121 ✗ pinocchio::Model model;
122 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
123 ✗ model.gravity.linear(Eigen::Vector3d(0, 0, -9.8));
124 ✗ pinocchio::Data data(model);
125
126 ✗ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
127 ✗ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
128 ✗ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
129
130 ✗ std::cout << std::setprecision(10);
131
132 ✗ Eigen::VectorXd expected(model.nv);
133 ✗ expected << 5.367234435e-15, -2.587860481e-14, 1281.84, -133.3062501, 198.975587,
134 ✗ -7.120345979e-16, 15.06850407, 58.39287139, -22.14971864, 17.14327289, 1.291543104,
135 ✗ 0.7402017048, -4.386231387, 22.73949408, -19.01681794, 0.8839600793, -0.3197599308,
136 ✗ -0.466827706, 65.47086697, 32.71449398, -4.250622066, -0.7937685568, -0.15349648, -1.070480752,
137 ✗ -3.066302263, -0.3557903212, -0.2183951073, 0.182684221, -0.6648425468, -2.902772493,
138 ✗ 0.1250340934, 0.4402877138, -0.3158584741, -0.0865162794, 0.3918733239;
139 ✗ for (int i = 7; i < model.nq; ++i)
140 ✗ q[i] = (i - 1) / 10.;
141 ✗ v = Eigen::VectorXd::Zero(model.nv);
142 ✗ a = Eigen::VectorXd::Zero(model.nv);
143 ✗ rnea(model, data, q, v, a);
144 // std::cout << expected << "\n ---------------- \n"
145 // << data.tau << std::endl;
146
147 ✗ BOOST_CHECK(expected.isApprox(data.tau, 1e-6));
148 }
149
150 ✗ BOOST_AUTO_TEST_CASE(test_QVA)
151 {
152 ✗ std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
153
154 ✗ pinocchio::Model model;
155 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
156 ✗ model.gravity.linear(Eigen::Vector3d(0, 0, -9.8));
157 ✗ pinocchio::Data data(model);
158
159 ✗ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
160 ✗ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
161 ✗ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
162
163 ✗ std::cout << std::setprecision(10);
164 ✗ Eigen::VectorXd expected(model.nv);
165 ✗ expected << -1.911650826, 1.250211406, 1284.82058, -139.0188156, 201.744449, 1.554847332,
166 ✗ 15.1910084, 59.27339983, -21.70753738, 17.84339797, 1.754639468, 0.670280632, -2.968778954,
167 ✗ 23.0776205, -17.56870284, 1.765761886, 0.2889992363, -0.392159764, 68.83598707, 34.59002827,
168 ✗ -4.604435817, -0.3832225891, 1.085231916, -0.348635267, -2.831371037, -1.047616506,
169 ✗ -0.228384161, 0.5656880079, 1.302869049, 0.8481280783, 0.7042182131, 1.554751317, -0.3908790552,
170 ✗ -0.1294643218, 1.421077555;
171 ✗ for (int i = 7; i < model.nq; ++i)
172 ✗ q[i] = (i - 1) / 10.;
173 ✗ for (int i = 6; i < model.nv; ++i)
174 ✗ v[i] = i / 100.;
175 ✗ for (int i = 6; i < model.nv; ++i)
176 ✗ a[i] = i / 100.;
177 ✗ rnea(model, data, q, v, a);
178 ✗ BOOST_CHECK(expected.isApprox(data.tau, 1e-7));
179 }
180
181 BOOST_AUTO_TEST_SUITE_END()
182