GCC Code Coverage Report


Directory: ./
File: benchmark/lqr_optctrl.cpp
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 0 52 0.0%
Branches: 0 130 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 "crocoddyl/core/actions/lqr.hpp"
11 #include "crocoddyl/core/solvers/fddp.hpp"
12 #include "crocoddyl/core/states/euclidean.hpp"
13 #include "crocoddyl/core/utils/callbacks.hpp"
14 #include "crocoddyl/core/utils/timer.hpp"
15
16 int main(int argc, char* argv[]) {
17 unsigned int NX = 37;
18 unsigned int NU = 12;
19 bool CALLBACKS = false;
20 unsigned int N = 100; // number of nodes
21 unsigned int T = 5e3; // number of trials
22 unsigned int MAXITER = 1;
23 if (argc > 1) {
24 T = atoi(argv[1]);
25 }
26
27 // Creating the action models and warm point for the LQR system
28 Eigen::VectorXd x0 = Eigen::VectorXd::Zero(NX);
29 std::shared_ptr<crocoddyl::ActionModelAbstract> model =
30 std::make_shared<crocoddyl::ActionModelLQR>(NX, NU);
31 std::vector<Eigen::VectorXd> xs(N + 1, x0);
32 std::vector<Eigen::VectorXd> us(N, Eigen::VectorXd::Zero(NU));
33 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract> > runningModels(
34 N, model);
35
36 // Formulating the optimal control problem
37 std::shared_ptr<crocoddyl::ShootingProblem> problem =
38 std::make_shared<crocoddyl::ShootingProblem>(x0, runningModels, model);
39 crocoddyl::SolverFDDP solver(problem);
40 if (CALLBACKS) {
41 std::vector<std::shared_ptr<crocoddyl::CallbackAbstract> > cbs;
42 cbs.push_back(std::make_shared<crocoddyl::CallbackVerbose>());
43 solver.setCallbacks(cbs);
44 }
45
46 std::cout << "NQ: "
47 << solver.get_problem()->get_terminalModel()->get_state()->get_nq()
48 << std::endl;
49 std::cout << "Number of nodes: " << N << std::endl;
50
51 // Solving the optimal control problem
52 Eigen::ArrayXd duration(T);
53 for (unsigned int i = 0; i < T; ++i) {
54 crocoddyl::Timer timer;
55 solver.solve(xs, us, MAXITER);
56 duration[i] = timer.get_duration();
57 }
58
59 double avrg_duration = duration.sum() / T;
60 double min_duration = duration.minCoeff();
61 double max_duration = duration.maxCoeff();
62 std::cout << " FDDP.solve [ms]: " << avrg_duration << " (" << min_duration
63 << "-" << max_duration << ")" << std::endl;
64
65 // Running calc
66 for (unsigned int i = 0; i < T; ++i) {
67 crocoddyl::Timer timer;
68 problem->calc(xs, us);
69 duration[i] = timer.get_duration();
70 }
71
72 avrg_duration = duration.sum() / T;
73 min_duration = duration.minCoeff();
74 max_duration = duration.maxCoeff();
75 std::cout << " ShootingProblem.calc [ms]: " << avrg_duration << " ("
76 << min_duration << "-" << max_duration << ")" << std::endl;
77
78 // Running calcDiff
79 for (unsigned int i = 0; i < T; ++i) {
80 crocoddyl::Timer timer;
81 problem->calcDiff(xs, us);
82 duration[i] = timer.get_duration();
83 }
84
85 avrg_duration = duration.sum() / T;
86 min_duration = duration.minCoeff();
87 max_duration = duration.maxCoeff();
88 std::cout << " ShootingProblem.calcDiff [ms]: " << avrg_duration << " ("
89 << min_duration << "-" << max_duration << ")" << std::endl;
90 }
91