GCC Code Coverage Report

Directory: ./
File: benchmark/arm_manipulation_timings.cpp
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 0 338 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 std::shared_ptr<crocoddyl::StateMultibody> state =
73 std::make_shared<crocoddyl::StateMultibody>(
74 ✗ std::make_shared<pinocchio::Model>(model));
75 std::shared_ptr<crocoddyl::ActuationModelFull> actuation =
76 ✗ std::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 std::shared_ptr<crocoddyl::CostModelAbstract> goalTrackingCost =
85 ✗ std::make_shared<crocoddyl::CostModelResidual>(
86 ✗ state, std::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 std::shared_ptr<crocoddyl::CostModelAbstract> xRegCost =
93 ✗ std::make_shared<crocoddyl::CostModelResidual>(
94 ✗ state, std::make_shared<crocoddyl::ResidualModelState>(
95 ✗ state, actuation->get_nu()));
96 std::shared_ptr<crocoddyl::CostModelAbstract> uRegCost =
97 ✗ std::make_shared<crocoddyl::CostModelResidual>(
98 ✗ state, std::make_shared<crocoddyl::ResidualModelControl>(
99 ✗ state, actuation->get_nu()));
100
101 std::shared_ptr<crocoddyl::CostModelSum> runningCostModel =
102 ✗ std::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
103 std::shared_ptr<crocoddyl::CostModelSum> terminalCostModel =
104 ✗ std::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 std::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
112 runningDAM =
113 std::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
114 ✗ state, actuation, runningCostModel);
115
116 std::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
117 terminalDAM =
118 std::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
119 ✗ state, actuation, terminalCostModel);
120
121 std::shared_ptr<crocoddyl::ActionModelAbstract> runningModelWithEuler =
122 ✗ std::make_shared<crocoddyl::IntegratedActionModelEuler>(runningDAM, 1e-3);
123 std::shared_ptr<crocoddyl::ActionModelAbstract> runningModelWithRK4 =
124 ✗ std::make_shared<crocoddyl::IntegratedActionModelRK>(
125 ✗ runningDAM, crocoddyl::RKType::four, 1e-3);
126 std::shared_ptr<crocoddyl::ActionModelAbstract> terminalModel =
127 ✗ std::make_shared<crocoddyl::IntegratedActionModelEuler>(terminalDAM,
128 ✗ 1e-3);
129
130 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> >
131 ✗ runningModelsWithEuler(N, runningModelWithEuler);
132 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> >
133 ✗ runningModelsWithRK4(N, runningModelWithRK4);
134
135 std::shared_ptr<crocoddyl::ShootingProblem> problemWithEuler =
136 std::make_shared<crocoddyl::ShootingProblem>(x0, runningModelsWithEuler,
137 ✗ terminalModel);
138 std::shared_ptr<crocoddyl::ShootingProblem> problemWithRK4 =
139 std::make_shared<crocoddyl::ShootingProblem>(x0, runningModelsWithRK4,
140 ✗ terminalModel);
141 ✗ std::vector<Eigen::VectorXd> xs(N + 1, x0);
142
143 /***************************************************************/
144
145 std::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithEuler_data =
146 ✗ runningModelWithEuler->createData();
147 std::shared_ptr<crocoddyl::ActionDataAbstract> runningModelWithRK4_data =
148 ✗ runningModelWithRK4->createData();
149 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> runningDAM_data =
150 ✗ runningDAM->createData();
151 crocoddyl::DifferentialActionDataFreeFwdDynamics* d =
152 static_cast<crocoddyl::DifferentialActionDataFreeFwdDynamics*>(
153 ✗ runningDAM_data.get());
154 std::shared_ptr<crocoddyl::ActuationDataAbstract> actuation_data =
155 ✗ actuation->createData();
156 std::shared_ptr<crocoddyl::CostDataAbstract> goalTrackingCost_data =
157 ✗ goalTrackingCost->createData(&d->multibody);
158 std::shared_ptr<crocoddyl::CostDataAbstract> xRegCost_data =
159 ✗ xRegCost->createData(&d->multibody);
160 std::shared_ptr<crocoddyl::CostDataAbstract> uRegCost_data =
161 ✗ uRegCost->createData(&d->multibody);
162
163 std::shared_ptr<crocoddyl::CostDataSum> runningCostModel_data =
164 ✗ runningCostModel->createData(&d->multibody);
165
166 std::shared_ptr<crocoddyl::ActivationModelAbstract> activationQuad =
167 ✗ xRegCost->get_activation();
168 std::shared_ptr<crocoddyl::ActivationDataAbstract> activationQuad_data =
169 ✗ activationQuad->createData();
170
171 /********************************************************************/
172
173 ✗ Eigen::ArrayXd duration(T);
174
175 ✗ std::vector<Eigen::VectorXd> x0s;
176 ✗ std::vector<Eigen::VectorXd> u0s;
177 ✗ PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) x1s; // (T, state->rand());
178 ✗ PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) x2s; // (T, state->rand());
179 ✗ PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd) us; // (T, state->rand());
180 PINOCCHIO_ALIGNED_STD_VECTOR(Eigen::VectorXd)
181 ✗ dxs(T, Eigen::VectorXd::Zero(2 * model.nv));
182
183 ✗ for (size_t i = 0; i < T; ++i) {
184 ✗ x1s.push_back(state->rand());
185 ✗ x2s.push_back(state->rand());
186 ✗ us.push_back(Eigen::VectorXd(actuation->get_nu()));
187 }
188 ✗ for (size_t i = 0; i < N; ++i) {
189 ✗ x0s.push_back(state->rand());
190 ✗ u0s.push_back(Eigen::VectorXd(actuation->get_nu()));
191 }
192 ✗ x0s.push_back(state->rand());
193
194 /*********************State**********************************/
195 ✗ std::cout << std::left << std::setw(42) << "Function call"
196 ✗ << " " << std::left << std::setw(15) << "AVG (us)" << std::left
197 ✗ << std::setw(15) << "STDDEV (us)" << std::left << std::setw(15)
198 ✗ << "MAX (us)" << std::left << std::setw(15) << "MIN (us)"
199 ✗ << std::endl;
200
201 ✗ duration.setZero();
202 ✗ SMOOTH(T) {
203 ✗ timer.reset();
204 ✗ pinocchio::difference(model, x1s[_smooth].head(model.nq),
205 ✗ x2s[_smooth].head(model.nq),
206 ✗ dxs[_smooth].head(model.nv));
207 ✗ duration[_smooth] = timer.get_us_duration();
208 }
209 ✗ std::cout << "pinocchio" << std::endl;
210 ✗ printStatistics("difference", duration);
211
212 ✗ duration.setZero();
213 ✗ SMOOTH(T) {
214 ✗ timer.reset();
215 ✗ pinocchio::integrate(model, x1s[_smooth].head(model.nq),
216 ✗ dxs[_smooth].head(model.nv),
217 ✗ x2s[_smooth].head(model.nq));
218 ✗ duration[_smooth] = timer.get_us_duration();
219 }
220 ✗ printStatistics("integrate", duration);
221
222 ✗ duration.setZero();
223 ✗ SMOOTH(T) {
224 ✗ timer.reset();
225 ✗ pinocchio::dIntegrate(
226 ✗ model, x1s[_smooth].head(model.nq), dxs[_smooth].head(model.nv),
227 ✗ Jsecond.bottomLeftCorner(model.nv, model.nv), pinocchio::ARG1);
228 ✗ duration[_smooth] = timer.get_us_duration();
229 }
230 ✗ printStatistics("dIntegrate ARG1", duration);
231
232 ✗ duration.setZero();
233 ✗ SMOOTH(T) {
234 ✗ timer.reset();
235 ✗ pinocchio::dIntegrate(
236 ✗ model, x1s[_smooth].head(model.nq), dxs[_smooth].head(model.nv),
237 ✗ Jsecond.bottomLeftCorner(model.nv, model.nv), pinocchio::ARG0);
238 ✗ duration[_smooth] = timer.get_us_duration();
239 }
240 ✗ printStatistics("dIntegrate ARG0", duration);
241
242 ✗ duration.setZero();
243 ✗ Eigen::MatrixXd Jin(Eigen::MatrixXd::Random(model.nv, 2 * model.nv));
244 ✗ SMOOTH(T) {
245 ✗ timer.reset();
246 ✗ pinocchio::dIntegrateTransport(model, x1s[_smooth].head(model.nq),
247 ✗ dxs[_smooth].head(model.nv), Jin,
248 pinocchio::ARG0);
249 ✗ duration[_smooth] = timer.get_us_duration();
250 }
251 ✗ printStatistics("dIntegrateTransport with aliasing", duration);
252
253 ✗ duration.setZero();
254 ✗ Jin = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
255 ✗ Eigen::MatrixXd Jout(Eigen::MatrixXd::Random(model.nv, 2 * model.nv));
256 ✗ SMOOTH(T) {
257 ✗ timer.reset();
258 ✗ pinocchio::dIntegrateTransport(model, x1s[_smooth].head(model.nq),
259 ✗ dxs[_smooth].head(model.nv), Jin, Jout,
260 pinocchio::ARG0);
261 ✗ duration[_smooth] = timer.get_us_duration();
262 }
263 ✗ printStatistics("dIntegrateTransport w/o aliasing", duration);
264
265 ✗ duration.setZero();
266 ✗ SMOOTH(T) {
267 ✗ timer.reset();
268 ✗ state->diff(x1s[_smooth], x2s[_smooth], dxs[_smooth]);
269 ✗ duration[_smooth] = timer.get_us_duration();
270 }
271 ✗ std::cout << "StateMultibody" << std::endl;
272 ✗ printStatistics("diff", duration);
273
274 ✗ duration.setZero();
275 ✗ SMOOTH(T) {
276 ✗ timer.reset();
277 ✗ state->integrate(x1s[_smooth], dxs[_smooth], x2s[_smooth]);
278 ✗ duration[_smooth] = timer.get_us_duration();
279 }
280 ✗ printStatistics("integrate", duration);
281
282 ✗ duration.setZero();
283 ✗ SMOOTH(T) {
284 ✗ timer.reset();
285 ✗ state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond, crocoddyl::both);
286 ✗ duration[_smooth] = timer.get_us_duration();
287 }
288 ✗ printStatistics("Jdiff both", duration);
289
290 ✗ duration.setZero();
291 ✗ SMOOTH(T) {
292 ✗ timer.reset();
293 ✗ state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond, crocoddyl::first);
294 ✗ duration[_smooth] = timer.get_us_duration();
295 }
296 ✗ printStatistics("Jdiff first", duration);
297
298 ✗ duration.setZero();
299 ✗ SMOOTH(T) {
300 ✗ timer.reset();
301 ✗ state->Jdiff(x1s[_smooth], x2s[_smooth], Jfirst, Jsecond,
302 crocoddyl::second);
303 ✗ duration[_smooth] = timer.get_us_duration();
304 }
305 ✗ printStatistics("Jdiff second", duration);
306
307 ✗ duration.setZero();
308 ✗ SMOOTH(T) {
309 ✗ timer.reset();
310 ✗ state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
311 crocoddyl::both);
312 ✗ duration[_smooth] = timer.get_us_duration();
313 }
314 ✗ printStatistics("Jintegrate both", duration);
315
316 ✗ duration.setZero();
317 ✗ SMOOTH(T) {
318 ✗ timer.reset();
319 ✗ state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
320 crocoddyl::first);
321 ✗ duration[_smooth] = timer.get_us_duration();
322 }
323 ✗ printStatistics("Jintegrate first", duration);
324
325 ✗ duration.setZero();
326 ✗ SMOOTH(T) {
327 ✗ timer.reset();
328 ✗ state->Jintegrate(x1s[_smooth], dxs[_smooth], Jfirst, Jsecond,
329 crocoddyl::second);
330 ✗ duration[_smooth] = timer.get_us_duration();
331 }
332 ✗ printStatistics("Jintegrate second", duration);
333
334 /**************************************************************/
335
336 ✗ duration.setZero();
337 ✗ SMOOTH(T) {
338 ✗ timer.reset();
339 ✗ activationQuad->calc(activationQuad_data, dxs[_smooth]);
340 ✗ duration[_smooth] = timer.get_us_duration();
341 }
342 ✗ std::cout << "ActivationModelQuad" << std::endl;
343 ✗ printStatistics("calc", duration);
344
345 ✗ duration.setZero();
346 ✗ SMOOTH(T) {
347 ✗ timer.reset();
348 ✗ activationQuad->calcDiff(activationQuad_data, dxs[_smooth]);
349 ✗ duration[_smooth] = timer.get_us_duration();
350 }
351 ✗ printStatistics("calcDiff", duration);
352
353 /*************************************Actuation*******************/
354
355 ✗ duration.setZero();
356 ✗ SMOOTH(T) {
357 ✗ timer.reset();
358 ✗ actuation->calc(actuation_data, x1s[_smooth], us[_smooth]);
359 ✗ duration[_smooth] = timer.get_us_duration();
360 }
361 ✗ std::cout << "ActuationModelFull" << std::endl;
362 ✗ printStatistics("calc", duration);
363
364 ✗ duration.setZero();
365 ✗ SMOOTH(T) {
366 ✗ timer.reset();
367 ✗ actuation->calcDiff(actuation_data, x1s[_smooth], us[_smooth]);
368 ✗ duration[_smooth] = timer.get_us_duration();
369 }
370 ✗ printStatistics("calcDiff", duration);
371
372 /*******************************Cost****************************/
373 ✗ duration.setZero();
374 ✗ SMOOTH(T) {
375 ✗ timer.reset();
376 ✗ goalTrackingCost->calc(goalTrackingCost_data, x1s[_smooth], us[_smooth]);
377 ✗ duration[_smooth] = timer.get_us_duration();
378 }
379 ✗ std::cout << "CostModelFramePlacement" << std::endl;
380 ✗ printStatistics("calc", duration);
381
382 ✗ duration.setZero();
383 ✗ SMOOTH(T) {
384 ✗ timer.reset();
385 ✗ goalTrackingCost->calcDiff(goalTrackingCost_data, x1s[_smooth],
386 ✗ us[_smooth]);
387 ✗ duration[_smooth] = timer.get_us_duration();
388 }
389 ✗ printStatistics("calcDiff", duration);
390
391 ✗ duration.setZero();
392 ✗ SMOOTH(T) {
393 ✗ timer.reset();
394 ✗ xRegCost->calc(xRegCost_data, x1s[_smooth], us[_smooth]);
395 ✗ duration[_smooth] = timer.get_us_duration();
396 }
397 ✗ std::cout << "CostModelState" << std::endl;
398 ✗ printStatistics("calc", duration);
399
400 ✗ duration.setZero();
401 ✗ SMOOTH(T) {
402 ✗ timer.reset();
403 ✗ xRegCost->calcDiff(xRegCost_data, x1s[_smooth], us[_smooth]);
404 ✗ duration[_smooth] = timer.get_us_duration();
405 }
406 ✗ printStatistics("calcDiff", duration);
407
408 ✗ duration.setZero();
409 ✗ SMOOTH(T) {
410 ✗ timer.reset();
411 ✗ uRegCost->calc(uRegCost_data, x1s[_smooth], us[_smooth]);
412 ✗ duration[_smooth] = timer.get_us_duration();
413 }
414 ✗ std::cout << "CostModelControl" << std::endl;
415 ✗ printStatistics("calc", duration);
416
417 ✗ duration.setZero();
418 ✗ SMOOTH(T) {
419 ✗ timer.reset();
420 ✗ uRegCost->calcDiff(uRegCost_data, x1s[_smooth], us[_smooth]);
421 ✗ duration[_smooth] = timer.get_us_duration();
422 }
423 ✗ printStatistics("calcDiff", duration);
424
425 ✗ duration.setZero();
426 ✗ SMOOTH(T) {
427 ✗ timer.reset();
428 ✗ runningCostModel->calc(runningCostModel_data, x1s[_smooth], us[_smooth]);
429 ✗ duration[_smooth] = timer.get_us_duration();
430 }
431 ✗ std::cout << "CostModelSum" << std::endl;
432 ✗ printStatistics("calc", duration);
433
434 ✗ duration.setZero();
435 ✗ SMOOTH(T) {
436 ✗ timer.reset();
437 ✗ runningCostModel->calcDiff(runningCostModel_data, x1s[_smooth],
438 ✗ us[_smooth]);
439 ✗ duration[_smooth] = timer.get_us_duration();
440 }
441 ✗ printStatistics("calcDiff", duration);
442
443 ✗ duration.setZero();
444 ✗ SMOOTH(T) {
445 ✗ timer.reset();
446 ✗ runningDAM->calc(runningDAM_data, x1s[_smooth], us[_smooth]);
447 ✗ duration[_smooth] = timer.get_us_duration();
448 }
449 ✗ std::cout << "FreeFwdDynamics" << std::endl;
450 ✗ printStatistics("calc", duration);
451
452 ✗ duration.setZero();
453 ✗ SMOOTH(T) {
454 ✗ timer.reset();
455 ✗ runningDAM->calcDiff(runningDAM_data, x1s[_smooth], us[_smooth]);
456 ✗ duration[_smooth] = timer.get_us_duration();
457 }
458 ✗ printStatistics("calcDiff", duration);
459
460 ✗ duration.setZero();
461 ✗ SMOOTH(T) {
462 ✗ timer.reset();
463 ✗ runningModelWithEuler->calc(runningModelWithEuler_data, x1s[_smooth],
464 ✗ us[_smooth]);
465 ✗ duration[_smooth] = timer.get_us_duration();
466 }
467 ✗ std::cout << "FreeFwdDynamics+Euler" << std::endl;
468 ✗ printStatistics("calc", duration);
469
470 ✗ duration.setZero();
471 ✗ SMOOTH(T) {
472 ✗ timer.reset();
473 ✗ runningModelWithEuler->calcDiff(runningModelWithEuler_data, x1s[_smooth],
474 ✗ us[_smooth]);
475 ✗ duration[_smooth] = timer.get_us_duration();
476 }
477 ✗ printStatistics("calcDiff", duration);
478
479 ✗ duration.setZero();
480 ✗ SMOOTH(T) {
481 ✗ timer.reset();
482 ✗ runningModelWithRK4->calc(runningModelWithRK4_data, x1s[_smooth],
483 ✗ us[_smooth]);
484 ✗ duration[_smooth] = timer.get_us_duration();
485 }
486 ✗ std::cout << "FreeFwdDynamics+RK4" << std::endl;
487 ✗ printStatistics("calc", duration);
488
489 ✗ duration.setZero();
490 ✗ SMOOTH(T) {
491 ✗ timer.reset();
492 ✗ runningModelWithRK4->calcDiff(runningModelWithRK4_data, x1s[_smooth],
493 ✗ us[_smooth]);
494 ✗ duration[_smooth] = timer.get_us_duration();
495 }
496 ✗ printStatistics("calcDiff", duration);
497
498 ✗ duration = Eigen::ArrayXd(T / N);
499 ✗ SMOOTH(T / N) {
500 ✗ timer.reset();
501 ✗ problemWithEuler->calc(x0s, u0s);
502 ✗ duration[_smooth] = timer.get_us_duration();
503 }
504 ✗ std::cout << "Problem+Euler" << std::endl;
505 ✗ printStatistics("calc", duration);
506
507 ✗ duration.setZero();
508 ✗ SMOOTH(T / N) {
509 ✗ timer.reset();
510 ✗ problemWithEuler->calcDiff(x0s, u0s);
511 ✗ duration[_smooth] = timer.get_us_duration();
512 }
513 ✗ printStatistics("calcDiff", duration);
514
515 ✗ duration.setZero();
516 ✗ SMOOTH(T / N) {
517 ✗ timer.reset();
518 ✗ problemWithRK4->calc(x0s, u0s);
519 ✗ duration[_smooth] = timer.get_us_duration();
520 }
521 ✗ std::cout << "Problem+RK4" << std::endl;
522 ✗ printStatistics("calc", duration);
523
524 ✗ duration.setZero();
525 ✗ SMOOTH(T / N) {
526 ✗ timer.reset();
527 ✗ problemWithRK4->calcDiff(x0s, u0s);
528 ✗ duration[_smooth] = timer.get_us_duration();
529 }
530 ✗ printStatistics("calcDiff", duration);
531 }
532