GCC Code Coverage Report

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