GCC Code Coverage Report

Directory: ./
File: unittest/test_problem.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 314 314 100.0%
Branches: 912 1770 51.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2025, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #define BOOST_TEST_NO_MAIN
10 #define BOOST_TEST_ALTERNATIVE_INIT_API
11
12 #include "crocoddyl/core/integrator/euler.hpp"
13 #include "crocoddyl/core/optctrl/shooting.hpp"
14 #include "crocoddyl/core/utils/conversions.hpp"
15 #include "factory/action.hpp"
16 #include "factory/diff_action.hpp"
17 #include "factory/integrator.hpp"
18 #include "unittest_common.hpp"
19
20 using namespace boost::unit_test;
21 using namespace crocoddyl::unittest;
22
23 //----------------------------------------------------------------------------//
24
25 7 void test_calc(ActionModelTypes::Type action_model_type) {
26 // create the model
27
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ActionModelFactory factory;
28 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
29
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(action_model_type);
30
31 // create two shooting problems (with and without data allocation)
32 7 std::size_t T = 20;
33
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 const Eigen::VectorXd& x0 = model->get_state()->rand();
34
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
35
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
36
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
37
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 140 datas[i] = model->createData();
38 }
39
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 crocoddyl::ShootingProblem problem1(x0, models, model);
40 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
41
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 14 model->createData());
42
43 // Run the print function
44
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 std::ostringstream tmp;
45
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 tmp << problem1;
46
47 // create random trajectory
48
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<Eigen::VectorXd> xs(T + 1);
49
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<Eigen::VectorXd> us(T);
50
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
51
2/4
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 140 times.
✗ Branch 7 not taken.
 140 xs[i] = model->get_state()->rand();
52
3/6
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 140 times.
✗ Branch 10 not taken.
 140 us[i] = Eigen::VectorXd::Random(model->get_nu());
53 }
54
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 xs.back() = model->get_state()->rand();
55
56 // check the state and cost in each node
57
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 double cost = problem1.calc(xs, us);
58
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 problem2.calc(xs, us);
59
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
60 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
61
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 140 model->createData();
62
3/6
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 140 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 140 times.
✗ Branch 11 not taken.
 140 model->calc(data, xs[i], us[i]);
63
7/14
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 140 times.
✗ Branch 15 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 140 times.
 140 BOOST_CHECK(problem1.get_runningDatas()[i]->cost == data->cost);
64
7/14
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 140 times.
✗ Branch 15 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 140 times.
 140 BOOST_CHECK(problem2.get_runningDatas()[i]->cost == data->cost);
65
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK(
66 (problem1.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9));
67
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK(
68 (problem2.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9));
69 140 }
70 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
71
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 model->createData();
72
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 model->calc(data, xs.back());
73
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 7 times.
 7 BOOST_CHECK(problem1.get_terminalData()->cost == data->cost);
74
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 7 times.
 7 BOOST_CHECK(problem2.get_terminalData()->cost == data->cost);
75
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 7 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 7 times.
 7 BOOST_CHECK((problem1.get_terminalData()->xnext - data->xnext).isZero(1e-9));
76
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 7 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 7 times.
 7 BOOST_CHECK((problem2.get_terminalData()->xnext - data->xnext).isZero(1e-9));
