GCC Code Coverage Report

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