| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
|||
2 |
#include <eigen3/Eigen/Core> |
||
3 |
#include <gtest/gtest.h> |
||
4 |
|||
5 |
#include <math.h> |
||
6 |
#include <Eigen/Dense> |
||
7 |
|||
8 |
|||
9 |
#include "mpi_cpp_tools/basic_tools.hpp" |
||
10 |
#include "mpi_cpp_tools/math.hpp" |
||
11 |
#include "mpi_cpp_tools/dynamical_systems.hpp" |
||
12 |
|||
13 |
|||
14 |
using namespace mct; |
||
15 |
|||
16 |
double epsilon = 1e-10; |
||
17 |
|||
18 |
|||
19 |
|||
20 |
|||
21 |
✗✓ | 5 |
TEST(linear_dynamics, evolution_with_zero_initial_position) |
22 |
{ |
||
23 |
1 |
double jerk = 1.0; |
|
24 |
1 |
LinearDynamics linear_dynamics(jerk, 0.0, 0.0, 0.0); |
|
25 |
|||
26 |
✓✓ | 101 |
for(size_t t = 0; t < 100; t++) |
27 |
{ |
||
28 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(approx_equal(linear_dynamics.get_acceleration(t), |
29 |
t * jerk)); |
||
30 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(approx_equal(linear_dynamics.get_velocity(t), |
31 |
0.5 * t * t * jerk)); |
||
32 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(approx_equal(linear_dynamics.get_position(t), |
33 |
0.5 / 3.0 * t * t * t * jerk)); |
||
34 |
} |
||
35 |
1 |
} |
|
36 |
|||
37 |
// should be linear in initial state and control |
||
38 |
✗✓ | 5 |
TEST(linear_dynamics, check_linearity) |
39 |
{ |
||
40 |
✓✗ | 1 |
Eigen::Vector4d parameters_a = Eigen::Vector4d(3.4, -5.6, 2.3, 7.2); |
41 |
✓✗ | 1 |
Eigen::Vector4d parameters_b = Eigen::Vector4d(-2.1, -3.0, 5.5, -10.2); |
42 |
|||
43 |
✓✗✓✗ |
1 |
LinearDynamics linear_dynamics_a(parameters_a); |
44 |
✓✗✓✗ |
1 |
LinearDynamics linear_dynamics_b(parameters_b); |
45 |
✓✗✓✗ ✓✗ |
1 |
LinearDynamics linear_dynamics_sum(parameters_a + parameters_b); |
46 |
|||
47 |
✓✓ | 101 |
for(size_t t = 0; t < 100; t++) |
48 |
{ |
||
49 |
✓✗✓✗ ✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(approx_equal(linear_dynamics_a.get_acceleration(t) + |
50 |
linear_dynamics_b.get_acceleration(t), |
||
51 |
linear_dynamics_sum.get_acceleration(t), |
||
52 |
1e-6)); |
||
53 |
✓✗✓✗ ✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(approx_equal(linear_dynamics_a.get_velocity(t) + |
54 |
linear_dynamics_b.get_velocity(t), |
||
55 |
linear_dynamics_sum.get_velocity(t), |
||
56 |
1e-6)); |
||
57 |
✓✗✓✗ ✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(approx_equal(linear_dynamics_a.get_position(t) + |
58 |
linear_dynamics_b.get_position(t), |
||
59 |
linear_dynamics_sum.get_position(t), |
||
60 |
1e-6)); |
||
61 |
} |
||
62 |
1 |
} |
|
63 |
|||
64 |
|||
65 |
✗✓ | 5 |
TEST(linear_dynamics, test_find_t_given_velocity_basic) |
66 |
{ |
||
67 |
✓✗ | 1 |
Eigen::Vector4d parameters_basic = Eigen::Vector4d(1.0, -2.0, 0.0, 0.0); |
68 |
✓✗✓✗ |
1 |
LinearDynamics dynamics_basic(parameters_basic); |
69 |
|||
70 |
LinearDynamics::Vector solutions = |
||
71 |
✓✗ | 1 |
dynamics_basic.find_t_given_velocity(- 3.0 / 2.0); |
72 |
|||
73 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
1 |
EXPECT_TRUE(solutions.size() == 2); |
74 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
1 |
EXPECT_TRUE(contains(solutions, 1)); |
75 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
1 |
EXPECT_TRUE(contains(solutions, 3)); |
76 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
1 |
EXPECT_FALSE(contains(solutions, 4)); |
77 |
1 |
} |
|
78 |
|||
79 |
|||
80 |
✗✓ | 5 |
TEST(linear_dynamics, test_find_t_given_velocity_consistency) |
81 |
{ |
||
82 |
✓✗ | 1 |
Eigen::Vector4d parameters_a = Eigen::Vector4d(3.4, -5.6, 2.3, 7.2); |
83 |
✓✗ | 1 |
Eigen::Vector4d parameters_b = Eigen::Vector4d(-2.1, -3.0, 5.5, -10.2); |
84 |
✓✗✓✗ |
1 |
LinearDynamics linear_dynamics_a(parameters_a); |
85 |
✓✗✓✗ |
1 |
LinearDynamics linear_dynamics_b(parameters_b); |
86 |
|||
87 |
✓✓ | 101 |
for(size_t t = 0; t < 100; t++) |
88 |
{ |
||
89 |
auto solutions_a = linear_dynamics_a.find_t_given_velocity( |
||
90 |
✓✗✓✗ |
100 |
linear_dynamics_a.get_velocity(t)); |
91 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(contains(solutions_a, t)); |
92 |
auto solutions_b = linear_dynamics_b.find_t_given_velocity( |
||
93 |
✓✗✓✗ |
100 |
linear_dynamics_b.get_velocity(t)); |
94 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(contains(solutions_b, t)); |
95 |
} |
||
96 |
1 |
} |
|
97 |
|||
98 |
|||
99 |
|||
100 |
✗✓ | 5 |
TEST(linear_dynamics_with_acceleration_constraint, test_time_evolution) |
101 |
{ |
||
102 |
✓✗ | 1 |
Eigen::Matrix<double, 5, 1> parameters; |
103 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
parameters << -2.0, 15.0, 5.5, -10.2, 17.0; |
104 |
1 |
double jerk_duration = (17.0 + 15.0) / 2.0; |
|
105 |
|||
106 |
|||
107 |
✓✗✓✗ |
1 |
LinearDynamicsWithAccelerationConstraint constrained_dynamics(parameters); |
108 |
✓✗✓✗ ✓✗ |
1 |
LinearDynamics dynamics(parameters.topRows(4)); |
109 |
LinearDynamicsWithAccelerationConstraint |
||
110 |
constrained_dynamics_b(-2.0, |
||
111 |
dynamics.get_acceleration(jerk_duration), |
||
112 |
dynamics.get_velocity(jerk_duration), |
||
113 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
dynamics.get_position(jerk_duration), 17.0); |
114 |
|||
115 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
1 |
EXPECT_TRUE(approx_equal(dynamics.get_acceleration(jerk_duration), -17)); |
116 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
1 |
EXPECT_TRUE(approx_equal(-17, constrained_dynamics.get_acceleration( |
117 |
jerk_duration + 100))); |
||
118 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗ |
1 |
EXPECT_TRUE(approx_equal(dynamics.get_position(jerk_duration), |
119 |
constrained_dynamics_b.get_position(0))); |
||
120 |
|||
121 |
|||
122 |
// make sure that ther is no discontinuity at jerk_duration ---------------- |
||
123 |
1 |
double large_epsilon = std::sqrt(epsilon) / 100.0; |
|
124 |
double numeric, exact; |
||
125 |
|||
126 |
✓✗ | 2 |
numeric = constrained_dynamics.get_position(jerk_duration) |
127 |
✓✗ | 1 |
+ large_epsilon * constrained_dynamics.get_velocity(jerk_duration); |
128 |
✓✗ | 1 |
exact = constrained_dynamics.get_position(jerk_duration + large_epsilon); |
129 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗ |
1 |
ASSERT_TRUE(approx_equal(numeric, exact)); |
130 |
|||
131 |
✓✗ | 2 |
numeric = constrained_dynamics.get_position(jerk_duration) |
132 |
✓✗ | 1 |
- large_epsilon * constrained_dynamics.get_velocity(jerk_duration); |
133 |
✓✗ | 1 |
exact = constrained_dynamics.get_position(jerk_duration - large_epsilon); |
134 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗ |
1 |
ASSERT_TRUE(approx_equal(numeric, exact)); |
135 |
|||
136 |
// make sure dynamics coincide |
||
137 |
✓✓ | 10001 |
for(size_t t = 0; t < 10000; t++) |
138 |
{ |
||
139 |
✓✗ | 10000 |
double acceleration_b = dynamics.get_acceleration(jerk_duration); |
140 |
double velocity_b = |
||
141 |
✓✗ | 20000 |
dynamics.get_velocity(jerk_duration) + |
142 |
✓✗ | 10000 |
dynamics.get_acceleration(jerk_duration) * t; |
143 |
double position_b = |
||
144 |
✓✗ | 30000 |
dynamics.get_position(jerk_duration) + |
145 |
✓✗ | 20000 |
dynamics.get_velocity(jerk_duration) * t + |
146 |
✓✗ | 10000 |
dynamics.get_acceleration(jerk_duration) * 0.5 * pow(t, 2); |
147 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
10000 |
EXPECT_TRUE(approx_equal(acceleration_b, |
148 |
constrained_dynamics_b.get_acceleration(t))); |
||
149 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
10000 |
EXPECT_TRUE(approx_equal(velocity_b, |
150 |
constrained_dynamics_b.get_velocity(t))); |
||
151 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
10000 |
EXPECT_TRUE(approx_equal(position_b, |
152 |
constrained_dynamics_b.get_position(t))); |
||
153 |
|||
154 |
|||
155 |
|||
156 |
✓✗✓✗ ✓✗✓✗ ✗✓✗✗ ✗✗✗✗ |
10000 |
EXPECT_TRUE(contains(constrained_dynamics.find_t_given_velocity( |
157 |
constrained_dynamics.get_velocity(t)), t)); |
||
158 |
|||
159 |
|||
160 |
✓✓ | 10000 |
if(t <= jerk_duration) |
161 |
{ |
||
162 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗ |
17 |
EXPECT_TRUE(approx_equal(dynamics.get_acceleration(t), |
163 |
constrained_dynamics.get_acceleration(t))); |
||
164 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗ |
17 |
EXPECT_TRUE(approx_equal(dynamics.get_velocity(t), |
165 |
constrained_dynamics.get_velocity(t))); |
||
166 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗ |
17 |
EXPECT_TRUE(approx_equal(dynamics.get_position(t), |
167 |
constrained_dynamics.get_position(t))); |
||
168 |
} |
||
169 |
else |
||
170 |
{ |
||
171 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗ |
9983 |
EXPECT_TRUE(approx_equal(constrained_dynamics_b.get_acceleration(t - jerk_duration), |
172 |
constrained_dynamics.get_acceleration(t))); |
||
173 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗ |
9983 |
EXPECT_TRUE(approx_equal(constrained_dynamics_b.get_velocity(t - jerk_duration), |
174 |
constrained_dynamics.get_velocity(t))); |
||
175 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗ |
9983 |
EXPECT_TRUE(approx_equal(constrained_dynamics_b.get_position(t - jerk_duration), |
176 |
constrained_dynamics.get_position(t))); |
||
177 |
} |
||
178 |
} |
||
179 |
} |
||
180 |
|||
181 |
✗✓ | 5 |
TEST(linear_dynamics_with_acceleration_constraint, consistency_with_linear_dynamics) |
182 |
{ |
||
183 |
|||
184 |
LinearDynamicsWithAccelerationConstraint |
||
185 |
✓✗✓✗ |
1 |
constrained_dynamics(-1.5, -13.0, 5.5, 10.2, 13.0); |
186 |
1 |
LinearDynamics dynamics(0.0, -13.0, 5.5, 10.2); |
|
187 |
|||
188 |
// make sure dynamics coincide |
||
189 |
✓✓ | 101 |
for(size_t t = 0; t < 100; t++) |
190 |
{ |
||
191 |
|||
192 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗ |
100 |
EXPECT_TRUE(approx_equal(dynamics.get_acceleration(t), |
193 |
constrained_dynamics.get_acceleration(t))); |
||
194 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗ |
100 |
EXPECT_TRUE(approx_equal(dynamics.get_velocity(t), |
195 |
constrained_dynamics.get_velocity(t))); |
||
196 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗ |
100 |
EXPECT_TRUE(approx_equal(dynamics.get_position(t), |
197 |
constrained_dynamics.get_position(t))); |
||
198 |
|||
199 |
LinearDynamics::Vector solutions = |
||
200 |
✓✗ | 100 |
dynamics.find_t_given_velocity(t - 50.0); |
201 |
LinearDynamics::Vector constrained_solutions = |
||
202 |
✓✗ | 100 |
constrained_dynamics.find_t_given_velocity(t - 50.0); |
203 |
|||
204 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(solutions.size() == constrained_solutions.size()); |
205 |
|||
206 |
✓✗✓✓ |
156 |
for(size_t i = 0; i < solutions.size(); i++) |
207 |
{ |
||
208 |
✓✗✓✗ ✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
56 |
EXPECT_TRUE(contains(solutions, constrained_solutions[i])); |
209 |
✓✗✓✗ ✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
56 |
EXPECT_TRUE(contains(constrained_solutions, solutions[i])); |
210 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
56 |
EXPECT_TRUE(approx_equal(solutions[i], - (t - 50.0 -5.5) / 13.0)); |
211 |
} |
||
212 |
} |
||
213 |
1 |
} |
|
214 |
|||
215 |
✗✓ | 5 |
TEST(linear_dynamics_with_acceleration_constraint, |
216 |
test_find_t_given_velocity_basic_1) |
||
217 |
{ |
||
218 |
LinearDynamicsWithAccelerationConstraint |
||
219 |
✓✗✓✗ |
1 |
dynamics_basic(1.0, -2.0, 0.0, 0.0, 100.0); |
220 |
|||
221 |
LinearDynamics::Vector solutions = |
||
222 |
✓✗ | 1 |
dynamics_basic.find_t_given_velocity(- 3.0 / 2.0); |
223 |
|||
224 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
1 |
EXPECT_TRUE(solutions.size() == 2); |
225 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
1 |
EXPECT_TRUE(contains(solutions, 1)); |
226 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
1 |
EXPECT_TRUE(contains(solutions, 3)); |
227 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
1 |
EXPECT_FALSE(contains(solutions, 4)); |
228 |
1 |
} |
|
229 |
|||
230 |
|||
231 |
✗✓ | 5 |
TEST(linear_dynamics_with_acceleration_constraint, |
232 |
test_find_t_given_velocity_basic_2) |
||
233 |
{ |
||
234 |
LinearDynamicsWithAccelerationConstraint |
||
235 |
✓✗✓✗ |
1 |
dynamics_basic(1.0, -5.0, 3.0, -10.0, 10.0); |
236 |
|||
237 |
LinearDynamics::Vector solutions = |
||
238 |
✓✗ | 1 |
dynamics_basic.find_t_given_velocity(-9); |
239 |
|||
240 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
1 |
EXPECT_TRUE(solutions.size() == 2); |
241 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
1 |
EXPECT_TRUE(contains(solutions, 4)); |
242 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
1 |
EXPECT_TRUE(contains(solutions, 6)); |
243 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
1 |
EXPECT_FALSE(contains(solutions, 7)); |
244 |
1 |
} |
|
245 |
|||
246 |
|||
247 |
✗✓ | 5 |
TEST(linear_dynamics_with_acceleration_constraint, |
248 |
test_find_t_given_velocity_basic_3) |
||
249 |
{ |
||
250 |
LinearDynamicsWithAccelerationConstraint |
||
251 |
✓✗✓✗ |
1 |
dynamics(1.0, -1.0, 3.0, -2.0, 2.0); |
252 |
|||
253 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
1 |
EXPECT_TRUE(approx_equal(dynamics.get_velocity(10.0), 18.5)); |
254 |
|||
255 |
✓✗ | 1 |
LinearDynamics::Vector solutions = dynamics.find_t_given_velocity(18.5); |
256 |
|||
257 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
1 |
EXPECT_TRUE(solutions.size() == 1); |
258 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
1 |
EXPECT_TRUE(approx_equal(solutions[0], 10.0)); |
259 |
1 |
} |
|
260 |
|||
261 |
|||
262 |
✗✓ | 5 |
TEST(linear_dynamics_with_acceleration_constraint, |
263 |
test_find_t_given_velocity_consistency) |
||
264 |
{ |
||
265 |
LinearDynamicsWithAccelerationConstraint |
||
266 |
✓✗✓✗ |
1 |
dynamics_a(3.4, -5.6, 2.3, 7.2, 20); |
267 |
LinearDynamicsWithAccelerationConstraint |
||
268 |
✓✗✓✗ |
1 |
dynamics_b(-2.1, -3.0, 5.5, -10.2, 44.3); |
269 |
|||
270 |
✓✓ | 101 |
for(size_t t = 0; t < 100; t++) |
271 |
{ |
||
272 |
auto solutions_a = dynamics_a.find_t_given_velocity( |
||
273 |
✓✗✓✗ |
100 |
dynamics_a.get_velocity(t)); |
274 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(contains(solutions_a, t)); |
275 |
auto solutions_b = dynamics_b.find_t_given_velocity( |
||
276 |
✓✗✓✗ |
100 |
dynamics_b.get_velocity(t)); |
277 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
100 |
EXPECT_TRUE(contains(solutions_b, t)); |
278 |
} |
||
279 |
1 |
} |
|
280 |
|||
281 |
|||
282 |
✗✓ | 5 |
TEST(linear_dynamics_with_acceleration_constraint, |
283 |
will_exceed_jointly) |
||
284 |
{ |
||
285 |
LinearDynamicsWithAccelerationConstraint |
||
286 |
✓✗✓✗ |
1 |
dynamics(-0.4, 2.6, 200.3, 7.2, 3); |
287 |
✓✗✓✗ |
1 |
double max_t = dynamics.find_t_given_velocity(0).maxCoeff() * 2; |
288 |
|||
289 |
2 |
std::vector<Eigen::Vector2d> trajectory; |
|
290 |
1 |
size_t n_iterations = 1000; |
|
291 |
✓✓ | 1001 |
for(size_t i = 0; i < n_iterations; i++) |
292 |
{ |
||
293 |
1000 |
double t = double(i) / n_iterations * max_t; |
|
294 |
|||
295 |
✓✗ | 1000 |
Eigen::Vector2d point; |
296 |
✓✗✓✗ |
1000 |
point[0] = dynamics.get_velocity(t); |
297 |
✓✗✓✗ |
1000 |
point[1] = dynamics.get_position(t); |
298 |
✓✗ | 1000 |
trajectory.push_back(point); |
299 |
} |
||
300 |
|||
301 |
✓✓ | 1001 |
for(auto& point : trajectory) |
302 |
{ |
||
303 |
2000 |
std::vector<Eigen::Vector2d> constraints; |
|
304 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(0.001, 0.001)); |
305 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point - Eigen::Vector2d(epsilon, epsilon)); |
306 |
|||
307 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(-epsilon, std::numeric_limits<double>::infinity())); |
308 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(-epsilon, -std::numeric_limits<double>::infinity())); |
309 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(0.001, -std::numeric_limits<double>::infinity())); |
310 |
|||
311 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(std::numeric_limits<double>::infinity(), -epsilon)); |
312 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(-std::numeric_limits<double>::infinity(), -epsilon)); |
313 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(-std::numeric_limits<double>::infinity(), 0.001)); |
314 |
|||
315 |
✓✓ | 9000 |
for(auto& constraint : constraints) |
316 |
{ |
||
317 |
|||
318 |
8000 |
bool label = false; |
|
319 |
✓✓ | 3349475 |
for(auto& point : trajectory) |
320 |
{ |
||
321 |
✓✗✓✗ ✓✓✓✗ ✓✗✓✓ ✓✓ |
3347013 |
if(point[0] > constraint[0] && point[1] > constraint[1]) |
322 |
{ |
||
323 |
// std::cout << "point: " << point.transpose() << std::endl; |
||
324 |
5538 |
label = true; |
|
325 |
5538 |
break; |
|
326 |
} |
||
327 |
} |
||
328 |
|||
329 |
double certificate_time; |
||
330 |
bool assigned_label = |
||
331 |
✓✗✓✗ |
8000 |
dynamics.will_exceed_jointly(constraint[0], constraint[1], |
332 |
✓✗ | 8000 |
certificate_time); |
333 |
|||
334 |
✗✓ | 8000 |
if(label != assigned_label) |
335 |
{ |
||
336 |
std::cout << "---------------------" << std::endl; |
||
337 |
std::cout << "constraint: " << constraint.transpose() << std::endl; |
||
338 |
|||
339 |
std::cout << "label: " << label << " assigned_label: " << assigned_label << std::endl; |
||
340 |
std::cout << "certificate time: " << certificate_time |
||
341 |
<< " certificate point: " |
||
342 |
<< dynamics.get_velocity(certificate_time) << ", " |
||
343 |
<< dynamics.get_position(certificate_time) << std::endl; |
||
344 |
} |
||
345 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
8000 |
EXPECT_TRUE(label == assigned_label); |
346 |
} |
||
347 |
} |
||
348 |
1 |
} |
|
349 |
|||
350 |
|||
351 |
|||
352 |
✗✓ | 5 |
TEST(linear_dynamics_with_acceleration_constraint, |
353 |
will_exceed_jointly_2) |
||
354 |
{ |
||
355 |
LinearDynamicsWithAccelerationConstraint |
||
356 |
✓✗✓✗ |
1 |
dynamics(-0.4, 20.6, -2.3, 7.2, 30); |
357 |
✓✗✓✗ |
1 |
double max_t = dynamics.find_t_given_velocity(0).maxCoeff() * 2; |
358 |
|||
359 |
2 |
std::vector<Eigen::Vector2d> trajectory; |
|
360 |
1 |
size_t n_iterations = 1000; |
|
361 |
✓✓ | 1001 |
for(size_t i = 0; i < n_iterations; i++) |
362 |
{ |
||
363 |
1000 |
double t = double(i) / n_iterations * max_t; |
|
364 |
|||
365 |
✓✗ | 1000 |
Eigen::Vector2d point; |
366 |
✓✗✓✗ |
1000 |
point[0] = dynamics.get_velocity(t); |
367 |
✓✗✓✗ |
1000 |
point[1] = dynamics.get_position(t); |
368 |
✓✗ | 1000 |
trajectory.push_back(point); |
369 |
} |
||
370 |
|||
371 |
✓✓ | 1001 |
for(auto& point : trajectory) |
372 |
{ |
||
373 |
2000 |
std::vector<Eigen::Vector2d> constraints; |
|
374 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(0.001, 0.001)); |
375 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point - Eigen::Vector2d(epsilon, epsilon)); |
376 |
|||
377 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(-epsilon, std::numeric_limits<double>::infinity())); |
378 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(-epsilon, -std::numeric_limits<double>::infinity())); |
379 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(0.001, -std::numeric_limits<double>::infinity())); |
380 |
|||
381 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(std::numeric_limits<double>::infinity(), -epsilon)); |
382 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(-std::numeric_limits<double>::infinity(), -epsilon)); |
383 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(-std::numeric_limits<double>::infinity(), 0.001)); |
384 |
|||
385 |
✓✓ | 9000 |
for(auto& constraint : constraints) |
386 |
{ |
||
387 |
|||
388 |
8000 |
bool label = false; |
|
389 |
✓✓ | 3124708 |
for(auto& point : trajectory) |
390 |
{ |
||
391 |
✓✗✓✗ ✓✓✓✗ ✓✗✓✓ ✓✓ |
3122455 |
if(point[0] > constraint[0] && point[1] > constraint[1]) |
392 |
{ |
||
393 |
// std::cout << "point: " << point.transpose() << std::endl; |
||
394 |
5747 |
label = true; |
|
395 |
5747 |
break; |
|
396 |
} |
||
397 |
} |
||
398 |
|||
399 |
double certificate_time; |
||
400 |
bool assigned_label = |
||
401 |
✓✗✓✗ |
8000 |
dynamics.will_exceed_jointly(constraint[0], constraint[1], |
402 |
✓✗ | 8000 |
certificate_time); |
403 |
|||
404 |
✗✓ | 8000 |
if(label != assigned_label) |
405 |
{ |
||
406 |
std::cout << "---------------------" << std::endl; |
||
407 |
std::cout << "constraint: " << constraint.transpose() << std::endl; |
||
408 |
|||
409 |
std::cout << "label: " << label << " assigned_label: " << assigned_label << std::endl; |
||
410 |
std::cout << "certificate time: " << certificate_time |
||
411 |
<< " certificate point: " |
||
412 |
<< dynamics.get_velocity(certificate_time) << ", " |
||
413 |
<< dynamics.get_position(certificate_time) << std::endl; |
||
414 |
} |
||
415 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
8000 |
EXPECT_TRUE(label == assigned_label); |
416 |
} |
||
417 |
} |
||
418 |
1 |
} |
|
419 |
|||
420 |
|||
421 |
✗✓ | 5 |
TEST(linear_dynamics_with_acceleration_constraint, |
422 |
will_deceed_jointly) |
||
423 |
{ |
||
424 |
LinearDynamicsWithAccelerationConstraint |
||
425 |
✓✗✓✗ |
1 |
dynamics(0.4, 2.6, -200.3, 7.2, 3); |
426 |
✓✗✓✗ |
1 |
double max_t = dynamics.find_t_given_velocity(0).maxCoeff() * 2; |
427 |
|||
428 |
2 |
std::vector<Eigen::Vector2d> trajectory; |
|
429 |
1 |
size_t n_iterations = 1000; |
|
430 |
✓✓ | 1001 |
for(size_t i = 0; i < n_iterations; i++) |
431 |
{ |
||
432 |
1000 |
double t = double(i) / n_iterations * max_t; |
|
433 |
|||
434 |
✓✗ | 1000 |
Eigen::Vector2d point; |
435 |
✓✗✓✗ |
1000 |
point[0] = dynamics.get_velocity(t); |
436 |
✓✗✓✗ |
1000 |
point[1] = dynamics.get_position(t); |
437 |
✓✗ | 1000 |
trajectory.push_back(point); |
438 |
} |
||
439 |
|||
440 |
✓✓ | 1001 |
for(auto& point : trajectory) |
441 |
{ |
||
442 |
2000 |
std::vector<Eigen::Vector2d> constraints; |
|
443 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point - Eigen::Vector2d(0.001, 0.001)); |
444 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(epsilon, epsilon)); |
445 |
|||
446 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(epsilon, std::numeric_limits<double>::infinity())); |
447 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(epsilon, -std::numeric_limits<double>::infinity())); |
448 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(-0.001, std::numeric_limits<double>::infinity())); |
449 |
|||
450 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(std::numeric_limits<double>::infinity(), epsilon)); |
451 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(-std::numeric_limits<double>::infinity(), epsilon)); |
452 |
✓✗✓✗ ✓✗✓✗ |
1000 |
constraints.push_back(point + Eigen::Vector2d(std::numeric_limits<double>::infinity(), -0.001)); |
453 |
|||
454 |
✓✓ | 9000 |
for(auto& constraint : constraints) |
455 |
{ |
||
456 |
|||
457 |
8000 |
bool label = false; |
|
458 |
✓✓ | 3386738 |
for(auto& point : trajectory) |
459 |
{ |
||
460 |
✓✗✓✗ ✓✓✓✗ ✓✗✓✓ ✓✓ |
3384235 |
if(point[0] < constraint[0] && point[1] < constraint[1]) |
461 |
{ |
||
462 |
// std::cout << "point: " << point.transpose() << std::endl; |
||
463 |
5497 |
label = true; |
|
464 |
5497 |
break; |
|
465 |
} |
||
466 |
} |
||
467 |
|||
468 |
double certificate_time; |
||
469 |
bool assigned_label = |
||
470 |
✓✗✓✗ |
8000 |
dynamics.will_deceed_jointly(constraint[0], constraint[1], |
471 |
✓✗ | 8000 |
certificate_time); |
472 |
|||
473 |
✗✓ | 8000 |
if(label != assigned_label) |
474 |
{ |
||
475 |
std::cout << "---------------------" << std::endl; |
||
476 |
std::cout << "constraint: " << constraint.transpose() << std::endl; |
||
477 |
|||
478 |
std::cout << "label: " << label << " assigned_label: " << assigned_label << std::endl; |
||
479 |
std::cout << "certificate time: " << certificate_time |
||
480 |
<< " certificate point: " |
||
481 |
<< dynamics.get_velocity(certificate_time) << ", " |
||
482 |
<< dynamics.get_position(certificate_time) << std::endl; |
||
483 |
} |
||
484 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
8000 |
EXPECT_TRUE(label == assigned_label); |
485 |
} |
||
486 |
} |
||
487 |
1 |
} |
|
488 |
|||
489 |
60560 |
double sample_uniformely(const double& min, const double& max) |
|
490 |
{ |
||
491 |
60560 |
return min + double(rand()) / RAND_MAX * (max - min); |
|
492 |
} |
||
493 |
|||
494 |
✗✓ | 5 |
TEST(find_max_admissible_acceleration, generated_trajectories) |
495 |
{ |
||
496 |
1 |
srand(0); |
|
497 |
|||
498 |
✓✓ | 101 |
for(size_t unused = 0; unused < 100; unused++) |
499 |
{ |
||
500 |
// initialize parameters randomly -------------------------------------- |
||
501 |
|||
502 |
|||
503 |
100 |
double initial_velocity = sample_uniformely(-20.0, 20.0); |
|
504 |
100 |
double initial_position = sample_uniformely(-20.0, 20.0); |
|
505 |
✓✗ | 100 |
NonnegDouble abs_jerk_limit = sample_uniformely(epsilon, 20.0); |
506 |
✓✗ | 100 |
NonnegDouble abs_acceleration_limit = sample_uniformely(epsilon, 20.0); |
507 |
|||
508 |
200 |
double initial_acceleration = sample_uniformely(-abs_acceleration_limit, |
|
509 |
200 |
abs_acceleration_limit); |
|
510 |
|||
511 |
|||
512 |
|||
513 |
// find some constraints which is just barely satisfied -------------------- |
||
514 |
100 |
LinearDynamicsWithAccelerationConstraint dynamics(-abs_jerk_limit, |
|
515 |
initial_acceleration, |
||
516 |
initial_velocity, |
||
517 |
initial_position, |
||
518 |
✓✗ | 200 |
abs_acceleration_limit); |
519 |
|||
520 |
100 |
double max_t = 20.0; |
|
521 |
✓✗✓✗ ✓✓ |
100 |
if(dynamics.find_t_given_velocity(0).size() > 0) |
522 |
{ |
||
523 |
✓✗✓✗ |
51 |
max_t = dynamics.find_t_given_velocity(0).maxCoeff() * 2; |
524 |
} |
||
525 |
|||
526 |
100 |
size_t n_iterations = 10000; |
|
527 |
100 |
int T = rand() % n_iterations; |
|
528 |
✓✗ | 100 |
Eigen::Vector2d constraint; |
529 |
✓✗✓✗ |
100 |
constraint[0] = dynamics.get_velocity(T); |
530 |
✓✗✓✗ |
100 |
constraint[1] = dynamics.get_position(T); |
531 |
|||
532 |
100 |
Eigen::Vector2d position_constraint(-std::numeric_limits<double>::infinity(), |
|
533 |
✓✗ | 200 |
-std::numeric_limits<double>::infinity()); |
534 |
100 |
Eigen::Vector2d velocity_constraint(-std::numeric_limits<double>::infinity(), |
|
535 |
✓✗ | 200 |
-std::numeric_limits<double>::infinity()); |
536 |
|||
537 |
✓✓ | 1000100 |
for(size_t i = 0; i < n_iterations; i++) |
538 |
{ |
||
539 |
1000000 |
double t = double(i) / n_iterations * max_t; |
|
540 |
✓✗ | 1000000 |
Eigen::Vector2d point; |
541 |
✓✗✓✗ |
1000000 |
point[0] = dynamics.get_velocity(t); |
542 |
✓✗✓✗ |
1000000 |
point[1] = dynamics.get_position(t); |
543 |
✓✗✓✗ ✓✓✓✗ ✓✗✓✓ ✓✓ |
1000000 |
if(point[0] > constraint[0] && point[1] > constraint[1]) |
544 |
✓✗ | 36141 |
constraint = point; |
545 |
✓✗✓✗ ✓✗ |
1000000 |
velocity_constraint[0] = std::max(velocity_constraint[0], point[0]); |
546 |
✓✗✓✗ ✓✗ |
1000000 |
position_constraint[1] = std::max(position_constraint[1], point[1]); |
547 |
} |
||
548 |
|||
549 |
// test that we get the same initial acceleration -------------------------- |
||
550 |
✓✗✓✗ ✓✗✓✓ |
400 |
for(auto& c : {constraint, position_constraint, velocity_constraint}) |
551 |
{ |
||
552 |
|||
553 |
|||
554 |
double max_admissible_initial_acceleration = |
||
555 |
find_max_admissible_acceleration( |
||
556 |
initial_velocity, |
||
557 |
initial_position, |
||
558 |
✓✗ | 300 |
c[0], |
559 |
✓✗ | 300 |
c[1], |
560 |
abs_jerk_limit, |
||
561 |
✓✗ | 300 |
abs_acceleration_limit); |
562 |
|||
563 |
✓✓ | 600 |
if(std::fabs(initial_acceleration - |
564 |
300 |
max_admissible_initial_acceleration) > 0.02) |
|
565 |
{ |
||
566 |
✓✓ | 338 |
if(max_admissible_initial_acceleration - 0.0001 > |
567 |
169 |
-abs_acceleration_limit) |
|
568 |
{ |
||
569 |
LinearDynamicsWithAccelerationConstraint |
||
570 |
119 |
below_limit_dynamics(-abs_jerk_limit, |
|
571 |
max_admissible_initial_acceleration |
||
572 |
- 0.0001, |
||
573 |
initial_velocity, |
||
574 |
initial_position, |
||
575 |
✓✗ | 238 |
abs_acceleration_limit); |
576 |
✓✗✓✗ ✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗ |
119 |
ASSERT_TRUE(below_limit_dynamics.will_exceed_jointly(c[0], c[1]) |
577 |
== false); |
||
578 |
} |
||
579 |
|||
580 |
|||
581 |
✓✓ | 338 |
if(max_admissible_initial_acceleration + 0.0001 < |
582 |
169 |
abs_acceleration_limit) |
|
583 |
{ |
||
584 |
LinearDynamicsWithAccelerationConstraint |
||
585 |
111 |
above_limit_dynamics(-abs_jerk_limit, |
|
586 |
max_admissible_initial_acceleration |
||
587 |
+ 0.0001, |
||
588 |
initial_velocity, |
||
589 |
initial_position, |
||
590 |
✓✗ | 222 |
abs_acceleration_limit); |
591 |
✓✗✓✗ ✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗ |
111 |
ASSERT_TRUE(above_limit_dynamics.will_exceed_jointly(c[0], c[1]) |
592 |
== true); |
||
593 |
} |
||
594 |
|||
595 |
|||
596 |
// std::cout << "------------------------------------ " << std::endl; |
||
597 |
|||
598 |
|||
599 |
// dynamics.print_parameters(); |
||
600 |
|||
601 |
// std::cout << "constraint " << c << std::endl; |
||
602 |
|||
603 |
// std::cout << " initial_acceleration " << initial_acceleration |
||
604 |
// << " max_admissible_initial_acceleration " |
||
605 |
// << max_admissible_initial_acceleration << std::endl; |
||
606 |
|||
607 |
// bool will_exceed_epsilon = dynamics.will_exceed_jointly(c[0], c[1]); |
||
608 |
|||
609 |
// bool will_exceed_minus_epsilon = dynamics.will_exceed_jointly(c[0] - epsilon, c[1] - epsilon); |
||
610 |
|||
611 |
|||
612 |
// bool limit_dynamics_will_exceed = limit_dynamics.will_exceed_jointly(c[0] - epsilon, c[1] - epsilon); |
||
613 |
|||
614 |
|||
615 |
|||
616 |
// std::cout << " will_exceed_epsilon " << will_exceed_epsilon |
||
617 |
// << " will_exceed_minus_epsilon " << will_exceed_minus_epsilon |
||
618 |
// << " limit_dynamics_will_exceed " << limit_dynamics_will_exceed |
||
619 |
|||
620 |
// << std::endl; |
||
621 |
|||
622 |
✓✗✓✓ ✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗ |
169 |
ASSERT_TRUE(approx_equal(c[0], initial_velocity) || |
623 |
approx_equal(c[1], initial_position)); |
||
624 |
} |
||
625 |
|||
626 |
// ASSERT_TRUE(std::fabs(initial_acceleration - |
||
627 |
// max_admissible_initial_acceleration) <= 0.001); |
||
628 |
} |
||
629 |
} |
||
630 |
} |
||
631 |
|||
632 |
|||
633 |
✗✓ | 5 |
TEST(find_max_admissible_acceleration, random_points) |
634 |
{ |
||
635 |
1 |
srand(0); |
|
636 |
|||
637 |
✓✓ | 10001 |
for(size_t unused = 0; unused < 10000; unused++) |
638 |
{ |
||
639 |
// initialize parameters randomly -------------------------------------- |
||
640 |
10000 |
double initial_velocity = sample_uniformely(-20.0, 20.0); |
|
641 |
10000 |
double initial_position = sample_uniformely(-20.0, 20.0); |
|
642 |
|||
643 |
10000 |
double max_velocity = sample_uniformely(-20.0, 20.0); |
|
644 |
10000 |
double max_position = sample_uniformely(-20.0, 20.0); |
|
645 |
|||
646 |
✓✗ | 10000 |
NonnegDouble abs_jerk_limit = sample_uniformely(epsilon, 20.0); |
647 |
✓✗ | 10000 |
NonnegDouble abs_acceleration_limit = sample_uniformely(epsilon, 20.0); |
648 |
|||
649 |
|||
650 |
double max_admissible_acceleration = |
||
651 |
find_max_admissible_acceleration( |
||
652 |
initial_velocity, |
||
653 |
initial_position, |
||
654 |
max_velocity, |
||
655 |
max_position, |
||
656 |
abs_jerk_limit, |
||
657 |
✓✗ | 10000 |
abs_acceleration_limit); |
658 |
|||
659 |
✓✓ | 20000 |
if(std::fabs(max_admissible_acceleration - 0.0001) <= |
660 |
10000 |
abs_acceleration_limit) |
|
661 |
{ |
||
662 |
LinearDynamicsWithAccelerationConstraint |
||
663 |
6858 |
below_limit_dynamics(-abs_jerk_limit, |
|
664 |
max_admissible_acceleration |
||
665 |
- 0.0001, |
||
666 |
initial_velocity, |
||
667 |
initial_position, |
||
668 |
✓✗ | 13716 |
abs_acceleration_limit); |
669 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗ |
6858 |
ASSERT_TRUE(below_limit_dynamics.will_exceed_jointly(max_velocity, max_position) |
670 |
== false); |
||
671 |
} |
||
672 |
✓✓ | 20000 |
if(std::fabs(max_admissible_acceleration + 0.0001) <= |
673 |
10000 |
abs_acceleration_limit) |
|
674 |
{ |
||
675 |
LinearDynamicsWithAccelerationConstraint |
||
676 |
5731 |
above_limit_dynamics(-abs_jerk_limit, |
|
677 |
max_admissible_acceleration |
||
678 |
+ 0.0001, |
||
679 |
initial_velocity, |
||
680 |
initial_position, |
||
681 |
✓✗ | 11462 |
abs_acceleration_limit); |
682 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗ |
5731 |
ASSERT_TRUE(above_limit_dynamics.will_exceed_jointly(max_velocity, max_position) |
683 |
== true); |
||
684 |
} |
||
685 |
} |
||
686 |
} |
||
687 |
|||
688 |
|||
689 |
✗✓ | 5 |
TEST(find_min_admissible_acceleration, random_points) |
690 |
{ |
||
691 |
1 |
srand(0); |
|
692 |
|||
693 |
✓✓ | 11 |
for(size_t unused = 0; unused < 10; unused++) |
694 |
{ |
||
695 |
// initialize parameters randomly -------------------------------------- |
||
696 |
10 |
double initial_velocity = sample_uniformely(-20.0, 20.0); |
|
697 |
10 |
double initial_position = sample_uniformely(-20.0, 20.0); |
|
698 |
|||
699 |
10 |
double min_velocity = sample_uniformely(-20.0, 20.0); |
|
700 |
10 |
double min_position = sample_uniformely(-20.0, 20.0); |
|
701 |
|||
702 |
✓✗ | 10 |
NonnegDouble abs_jerk_limit = sample_uniformely(epsilon, 20.0); |
703 |
✓✗ | 10 |
NonnegDouble abs_acceleration_limit = sample_uniformely(epsilon, 20.0); |
704 |
|||
705 |
|||
706 |
double min_admissible_acceleration = |
||
707 |
find_min_admissible_acceleration( |
||
708 |
initial_velocity, |
||
709 |
initial_position, |
||
710 |
min_velocity, |
||
711 |
min_position, |
||
712 |
abs_jerk_limit, |
||
713 |
✓✗ | 10 |
abs_acceleration_limit); |
714 |
|||
715 |
✓✓ | 20 |
if(std::fabs(min_admissible_acceleration - 0.0001) <= |
716 |
10 |
abs_acceleration_limit) |
|
717 |
{ |
||
718 |
LinearDynamicsWithAccelerationConstraint |
||
719 |
below_limit_dynamics(abs_jerk_limit, |
||
720 |
min_admissible_acceleration |
||
721 |
- 0.0001, |
||
722 |
initial_velocity, |
||
723 |
initial_position, |
||
724 |
✓✗ | 6 |
abs_acceleration_limit); |
725 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗ |
6 |
ASSERT_TRUE(below_limit_dynamics.will_deceed_jointly(min_velocity, |
726 |
min_position) |
||
727 |
== true); |
||
728 |
} |
||
729 |
✓✓ | 20 |
if(std::fabs(min_admissible_acceleration + 0.0001) <= |
730 |
10 |
abs_acceleration_limit) |
|
731 |
{ |
||
732 |
LinearDynamicsWithAccelerationConstraint |
||
733 |
above_limit_dynamics(abs_jerk_limit, |
||
734 |
min_admissible_acceleration |
||
735 |
+ 0.0001, |
||
736 |
initial_velocity, |
||
737 |
initial_position, |
||
738 |
✓✗ | 9 |
abs_acceleration_limit); |
739 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗ |
9 |
ASSERT_TRUE(above_limit_dynamics.will_deceed_jointly(min_velocity, |
740 |
min_position) |
||
741 |
== false); |
||
742 |
} |
||
743 |
} |
||
744 |
✓✗✓✗ |
3 |
} |
745 |
|||
746 |
|||
747 |
|||
748 |
| Generated by: GCOVR (Version 4.2) |