77
78 // Checking that casted computation is the same
79 #ifdef NDEBUG // Run only in release mode
80 crocoddyl::ShootingProblemTpl<float> casted_problem1 = problem1.cast<float>();
81 crocoddyl::ShootingProblemTpl<float> casted_problem2 = problem2.cast<float>();
82 std::vector<Eigen::VectorXf> xs_f(T + 1);
83 std::vector<Eigen::VectorXf> us_f(T);
84 for (std::size_t i = 0; i < T; ++i) {
85 xs_f[i] = xs[i].cast<float>();
86 us_f[i] = us[i].cast<float>();
87 }
88 xs_f.back() = xs.back().cast<float>();
89 float cost_f = casted_problem1.calc(xs_f, us_f);
90 casted_problem2.calc(xs_f, us_f);
91 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
92 for (std::size_t i = 0; i < T; ++i) {
93 const std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>&
94 casted_model = model->cast<float>();
95 const std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>>&
96 casted_data = casted_model->createData();
97 casted_model->calc(casted_data, xs_f[i], us_f[i]);
98 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->cost ==
99 casted_data->cost);
100 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->cost ==
101 casted_data->cost);
102 BOOST_CHECK(
103 (casted_problem1.get_runningDatas()[i]->xnext - casted_data->xnext)
104 .isZero(1e-9f));
105 BOOST_CHECK(
106 (casted_problem2.get_runningDatas()[i]->xnext - casted_data->xnext)
107 .isZero(1e-9f));
108 BOOST_CHECK(float(problem1.get_runningDatas()[i]->cost) -
109 casted_data->cost <=
110 tol_f);
111 BOOST_CHECK((problem1.get_runningDatas()[i]->xnext.cast<float>() -
112 casted_data->xnext)
113 .isZero(tol_f));
114 }
115 BOOST_CHECK(std::abs(float(cost) - cost_f) <= tol_f);
116 #endif
117 7 }
118
119 92 void test_calc_diffAction(DifferentialActionModelTypes::Type action_model_type,
120 IntegratorTypes::Type integrator_type) {
121 // create the model
122
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 DifferentialActionModelFactory factory;
123 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& diffModel =
124
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 factory.create(action_model_type);
125
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 IntegratorFactory factory_int;
126 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
127
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 factory_int.create(integrator_type, diffModel);
128
129 // create two shooting problems (with and without data allocation)
130 92 std::size_t T = 20;
131
2/4
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 const Eigen::VectorXd& x0 = model->get_state()->rand();
132
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
133
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
134
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
135
1/2
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
 1840 datas[i] = model->createData();
136 }
137
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 crocoddyl::ShootingProblem problem1(x0, models, model);
138 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
139
2/4
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 184 model->createData());
140
141 // create random trajectory
142
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<Eigen::VectorXd> xs(T + 1);
143
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<Eigen::VectorXd> us(T);
144
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
145
2/4
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1840 times.
✗ Branch 7 not taken.
 1840 xs[i] = model->get_state()->rand();
146
3/6
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1840 times.
✗ Branch 10 not taken.
 1840 us[i] = Eigen::VectorXd::Random(model->get_nu());
147 }
148
2/4
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 xs.back() = model->get_state()->rand();
149
150 // check the state and cost in each node
151
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 double cost = problem1.calc(xs, us);
152
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 problem2.calc(xs, us);
153
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
154 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
155
1/2
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
 1840 model->createData();
156
3/6
✓ Branch 3 taken 1840 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1840 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1840 times.
✗ Branch 11 not taken.
 1840 model->calc(data, xs[i], us[i]);
157
7/14
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1840 times.
✗ Branch 15 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1840 times.
 1840 BOOST_CHECK(problem1.get_runningDatas()[i]->cost == data->cost);
158
7/14
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1840 times.
✗ Branch 15 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1840 times.
 1840 BOOST_CHECK(problem2.get_runningDatas()[i]->cost == data->cost);
159
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK(
160 (problem1.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9));
161
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK(
162 (problem2.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9));
163 1840 }
164 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
165
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 model->createData();
166
2/4
✓ Branch 3 taken 92 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 model->calc(data, xs.back());
167
7/14
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 92 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 92 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 92 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 92 times.
 92 BOOST_CHECK(problem1.get_terminalData()->cost == data->cost);
168
7/14
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 92 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 92 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 92 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 92 times.
 92 BOOST_CHECK(problem2.get_terminalData()->cost == data->cost);
169
9/18
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 92 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 92 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 92 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 92 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 92 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 92 times.
 92 BOOST_CHECK((problem1.get_terminalData()->xnext - data->xnext).isZero(1e-9));
170
9/18
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 92 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 92 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 92 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 92 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 92 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 92 times.
 92 BOOST_CHECK((problem2.get_terminalData()->xnext - data->xnext).isZero(1e-9));
