| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2020-2021, 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 "factory/action.hpp" |
||
15 |
#include "factory/diff_action.hpp" |
||
16 |
#include "factory/integrator.hpp" |
||
17 |
#include "unittest_common.hpp" |
||
18 |
|||
19 |
using namespace boost::unit_test; |
||
20 |
using namespace crocoddyl::unittest; |
||
21 |
|||
22 |
//----------------------------------------------------------------------------// |
||
23 |
|||
24 |
5 |
void test_calc(ActionModelTypes::Type action_model_type) { |
|
25 |
// create the model |
||
26 |
✓✗ | 10 |
ActionModelFactory factory; |
27 |
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model = |
||
28 |
✓✗ | 10 |
factory.create(action_model_type); |
29 |
|||
30 |
// create two shooting problems (with and without data allocation) |
||
31 |
5 |
std::size_t T = 20; |
|
32 |
✓✗ | 10 |
const Eigen::VectorXd& x0 = model->get_state()->rand(); |
33 |
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > models(T, |
||
34 |
✓✗ | 10 |
model); |
35 |
✓✗ | 10 |
std::vector<boost::shared_ptr<crocoddyl::ActionDataAbstract> > datas(T); |
36 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
37 |
✓✗ | 100 |
datas[i] = model->createData(); |
38 |
} |
||
39 |
✓✗ | 10 |
crocoddyl::ShootingProblem problem1(x0, models, model); |
40 |
crocoddyl::ShootingProblem problem2(x0, models, model, datas, |
||
41 |
✓✗✓✗ |
15 |
model->createData()); |
42 |
|||
43 |
// Run the print function |
||
44 |
✓✗ | 10 |
std::ostringstream tmp; |
45 |
✓✗ | 5 |
tmp << problem1; |
46 |
|||
47 |
// create random trajectory |
||
48 |
✓✗ | 10 |
std::vector<Eigen::VectorXd> xs(T + 1); |
49 |
✓✗ | 10 |
std::vector<Eigen::VectorXd> us(T); |
50 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
51 |
✓✗ | 100 |
xs[i] = model->get_state()->rand(); |
52 |
✓✗✓✗ |
100 |
us[i] = Eigen::VectorXd::Random(model->get_nu()); |
53 |
} |
||
54 |
✓✗ | 5 |
xs.back() = model->get_state()->rand(); |
55 |
|||
56 |
// check the state and cost in each node |
||
57 |
✓✗ | 5 |
problem1.calc(xs, us); |
58 |
✓✗ | 5 |
problem2.calc(xs, us); |
59 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
60 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
61 |
✓✗ | 200 |
model->createData(); |
62 |
✓✗✓✗ ✓✗ |
100 |
model->calc(data, xs[i], us[i]); |
63 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK(problem1.get_runningDatas()[i]->cost == data->cost); |
64 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK(problem2.get_runningDatas()[i]->cost == data->cost); |
65 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK( |
66 |
(problem1.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9)); |
||
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK( |
68 |
(problem2.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9)); |
||
69 |
} |
||
70 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
71 |
✓✗ | 10 |
model->createData(); |
72 |
✓✗✓✗ |
5 |
model->calc(data, xs.back()); |
73 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
5 |
BOOST_CHECK(problem1.get_terminalData()->cost == data->cost); |
74 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
5 |
BOOST_CHECK(problem2.get_terminalData()->cost == data->cost); |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
5 |
BOOST_CHECK((problem1.get_terminalData()->xnext - data->xnext).isZero(1e-9)); |
76 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
5 |
BOOST_CHECK((problem2.get_terminalData()->xnext - data->xnext).isZero(1e-9)); |
77 |
5 |
} |
|
78 |
|||
79 |
88 |
void test_calc_diffAction(DifferentialActionModelTypes::Type action_model_type, |
|
80 |
IntegratorTypes::Type integrator_type) { |
||
81 |
// create the model |
||
82 |
✓✗ | 176 |
DifferentialActionModelFactory factory; |
83 |
const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& |
||
84 |
✓✗ | 176 |
diffModel = factory.create(action_model_type); |
85 |
✓✗ | 176 |
IntegratorFactory factory_int; |
86 |
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model = |
||
87 |
✓✗ | 176 |
factory_int.create(integrator_type, diffModel); |
88 |
|||
89 |
// create two shooting problems (with and without data allocation) |
||
90 |
88 |
std::size_t T = 20; |
|
91 |
✓✗ | 176 |
const Eigen::VectorXd& x0 = model->get_state()->rand(); |
92 |
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > models(T, |
||
93 |
✓✗ | 176 |
model); |
94 |
✓✗ | 176 |
std::vector<boost::shared_ptr<crocoddyl::ActionDataAbstract> > datas(T); |
95 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
96 |
✓✗ | 1760 |
datas[i] = model->createData(); |
97 |
} |
||
98 |
✓✗ | 176 |
crocoddyl::ShootingProblem problem1(x0, models, model); |
99 |
crocoddyl::ShootingProblem problem2(x0, models, model, datas, |
||
100 |
✓✗✓✗ |
264 |
model->createData()); |
101 |
|||
102 |
// create random trajectory |
||
103 |
✓✗ | 176 |
std::vector<Eigen::VectorXd> xs(T + 1); |
104 |
✓✗ | 176 |
std::vector<Eigen::VectorXd> us(T); |
105 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
106 |
✓✗ | 1760 |
xs[i] = model->get_state()->rand(); |
107 |
✓✗✓✗ |
1760 |
us[i] = Eigen::VectorXd::Random(model->get_nu()); |
108 |
} |
||
109 |
✓✗ | 88 |
xs.back() = model->get_state()->rand(); |
110 |
|||
111 |
// check the state and cost in each node |
||
112 |
✓✗ | 88 |
problem1.calc(xs, us); |
113 |
✓✗ | 88 |
problem2.calc(xs, us); |
114 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
115 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
116 |
✓✗ | 3520 |
model->createData(); |
117 |
✓✗✓✗ ✓✗ |
1760 |
model->calc(data, xs[i], us[i]); |
118 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK(problem1.get_runningDatas()[i]->cost == data->cost); |
119 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK(problem2.get_runningDatas()[i]->cost == data->cost); |
120 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK( |
121 |
(problem1.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9)); |
||
122 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK( |
123 |
(problem2.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9)); |
||
124 |
} |
||
125 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
126 |
✓✗ | 176 |
model->createData(); |
127 |
✓✗✓✗ |
88 |
model->calc(data, xs.back()); |
128 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
88 |
BOOST_CHECK(problem1.get_terminalData()->cost == data->cost); |
129 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
88 |
BOOST_CHECK(problem2.get_terminalData()->cost == data->cost); |
130 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
88 |
BOOST_CHECK((problem1.get_terminalData()->xnext - data->xnext).isZero(1e-9)); |
131 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
88 |
BOOST_CHECK((problem2.get_terminalData()->xnext - data->xnext).isZero(1e-9)); |
132 |
88 |
} |
|
133 |
|||
134 |
5 |
void test_calcDiff(ActionModelTypes::Type action_model_type) { |
|
135 |
// create the model |
||
136 |
✓✗ | 10 |
ActionModelFactory factory; |
137 |
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model = |
||
138 |
✓✗ | 10 |
factory.create(action_model_type); |
139 |
|||
140 |
// create two shooting problems (with and without data allocation) |
||
141 |
5 |
std::size_t T = 20; |
|
142 |
✓✗ | 10 |
const Eigen::VectorXd& x0 = model->get_state()->rand(); |
143 |
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > models(T, |
||
144 |
✓✗ | 10 |
model); |
145 |
✓✗ | 10 |
std::vector<boost::shared_ptr<crocoddyl::ActionDataAbstract> > datas(T); |
146 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
147 |
✓✗ | 100 |
datas[i] = model->createData(); |
148 |
} |
||
149 |
✓✗ | 10 |
crocoddyl::ShootingProblem problem1(x0, models, model); |
150 |
crocoddyl::ShootingProblem problem2(x0, models, model, datas, |
||
151 |
✓✗✓✗ |
15 |
model->createData()); |
152 |
|||
153 |
// create random trajectory |
||
154 |
✓✗ | 10 |
std::vector<Eigen::VectorXd> xs(T + 1); |
155 |
✓✗ | 10 |
std::vector<Eigen::VectorXd> us(T); |
156 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
157 |
✓✗ | 100 |
xs[i] = model->get_state()->rand(); |
158 |
✓✗✓✗ |
100 |
us[i] = Eigen::VectorXd::Random(model->get_nu()); |
159 |
} |
||
160 |
✓✗ | 5 |
xs.back() = model->get_state()->rand(); |
161 |
|||
162 |
// check the state and cost in each node |
||
163 |
✓✗ | 5 |
problem1.calc(xs, us); |
164 |
✓✗ | 5 |
problem2.calc(xs, us); |
165 |
✓✗ | 5 |
problem1.calcDiff(xs, us); |
166 |
✓✗ | 5 |
problem2.calcDiff(xs, us); |
167 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
168 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
169 |
✓✗ | 200 |
model->createData(); |
170 |
✓✗✓✗ ✓✗ |
100 |
model->calc(data, xs[i], us[i]); |
171 |
✓✗✓✗ ✓✗ |
100 |
model->calcDiff(data, xs[i], us[i]); |
172 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-9)); |
173 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-9)); |
174 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-9)); |
175 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-9)); |
176 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-9)); |
177 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-9)); |
178 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-9)); |
179 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-9)); |
180 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-9)); |
181 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-9)); |
182 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-9)); |
183 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-9)); |
184 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-9)); |
185 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-9)); |
186 |
} |
||
187 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
188 |
✓✗ | 10 |
model->createData(); |
189 |
✓✗✓✗ |
5 |
model->calc(data, xs.back()); |
190 |
✓✗✓✗ |
5 |
model->calcDiff(data, xs.back()); |
191 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
5 |
BOOST_CHECK((problem1.get_terminalData()->Fx - data->Fx).isZero(1e-9)); |
192 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
5 |
BOOST_CHECK((problem2.get_terminalData()->Fx - data->Fx).isZero(1e-9)); |
193 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
5 |
BOOST_CHECK((problem1.get_terminalData()->Lx - data->Lx).isZero(1e-9)); |
194 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
5 |
BOOST_CHECK((problem2.get_terminalData()->Lx - data->Lx).isZero(1e-9)); |
195 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
5 |
BOOST_CHECK((problem1.get_terminalData()->Lxx - data->Lxx).isZero(1e-9)); |
196 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
5 |
BOOST_CHECK((problem2.get_terminalData()->Lxx - data->Lxx).isZero(1e-9)); |
197 |
5 |
} |
|
198 |
|||
199 |
88 |
void test_calcDiff_diffAction( |
|
200 |
DifferentialActionModelTypes::Type action_model_type, |
||
201 |
IntegratorTypes::Type integrator_type) { |
||
202 |
// create the model |
||
203 |
✓✗ | 176 |
DifferentialActionModelFactory factory; |
204 |
const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& |
||
205 |
✓✗ | 176 |
diffModel = factory.create(action_model_type); |
206 |
✓✗ | 176 |
IntegratorFactory factory_int; |
207 |
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model = |
||
208 |
✓✗ | 176 |
factory_int.create(integrator_type, diffModel); |
209 |
|||
210 |
// create two shooting problems (with and without data allocation) |
||
211 |
88 |
std::size_t T = 20; |
|
212 |
✓✗ | 176 |
const Eigen::VectorXd& x0 = model->get_state()->rand(); |
213 |
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > models(T, |
||
214 |
✓✗ | 176 |
model); |
215 |
✓✗ | 176 |
std::vector<boost::shared_ptr<crocoddyl::ActionDataAbstract> > datas(T); |
216 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
217 |
✓✗ | 1760 |
datas[i] = model->createData(); |
218 |
} |
||
219 |
✓✗ | 176 |
crocoddyl::ShootingProblem problem1(x0, models, model); |
220 |
crocoddyl::ShootingProblem problem2(x0, models, model, datas, |
||
221 |
✓✗✓✗ |
264 |
model->createData()); |
222 |
|||
223 |
// create random trajectory |
||
224 |
✓✗ | 176 |
std::vector<Eigen::VectorXd> xs(T + 1); |
225 |
✓✗ | 176 |
std::vector<Eigen::VectorXd> us(T); |
226 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
227 |
✓✗ | 1760 |
xs[i] = model->get_state()->rand(); |
228 |
✓✗✓✗ |
1760 |
us[i] = Eigen::VectorXd::Random(model->get_nu()); |
229 |
} |
||
230 |
✓✗ | 88 |
xs.back() = model->get_state()->rand(); |
231 |
|||
232 |
// check the state and cost in each node |
||
233 |
✓✗ | 88 |
problem1.calc(xs, us); |
234 |
✓✗ | 88 |
problem2.calc(xs, us); |
235 |
✓✗ | 88 |
problem1.calcDiff(xs, us); |
236 |
✓✗ | 88 |
problem2.calcDiff(xs, us); |
237 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
238 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
239 |
✓✗ | 3520 |
model->createData(); |
240 |
✓✗✓✗ ✓✗ |
1760 |
model->calc(data, xs[i], us[i]); |
241 |
✓✗✓✗ ✓✗ |
1760 |
model->calcDiff(data, xs[i], us[i]); |
242 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-7)); |
243 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-7)); |
244 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-7)); |
245 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-7)); |
246 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-7)); |
247 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-7)); |
248 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-7)); |
249 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-7)); |
250 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-7)); |
251 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-7)); |
252 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-7)); |
253 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-7)); |
254 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem1.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-7)); |
255 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((problem2.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-7)); |
256 |
} |
||
257 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
258 |
✓✗ | 176 |
model->createData(); |
259 |
✓✗✓✗ |
88 |
model->calc(data, xs.back()); |
260 |
✓✗✓✗ |
88 |
model->calcDiff(data, xs.back()); |
261 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
88 |
BOOST_CHECK((problem1.get_terminalData()->Fx - data->Fx).isZero(1e-7)); |
262 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
88 |
BOOST_CHECK((problem2.get_terminalData()->Fx - data->Fx).isZero(1e-7)); |
263 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
88 |
BOOST_CHECK((problem1.get_terminalData()->Lx - data->Lx).isZero(1e-7)); |
264 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
88 |
BOOST_CHECK((problem2.get_terminalData()->Lx - data->Lx).isZero(1e-7)); |
265 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
88 |
BOOST_CHECK((problem1.get_terminalData()->Lxx - data->Lxx).isZero(1e-7)); |
266 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
88 |
BOOST_CHECK((problem2.get_terminalData()->Lxx - data->Lxx).isZero(1e-7)); |
267 |
88 |
} |
|
268 |
|||
269 |
5 |
void test_rollout(ActionModelTypes::Type action_model_type) { |
|
270 |
// create the model |
||
271 |
✓✗ | 10 |
ActionModelFactory factory; |
272 |
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model = |
||
273 |
✓✗ | 10 |
factory.create(action_model_type); |
274 |
|||
275 |
// create the shooting problem |
||
276 |
5 |
std::size_t T = 20; |
|
277 |
✓✗ | 10 |
const Eigen::VectorXd& x0 = model->get_state()->rand(); |
278 |
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > models(T, |
||
279 |
✓✗ | 10 |
model); |
280 |
✓✗ | 10 |
crocoddyl::ShootingProblem problem(x0, models, model); |
281 |
|||
282 |
// create random trajectory |
||
283 |
✓✗ | 10 |
std::vector<Eigen::VectorXd> xs(T + 1); |
284 |
✓✗ | 10 |
std::vector<Eigen::VectorXd> us(T); |
285 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
286 |
✓✗ | 100 |
xs[i] = model->get_state()->zero(); |
287 |
✓✗✓✗ |
100 |
us[i] = Eigen::VectorXd::Random(model->get_nu()); |
288 |
} |
||
289 |
✓✗ | 5 |
xs.back() = model->get_state()->zero(); |
290 |
|||
291 |
// check the state and cost in each node |
||
292 |
✓✗ | 5 |
problem.rollout(us, xs); |
293 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
294 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
295 |
✓✗ | 200 |
model->createData(); |
296 |
✓✗✓✗ ✓✗ |
100 |
model->calc(data, xs[i], us[i]); |
297 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((xs[i + 1] - data->xnext).isZero(1e-7)); |
298 |
} |
||
299 |
5 |
} |
|
300 |
|||
301 |
88 |
void test_rollout_diffAction( |
|
302 |
DifferentialActionModelTypes::Type action_model_type, |
||
303 |
IntegratorTypes::Type integrator_type) { |
||
304 |
// create the model |
||
305 |
✓✗ | 176 |
DifferentialActionModelFactory factory; |
306 |
const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& |
||
307 |
✓✗ | 176 |
diffModel = factory.create(action_model_type); |
308 |
✓✗ | 176 |
IntegratorFactory factory_int; |
309 |
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model = |
||
310 |
✓✗ | 176 |
factory_int.create(integrator_type, diffModel); |
311 |
|||
312 |
// create the shooting problem |
||
313 |
88 |
std::size_t T = 20; |
|
314 |
✓✗ | 176 |
const Eigen::VectorXd& x0 = model->get_state()->rand(); |
315 |
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > models(T, |
||
316 |
✓✗ | 176 |
model); |
317 |
✓✗ | 176 |
crocoddyl::ShootingProblem problem(x0, models, model); |
318 |
|||
319 |
// create random trajectory |
||
320 |
✓✗ | 176 |
std::vector<Eigen::VectorXd> xs(T + 1); |
321 |
✓✗ | 176 |
std::vector<Eigen::VectorXd> us(T); |
322 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
323 |
✓✗ | 1760 |
xs[i] = model->get_state()->zero(); |
324 |
✓✗✓✗ |
1760 |
us[i] = Eigen::VectorXd::Random(model->get_nu()); |
325 |
} |
||
326 |
✓✗ | 88 |
xs.back() = model->get_state()->zero(); |
327 |
|||
328 |
// check the state and cost in each node |
||
329 |
✓✗ | 88 |
problem.rollout(us, xs); |
330 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
331 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
332 |
✓✗ | 3520 |
model->createData(); |
333 |
✓✗✓✗ ✓✗ |
1760 |
model->calc(data, xs[i], us[i]); |
334 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((xs[i + 1] - data->xnext).isZero(1e-7)); |
335 |
} |
||
336 |
88 |
} |
|
337 |
|||
338 |
5 |
void test_quasiStatic(ActionModelTypes::Type action_model_type) { |
|
339 |
// create the model |
||
340 |
✓✗ | 10 |
ActionModelFactory factory; |
341 |
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model = |
||
342 |
✓✗ | 10 |
factory.create(action_model_type); |
343 |
|||
344 |
// create two shooting problems (with and without data allocation) |
||
345 |
5 |
std::size_t T = 20; |
|
346 |
✓✗ | 10 |
const Eigen::VectorXd& x0 = model->get_state()->rand(); |
347 |
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > models(T, |
||
348 |
✓✗ | 10 |
model); |
349 |
✓✗ | 10 |
std::vector<boost::shared_ptr<crocoddyl::ActionDataAbstract> > datas(T); |
350 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
351 |
✓✗ | 100 |
datas[i] = model->createData(); |
352 |
} |
||
353 |
✓✗ | 10 |
crocoddyl::ShootingProblem problem1(x0, models, model); |
354 |
crocoddyl::ShootingProblem problem2(x0, models, model, datas, |
||
355 |
✓✗✓✗ |
15 |
model->createData()); |
356 |
|||
357 |
// create random trajectory |
||
358 |
✓✗ | 10 |
std::vector<Eigen::VectorXd> xs(T); |
359 |
✓✗ | 10 |
std::vector<Eigen::VectorXd> us(T); |
360 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
361 |
✓✗ | 100 |
xs[i] = model->get_state()->rand(); |
362 |
✓✗✓✗ |
100 |
xs[i].tail(model->get_state()->get_nv()) *= 0; |
363 |
✓✗✓✗ |
100 |
us[i] = Eigen::VectorXd::Zero(model->get_nu()); |
364 |
} |
||
365 |
|||
366 |
// check the state and cost in each node |
||
367 |
✓✗ | 5 |
problem1.quasiStatic(us, xs); |
368 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
369 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
370 |
✓✗ | 200 |
model->createData(); |
371 |
✓✗✓✗ |
200 |
Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu()); |
372 |
✓✗✓✗ ✓✗ |
100 |
model->quasiStatic(data, u, xs[i]); |
373 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((u - us[i]).isZero(1e-7)); |
374 |
} |
||
375 |
✓✗ | 5 |
problem2.quasiStatic(us, xs); |
376 |
✓✓ | 105 |
for (std::size_t i = 0; i < T; ++i) { |
377 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
378 |
✓✗ | 200 |
model->createData(); |
379 |
✓✗✓✗ |
200 |
Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu()); |
380 |
✓✗✓✗ ✓✗ |
100 |
model->quasiStatic(data, u, xs[i]); |
381 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
100 |
BOOST_CHECK((u - us[i]).isZero(1e-7)); |
382 |
} |
||
383 |
5 |
} |
|
384 |
|||
385 |
88 |
void test_quasiStatic_diffAction( |
|
386 |
DifferentialActionModelTypes::Type action_model_type, |
||
387 |
IntegratorTypes::Type integrator_type) { |
||
388 |
// create the model |
||
389 |
✓✗ | 176 |
DifferentialActionModelFactory factory; |
390 |
const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& |
||
391 |
✓✗ | 176 |
diffModel = factory.create(action_model_type); |
392 |
✓✗ | 176 |
IntegratorFactory factory_int; |
393 |
const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model = |
||
394 |
✓✗ | 176 |
factory_int.create(integrator_type, diffModel); |
395 |
|||
396 |
// create two shooting problems (with and without data allocation) |
||
397 |
88 |
std::size_t T = 20; |
|
398 |
✓✗ | 176 |
const Eigen::VectorXd& x0 = model->get_state()->rand(); |
399 |
std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > models(T, |
||
400 |
✓✗ | 176 |
model); |
401 |
✓✗ | 176 |
std::vector<boost::shared_ptr<crocoddyl::ActionDataAbstract> > datas(T); |
402 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
403 |
✓✗ | 1760 |
datas[i] = model->createData(); |
404 |
} |
||
405 |
✓✗ | 176 |
crocoddyl::ShootingProblem problem1(x0, models, model); |
406 |
crocoddyl::ShootingProblem problem2(x0, models, model, datas, |
||
407 |
✓✗✓✗ |
264 |
model->createData()); |
408 |
|||
409 |
// create random trajectory |
||
410 |
✓✗ | 176 |
std::vector<Eigen::VectorXd> xs(T); |
411 |
✓✗ | 176 |
std::vector<Eigen::VectorXd> us(T); |
412 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
413 |
✓✗ | 1760 |
xs[i] = model->get_state()->rand(); |
414 |
✓✗✓✗ |
1760 |
xs[i].tail(model->get_state()->get_nv()) *= 0; |
415 |
✓✗✓✗ |
1760 |
us[i] = Eigen::VectorXd::Zero(model->get_nu()); |
416 |
} |
||
417 |
|||
418 |
// check the state and cost in each node |
||
419 |
✓✗ | 88 |
problem1.quasiStatic(us, xs); |
420 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
421 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
422 |
✓✗ | 3520 |
model->createData(); |
423 |
✓✗✓✗ |
3520 |
Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu()); |
424 |
✓✗✓✗ ✓✗ |
1760 |
model->quasiStatic(data, u, xs[i]); |
425 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((u - us[i]).isZero(1e-7)); |
426 |
} |
||
427 |
✓✗ | 88 |
problem2.quasiStatic(us, xs); |
428 |
✓✓ | 1848 |
for (std::size_t i = 0; i < T; ++i) { |
429 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
430 |
✓✗ | 3520 |
model->createData(); |
431 |
✓✗✓✗ |
3520 |
Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu()); |
432 |
✓✗✓✗ ✓✗ |
1760 |
model->quasiStatic(data, u, xs[i]); |
433 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1760 |
BOOST_CHECK((u - us[i]).isZero(1e-7)); |
434 |
} |
||
435 |
88 |
} |
|
436 |
|||
437 |
//----------------------------------------------------------------------------// |
||
438 |
|||
439 |
5 |
void register_action_model_unit_tests( |
|
440 |
ActionModelTypes::Type action_model_type) { |
||
441 |
✓✗✓✗ |
10 |
boost::test_tools::output_test_stream test_name; |
442 |
✓✗✓✗ |
5 |
test_name << "test_" << action_model_type; |
443 |
✓✗✓✗ ✓✗✓✗ |
5 |
std::cout << "Running " << test_name.str() << std::endl; |
444 |
✓✗✓✗ ✓✗✓✗ |
5 |
test_suite* ts = BOOST_TEST_SUITE(test_name.str()); |
445 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
5 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_calc, action_model_type))); |
446 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
5 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff, action_model_type))); |
447 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
5 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_quasiStatic, action_model_type))); |
448 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
5 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_rollout, action_model_type))); |
449 |
✓✗✓✗ ✓✗ |
5 |
framework::master_test_suite().add(ts); |
450 |
5 |
} |
|
451 |
|||
452 |
88 |
void register_diff_action_model_unit_tests( |
|
453 |
DifferentialActionModelTypes::Type action_model_type, |
||
454 |
IntegratorTypes::Type integrator_type) { |
||
455 |
✓✗✓✗ |
176 |
boost::test_tools::output_test_stream test_name; |
456 |
✓✗✓✗ ✓✗✓✗ |
88 |
test_name << "test_" << action_model_type << "_" << integrator_type; |
457 |
✓✗✓✗ ✓✗✓✗ |
88 |
std::cout << "Running " << test_name.str() << std::endl; |
458 |
✓✗✓✗ ✓✗✓✗ |
88 |
test_suite* ts = BOOST_TEST_SUITE(test_name.str()); |
459 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
88 |
ts->add(BOOST_TEST_CASE( |
460 |
boost::bind(&test_calc_diffAction, action_model_type, integrator_type))); |
||
461 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
88 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff_diffAction, |
462 |
action_model_type, integrator_type))); |
||
463 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
88 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_quasiStatic_diffAction, |
464 |
action_model_type, integrator_type))); |
||
465 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
88 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_rollout_diffAction, |
466 |
action_model_type, integrator_type))); |
||
467 |
✓✗✓✗ ✓✗ |
88 |
framework::master_test_suite().add(ts); |
468 |
88 |
} |
|
469 |
|||
470 |
1 |
bool init_function() { |
|
471 |
✓✓ | 6 |
for (size_t i = 0; i < ActionModelTypes::all.size(); ++i) { |
472 |
5 |
register_action_model_unit_tests(ActionModelTypes::all[i]); |
|
473 |
} |
||
474 |
✓✓ | 23 |
for (size_t i = 0; i < DifferentialActionModelTypes::all.size(); ++i) { |
475 |
✓✓ | 110 |
for (size_t j = 0; j < IntegratorTypes::all.size(); ++j) { |
476 |
88 |
register_diff_action_model_unit_tests( |
|
477 |
88 |
DifferentialActionModelTypes::all[i], IntegratorTypes::all[j]); |
|
478 |
} |
||
479 |
} |
||
480 |
1 |
return true; |
|
481 |
} |
||
482 |
|||
483 |
1 |
int main(int argc, char** argv) { |
|
484 |
1 |
return ::boost::unit_test::unit_test_main(&init_function, argc, argv); |
|
485 |
} |
| Generated by: GCOVR (Version 4.2) |