Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2019 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
/* |
6 |
|
|
* Validate the sparse Cholesky decomposition of the mass matrix. The code |
7 |
|
|
* tests both the numerical value and the computation time. For a strong |
8 |
|
|
* computation benchmark, see benchmark/timings. |
9 |
|
|
* |
10 |
|
|
*/ |
11 |
|
|
|
12 |
|
|
#include "pinocchio/spatial/se3.hpp" |
13 |
|
|
#include "pinocchio/multibody/model.hpp" |
14 |
|
|
#include "pinocchio/multibody/data.hpp" |
15 |
|
|
#include "pinocchio/algorithm/crba.hpp" |
16 |
|
|
#include "pinocchio/algorithm/cholesky.hpp" |
17 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
18 |
|
|
#include "pinocchio/utils/timer.hpp" |
19 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
20 |
|
|
|
21 |
|
|
#include <iostream> |
22 |
|
|
#ifdef NDEBUG |
23 |
|
|
#include <Eigen/Cholesky> |
24 |
|
|
#endif |
25 |
|
|
|
26 |
|
|
#include <boost/test/unit_test.hpp> |
27 |
|
|
#include <boost/utility/binary.hpp> |
28 |
|
|
|
29 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
30 |
|
|
|
31 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_cholesky) |
32 |
|
|
{ |
33 |
|
|
using namespace Eigen; |
34 |
|
|
using namespace pinocchio; |
35 |
|
|
|
36 |
|
✗ |
pinocchio::Model model; |
37 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
38 |
|
✗ |
pinocchio::Data data(model); |
39 |
|
|
|
40 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
41 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
42 |
|
✗ |
VectorXd q = randomConfiguration(model); |
43 |
|
✗ |
data.M.fill(0); // Only nonzero coeff of M are initialized by CRBA. |
44 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
45 |
|
|
|
46 |
|
✗ |
pinocchio::cholesky::decompose(model, data); |
47 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
48 |
|
✗ |
data.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
49 |
|
|
|
50 |
|
✗ |
const Eigen::MatrixXd & U = data.U; |
51 |
|
✗ |
const Eigen::VectorXd & D = data.D; |
52 |
|
✗ |
const Eigen::MatrixXd & M = data.M; |
53 |
|
|
|
54 |
|
|
#ifndef NDEBUG |
55 |
|
✗ |
std::cout << "M = [\n" << M << "];" << std::endl; |
56 |
|
✗ |
std::cout << "U = [\n" << U << "];" << std::endl; |
57 |
|
✗ |
std::cout << "D = [\n" << D.transpose() << "];" << std::endl; |
58 |
|
|
#endif |
59 |
|
|
|
60 |
|
✗ |
BOOST_CHECK(M.isApprox(U * D.asDiagonal() * U.transpose(), 1e-12)); |
61 |
|
|
|
62 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); |
63 |
|
|
// std::cout << "v = [" << v.transpose() << "]';" << std::endl; |
64 |
|
|
|
65 |
|
✗ |
Eigen::VectorXd Uv = v; |
66 |
|
✗ |
pinocchio::cholesky::Uv(model, data, Uv); |
67 |
|
✗ |
BOOST_CHECK(Uv.isApprox(U * v, 1e-12)); |
68 |
|
|
|
69 |
|
✗ |
Eigen::VectorXd Utv = v; |
70 |
|
✗ |
pinocchio::cholesky::Utv(model, data, Utv); |
71 |
|
✗ |
BOOST_CHECK(Utv.isApprox(U.transpose() * v, 1e-12)); |
72 |
|
|
|
73 |
|
✗ |
Eigen::VectorXd Uiv = v; |
74 |
|
✗ |
pinocchio::cholesky::Uiv(model, data, Uiv); |
75 |
|
✗ |
BOOST_CHECK(Uiv.isApprox(U.inverse() * v, 1e-12)); |
76 |
|
|
|
77 |
|
✗ |
Eigen::VectorXd Utiv = v; |
78 |
|
✗ |
pinocchio::cholesky::Utiv(model, data, Utiv); |
79 |
|
✗ |
BOOST_CHECK(Utiv.isApprox(U.transpose().inverse() * v, 1e-12)); |
80 |
|
|
|
81 |
|
✗ |
Eigen::VectorXd Miv = v; |
82 |
|
✗ |
pinocchio::cholesky::solve(model, data, Miv); |
83 |
|
✗ |
BOOST_CHECK(Miv.isApprox(M.inverse() * v, 1e-12)); |
84 |
|
|
|
85 |
|
✗ |
Eigen::VectorXd Mv = v; |
86 |
|
✗ |
Mv = pinocchio::cholesky::Mv(model, data, Mv); |
87 |
|
✗ |
BOOST_CHECK(Mv.isApprox(M * v, 1e-12)); |
88 |
|
✗ |
Mv = v; |
89 |
|
✗ |
pinocchio::cholesky::UDUtv(model, data, Mv); |
90 |
|
✗ |
BOOST_CHECK(Mv.isApprox(M * v, 1e-12)); |
91 |
|
|
} |
92 |
|
|
|
93 |
|
|
/* The flag triger the following timers: |
94 |
|
|
* 000001: sparse UDUt cholesky |
95 |
|
|
* 000010: dense Eigen LDLt cholesky (with pivot) |
96 |
|
|
* 000100: sparse resolution |
97 |
|
|
* 001000: sparse U*v multiplication |
98 |
|
|
* 010000: sparse U\v substitution |
99 |
|
|
* 100000: sparse M*v multiplication without Cholesky |
100 |
|
|
*/ |
101 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_timings) |
102 |
|
|
{ |
103 |
|
|
using namespace Eigen; |
104 |
|
|
using namespace pinocchio; |
105 |
|
|
|
106 |
|
✗ |
pinocchio::Model model; |
107 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
108 |
|
✗ |
pinocchio::Data data(model); |
109 |
|
|
|
110 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
111 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
112 |
|
✗ |
VectorXd q = randomConfiguration(model); |
113 |
|
✗ |
data.M.fill(0); // Only nonzero coeff of M are initialized by CRBA. |
114 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
115 |
|
|
|
116 |
|
✗ |
long flag = BOOST_BINARY(1111111); |
117 |
|
✗ |
PinocchioTicToc timer(PinocchioTicToc::US); |
118 |
|
|
#ifdef NDEBUG |
119 |
|
|
#ifdef _INTENSE_TESTING_ |
120 |
|
|
const size_t NBT = 1000 * 1000; |
121 |
|
|
#else |
122 |
|
|
const size_t NBT = 10; |
123 |
|
|
#endif |
124 |
|
|
#else |
125 |
|
✗ |
const size_t NBT = 1; |
126 |
|
✗ |
std::cout << "(the time score in debug mode is not relevant) "; |
127 |
|
|
#endif |
128 |
|
|
|
129 |
|
✗ |
bool verbose = flag & (flag - 1); // True is two or more binaries of the flag are 1. |
130 |
|
✗ |
if (verbose) |
131 |
|
✗ |
std::cout << "--" << std::endl; |
132 |
|
|
|
133 |
|
✗ |
if (flag >> 0 & 1) |
134 |
|
|
{ |
135 |
|
✗ |
timer.tic(); |
136 |
|
✗ |
SMOOTH(NBT) |
137 |
|
|
{ |
138 |
|
✗ |
pinocchio::cholesky::decompose(model, data); |
139 |
|
|
} |
140 |
|
✗ |
if (verbose) |
141 |
|
✗ |
std::cout << "Decompose =\t"; |
142 |
|
✗ |
timer.toc(std::cout, NBT); |
143 |
|
|
} |
144 |
|
|
|
145 |
|
✗ |
if (flag >> 1 & 1) |
146 |
|
|
{ |
147 |
|
✗ |
timer.tic(); |
148 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); |
149 |
|
✗ |
Eigen::VectorXd res(model.nv); |
150 |
|
✗ |
SMOOTH(NBT) |
151 |
|
|
{ |
152 |
|
✗ |
Eigen::LDLT<Eigen::MatrixXd> Mchol(data.M); |
153 |
|
✗ |
res = Mchol.solve(v); |
154 |
|
|
} |
155 |
|
✗ |
if (verbose) |
156 |
|
✗ |
std::cout << "Eigen::LDLt =\t"; |
157 |
|
✗ |
timer.toc(std::cout, NBT); |
158 |
|
|
} |
159 |
|
|
|
160 |
|
✗ |
if (flag >> 2 & 31) |
161 |
|
|
{ |
162 |
|
✗ |
std::vector<Eigen::VectorXd> randvec(NBT); |
163 |
|
✗ |
for (size_t i = 0; i < NBT; ++i) |
164 |
|
✗ |
randvec[i] = Eigen::VectorXd::Random(model.nv); |
165 |
|
✗ |
Eigen::VectorXd zero = Eigen::VectorXd::Zero(model.nv); |
166 |
|
✗ |
Eigen::VectorXd res(model.nv); |
167 |
|
|
|
168 |
|
✗ |
if (flag >> 2 & 1) |
169 |
|
|
{ |
170 |
|
✗ |
timer.tic(); |
171 |
|
✗ |
SMOOTH(NBT) |
172 |
|
|
{ |
173 |
|
✗ |
pinocchio::cholesky::solve(model, data, randvec[_smooth]); |
174 |
|
|
} |
175 |
|
✗ |
if (verbose) |
176 |
|
✗ |
std::cout << "solve =\t\t"; |
177 |
|
✗ |
timer.toc(std::cout, NBT); |
178 |
|
|
} |
179 |
|
|
|
180 |
|
✗ |
if (flag >> 3 & 1) |
181 |
|
|
{ |
182 |
|
✗ |
timer.tic(); |
183 |
|
✗ |
SMOOTH(NBT) |
184 |
|
|
{ |
185 |
|
✗ |
pinocchio::cholesky::Uv(model, data, randvec[_smooth]); |
186 |
|
|
} |
187 |
|
✗ |
if (verbose) |
188 |
|
✗ |
std::cout << "Uv =\t\t"; |
189 |
|
✗ |
timer.toc(std::cout, NBT); |
190 |
|
|
} |
191 |
|
|
|
192 |
|
✗ |
if (flag >> 4 & 1) |
193 |
|
|
{ |
194 |
|
✗ |
timer.tic(); |
195 |
|
✗ |
SMOOTH(NBT) |
196 |
|
|
{ |
197 |
|
✗ |
pinocchio::cholesky::Uiv(model, data, randvec[_smooth]); |
198 |
|
|
} |
199 |
|
✗ |
if (verbose) |
200 |
|
✗ |
std::cout << "Uiv =\t\t"; |
201 |
|
✗ |
timer.toc(std::cout, NBT); |
202 |
|
|
} |
203 |
|
✗ |
if (flag >> 5 & 1) |
204 |
|
|
{ |
205 |
|
✗ |
timer.tic(); |
206 |
|
✗ |
Eigen::VectorXd res; |
207 |
|
✗ |
SMOOTH(NBT) |
208 |
|
|
{ |
209 |
|
✗ |
res = pinocchio::cholesky::Mv(model, data, randvec[_smooth]); |
210 |
|
|
} |
211 |
|
✗ |
if (verbose) |
212 |
|
✗ |
std::cout << "Mv =\t\t"; |
213 |
|
✗ |
timer.toc(std::cout, NBT); |
214 |
|
|
} |
215 |
|
✗ |
if (flag >> 6 & 1) |
216 |
|
|
{ |
217 |
|
✗ |
timer.tic(); |
218 |
|
✗ |
SMOOTH(NBT) |
219 |
|
|
{ |
220 |
|
✗ |
pinocchio::cholesky::UDUtv(model, data, randvec[_smooth]); |
221 |
|
|
} |
222 |
|
✗ |
if (verbose) |
223 |
|
✗ |
std::cout << "UDUtv =\t\t"; |
224 |
|
✗ |
timer.toc(std::cout, NBT); |
225 |
|
|
} |
226 |
|
|
} |
227 |
|
|
} |
228 |
|
|
|
229 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_Minv_from_cholesky) |
230 |
|
|
{ |
231 |
|
|
using namespace Eigen; |
232 |
|
|
using namespace pinocchio; |
233 |
|
|
|
234 |
|
✗ |
pinocchio::Model model; |
235 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
236 |
|
✗ |
pinocchio::Data data(model); |
237 |
|
|
|
238 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
239 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
240 |
|
✗ |
VectorXd q = randomConfiguration(model); |
241 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
242 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
243 |
|
✗ |
data.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
244 |
|
✗ |
MatrixXd Minv_ref(data.M.inverse()); |
245 |
|
|
|
246 |
|
✗ |
cholesky::decompose(model, data); |
247 |
|
✗ |
VectorXd v_unit(VectorXd::Unit(model.nv, 0)); |
248 |
|
|
|
249 |
|
✗ |
VectorXd Ui_v_unit(model.nv); |
250 |
|
✗ |
VectorXd Ui_v_unit_ref(model.nv); |
251 |
|
|
|
252 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
253 |
|
|
{ |
254 |
|
✗ |
v_unit = VectorXd::Unit(model.nv, k); |
255 |
|
✗ |
Ui_v_unit.setZero(); |
256 |
|
✗ |
cholesky::internal::Miunit(model, data, k, Ui_v_unit); |
257 |
|
✗ |
Ui_v_unit_ref = v_unit; |
258 |
|
✗ |
cholesky::Uiv(model, data, Ui_v_unit_ref); |
259 |
|
✗ |
Ui_v_unit_ref.array() *= data.Dinv.array(); |
260 |
|
✗ |
cholesky::Utiv(model, data, Ui_v_unit_ref); |
261 |
|
✗ |
BOOST_CHECK(Ui_v_unit.head(k + 1).isApprox(Ui_v_unit_ref.head(k + 1))); |
262 |
|
|
|
263 |
|
✗ |
Ui_v_unit_ref = v_unit; |
264 |
|
✗ |
cholesky::solve(model, data, Ui_v_unit_ref); |
265 |
|
✗ |
BOOST_CHECK(Ui_v_unit.head(k + 1).isApprox(Ui_v_unit_ref.head(k + 1))); |
266 |
|
|
} |
267 |
|
|
|
268 |
|
✗ |
MatrixXd Minv(model.nv, model.nv); |
269 |
|
✗ |
Minv.setZero(); |
270 |
|
✗ |
cholesky::computeMinv(model, data, Minv); |
271 |
|
|
|
272 |
|
✗ |
BOOST_CHECK(Minv.isApprox(Minv_ref)); |
273 |
|
|
|
274 |
|
|
// Check second call to cholesky::computeMinv |
275 |
|
✗ |
cholesky::computeMinv(model, data, Minv); |
276 |
|
✗ |
BOOST_CHECK(Minv.isApprox(Minv_ref)); |
277 |
|
|
|
278 |
|
|
// Call the second signature of cholesky::computeMinv |
279 |
|
✗ |
Data data_bis(model); |
280 |
|
✗ |
crba(model, data_bis, q, Convention::WORLD); |
281 |
|
✗ |
cholesky::decompose(model, data_bis); |
282 |
|
✗ |
cholesky::computeMinv(model, data_bis); |
283 |
|
✗ |
BOOST_CHECK(data_bis.Minv.isApprox(Minv_ref)); |
284 |
|
|
} |
285 |
|
|
|
286 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
287 |
|
|
|