171
172 // Checking that casted computation is the same
173 #ifdef NDEBUG // Run only in release mode
174 crocoddyl::ShootingProblemTpl<float> casted_problem1 = problem1.cast<float>();
175 crocoddyl::ShootingProblemTpl<float> casted_problem2 = problem2.cast<float>();
176 std::vector<Eigen::VectorXf> xs_f(T + 1);
177 std::vector<Eigen::VectorXf> us_f(T);
178 for (std::size_t i = 0; i < T; ++i) {
179 xs_f[i] = xs[i].cast<float>();
180 us_f[i] = us[i].cast<float>();
181 }
182 xs_f.back() = xs.back().cast<float>();
183 float cost_f = casted_problem1.calc(xs_f, us_f);
184 casted_problem2.calc(xs_f, us_f);
185 float tol_f = 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
186 for (std::size_t i = 0; i < T; ++i) {
187 const std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>&
188 casted_model = model->cast<float>();
189 const std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>>&
190 casted_data = casted_model->createData();
191 casted_model->calc(casted_data, xs_f[i], us_f[i]);
192 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->cost ==
193 casted_data->cost);
194 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->cost ==
195 casted_data->cost);
196 BOOST_CHECK(
197 (casted_problem1.get_runningDatas()[i]->xnext - casted_data->xnext)
198 .isZero(1e-9f));
199 BOOST_CHECK(
200 (casted_problem2.get_runningDatas()[i]->xnext - casted_data->xnext)
201 .isZero(1e-9f));
202 BOOST_CHECK(std::abs(float(problem1.get_runningDatas()[i]->cost) -
203 casted_data->cost) <= tol_f);
204 BOOST_CHECK((problem1.get_runningDatas()[i]->xnext.cast<float>() -
205 casted_data->xnext)
206 .isZero(tol_f));
207 }
208 BOOST_CHECK(std::abs(float(cost) - cost_f) <= tol_f);
209 #endif
210 92 }
211
212 7 void test_calcDiff(ActionModelTypes::Type action_model_type) {
213 // create the model
214
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ActionModelFactory factory;
215 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
216
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(action_model_type);
217
218 // create two shooting problems (with and without data allocation)
219 7 std::size_t T = 20;
220
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 const Eigen::VectorXd& x0 = model->get_state()->rand();
221
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
222
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
223
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
224
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 140 datas[i] = model->createData();
225 }
226
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 crocoddyl::ShootingProblem problem1(x0, models, model);
227 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
228
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 14 model->createData());
229
230 // create random trajectory
231
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<Eigen::VectorXd> xs(T + 1);
232
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<Eigen::VectorXd> us(T);
233
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
234
2/4
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 140 times.
✗ Branch 7 not taken.
 140 xs[i] = model->get_state()->rand();
235
3/6
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 140 times.
✗ Branch 10 not taken.
 140 us[i] = Eigen::VectorXd::Random(model->get_nu());
236 }
237
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 xs.back() = model->get_state()->rand();
238
239 // check the state and cost in each node
240
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 problem1.calc(xs, us);
241
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 problem2.calc(xs, us);
242
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 problem1.calcDiff(xs, us);
243
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 problem2.calcDiff(xs, us);
244
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
245 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
246
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 140 model->createData();
247
3/6
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 140 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 140 times.
✗ Branch 11 not taken.
 140 model->calc(data, xs[i], us[i]);
248
3/6
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 140 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 140 times.
✗ Branch 11 not taken.
 140 model->calcDiff(data, xs[i], us[i]);
249
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem1.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-9));
250
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem2.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-9));
251
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem1.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-9));
252
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem2.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-9));
253
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem1.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-9));
254
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem2.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-9));
255
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem1.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-9));
256
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem2.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-9));
257
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem1.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-9));
258
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem2.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-9));
259
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem1.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-9));
260
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem2.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-9));
261
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem1.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-9));
262
9/18
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 140 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 140 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 140 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 140 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 140 times.
 140 BOOST_CHECK((problem2.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-9));
263 140 }
264 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
265
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 model->createData();
266
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 model->calc(data, xs.back());
267
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 model->calcDiff(data, xs.back());
268
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 7 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 7 times.
 7 BOOST_CHECK((problem1.get_terminalData()->Fx - data->Fx).isZero(1e-9));
269
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 7 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 7 times.
 7 BOOST_CHECK((problem2.get_terminalData()->Fx - data->Fx).isZero(1e-9));
270
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 7 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 7 times.
 7 BOOST_CHECK((problem1.get_terminalData()->Lx - data->Lx).isZero(1e-9));
271
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 7 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 7 times.
 7 BOOST_CHECK((problem2.get_terminalData()->Lx - data->Lx).isZero(1e-9));
272
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 7 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 7 times.
 7 BOOST_CHECK((problem1.get_terminalData()->Lxx - data->Lxx).isZero(1e-9));
