Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
constant_velocity_model_test.cpp
Go to the documentation of this file.
1#include "motion_lib/v2p_models/constant_velocity_model.hpp"
2
3#include <gtest/gtest.h>
4
5#include <cmath>
6
10TEST(CONSTANT_VELOCITY_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_1) {
11 // Arrange
12 const Eigen::Vector3d previous_pose(0, 0, 0);
13 const Eigen::Vector3d velocities(1, 0, 0);
14 const double delta_t = 1;
15 ConstantVelocityModel model;
16
17 // Act
18 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
19 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
20
21 // Assert
22 EXPECT_NEAR(next_pose(0), 1, 0.000001);
23 EXPECT_NEAR(next_pose(1), 0, 0.000001);
24 EXPECT_NEAR(next_pose(2), 0, 0.000001);
25 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
26 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
27 EXPECT_FLOAT_EQ(pose_jacobian(0, 2), 0);
28 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
29 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
30 EXPECT_FLOAT_EQ(pose_jacobian(1, 2), 1);
31 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
32 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
33 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
34}
35
39TEST(CONSTANT_VELOCITY_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_2) {
40 // Arrange
41 const Eigen::Vector3d previous_pose(0, 0, M_PI / 4);
42 const Eigen::Vector3d velocities(2, 0, 0);
43 const double delta_t = 1;
44 ConstantVelocityModel model;
45
46 // Act
47 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
48 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
49
50 // Assert
51 EXPECT_NEAR(next_pose(0), 1.41, 0.01);
52 EXPECT_NEAR(next_pose(1), 1.41, 0.01);
53 EXPECT_NEAR(next_pose(2), M_PI / 4, 0.000001);
54 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
55 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
56 EXPECT_NEAR(pose_jacobian(0, 2), -1.41, 0.01);
57 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
58 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
59 EXPECT_NEAR(pose_jacobian(1, 2), 1.41, 0.01);
60 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
61 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
62 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
63}
64
68TEST(CONSTANT_VELOCITY_MODEL, BACKWARDS_MOVEMENT_TEST_1) {
69 // Arrange
70 const Eigen::Vector3d previous_pose(0, 0, 0.0);
71 const Eigen::Vector3d velocities(-1, 0.0, 0);
72 const double delta_t = 1;
73 ConstantVelocityModel model;
74
75 // Act
76 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
77 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
78
79 // Assert
80 EXPECT_NEAR(next_pose(0), -1.0, 0.000001);
81 EXPECT_NEAR(next_pose(1), 0, 0.000001);
82 EXPECT_NEAR(abs(next_pose(2)), 0.0, 0.000001);
83 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
84 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
85 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
86 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
87 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
88 EXPECT_NEAR(pose_jacobian(1, 2), -1, 0.01);
89 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
90 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
91 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
92}
93
98TEST(CONSTANT_VELOCITY_MODEL, BACKWARDS_MOVEMENT_TEST_2) {
99 // Arrange
100 const Eigen::Vector3d previous_pose(0, 0, M_PI);
101 const Eigen::Vector3d velocities(1, 0.0, 0);
102 const double delta_t = 1;
103 ConstantVelocityModel model;
104
105 // Act
106 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
107 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
108
109 // Assert
110 EXPECT_NEAR(next_pose(0), -1.00, 0.000001);
111 EXPECT_NEAR(next_pose(1), 0, 0.000001);
112 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
113 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
114 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
115 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
116 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
117 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
118 EXPECT_NEAR(pose_jacobian(1, 2), -1, 0.01);
119 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
120 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
121 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
122}
123
128TEST(CONSTANT_VELOCITY_MODEL, BACKWARDS_MOVEMENT_TEST_3) {
129 // Arrange
130 const Eigen::Vector3d previous_pose(0, 0, -M_PI);
131 const Eigen::Vector3d velocities(-1, 0.0, 0);
132 const double delta_t = 1;
133 ConstantVelocityModel model;
134
135 // Act
136 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
137 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
138
139 // Assert
140 EXPECT_NEAR(next_pose(0), 1.00, 0.000001);
141 EXPECT_NEAR(next_pose(1), 0, 0.000001);
142 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
143 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
144 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
145 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
146 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
147 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
148 EXPECT_NEAR(pose_jacobian(1, 2), 1, 0.01);
149 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
150 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
151 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
152}
153
157TEST(CONSTANT_VELOCITY_MODEL, ORIENTATION_ANGLE_CAP_TEST_1) {
158 // Arrange
159 const Eigen::Vector3d previous_pose(0, 0, 2 * M_PI);
160 const Eigen::Vector3d velocities(0.0, 0.0, 0);
161 const double delta_t = 1;
162 ConstantVelocityModel model;
163
164 // Act
165 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
166 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
167
168 // Assert
169 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
170 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
171 EXPECT_NEAR(next_pose(2), 0.0, 0.000001);
172 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
173 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
174 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
175 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
176 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
177 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
178 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
179 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
180 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
181}
182
186TEST(CONSTANT_VELOCITY_MODEL, ORIENTATION_ANGLE_CAP_TEST_2) {
187 // Arrange
188 const Eigen::Vector3d previous_pose(0, 0, 3 * M_PI);
189 const Eigen::Vector3d velocities(0.0, 0.0, 0);
190 const double delta_t = 1;
191 ConstantVelocityModel model;
192
193 // Act
194 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
195 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
196
197 // Assert
198 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
199 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
200 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
201 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
202 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
203 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
204 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
205 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
206 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
207 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
208 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
209 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
210}
211
215TEST(CONSTANT_VELOCITY_MODEL, ORIENTATION_ANGLE_CAP_TEST_3) {
216 // Arrange
217 const Eigen::Vector3d previous_pose(0, 0, -3 * M_PI);
218 const Eigen::Vector3d velocities(0.0, 0.0, 0);
219 const double delta_t = 1;
220 ConstantVelocityModel model;
221
222 // Act
223 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
224 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
225
226 // Assert
227 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
228 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
229 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
230 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
231 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
232 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
233 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
234 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
235 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
236 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
237 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
238 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
239}
240
244TEST(CONSTANT_VELOCITY_MODEL, ORIENTATION_ANGLE_CAP_TEST_4) {
245 // Arrange
246 const Eigen::Vector3d previous_pose(0, 0, 1.5 * M_PI);
247 const Eigen::Vector3d velocities(0.0, 0.0, 0);
248 const double delta_t = 1;
249 ConstantVelocityModel model;
250
251 // Act
252 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
253 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
254
255 // Assert
256 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
257 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
258 EXPECT_NEAR(next_pose(2), -M_PI / 2, 0.000001);
259 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
260 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
261 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
262 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
263 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
264 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
265 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
266 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
267 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
268}
269
273TEST(CONSTANT_VELOCITY_MODEL, ORIENTATION_ANGLE_CAP_TEST_5) {
274 // Arrange
275 const Eigen::Vector3d previous_pose(0, 0, -2.5 * M_PI);
276 const Eigen::Vector3d velocities(0.0, 0.0, 0);
277 const double delta_t = 1;
278 ConstantVelocityModel model;
279
280 // Act
281 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
282 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
283
284 // Assert
285 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
286 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
287 EXPECT_NEAR(next_pose(2), -M_PI / 2, 0.000001);
288 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
289 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
290 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
291 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
292 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
293 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
294 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
295 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
296 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
297}
298
302TEST(CONSTANT_VELOCITY_MODEL, CURVILINEAR_MOVEMENT_TEST_1) {
303 // Arrange
304 const Eigen::Vector3d previous_pose(1, 2, M_PI / 4);
305 const Eigen::Vector3d velocities(3, 3, M_PI / 16);
306 const double delta_t = 1;
307 ConstantVelocityModel model;
308
309 // Act
310 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
311 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
312
313 // Assert
314 EXPECT_NEAR(next_pose(0), 1, 0.01);
315 EXPECT_NEAR(next_pose(1), 6.24, 0.01);
316 EXPECT_NEAR(next_pose(2), 0.98, 0.01);
317 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
318 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
319 EXPECT_NEAR(pose_jacobian(0, 2), -4.24, 0.01);
320 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
321 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
322 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
323 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
324 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
325 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
326}
327
331TEST(CONSTANT_VELOCITY_MODEL, CURVILINEAR_MOVEMENT_TEST_2) {
332 // Arrange
333 const Eigen::Vector3d previous_pose(1, 2, -M_PI / 4);
334 const Eigen::Vector3d velocities(3, -0.5, -M_PI / 8);
335 const double delta_t = 1;
336 ConstantVelocityModel model;
337
338 // Act
339 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
340 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
341
342 // Assert
343 EXPECT_NEAR(next_pose(0), 2.76, 0.01);
344 EXPECT_NEAR(next_pose(1), -0.48, 0.01);
345 EXPECT_NEAR(next_pose(2), -3 * M_PI / 8, 0.000001);
346 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
347 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
348 EXPECT_NEAR(pose_jacobian(0, 2), 2.48, 0.01);
349 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
350 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
351 EXPECT_NEAR(pose_jacobian(1, 2), 1.76, 0.01);
352 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
353 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
354 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
355}
356
361TEST(CONSTANT_VELOCITY_MODEL, CURVILINEAR_MOVEMENT_TEST_3) {
362 // Arrange
363 const Eigen::Vector3d previous_pose(1, 2, -M_PI / 2);
364 const Eigen::Vector3d velocities(3, 1.5, M_PI / 4);
365 const double delta_t = 2;
366 ConstantVelocityModel model;
367
368 // Act
369 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
370 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
371
372 // Assert
373 EXPECT_NEAR(next_pose(0), 4, 0.01);
374 EXPECT_NEAR(next_pose(1), -4, 0.01);
375 EXPECT_NEAR(next_pose(2), 0.0, 0.01);
376 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
377 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
378 EXPECT_NEAR(pose_jacobian(0, 2), 6, 0.01);
379 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
380 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
381 EXPECT_NEAR(pose_jacobian(1, 2), 3, 0.01);
382 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
383 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
384 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
385}
ConstantVelocityModel
Motion model that predicts the pose of the robot given the velocities with a constant velocity model.
Definition constant_velocity_model.hpp:11
V2PMotionModel::get_next_pose
virtual Eigen::Vector3d get_next_pose(const Eigen::Vector3d &previous_pose, const Eigen::VectorXd &motion_data, const double delta_t)
Predict the pose of the robot given the motion data.
Definition base_v2p_motion_model.cpp:5
constant_velocity_model.hpp
TEST
TEST(CONSTANT_VELOCITY_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_1)
Test the constant velocity model with a straight line movement in the x axis - positive vx.
Definition constant_velocity_model_test.cpp:10
model
std::shared_ptr< IVehicleModel > model
Definition pacsim_main.cpp:61