GCC Code Coverage Report

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