273
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 7 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 7 times.
 7 BOOST_CHECK((problem2.get_terminalData()->Lxx - data->Lxx).isZero(1e-9));
274
275 // Checking that casted computation is the same
276 #ifdef NDEBUG // Run only in release mode
277 crocoddyl::ShootingProblemTpl<float> casted_problem1 = problem1.cast<float>();
278 crocoddyl::ShootingProblemTpl<float> casted_problem2 = problem2.cast<float>();
279 std::vector<Eigen::VectorXf> xs_f(T + 1);
280 std::vector<Eigen::VectorXf> us_f(T);
281 for (std::size_t i = 0; i < T; ++i) {
282 xs_f[i] = xs[i].cast<float>();
283 us_f[i] = us[i].cast<float>();
284 }
285 xs_f.back() = xs.back().cast<float>();
286 casted_problem1.calc(xs_f, us_f);
287 casted_problem1.calcDiff(xs_f, us_f);
288 casted_problem2.calc(xs_f, us_f);
289 casted_problem2.calcDiff(xs_f, us_f);
290 float tol_f = 10.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
291 for (std::size_t i = 0; i < T; ++i) {
292 const std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>&
293 casted_model = model->cast<float>();
294 const std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>>&
295 casted_data = casted_model->createData();
296 casted_model->calc(casted_data, xs_f[i], us_f[i]);
297 casted_model->calcDiff(casted_data, xs_f[i], us_f[i]);
298 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Fx == casted_data->Fx);
299 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Fx == casted_data->Fx);
300 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Fu == casted_data->Fu);
301 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Fu == casted_data->Fu);
302 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lx == casted_data->Lx);
303 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lx == casted_data->Lx);
304 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lu == casted_data->Lu);
305 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lu == casted_data->Lu);
306 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lxx == casted_data->Lxx);
307 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lxx == casted_data->Lxx);
308 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lxu == casted_data->Lxu);
309 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lxu == casted_data->Lxu);
310 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Luu == casted_data->Luu);
311 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Luu == casted_data->Luu);
312 BOOST_CHECK(
313 (problem1.get_runningDatas()[i]->Fx.cast<float>() - casted_data->Fx)
314 .isZero(tol_f));
315 BOOST_CHECK(
316 (problem1.get_runningDatas()[i]->Fu.cast<float>() - casted_data->Fu)
317 .isZero(tol_f));
318 BOOST_CHECK(
319 (problem1.get_runningDatas()[i]->Lx.cast<float>() - casted_data->Lx)
320 .isZero(tol_f));
321 BOOST_CHECK(
322 (problem1.get_runningDatas()[i]->Lu.cast<float>() - casted_data->Lu)
323 .isZero(tol_f));
324 BOOST_CHECK(
325 (problem1.get_runningDatas()[i]->Lxx.cast<float>() - casted_data->Lxx)
326 .isZero(tol_f));
327 BOOST_CHECK(
328 (problem1.get_runningDatas()[i]->Lxu.cast<float>() - casted_data->Lxu)
329 .isZero(tol_f));
330 BOOST_CHECK(
331 (problem1.get_runningDatas()[i]->Luu.cast<float>() - casted_data->Luu)
332 .isZero(tol_f));
333 }
334 #endif
335 7 }
336
337 92 void test_calcDiff_diffAction(
338 DifferentialActionModelTypes::Type action_model_type,
339 IntegratorTypes::Type integrator_type) {
340 // create the model
341
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 DifferentialActionModelFactory factory;
342 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& diffModel =
343
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 factory.create(action_model_type);
344
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 IntegratorFactory factory_int;
345 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
346
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 factory_int.create(integrator_type, diffModel);
347
348 // create two shooting problems (with and without data allocation)
349 92 std::size_t T = 20;
350
2/4
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 const Eigen::VectorXd& x0 = model->get_state()->rand();
351
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
352
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
353
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
354
1/2
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
 1840 datas[i] = model->createData();
355 }
356
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 crocoddyl::ShootingProblem problem1(x0, models, model);
357 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
358
2/4
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 184 model->createData());
359
360 // create random trajectory
361
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<Eigen::VectorXd> xs(T + 1);
362
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<Eigen::VectorXd> us(T);
363
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
364
2/4
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1840 times.
✗ Branch 7 not taken.
 1840 xs[i] = model->get_state()->rand();
