1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#ifdef CROCODDYL_WITH_MULTITHREADING |
10 |
|
|
#include <omp.h> |
11 |
|
|
#endif |
12 |
|
|
#ifndef CROCODDYL_WITH_NTHREADS |
13 |
|
|
#define CROCODDYL_WITH_NTHREADS 1 |
14 |
|
|
#endif |
15 |
|
|
|
16 |
|
|
#ifdef CROCODDYL_WITH_CODEGEN |
17 |
|
|
#include "crocoddyl/core/codegen/action-base.hpp" |
18 |
|
|
#endif |
19 |
|
|
|
20 |
|
|
#include "crocoddyl/core/solvers/fddp.hpp" |
21 |
|
|
#include "crocoddyl/core/utils/file-io.hpp" |
22 |
|
|
#include "crocoddyl/core/utils/timer.hpp" |
23 |
|
|
#include "factory/arm-kinova.hpp" |
24 |
|
|
#include "factory/arm.hpp" |
25 |
|
|
#include "factory/legged-robots.hpp" |
26 |
|
|
|
27 |
|
|
#define STDDEV(vec) \ |
28 |
|
|
std::sqrt(((vec - vec.mean())).square().sum() / \ |
29 |
|
|
(static_cast<double>(vec.size()) - 1)) |
30 |
|
|
#define AVG(vec) (vec.mean()) |
31 |
|
|
|
32 |
|
|
void print_benchmark(RobotEENames robot) { |
33 |
|
|
unsigned int N = 100; // number of nodes |
34 |
|
|
unsigned int T = 1e3; // number of trials |
35 |
|
|
|
36 |
|
|
// Building the running and terminal models |
37 |
|
|
boost::shared_ptr<crocoddyl::ActionModelAbstract> runningModel, terminalModel; |
38 |
|
|
if (robot.robot_name == "Talos_arm") { |
39 |
|
|
crocoddyl::benchmark::build_arm_action_models(runningModel, terminalModel); |
40 |
|
|
} else if (robot.robot_name == "Kinova_arm") { |
41 |
|
|
crocoddyl::benchmark::build_arm_kinova_action_models(runningModel, |
42 |
|
|
terminalModel); |
43 |
|
|
} else { |
44 |
|
|
crocoddyl::benchmark::build_contact_action_models(robot, runningModel, |
45 |
|
|
terminalModel); |
46 |
|
|
} |
47 |
|
|
|
48 |
|
|
// Get the initial state |
49 |
|
|
boost::shared_ptr<crocoddyl::StateMultibody> state = |
50 |
|
|
boost::static_pointer_cast<crocoddyl::StateMultibody>( |
51 |
|
|
runningModel->get_state()); |
52 |
|
|
std::cout << "NQ: " << state->get_nq() << std::endl; |
53 |
|
|
std::cout << "Number of nodes: " << N << std::endl << std::endl; |
54 |
|
|
|
55 |
|
|
Eigen::VectorXd default_state(state->get_nq() + state->get_nv()); |
56 |
|
|
boost::shared_ptr<crocoddyl::IntegratedActionModelEulerTpl<double> > rm = |
57 |
|
|
boost::static_pointer_cast< |
58 |
|
|
crocoddyl::IntegratedActionModelEulerTpl<double> >(runningModel); |
59 |
|
|
if (robot.robot_name == "Talos_arm" || robot.robot_name == "Kinova_arm") { |
60 |
|
|
boost::shared_ptr< |
61 |
|
|
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl<double> > |
62 |
|
|
dm = boost::static_pointer_cast< |
63 |
|
|
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl<double> >( |
64 |
|
|
rm->get_differential()); |
65 |
|
|
default_state |
66 |
|
|
<< dm->get_pinocchio().referenceConfigurations[robot.reference_conf], |
67 |
|
|
Eigen::VectorXd::Zero(state->get_nv()); |
68 |
|
|
} else { |
69 |
|
|
boost::shared_ptr< |
70 |
|
|
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl<double> > |
71 |
|
|
dm = boost::static_pointer_cast< |
72 |
|
|
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl<double> >( |
73 |
|
|
rm->get_differential()); |
74 |
|
|
default_state |
75 |
|
|
<< dm->get_pinocchio().referenceConfigurations[robot.reference_conf], |
76 |
|
|
Eigen::VectorXd::Zero(state->get_nv()); |
77 |
|
|
} |
78 |
|
|
Eigen::VectorXd x0(default_state); |
79 |
|
|
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > runningModels( |
80 |
|
|
N, runningModel); |
81 |
|
|
boost::shared_ptr<crocoddyl::ShootingProblem> problem = |
82 |
|
|
boost::make_shared<crocoddyl::ShootingProblem>(x0, runningModels, |
83 |
|
|
terminalModel); |
84 |
|
|
// Computing the warm-start |
85 |
|
|
std::vector<Eigen::VectorXd> xs(N + 1, x0); |
86 |
|
|
std::vector<Eigen::VectorXd> us( |
87 |
|
|
N, Eigen::VectorXd::Zero(runningModel->get_nu())); |
88 |
|
|
for (unsigned int i = 0; i < N; ++i) { |
89 |
|
|
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model = |
90 |
|
|
problem->get_runningModels()[i]; |
91 |
|
|
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
92 |
|
|
problem->get_runningDatas()[i]; |
93 |
|
|
model->quasiStatic(data, us[i], x0); |
94 |
|
|
} |
95 |
|
|
crocoddyl::SolverFDDP ddp(problem); |
96 |
|
|
ddp.setCandidate(xs, us, false); |
97 |
|
|
boost::shared_ptr<crocoddyl::ActionDataAbstract> runningData = |
98 |
|
|
runningModel->createData(); |
99 |
|
|
|
100 |
|
|
#ifdef CROCODDYL_WITH_CODEGEN |
101 |
|
|
// Code generation of the running an terminal models |
102 |
|
|
typedef CppAD::AD<CppAD::cg::CG<double> > ADScalar; |
103 |
|
|
boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<ADScalar> > |
104 |
|
|
ad_runningModel, ad_terminalModel; |
105 |
|
|
if (robot.robot_name == "Talos_arm") { |
106 |
|
|
crocoddyl::benchmark::build_arm_action_models(ad_runningModel, |
107 |
|
|
ad_terminalModel); |
108 |
|
|
} else if (robot.robot_name == "Kinova_arm") { |
109 |
|
|
crocoddyl::benchmark::build_arm_kinova_action_models(ad_runningModel, |
110 |
|
|
ad_terminalModel); |
111 |
|
|
} else { |
112 |
|
|
crocoddyl::benchmark::build_contact_action_models(robot, ad_runningModel, |
113 |
|
|
ad_terminalModel); |
114 |
|
|
} |
115 |
|
|
|
116 |
|
|
boost::shared_ptr<crocoddyl::ActionModelAbstract> cg_runningModel = |
117 |
|
|
boost::make_shared<crocoddyl::ActionModelCodeGen>( |
118 |
|
|
ad_runningModel, runningModel, robot.robot_name + "_running_cg"); |
119 |
|
|
boost::shared_ptr<crocoddyl::ActionModelAbstract> cg_terminalModel = |
120 |
|
|
boost::make_shared<crocoddyl::ActionModelCodeGen>( |
121 |
|
|
ad_terminalModel, terminalModel, robot.robot_name + "_terminal_cg"); |
122 |
|
|
|
123 |
|
|
// Defining the shooting problem for both cases: with and without code |
124 |
|
|
// generation |
125 |
|
|
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > |
126 |
|
|
cg_runningModels(N, cg_runningModel); |
127 |
|
|
boost::shared_ptr<crocoddyl::ShootingProblem> cg_problem = |
128 |
|
|
boost::make_shared<crocoddyl::ShootingProblem>(x0, cg_runningModels, |
129 |
|
|
cg_terminalModel); |
130 |
|
|
|
131 |
|
|
// Check that code-generated action model is the same as original. |
132 |
|
|
/**************************************************************************/ |
133 |
|
|
crocoddyl::SolverFDDP cg_ddp(cg_problem); |
134 |
|
|
cg_ddp.setCandidate(xs, us, false); |
135 |
|
|
boost::shared_ptr<crocoddyl::ActionDataAbstract> cg_runningData = |
136 |
|
|
cg_runningModel->createData(); |
137 |
|
|
Eigen::VectorXd x_rand = cg_runningModel->get_state()->rand(); |
138 |
|
|
Eigen::VectorXd u_rand = Eigen::VectorXd::Random(cg_runningModel->get_nu()); |
139 |
|
|
runningModel->calc(runningData, x_rand, u_rand); |
140 |
|
|
runningModel->calcDiff(runningData, x_rand, u_rand); |
141 |
|
|
cg_runningModel->calc(cg_runningData, x_rand, u_rand); |
142 |
|
|
cg_runningModel->calcDiff(cg_runningData, x_rand, u_rand); |
143 |
|
|
assert_pretty(cg_runningData->xnext.isApprox(runningData->xnext), |
144 |
|
|
"Problem in xnext"); |
145 |
|
|
assert_pretty(std::abs(cg_runningData->cost - runningData->cost) < 1e-10, |
146 |
|
|
"Problem in cost"); |
147 |
|
|
assert_pretty(cg_runningData->Lx.isApprox(runningData->Lx), "Problem in Lx"); |
148 |
|
|
assert_pretty(cg_runningData->Lu.isApprox(runningData->Lu), "Problem in Lu"); |
149 |
|
|
assert_pretty(cg_runningData->Lxx.isApprox(runningData->Lxx), |
150 |
|
|
"Problem in Lxx"); |
151 |
|
|
assert_pretty(cg_runningData->Lxu.isApprox(runningData->Lxu), |
152 |
|
|
"Problem in Lxu"); |
153 |
|
|
assert_pretty(cg_runningData->Luu.isApprox(runningData->Luu), |
154 |
|
|
"Problem in Luu"); |
155 |
|
|
assert_pretty(cg_runningData->Fx.isApprox(runningData->Fx), "Problem in Fx"); |
156 |
|
|
assert_pretty(cg_runningData->Fu.isApprox(runningData->Fu), "Problem in Fu"); |
157 |
|
|
#endif // CROCODDYL_WITH_CODEGEN |
158 |
|
|
|
159 |
|
|
/******************************* create csv file |
160 |
|
|
* *******************************/ |
161 |
|
|
const std::string csv_filename = "/tmp/" + robot.robot_name + "_" + |
162 |
|
|
std::to_string(state->get_nq()) + |
163 |
|
|
"DoF.bench"; |
164 |
|
|
CsvStream csv(csv_filename); |
165 |
|
|
csv << "fn_name" |
166 |
|
|
<< "nthreads" |
167 |
|
|
<< "with_cg" |
168 |
|
|
<< "mean" |
169 |
|
|
<< "stddev" |
170 |
|
|
<< "max" |
171 |
|
|
<< "min" |
172 |
|
|
<< "mean_per_nodes" |
173 |
|
|
<< "stddev_per_nodes" << csv.endl; |
174 |
|
|
|
175 |
|
|
/*******************************************************************************/ |
176 |
|
|
/*********************************** TIMINGS |
177 |
|
|
* ***********************************/ |
178 |
|
|
Eigen::ArrayXd duration(T); |
179 |
|
|
Eigen::ArrayXd avg(CROCODDYL_WITH_NTHREADS); |
180 |
|
|
Eigen::ArrayXd stddev(CROCODDYL_WITH_NTHREADS); |
181 |
|
|
|
182 |
|
|
/*******************************************************************************/ |
183 |
|
|
/****************************** ACTION MODEL TIMINGS |
184 |
|
|
* ***************************/ |
185 |
|
|
std::cout << "Without Code Generation:" << std::endl; |
186 |
|
|
problem->calc(xs, us); |
187 |
|
|
// calcDiff timings |
188 |
|
|
for (int ithread = 0; ithread < CROCODDYL_WITH_NTHREADS; ++ithread) { |
189 |
|
|
duration.setZero(); |
190 |
|
|
for (unsigned int i = 0; i < T; ++i) { |
191 |
|
|
crocoddyl::Timer timer; |
192 |
|
|
#ifdef CROCODDYL_WITH_MULTITHREADING |
193 |
|
|
#pragma omp parallel for num_threads(ithread + 1) |
194 |
|
|
#endif |
195 |
|
|
for (unsigned int j = 0; j < N; ++j) { |
196 |
|
|
runningModels[j]->calcDiff(problem->get_runningDatas()[j], xs[j], |
197 |
|
|
us[j]); |
198 |
|
|
} |
199 |
|
|
duration[i] = timer.get_us_duration(); |
200 |
|
|
} |
201 |
|
|
avg[ithread] = AVG(duration); |
202 |
|
|
stddev[ithread] = STDDEV(duration); |
203 |
|
|
std::cout << ithread + 1 << " threaded calcDiff [us]:\t" << avg[ithread] |
204 |
|
|
<< " +- " << stddev[ithread] << " (max: " << duration.maxCoeff() |
205 |
|
|
<< ", min: " << duration.minCoeff() |
206 |
|
|
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " |
207 |
|
|
<< stddev[ithread] * (ithread + 1) / N << ")" << std::endl; |
208 |
|
|
csv << "calcDiff" << (ithread + 1) << false << avg[ithread] |
209 |
|
|
<< stddev[ithread] << duration.maxCoeff() << duration.minCoeff() |
210 |
|
|
<< avg[ithread] * (ithread + 1) / N |
211 |
|
|
<< stddev[ithread] * (ithread + 1) / N << csv.endl; |
212 |
|
|
} |
213 |
|
|
|
214 |
|
|
// calc timings |
215 |
|
|
for (int ithread = 0; ithread < CROCODDYL_WITH_NTHREADS; ++ithread) { |
216 |
|
|
duration.setZero(); |
217 |
|
|
for (unsigned int i = 0; i < T; ++i) { |
218 |
|
|
crocoddyl::Timer timer; |
219 |
|
|
#ifdef CROCODDYL_WITH_MULTITHREADING |
220 |
|
|
#pragma omp parallel for num_threads(ithread + 1) |
221 |
|
|
#endif |
222 |
|
|
for (unsigned int j = 0; j < N; ++j) { |
223 |
|
|
runningModels[j]->calc(problem->get_runningDatas()[j], xs[j], us[j]); |
224 |
|
|
} |
225 |
|
|
duration[i] = timer.get_us_duration(); |
226 |
|
|
} |
227 |
|
|
avg[ithread] = AVG(duration); |
228 |
|
|
stddev[ithread] = STDDEV(duration); |
229 |
|
|
std::cout << ithread + 1 << " threaded calc [us]: \t" << avg[ithread] |
230 |
|
|
<< " +- " << stddev[ithread] << " (max: " << duration.maxCoeff() |
231 |
|
|
<< ", min: " << duration.minCoeff() |
232 |
|
|
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " |
233 |
|
|
<< stddev[ithread] * (ithread + 1) / N << ")" << std::endl; |
234 |
|
|
csv << "calc" << (ithread + 1) << false << avg[ithread] << stddev[ithread] |
235 |
|
|
<< duration.maxCoeff() << duration.minCoeff() |
236 |
|
|
<< avg[ithread] * (ithread + 1) / N |
237 |
|
|
<< stddev[ithread] * (ithread + 1) / N << csv.endl; |
238 |
|
|
} |
239 |
|
|
|
240 |
|
|
/*******************************************************************************/ |
241 |
|
|
/******************* DDP BACKWARD AND FORWARD PASSES TIMINGS |
242 |
|
|
* *******************/ |
243 |
|
|
// Backward pass timings |
244 |
|
|
ddp.calcDiff(); |
245 |
|
|
duration.setZero(); |
246 |
|
|
for (unsigned int i = 0; i < T; ++i) { |
247 |
|
|
crocoddyl::Timer timer; |
248 |
|
|
ddp.backwardPass(); |
249 |
|
|
duration[i] = timer.get_us_duration(); |
250 |
|
|
} |
251 |
|
|
double avg_bp = AVG(duration); |
252 |
|
|
double stddev_bp = STDDEV(duration); |
253 |
|
|
std::cout << "backwardPass [us]:\t\t" << avg_bp << " +- " << stddev_bp |
254 |
|
|
<< " (max: " << duration.maxCoeff() |
255 |
|
|
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_bp / N |
256 |
|
|
<< " +- " << stddev_bp / N << ")" << std::endl; |
257 |
|
|
|
258 |
|
|
csv << "backwardPass" << 1 << false << avg_bp << stddev_bp |
259 |
|
|
<< duration.maxCoeff() << duration.minCoeff() << avg_bp / N |
260 |
|
|
<< stddev_bp / N << csv.endl; |
261 |
|
|
|
262 |
|
|
// Forward pass timings |
263 |
|
|
duration.setZero(); |
264 |
|
|
for (unsigned int i = 0; i < T; ++i) { |
265 |
|
|
crocoddyl::Timer timer; |
266 |
|
|
ddp.forwardPass(0.005); |
267 |
|
|
duration[i] = timer.get_us_duration(); |
268 |
|
|
} |
269 |
|
|
double avg_fp = AVG(duration); |
270 |
|
|
double stddev_fp = STDDEV(duration); |
271 |
|
|
std::cout << "forwardPass [us]: \t\t" << avg_fp << " +- " << stddev_fp |
272 |
|
|
<< " (max: " << duration.maxCoeff() |
273 |
|
|
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_fp / N |
274 |
|
|
<< " +- " << stddev_fp / N << ")" << std::endl; |
275 |
|
|
|
276 |
|
|
csv << "forwardPass" << 1 << false << avg_fp << stddev_fp |
277 |
|
|
<< duration.maxCoeff() << duration.minCoeff() << avg_fp / N |
278 |
|
|
<< stddev_fp / N << csv.endl; |
279 |
|
|
|
280 |
|
|
#ifdef CROCODDYL_WITH_CODEGEN |
281 |
|
|
|
282 |
|
|
/*******************************************************************************/ |
283 |
|
|
/*************************** CODE GENERATION TIMINGS |
284 |
|
|
* ***************************/ |
285 |
|
|
/*******************************************************************************/ |
286 |
|
|
|
287 |
|
|
/*******************************************************************************/ |
288 |
|
|
/****************************** ACTION MODEL TIMINGS |
289 |
|
|
* ***************************/ |
290 |
|
|
std::cout << std::endl << "With Code Generation:" << std::endl; |
291 |
|
|
// calcDiff timings |
292 |
|
|
cg_problem->calc(xs, us); |
293 |
|
|
for (int ithread = 0; ithread < CROCODDYL_WITH_NTHREADS; ++ithread) { |
294 |
|
|
duration.setZero(); |
295 |
|
|
for (unsigned int i = 0; i < T; ++i) { |
296 |
|
|
crocoddyl::Timer timer; |
297 |
|
|
#ifdef CROCODDYL_WITH_MULTITHREADING |
298 |
|
|
#pragma omp parallel for num_threads(ithread + 1) |
299 |
|
|
#endif |
300 |
|
|
for (unsigned int j = 0; j < N; ++j) { |
301 |
|
|
cg_runningModels[j]->calcDiff(cg_problem->get_runningDatas()[j], xs[j], |
302 |
|
|
us[j]); |
303 |
|
|
} |
304 |
|
|
duration[i] = timer.get_us_duration(); |
305 |
|
|
} |
306 |
|
|
avg[ithread] = AVG(duration); |
307 |
|
|
stddev[ithread] = STDDEV(duration); |
308 |
|
|
std::cout << ithread + 1 << " threaded calcDiff [us]:\t" << avg[ithread] |
309 |
|
|
<< " +- " << stddev[ithread] << " (max: " << duration.maxCoeff() |
310 |
|
|
<< ", min: " << duration.minCoeff() |
311 |
|
|
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " |
312 |
|
|
<< stddev[ithread] * (ithread + 1) / N << ")" << std::endl; |
313 |
|
|
csv << "calcDiff" << (ithread + 1) << true << avg[ithread] |
314 |
|
|
<< stddev[ithread] << duration.maxCoeff() << duration.minCoeff() |
315 |
|
|
<< avg[ithread] * (ithread + 1) / N |
316 |
|
|
<< stddev[ithread] * (ithread + 1) / N << csv.endl; |
317 |
|
|
} |
318 |
|
|
|
319 |
|
|
// calc timings |
320 |
|
|
for (int ithread = 0; ithread < CROCODDYL_WITH_NTHREADS; ++ithread) { |
321 |
|
|
duration.setZero(); |
322 |
|
|
for (unsigned int i = 0; i < T; ++i) { |
323 |
|
|
crocoddyl::Timer timer; |
324 |
|
|
#ifdef CROCODDYL_WITH_MULTITHREADING |
325 |
|
|
#pragma omp parallel for num_threads(ithread + 1) |
326 |
|
|
#endif |
327 |
|
|
for (unsigned int j = 0; j < N; ++j) { |
328 |
|
|
cg_runningModels[j]->calc(cg_problem->get_runningDatas()[j], xs[j], |
329 |
|
|
us[j]); |
330 |
|
|
} |
331 |
|
|
duration[i] = timer.get_us_duration(); |
332 |
|
|
} |
333 |
|
|
avg[ithread] = AVG(duration); |
334 |
|
|
stddev[ithread] = STDDEV(duration); |
335 |
|
|
std::cout << ithread + 1 << " threaded calc [us]: \t" << avg[ithread] |
336 |
|
|
<< " +- " << stddev[ithread] << " (max: " << duration.maxCoeff() |
337 |
|
|
<< ", min: " << duration.minCoeff() |
338 |
|
|
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " |
339 |
|
|
<< stddev[ithread] * (ithread + 1) / N << ")" << std::endl; |
340 |
|
|
csv << "calc" << (ithread + 1) << true << avg[ithread] << stddev[ithread] |
341 |
|
|
<< duration.maxCoeff() << duration.minCoeff() |
342 |
|
|
<< avg[ithread] * (ithread + 1) / N |
343 |
|
|
<< stddev[ithread] * (ithread + 1) / N << csv.endl; |
344 |
|
|
} |
345 |
|
|
|
346 |
|
|
/*******************************************************************************/ |
347 |
|
|
/******************* DDP BACKWARD AND FORWARD PASSES TIMINGS |
348 |
|
|
* *******************/ |
349 |
|
|
// Backward pass timings |
350 |
|
|
cg_ddp.calcDiff(); |
351 |
|
|
for (unsigned int i = 0; i < T; ++i) { |
352 |
|
|
crocoddyl::Timer timer; |
353 |
|
|
cg_ddp.backwardPass(); |
354 |
|
|
duration[i] = timer.get_us_duration(); |
355 |
|
|
} |
356 |
|
|
avg_bp = AVG(duration); |
357 |
|
|
stddev_bp = STDDEV(duration); |
358 |
|
|
std::cout << "backwardPass [us]:\t\t" << avg_bp << " +- " << stddev_bp |
359 |
|
|
<< " (max: " << duration.maxCoeff() |
360 |
|
|
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_bp / N |
361 |
|
|
<< " +- " << stddev_bp / N << ")" << std::endl; |
362 |
|
|
csv << "backwardPass" << 1 << true << avg_bp << stddev_bp |
363 |
|
|
<< duration.maxCoeff() << duration.minCoeff() << avg_bp / N |
364 |
|
|
<< stddev_bp / N << csv.endl; |
365 |
|
|
|
366 |
|
|
// Forward pass timings |
367 |
|
|
for (unsigned int i = 0; i < T; ++i) { |
368 |
|
|
crocoddyl::Timer timer; |
369 |
|
|
cg_ddp.forwardPass(0.005); |
370 |
|
|
duration[i] = timer.get_us_duration(); |
371 |
|
|
} |
372 |
|
|
avg_fp = AVG(duration); |
373 |
|
|
stddev_fp = STDDEV(duration); |
374 |
|
|
std::cout << "forwardPass [us]: \t\t" << avg_fp << " +- " << stddev_fp |
375 |
|
|
<< " (max: " << duration.maxCoeff() |
376 |
|
|
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_fp / N |
377 |
|
|
<< " +- " << stddev_fp / N << ")" << std::endl; |
378 |
|
|
|
379 |
|
|
csv << "forwardPass" << 1 << true << avg_fp << stddev_fp |
380 |
|
|
<< duration.maxCoeff() << duration.minCoeff() << avg_fp / N |
381 |
|
|
<< stddev_fp / N << csv.endl; |
382 |
|
|
|
383 |
|
|
#endif // CROCODDYL_WITH_CODEGEN |
384 |
|
|
} |
385 |
|
|
|
386 |
|
|
int main() { |
387 |
|
|
// Arm Manipulation Benchmarks |
388 |
|
|
std::cout << "********************Talos 4DoF Arm******************" |
389 |
|
|
<< std::endl; |
390 |
|
|
std::vector<std::string> contact_names; |
391 |
|
|
std::vector<crocoddyl::ContactType> contact_types; |
392 |
|
|
RobotEENames talosArm4Dof( |
393 |
|
|
"Talos_arm", contact_names, contact_types, |
394 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_left_arm.urdf", |
395 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf", |
396 |
|
|
"gripper_left_joint", "half_sitting"); |
397 |
|
|
|
398 |
|
|
print_benchmark(talosArm4Dof); |
399 |
|
|
// Arm Manipulation Benchmarks |
400 |
|
|
std::cout << "******************** Kinova Arm ******************" |
401 |
|
|
<< std::endl; |
402 |
|
|
RobotEENames kinovaArm( |
403 |
|
|
"Kinova_arm", contact_names, contact_types, |
404 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR "/kinova_description/robots/kinova.urdf", |
405 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR "/kinova_description/srdf/kinova.srdf", |
406 |
|
|
"gripper_left_joint", "arm_up"); |
407 |
|
|
|
408 |
|
|
print_benchmark(kinovaArm); |
409 |
|
|
|
410 |
|
|
// Quadruped Solo Benchmarks |
411 |
|
|
std::cout << "********************Quadruped Solo******************" |
412 |
|
|
<< std::endl; |
413 |
|
|
contact_names.clear(); |
414 |
|
|
contact_types.clear(); |
415 |
|
|
contact_names.push_back("FR_KFE"); |
416 |
|
|
contact_names.push_back("HL_KFE"); |
417 |
|
|
contact_types.push_back(crocoddyl::Contact3D); |
418 |
|
|
contact_types.push_back(crocoddyl::Contact3D); |
419 |
|
|
RobotEENames quadrupedSolo( |
420 |
|
|
"Solo", contact_names, contact_types, |
421 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo.urdf", |
422 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/srdf/solo.srdf", "HL_KFE", |
423 |
|
|
"standing"); |
424 |
|
|
|
425 |
|
|
print_benchmark(quadrupedSolo); |
426 |
|
|
|
427 |
|
|
// Quadruped Anymal Benchmarks |
428 |
|
|
std::cout << "********************Quadruped Anymal******************" |
429 |
|
|
<< std::endl; |
430 |
|
|
contact_names.clear(); |
431 |
|
|
contact_types.clear(); |
432 |
|
|
contact_names.push_back("RF_KFE"); |
433 |
|
|
contact_names.push_back("LF_KFE"); |
434 |
|
|
contact_names.push_back("LH_KFE"); |
435 |
|
|
contact_types.push_back(crocoddyl::Contact3D); |
436 |
|
|
contact_types.push_back(crocoddyl::Contact3D); |
437 |
|
|
contact_types.push_back(crocoddyl::Contact3D); |
438 |
|
|
RobotEENames quadrupedAnymal( |
439 |
|
|
"Anymal", contact_names, contact_types, |
440 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR |
441 |
|
|
"/anymal_b_simple_description/robots/anymal.urdf", |
442 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR |
443 |
|
|
"/anymal_b_simple_description/srdf/anymal.srdf", |
444 |
|
|
"RH_KFE", "standing"); |
445 |
|
|
|
446 |
|
|
print_benchmark(quadrupedAnymal); |
447 |
|
|
|
448 |
|
|
// Quadruped HyQ Benchmarks |
449 |
|
|
std::cout << "******************** Quadruped HyQ ******************" |
450 |
|
|
<< std::endl; |
451 |
|
|
contact_names.clear(); |
452 |
|
|
contact_types.clear(); |
453 |
|
|
contact_names.push_back("rf_kfe_joint"); |
454 |
|
|
contact_names.push_back("lf_kfe_joint"); |
455 |
|
|
contact_names.push_back("lh_kfe_joint"); |
456 |
|
|
contact_types.push_back(crocoddyl::Contact3D); |
457 |
|
|
contact_types.push_back(crocoddyl::Contact3D); |
458 |
|
|
contact_types.push_back(crocoddyl::Contact3D); |
459 |
|
|
RobotEENames quadrupedHyQ("HyQ", contact_names, contact_types, |
460 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR |
461 |
|
|
"/hyq_description/robots/hyq_no_sensors.urdf", |
462 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR |
463 |
|
|
"/hyq_description/srdf/hyq.srdf", |
464 |
|
|
"rh_kfe_joint", "standing"); |
465 |
|
|
|
466 |
|
|
print_benchmark(quadrupedHyQ); |
467 |
|
|
|
468 |
|
|
// Biped icub Benchmarks |
469 |
|
|
std::cout << "********************Biped iCub ***********************" |
470 |
|
|
<< std::endl; |
471 |
|
|
contact_names.clear(); |
472 |
|
|
contact_types.clear(); |
473 |
|
|
contact_names.push_back("r_ankle_roll"); |
474 |
|
|
contact_names.push_back("l_ankle_roll"); |
475 |
|
|
contact_types.push_back(crocoddyl::Contact6D); |
476 |
|
|
contact_types.push_back(crocoddyl::Contact6D); |
477 |
|
|
|
478 |
|
|
RobotEENames bipedIcub( |
479 |
|
|
"iCub", contact_names, contact_types, |
480 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR "/icub_description/robots/icub_reduced.urdf", |
481 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR "/icub_description/srdf/icub.srdf", |
482 |
|
|
"r_wrist_yaw", "half_sitting"); |
483 |
|
|
print_benchmark(bipedIcub); |
484 |
|
|
|
485 |
|
|
// Biped icub Benchmarks |
486 |
|
|
std::cout << "********************Biped Talos***********************" |
487 |
|
|
<< std::endl; |
488 |
|
|
contact_names.clear(); |
489 |
|
|
contact_types.clear(); |
490 |
|
|
contact_names.push_back("leg_right_6_joint"); |
491 |
|
|
contact_names.push_back("leg_left_6_joint"); |
492 |
|
|
contact_types.push_back(crocoddyl::Contact6D); |
493 |
|
|
contact_types.push_back(crocoddyl::Contact6D); |
494 |
|
|
|
495 |
|
|
RobotEENames bipedTalos( |
496 |
|
|
"Talos", contact_names, contact_types, |
497 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_reduced.urdf", |
498 |
|
|
EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf", |
499 |
|
|
"arm_right_7_joint", "half_sitting"); |
500 |
|
|
print_benchmark(bipedTalos); |
501 |
|
|
|
502 |
|
|
return 0; |
503 |
|
|
} |