GCC Code Coverage Report

Directory: ./
File: benchmark/arm_manipulation_timings.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 0 339 0.0%
Branches: 0 962 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include <example-robot-data/path.hpp>
11 #include <pinocchio/algorithm/model.hpp>
12 #include <pinocchio/parsers/urdf.hpp>
13
14 #include "crocoddyl/core/costs/cost-sum.hpp"
15 #include "crocoddyl/core/costs/residual.hpp"
16 #include "crocoddyl/core/integrator/euler.hpp"
17 #include "crocoddyl/core/integrator/rk.hpp"
18 #include "crocoddyl/core/mathbase.hpp"
19 #include "crocoddyl/core/optctrl/shooting.hpp"
20 #include "crocoddyl/core/residuals/control.hpp"
21 #include "crocoddyl/core/utils/timer.hpp"
22 #include "crocoddyl/multibody/actions/free-fwddyn.hpp"
23 #include "crocoddyl/multibody/actuations/full.hpp"
24 #include "crocoddyl/multibody/residuals/frame-placement.hpp"
25 #include "crocoddyl/multibody/residuals/state.hpp"
26 #include "crocoddyl/multibody/states/multibody.hpp"
27
28 #define SMOOTH(s) for (size_t _smooth = 0; _smooth < s; ++_smooth)
29
30 #define STDDEV(vec) \
31 std::sqrt(((vec - vec.mean())).square().sum() / ((double)vec.size() - 1))
32 #define AVG(vec) (vec.mean())
33
34 ✗ void printStatistics(std::string name, Eigen::ArrayXd duration) {
35 ✗ std::cout << " " << std::left << std::setw(42) << name << std::left
36 ✗ << std::setw(15) << AVG(duration) << std::left << std::setw(15)
37 ✗ << STDDEV(duration) << std::left << std::setw(15)
38 ✗ << duration.maxCoeff() << std::left << std::setw(15)
39 ✗ << duration.minCoeff() << std::endl;
40 }
41
42 ✗ int main(int argc, char* argv[]) {
43 ✗ unsigned int N = 100; // number of nodes
44 ✗ unsigned int T = 5e4; // number of trials
45 ✗ if (argc > 1) {
46 ✗ T = atoi(argv[1]);
47 }
48
49 /**************************DOUBLE**********************/
50 /**************************DOUBLE**********************/
51 /**************************DOUBLE**********************/
52 ✗ pinocchio::Model model_full, model;
53 ✗ pinocchio::urdf::buildModel(EXAMPLE_ROBOT_DATA_MODEL_DIR
54 "/talos_data/robots/talos_left_arm.urdf",
55 model_full);
56 ✗ std::vector<pinocchio::JointIndex> locked_joints;
57 ✗ locked_joints.reserve(3);
58
59 ✗ locked_joints.push_back(5);
60 ✗ locked_joints.push_back(6);
61 ✗ locked_joints.push_back(7);
62
63 ✗ pinocchio::buildReducedModel(model_full, locked_joints,
64 ✗ Eigen::VectorXd::Zero(model_full.nq), model);
65
66 /*************************PINOCCHIO MODEL**************/
67
68 /************************* SETUP ***********************/
69 ✗ crocoddyl::Timer timer;
70 ✗ std::cout << "NQ: " << model.nq << std::endl;
71
72 boost::shared_ptr<crocoddyl::StateMultibody> state =
73 boost::make_shared<crocoddyl::StateMultibody>(
74 ✗ boost::make_shared<pinocchio::Model>(model));
75 boost::shared_ptr<crocoddyl::ActuationModelFull> actuation =
76 ✗ boost::make_shared<crocoddyl::ActuationModelFull>(state);
77
78 ✗ Eigen::VectorXd q0 = Eigen::VectorXd::Random(state->get_nq());
79 ✗ Eigen::VectorXd x0(state->get_nx());
80 ✗ x0 << q0, Eigen::VectorXd::Random(state->get_nv());
81 ✗ Eigen::MatrixXd Jfirst(2 * model.nv, 2 * model.nv),
82 ✗ Jsecond(2 * model.nv, 2 * model.nv);
83
84 boost::shared_ptr<crocoddyl::CostModelAbstract> goalTrackingCost =
85 ✗ boost::make_shared<crocoddyl::CostModelResidual>(
86 ✗ state, boost::make_shared<crocoddyl::ResidualModelFramePlacement>(
87 ✗ state, model.getFrameId("gripper_left_joint"),
88 ✗ pinocchio::SE3(Eigen::Matrix3d::Identity(),
89 ✗ Eigen::Vector3d(.0, .0, .4)),
90 ✗ actuation->get_nu()));
91
92 boost::shared_ptr<crocoddyl::CostModelAbstract> xRegCost =
93 ✗ boost::make_shared<crocoddyl::CostModelResidual>(
94 ✗ state, boost::make_shared<crocoddyl::ResidualModelState>(
95 ✗ state, actuation->get_nu()));
96 boost::shared_ptr<crocoddyl::CostModelAbstract> uRegCost =
97 ✗ boost::make_shared<crocoddyl::CostModelResidual>(
98 ✗ state, boost::make_shared<crocoddyl::ResidualModelControl>(
99 ✗ state, actuation->get_nu()));
100
101 boost::shared_ptr<crocoddyl::CostModelSum> runningCostModel =
102 ✗ boost::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
103 boost::shared_ptr<crocoddyl::CostModelSum> terminalCostModel =
104 ✗ boost::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
105
106 ✗ runningCostModel->addCost("gripperPose", goalTrackingCost, 1);
107 ✗ runningCostModel->addCost("xReg", xRegCost, 1e-4);
108 ✗ runningCostModel->addCost("uReg", uRegCost, 1e-4);
109 ✗ terminalCostModel->addCost("gripperPose", goalTrackingCost, 1);
110
111 boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
112 runningDAM =
113 boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
114 ✗ state, actuation, runningCostModel);
115
116 boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
117 terminalDAM =
118 boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
119 ✗ state, actuation, terminalCostModel);
120
121 boost::shared_ptr<crocoddyl::ActionModelAbstract> runningModelWithEuler =
122 ✗ boost::make_shared<crocoddyl::IntegratedActionModelEuler>(runningDAM,
123 ✗ 1e-3);
124 boost::shared_ptr<crocoddyl::ActionModelAbstract> runningModelWithRK4 =
125 ✗ boost::make_shared<crocoddyl::IntegratedActionModelRK>(
126 ✗ runningDAM, crocoddyl::RKType::four, 1e-3);
127 boost::shared_ptr<crocoddyl::ActionModelAbstract> terminalModel =
128 ✗ boost::make_shared<crocoddyl::IntegratedActionModelEuler>(terminalDAM,
129 ✗ 1e-3);
130
131 std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> >
132 ✗ runningModelsWithEuler(N, runningModelWithEuler);
133 std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> >
134 ✗ runningModelsWithRK4(N, runningModelWithRK4);
135
136 boost::shared_ptr<crocoddyl::ShootingProblem> problemWithEuler =
137 boost::make_shared<crocoddyl::ShootingProblem>(x0, runningModelsWithEuler,
138 ✗ terminalModel);
139 boost::shared_ptr<crocoddyl::ShootingProblem> problemWithRK4 =
140 boost::make_shared<crocoddyl::ShootingProblem>(x0, runningModelsWithRK4,
141 ✗ terminalModel);
142 ✗ std::vector<Eigen::VectorXd> xs(N + 1, x0);
143
144 /***************************************************************/
145
146 boost::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithEuler_data =
147 ✗ runningModelWithEuler->createData();
148 boost::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithRK4_data =
149 ✗ runningModelWithRK4->createData();
150 boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> runningDAM_data =
151 ✗ runningDAM->createData();
152 crocoddyl::DifferentialActionDataFreeFwdDynamics* d =
153 static_cast<crocoddyl::DifferentialActionDataFreeFwdDynamics*>(
154 ✗ runningDAM_data.get());
155 boost::shared_ptr<crocoddyl::ActuationDataAbstract> actuation_data =
156 ✗ actuation->createData();
157 boost::shared_ptr<crocoddyl::CostDataAbstract> goalTrackingCost_data =
158 ✗ goalTrackingCost->createData(&d->multibody);
159 boost::shared_ptr<crocoddyl::CostDataAbstract> xRegCost_data =
160 ✗ xRegCost->createData(&d->multibody);
161 boost::shared_ptr<crocoddyl::CostDataAbstract> uRegCost_data =
162 ✗ uRegCost->createData(&d->multibody);
163
164 boost::shared_ptr<crocoddyl::CostDataSum> runningCostModel_data =
165 ✗ runningCostModel->createData(&d->multibody);
166
167 boost::shared_ptr<crocoddyl::ActivationModelAbstract> activationQuad =
168 ✗ xRegCost->get_activation();
169 boost::shared_ptr<crocoddyl::ActivationDataAbstract> activationQuad_data =
170 ✗ activationQuad->createData();
171
172 /********************************************************************/
173
174 ✗ Eigen::ArrayXd duration(T);
175
176 ✗ std::vector<Eigen::VectorXd> x0s;
177 ✗ std::vector<Eigen::VectorXd> u0s;
178 ✗ PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) x1s; // (T, state->rand());
179 ✗ PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) x2s; // (T, state->rand());
180 ✗ PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) us; // (T, state->rand());
181 PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd)
182 ✗ dxs(T, Eigen::VectorXd::Zero(2 * model.nv));
183
184 ✗ for (size_t i = 0; i < T; ++i) {
185 ✗ x1s.push_back(state->rand());
186 ✗ x2s.push_back(state->rand());
187 ✗ us.push_back(Eigen::VectorXd(actuation->get_nu()));
188 }
189 ✗ for (size_t i = 0; i < N; ++i) {
190 ✗ x0s.push_back(state->rand());
191 ✗ u0s.push_back(Eigen::VectorXd(actuation->get_nu()));
192 }
193 ✗ x0s.push_back(state->rand());
194
195 /*********************State**********************************/
196 ✗ std::cout << std::left << std::setw(42) << "Function call"
197 ✗ << " " << std::left << std::setw(15) << "AVG (us)" << std::left
198 ✗ << std::setw(15) << "STDDEV (us)" << std::left << std::setw(15)
199 ✗ << "MAX (us)" << std::left << std::setw(15) << "MIN (us)"
200 ✗ << std::endl;
201
202 ✗ duration.setZero();
203 ✗ SMOOTH(T) {
204 ✗ timer.reset();
205 ✗ pinocchio::difference(model, x1s[_smooth].head(model.nq),
206 ✗ x2s[_smooth].head(model.nq),
207 ✗ dxs[_smooth].head(model.nv));
208 ✗ duration[_smooth] = timer.get_us_duration();
209 }
210 ✗ std::cout << "pinocchio" << std::endl;
211 ✗ printStatistics("difference", duration);
212
213 ✗ duration.setZero();
214 ✗ SMOOTH(T) {
215 ✗ timer.reset();
216 ✗ pinocchio::integrate(model, x1s[_smooth].head(model.nq),
217 ✗ dxs[_smooth].head(model.nv),
218 ✗ x2s[_smooth].head(model.nq));
219 ✗ duration[_smooth] = timer.get_us_duration();
220 }
221 ✗ printStatistics("integrate", duration);
222
223 ✗ duration.setZero();
224 ✗ SMOOTH(T) {
225 ✗ timer.reset();
226 ✗ pinocchio::dIntegrate(
227 ✗ model, x1s[_smooth].head(model.nq), dxs[_smooth].head(model.nv),
228 ✗ Jsecond.bottomLeftCorner(model.nv, model.nv), pinocchio::ARG1);
229 ✗ duration[_smooth] = timer.get_us_duration();
230 }
231 ✗ printStatistics("dIntegrate ARG1", duration);
232
233 ✗ duration.setZero();
234 ✗ SMOOTH(T) {
235 ✗ timer.reset();
236 ✗ pinocchio::dIntegrate(
237 ✗ model, x1s[_smooth].head(model.nq), dxs[_smooth].head(model.nv),
238 ✗ Jsecond.bottomLeftCorner(model.nv, model.nv), pinocchio::ARG0);
239 ✗ duration[_smooth] = timer.get_us_duration();
240 }
241 ✗ printStatistics("dIntegrate ARG0", duration);
242
243 ✗ duration.setZero();
244 ✗ Eigen::MatrixXd Jin(Eigen::MatrixXd::Random(model.nv, 2 * model.nv));
245 ✗ SMOOTH(T) {
246 ✗ timer.reset();
247 ✗ pinocchio::dIntegrateTransport(model, x1s[_smooth].head(model.nq),
248 ✗ dxs[_smooth].head(model.nv), Jin,
249 pinocchio::ARG0);
250 ✗ duration[_smooth] = timer.get_us_duration();
251 }
252 ✗ printStatistics("dIntegrateTransport with aliasing", duration);
253
254 ✗ duration.setZero();
255 ✗ Jin = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
256 ✗ Eigen::MatrixXd Jout(Eigen::MatrixXd::Random(model.nv, 2 * model.nv));
257 ✗ SMOOTH(T) {
258 ✗ timer.reset();
259 ✗ pinocchio::dIntegrateTransport(model, x1s[_smooth].head(model.nq),
260 ✗ dxs[_smooth].head(model.nv), Jin, Jout,
261 pinocchio::ARG0);
262 ✗ duration[_smooth] = timer.get_us_duration();
263 }
264 ✗ printStatistics("dIntegrateTransport w/o aliasing", duration);
265
266 ✗ duration.setZero();
267 ✗ SMOOTH(T) {
268 ✗ timer.reset();
269 ✗ state->diff(x1s[_smooth], x2s[_smooth], dxs[_smooth]);
270 ✗ duration[_smooth] = timer.get_us_duration();
271 }
272 ✗ std::cout << "StateMultibody" << std::endl;
273 ✗ printStatistics("diff", duration);
274
275 ✗ duration.setZero();
276 ✗ SMOOTH(T) {
277 ✗ timer.reset();
278 ✗ state->integrate(x1s[_smooth], dxs[_smooth], x2s[_smooth]);
279 ✗ duration[_smooth] = timer.get_us_duration();
280 }
281 ✗ printStatistics("integrate", duration);
282
283 ✗ duration.setZero();
284 ✗ SMOOTH(T) {
285 ✗ timer.reset();
286 ✗ state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond, crocoddyl::both);
287 ✗ duration[_smooth] = timer.get_us_duration();
288 }
289 ✗ printStatistics("Jdiff both", duration);
290
291 ✗ duration.setZero();
292 ✗ SMOOTH(T) {
293 ✗ timer.reset();
294 ✗ state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond, crocoddyl::first);
295 ✗ duration[_smooth] = timer.get_us_duration();
296 }
297 ✗ printStatistics("Jdiff first", duration);
298
299 ✗ duration.setZero();
300 ✗ SMOOTH(T) {
301 ✗ timer.reset();
302 ✗ state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond,
303 crocoddyl::second);
304 ✗ duration[_smooth] = timer.get_us_duration();
305 }
306 ✗ printStatistics("Jdiff second", duration);
307
308 ✗ duration.setZero();
309 ✗ SMOOTH(T) {
310 ✗ timer.reset();
311 ✗ state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
312 crocoddyl::both);
313 ✗ duration[_smooth] = timer.get_us_duration();
314 }
315 ✗ printStatistics("Jintegrate both", duration);
316
317 ✗ duration.setZero();
318 ✗ SMOOTH(T) {
319 ✗ timer.reset();
320 ✗ state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
321 crocoddyl::first);
322 ✗ duration[_smooth] = timer.get_us_duration();
323 }
324 ✗ printStatistics("Jintegrate first", duration);
325
326 ✗ duration.setZero();
327 ✗ SMOOTH(T) {
328 ✗ timer.reset();
329 ✗ state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
330 crocoddyl::second);
331 ✗ duration[_smooth] = timer.get_us_duration();
332 }
333 ✗ printStatistics("Jintegrate second", duration);
334
335 /**************************************************************/
336
337 ✗ duration.setZero();
338 ✗ SMOOTH(T) {
339 ✗ timer.reset();
340 ✗ activationQuad->calc(activationQuad_data, dxs[_smooth]);
341 ✗ duration[_smooth] = timer.get_us_duration();
342 }
343 ✗ std::cout << "ActivationModelQuad" << std::endl;
344 ✗ printStatistics("calc", duration);
345
346 ✗ duration.setZero();
347 ✗ SMOOTH(T) {
348 ✗ timer.reset();
349 ✗ activationQuad->calcDiff(activationQuad_data, dxs[_smooth]);
350 ✗ duration[_smooth] = timer.get_us_duration();
351 }
352 ✗ printStatistics("calcDiff", duration);
353
354 /*************************************Actuation*******************/
355
356 ✗ duration.setZero();
357 ✗ SMOOTH(T) {
358 ✗ timer.reset();
359 ✗ actuation->calc(actuation_data, x1s[_smooth], us[_smooth]);
360 ✗ duration[_smooth] = timer.get_us_duration();
361 }
362 ✗ std::cout << "ActuationModelFull" << std::endl;
363 ✗ printStatistics("calc", duration);
364
365 ✗ duration.setZero();
366 ✗ SMOOTH(T) {
367 ✗ timer.reset();
368 ✗ actuation->calcDiff(actuation_data, x1s[_smooth], us[_smooth]);
369 ✗ duration[_smooth] = timer.get_us_duration();
370 }
371 ✗ printStatistics("calcDiff", duration);
372
373 /*******************************Cost****************************/
374 ✗ duration.setZero();
375 ✗ SMOOTH(T) {
376 ✗ timer.reset();
377 ✗ goalTrackingCost->calc(goalTrackingCost_data, x1s[_smooth], us[_smooth]);
378 ✗ duration[_smooth] = timer.get_us_duration();
379 }
380 ✗ std::cout << "CostModelFramePlacement" << std::endl;
381 ✗ printStatistics("calc", duration);
382
383 ✗ duration.setZero();
384 ✗ SMOOTH(T) {
385 ✗ timer.reset();
386 ✗ goalTrackingCost->calcDiff(goalTrackingCost_data, x1s[_smooth],
387 ✗ us[_smooth]);
388 ✗ duration[_smooth] = timer.get_us_duration();
389 }
390 ✗ printStatistics("calcDiff", duration);
391
392 ✗ duration.setZero();
393 ✗ SMOOTH(T) {
394 ✗ timer.reset();
395 ✗ xRegCost->calc(xRegCost_data, x1s[_smooth], us[_smooth]);
396 ✗ duration[_smooth] = timer.get_us_duration();
397 }
398 ✗ std::cout << "CostModelState" << std::endl;
399 ✗ printStatistics("calc", duration);
400
401 ✗ duration.setZero();
402 ✗ SMOOTH(T) {
403 ✗ timer.reset();
404 ✗ xRegCost->calcDiff(xRegCost_data, x1s[_smooth], us[_smooth]);
405 ✗ duration[_smooth] = timer.get_us_duration();
406 }
407 ✗ printStatistics("calcDiff", duration);
408
409 ✗ duration.setZero();
410 ✗ SMOOTH(T) {
411 ✗ timer.reset();
412 ✗ uRegCost->calc(uRegCost_data, x1s[_smooth], us[_smooth]);
413 ✗ duration[_smooth] = timer.get_us_duration();
414 }
415 ✗ std::cout << "CostModelControl" << std::endl;
416 ✗ printStatistics("calc", duration);
417
418 ✗ duration.setZero();
419 ✗ SMOOTH(T) {
420 ✗ timer.reset();
421 ✗ uRegCost->calcDiff(uRegCost_data, x1s[_smooth], us[_smooth]);
422 ✗ duration[_smooth] = timer.get_us_duration();
423 }
424 ✗ printStatistics("calcDiff", duration);
425
426 ✗ duration.setZero();
427 ✗ SMOOTH(T) {
428 ✗ timer.reset();
429 ✗ runningCostModel->calc(runningCostModel_data, x1s[_smooth], us[_smooth]);
430 ✗ duration[_smooth] = timer.get_us_duration();
431 }
432 ✗ std::cout << "CostModelSum" << std::endl;
433 ✗ printStatistics("calc", duration);
434
435 ✗ duration.setZero();
436 ✗ SMOOTH(T) {
437 ✗ timer.reset();
438 ✗ runningCostModel->calcDiff(runningCostModel_data, x1s[_smooth],
439 ✗ us[_smooth]);
440 ✗ duration[_smooth] = timer.get_us_duration();
441 }
442 ✗ printStatistics("calcDiff", duration);
443
444 ✗ duration.setZero();
445 ✗ SMOOTH(T) {
446 ✗ timer.reset();
447 ✗ runningDAM->calc(runningDAM_data, x1s[_smooth], us[_smooth]);
448 ✗ duration[_smooth] = timer.get_us_duration();
449 }
450 ✗ std::cout << "FreeFwdDynamics" << std::endl;
451 ✗ printStatistics("calc", duration);
452
453 ✗ duration.setZero();
454 ✗ SMOOTH(T) {
455 ✗ timer.reset();
456 ✗ runningDAM->calcDiff(runningDAM_data, x1s[_smooth], us[_smooth]);
457 ✗ duration[_smooth] = timer.get_us_duration();
458 }
459 ✗ printStatistics("calcDiff", duration);
460
461 ✗ duration.setZero();
462 ✗ SMOOTH(T) {
463 ✗ timer.reset();
464 ✗ runningModelWithEuler->calc(runningModelWithEuler_data, x1s[_smooth],
465 ✗ us[_smooth]);
466 ✗ duration[_smooth] = timer.get_us_duration();
467 }
468 ✗ std::cout << "FreeFwdDynamics+Euler" << std::endl;
469 ✗ printStatistics("calc", duration);
470
471 ✗ duration.setZero();
472 ✗ SMOOTH(T) {
473 ✗ timer.reset();
474 ✗ runningModelWithEuler->calcDiff(runningModelWithEuler_data, x1s[_smooth],
475 ✗ us[_smooth]);
476 ✗ duration[_smooth] = timer.get_us_duration();
477 }
478 ✗ printStatistics("calcDiff", duration);
479
480 ✗ duration.setZero();
481 ✗ SMOOTH(T) {
482 ✗ timer.reset();
483 ✗ runningModelWithRK4->calc(runningModelWithRK4_data, x1s[_smooth],
484 ✗ us[_smooth]);
485 ✗ duration[_smooth] = timer.get_us_duration();
486 }
487 ✗ std::cout << "FreeFwdDynamics+RK4" << std::endl;
488 ✗ printStatistics("calc", duration);
489
490 ✗ duration.setZero();
491 ✗ SMOOTH(T) {
492 ✗ timer.reset();
493 ✗ runningModelWithRK4->calcDiff(runningModelWithRK4_data, x1s[_smooth],
494 ✗ us[_smooth]);
495 ✗ duration[_smooth] = timer.get_us_duration();
496 }
497 ✗ printStatistics("calcDiff", duration);
498
499 ✗ duration = Eigen::ArrayXd(T / N);
500 ✗ SMOOTH(T / N) {
501 ✗ timer.reset();
502 ✗ problemWithEuler->calc(x0s, u0s);
503 ✗ duration[_smooth] = timer.get_us_duration();
504 }
505 ✗ std::cout << "Problem+Euler" << std::endl;
506 ✗ printStatistics("calc", duration);
507
508 ✗ duration.setZero();
509 ✗ SMOOTH(T / N) {
510 ✗ timer.reset();
511 ✗ problemWithEuler->calcDiff(x0s, u0s);
512 ✗ duration[_smooth] = timer.get_us_duration();
513 }
514 ✗ printStatistics("calcDiff", duration);
515
516 ✗ duration.setZero();
517 ✗ SMOOTH(T / N) {
518 ✗ timer.reset();
519 ✗ problemWithRK4->calc(x0s, u0s);
520 ✗ duration[_smooth] = timer.get_us_duration();
521 }
522 ✗ std::cout << "Problem+RK4" << std::endl;
523 ✗ printStatistics("calc", duration);
524
525 ✗ duration.setZero();
526 ✗ SMOOTH(T / N) {
527 ✗ timer.reset();
528 ✗ problemWithRK4->calcDiff(x0s, u0s);
529 ✗ duration[_smooth] = timer.get_us_duration();
530 }
531 ✗ printStatistics("calcDiff", duration);
532 }
533