365
3/6
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1840 times.
✗ Branch 10 not taken.
 1840 us[i] = Eigen::VectorXd::Random(model->get_nu());
366 }
367
2/4
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 xs.back() = model->get_state()->rand();
368
369 // check the state and cost in each node
370
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 problem1.calc(xs, us);
371
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 problem2.calc(xs, us);
372
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 problem1.calcDiff(xs, us);
373
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 problem2.calcDiff(xs, us);
374
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
375 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
376
1/2
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
 1840 model->createData();
377
3/6
✓ Branch 3 taken 1840 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1840 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1840 times.
✗ Branch 11 not taken.
 1840 model->calc(data, xs[i], us[i]);
378
3/6
✓ Branch 3 taken 1840 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1840 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1840 times.
✗ Branch 11 not taken.
 1840 model->calcDiff(data, xs[i], us[i]);
379
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem1.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-7));
380
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem2.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-7));
381
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem1.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-7));
382
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem2.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-7));
383
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem1.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-7));
384
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem2.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-7));
385
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem1.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-7));
386
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem2.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-7));
387
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem1.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-7));
388
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem2.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-7));
389
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem1.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-7));
390
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem2.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-7));
391
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem1.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-7));
392
9/18
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1840 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1840 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1840 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1840 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1840 times.
 1840 BOOST_CHECK((problem2.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-7));
393 1840 }
394 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
395
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 model->createData();
396
2/4
✓ Branch 3 taken 92 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 model->calc(data, xs.back());
397
2/4
✓ Branch 3 taken 92 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 model->calcDiff(data, xs.back());
398
9/18
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 92 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 92 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 92 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 92 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 92 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 92 times.
 92 BOOST_CHECK((problem1.get_terminalData()->Fx - data->Fx).isZero(1e-7));
399
9/18
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 92 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 92 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 92 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 92 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 92 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 92 times.
 92 BOOST_CHECK((problem2.get_terminalData()->Fx - data->Fx).isZero(1e-7));
400
9/18
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 92 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 92 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 92 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 92 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 92 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 92 times.
 92 BOOST_CHECK((problem1.get_terminalData()->Lx - data->Lx).isZero(1e-7));
401
9/18
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 92 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 92 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 92 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 92 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 92 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 92 times.
 92 BOOST_CHECK((problem2.get_terminalData()->Lx - data->Lx).isZero(1e-7));
402
9/18
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 92 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 92 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 92 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 92 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 92 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 92 times.
 92 BOOST_CHECK((problem1.get_terminalData()->Lxx - data->Lxx).isZero(1e-7));
403
9/18
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 92 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 92 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 92 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 92 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 92 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 92 times.
 92 BOOST_CHECK((problem2.get_terminalData()->Lxx - data->Lxx).isZero(1e-7));
