GCC Code Coverage Report

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