404
405 // Checking that casted computation is the same
406 #ifdef NDEBUG // Run only in release mode
407 crocoddyl::ShootingProblemTpl<float> casted_problem1 = problem1.cast<float>();
408 crocoddyl::ShootingProblemTpl<float> casted_problem2 = problem2.cast<float>();
409 std::vector<Eigen::VectorXf> xs_f(T + 1);
410 std::vector<Eigen::VectorXf> us_f(T);
411 for (std::size_t i = 0; i < T; ++i) {
412 xs_f[i] = xs[i].cast<float>();
413 us_f[i] = us[i].cast<float>();
414 }
415 xs_f.back() = xs.back().cast<float>();
416 casted_problem1.calc(xs_f, us_f);
417 casted_problem1.calcDiff(xs_f, us_f);
418 casted_problem2.calc(xs_f, us_f);
419 casted_problem2.calcDiff(xs_f, us_f);
420 float tol_f = 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
421 for (std::size_t i = 0; i < T; ++i) {
422 const std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>&
423 casted_model = model->cast<float>();
424 const std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>>&
425 casted_data = casted_model->createData();
426 casted_model->calc(casted_data, xs_f[i], us_f[i]);
427 casted_model->calcDiff(casted_data, xs_f[i], us_f[i]);
428 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Fx == casted_data->Fx);
429 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Fx == casted_data->Fx);
430 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Fu == casted_data->Fu);
431 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Fu == casted_data->Fu);
432 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lx == casted_data->Lx);
433 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lx == casted_data->Lx);
434 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lu == casted_data->Lu);
435 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lu == casted_data->Lu);
436 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lxx == casted_data->Lxx);
437 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lxx == casted_data->Lxx);
438 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lxu == casted_data->Lxu);
439 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lxu == casted_data->Lxu);
440 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Luu == casted_data->Luu);
441 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Luu == casted_data->Luu);
442 BOOST_CHECK(
443 (problem1.get_runningDatas()[i]->Fx.cast<float>() - casted_data->Fx)
444 .isZero(tol_f));
445 BOOST_CHECK(
446 (problem1.get_runningDatas()[i]->Fu.cast<float>() - casted_data->Fu)
447 .isZero(tol_f));
448 BOOST_CHECK(
449 (problem1.get_runningDatas()[i]->Lx.cast<float>() - casted_data->Lx)
450 .isZero(tol_f));
451 BOOST_CHECK(
452 (problem1.get_runningDatas()[i]->Lu.cast<float>() - casted_data->Lu)
453 .isZero(tol_f));
454 BOOST_CHECK(
455 (problem1.get_runningDatas()[i]->Lxx.cast<float>() - casted_data->Lxx)
456 .isZero(20.f * tol_f));
457 BOOST_CHECK(
458 (problem1.get_runningDatas()[i]->Lxu.cast<float>() - casted_data->Lxu)
459 .isZero(tol_f));
460 BOOST_CHECK(
461 (problem1.get_runningDatas()[i]->Luu.cast<float>() - casted_data->Luu)
462 .isZero(tol_f));
463 }
464 #endif
465 92 }
466
467 7 void test_rollout(ActionModelTypes::Type action_model_type) {
468 // create the model
469
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ActionModelFactory factory;
470 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
471
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(action_model_type);
472
473 // create the shooting problem
474 7 std::size_t T = 20;
475
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 const Eigen::VectorXd& x0 = model->get_state()->rand();
476
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
477
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 crocoddyl::ShootingProblem problem(x0, models, model);
478
479 // create random trajectory
480
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<Eigen::VectorXd> xs(T + 1);
481
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<Eigen::VectorXd> us(T);
482
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
483
2/4
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 140 times.
✗ Branch 7 not taken.
 140 xs[i] = model->get_state()->zero();
484
3/6
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 140 times.
✗ Branch 10 not taken.
 140 us[i] = Eigen::VectorXd::Random(model->get_nu());
485 }
486
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 xs.back() = model->get_state()->zero();
487
488 // check the state and cost in each node
489
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 problem.rollout(us, xs);
490
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
491 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
492
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 140 model->createData();
493
3/6
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 140 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 140 times.
✗ Branch 11 not taken.
 140 model->calc(data, xs[i], us[i]);
494
8/16
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 140 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 140 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 140 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 140 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 140 times.
 140 BOOST_CHECK((xs[i + 1] - data->xnext).isZero(1e-7));
495 140 }
496 7 }
497
498 92 void test_rollout_diffAction(
499 DifferentialActionModelTypes::Type action_model_type,
500 IntegratorTypes::Type integrator_type) {
501 // create the model
502
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 DifferentialActionModelFactory factory;
503 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& diffModel =
504
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 factory.create(action_model_type);
505
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 IntegratorFactory factory_int;
506 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
507
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 factory_int.create(integrator_type, diffModel);
508
509 // create the shooting problem
510 92 std::size_t T = 20;
511
2/4
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 const Eigen::VectorXd& x0 = model->get_state()->rand();
512
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
513
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 crocoddyl::ShootingProblem problem(x0, models, model);
514
515 // create random trajectory
516
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<Eigen::VectorXd> xs(T + 1);
517
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<Eigen::VectorXd> us(T);
518
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
519
2/4
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1840 times.
✗ Branch 7 not taken.
 1840 xs[i] = model->get_state()->zero();
520
3/6
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1840 times.
✗ Branch 10 not taken.
 1840 us[i] = Eigen::VectorXd::Random(model->get_nu());
521 }
522
2/4
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 xs.back() = model->get_state()->zero();
523
524 // check the state and cost in each node
525
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 problem.rollout(us, xs);
526
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
527 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
528
1/2
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
 1840 model->createData();
529
3/6
✓ Branch 3 taken 1840 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1840 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1840 times.
✗ Branch 11 not taken.
 1840 model->calc(data, xs[i], us[i]);
530
8/16
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1840 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1840 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1840 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1840 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1840 times.
 1840 BOOST_CHECK((xs[i + 1] - data->xnext).isZero(1e-7));
531 1840 }
532 92 }
533
534 7 void test_quasiStatic(ActionModelTypes::Type action_model_type) {
535 // create the model
536
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ActionModelFactory factory;
537 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
538
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(action_model_type);
539
540 // create two shooting problems (with and without data allocation)
541 7 std::size_t T = 20;
542
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 const Eigen::VectorXd& x0 = model->get_state()->rand();
543
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
544
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
545
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
546
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 140 datas[i] = model->createData();
547 }
548
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 crocoddyl::ShootingProblem problem1(x0, models, model);
549 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
550
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 14 model->createData());
551
552 // create random trajectory
553
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<Eigen::VectorXd> xs(T);
554
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 std::vector<Eigen::VectorXd> us(T);
555
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
556
2/4
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 140 times.
✗ Branch 7 not taken.
 140 xs[i] = model->get_state()->rand();
557
4/8
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 140 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 140 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 140 times.
✗ Branch 14 not taken.
 140 xs[i].tail(model->get_state()->get_nv()) *= 0;
558
3/6
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 140 times.
✗ Branch 10 not taken.
 140 us[i] = Eigen::VectorXd::Zero(model->get_nu());
559 }
560
561 // check the state and cost in each node
562
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 problem1.quasiStatic(us, xs);
563
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
564 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
565
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 140 model->createData();
566
3/6
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 140 times.
✗ Branch 9 not taken.
 140 Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
567
3/6
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 140 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 140 times.
✗ Branch 10 not taken.
 140 model->quasiStatic(data, u, xs[i]);
568
8/16
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 140 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 140 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 140 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 140 times.
 140 BOOST_CHECK((u - us[i]).isZero(1e-7));
569 140 }
570
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 problem2.quasiStatic(us, xs);
571
2/2
✓ Branch 0 taken 140 times.
✓ Branch 1 taken 7 times.
 147 for (std::size_t i = 0; i < T; ++i) {
572 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
573
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 140 model->createData();
574
3/6
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 140 times.
✗ Branch 9 not taken.
 140 Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
575
3/6
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 140 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 140 times.
✗ Branch 10 not taken.
 140 model->quasiStatic(data, u, xs[i]);
576
8/16
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 140 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 140 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 140 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 140 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 140 times.
 140 BOOST_CHECK((u - us[i]).isZero(1e-7));
577 140 }
578 7 }
579
580 92 void test_quasiStatic_diffAction(
581 DifferentialActionModelTypes::Type action_model_type,
582 IntegratorTypes::Type integrator_type) {
583 // create the model
584
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 DifferentialActionModelFactory factory;
585 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& diffModel =
586
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 factory.create(action_model_type);
587
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 IntegratorFactory factory_int;
588 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
589
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 factory_int.create(integrator_type, diffModel);
590
591 // create two shooting problems (with and without data allocation)
592 92 std::size_t T = 20;
593
2/4
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 const Eigen::VectorXd& x0 = model->get_state()->rand();
594
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
595
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
596
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
597
1/2
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
 1840 datas[i] = model->createData();
598 }
599
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 crocoddyl::ShootingProblem problem1(x0, models, model);
600 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
601
2/4
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 184 model->createData());
602
603 // create random trajectory
604
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<Eigen::VectorXd> xs(T);
605
1/2
✓ Branch 2 taken 92 times.
✗ Branch 3 not taken.
 92 std::vector<Eigen::VectorXd> us(T);
606
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
607
2/4
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1840 times.
✗ Branch 7 not taken.
 1840 xs[i] = model->get_state()->rand();
608
4/8
✓ Branch 3 taken 1840 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1840 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1840 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1840 times.
✗ Branch 14 not taken.
 1840 xs[i].tail(model->get_state()->get_nv()) *= 0;
609
3/6
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1840 times.
✗ Branch 10 not taken.
 1840 us[i] = Eigen::VectorXd::Zero(model->get_nu());
610 }
611
612 // check the state and cost in each node
613
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 problem1.quasiStatic(us, xs);
614
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
615 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
616
1/2
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
 1840 model->createData();
617
3/6
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1840 times.
✗ Branch 9 not taken.
 1840 Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
618
3/6
✓ Branch 3 taken 1840 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1840 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1840 times.
✗ Branch 10 not taken.
 1840 model->quasiStatic(data, u, xs[i]);
619
8/16
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1840 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1840 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1840 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1840 times.
 1840 BOOST_CHECK((u - us[i]).isZero(1e-7));
620 1840 }
621
1/2
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
 92 problem2.quasiStatic(us, xs);
622
2/2
✓ Branch 0 taken 1840 times.
✓ Branch 1 taken 92 times.
 1932 for (std::size_t i = 0; i < T; ++i) {
623 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
624
1/2
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
 1840 model->createData();
625
3/6
✓ Branch 2 taken 1840 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1840 times.
✗ Branch 9 not taken.
 1840 Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
626
3/6
✓ Branch 3 taken 1840 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1840 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1840 times.
✗ Branch 10 not taken.
 1840 model->quasiStatic(data, u, xs[i]);
627
8/16
✓ Branch 1 taken 1840 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1840 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1840 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1840 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1840 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1840 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1840 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1840 times.
 1840 BOOST_CHECK((u - us[i]).isZero(1e-7));
628 1840 }
629 92 }
630
631 //----------------------------------------------------------------------------//
632
633 7 void register_action_model_unit_tests(
634 ActionModelTypes::Type action_model_type) {
635
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 boost::test_tools::output_test_stream test_name;
636
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 test_name << "test_" << action_model_type;
637
4/8
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
 7 std::cout << "Running " << test_name.str() << std::endl;
638
4/8
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
639
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_calc, action_model_type)));
640
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff, action_model_type)));
641
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_quasiStatic, action_model_type)));
642
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_rollout, action_model_type)));
643
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 framework::master_test_suite().add(ts);
644 7 }
645
646 92 void register_diff_action_model_unit_tests(
647 DifferentialActionModelTypes::Type action_model_type,
648 IntegratorTypes::Type integrator_type) {
649
2/4
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92 times.
✗ Branch 5 not taken.
 92 boost::test_tools::output_test_stream test_name;
650
4/8
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 92 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 92 times.
✗ Branch 11 not taken.
 92 test_name << "test_" << action_model_type << "_" << integrator_type;
651
4/8
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 92 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 92 times.
✗ Branch 11 not taken.
 92 std::cout << "Running " << test_name.str() << std::endl;
652
4/8
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
 92 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
653
5/10
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 92 times.
✗ Branch 15 not taken.
 92 ts->add(BOOST_TEST_CASE(
654 boost::bind(&test_calc_diffAction, action_model_type, integrator_type)));
655
5/10
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 92 times.
✗ Branch 15 not taken.
 92 ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff_diffAction,
656 action_model_type, integrator_type)));
657
5/10
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 92 times.
✗ Branch 15 not taken.
 92 ts->add(BOOST_TEST_CASE(boost::bind(&test_quasiStatic_diffAction,
658 action_model_type, integrator_type)));
659
5/10
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 92 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 92 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 92 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 92 times.
✗ Branch 15 not taken.
 92 ts->add(BOOST_TEST_CASE(boost::bind(&test_rollout_diffAction,
660 action_model_type, integrator_type)));
661
3/6
✓ Branch 1 taken 92 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 92 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 92 times.
✗ Branch 7 not taken.
 92 framework::master_test_suite().add(ts);
662 92 }
663
664 1 bool init_function() {
665
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 1 times.
 8 for (size_t i = 0; i < ActionModelTypes::all.size(); ++i) {
666 7 register_action_model_unit_tests(ActionModelTypes::all[i]);
667 }
668
2/2
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 1 times.
 24 for (size_t i = 0; i < DifferentialActionModelTypes::all.size(); ++i) {
669
2/2
✓ Branch 1 taken 92 times.
✓ Branch 2 taken 23 times.
 115 for (size_t j = 0; j < IntegratorTypes::all.size(); ++j) {
670 92 register_diff_action_model_unit_tests(
671 92 DifferentialActionModelTypes::all[i], IntegratorTypes::all[j]);
672 }
673 }
674 1 return true;
675 }
676
677 1 int main(int argc, char** argv) {
678 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
679 }
680