Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
constant_acceleration_turnrate_model_test.cpp
Go to the documentation of this file.
1#include "motion_lib/v2p_models/constant_acceleration_turnrate_model.hpp"
2
3#include <gtest/gtest.h>
4
5#include <cmath>
6
7// TODO: improve tests: have tests be based off of a continuous example, with data regarding a
8// vehicle performing a complex movement or something similar.
9
15TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_1) {
16 // Arrange
17 Eigen::Vector3d previous_pose(0, 0, 0);
18 Eigen::Vector4d motion_data(1, 0, 0, 0);
19 double delta_t = 1;
20 ConstantAccelerationTurnrateModel model;
21
22 // Act
23 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
24 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
25
26 // Assert
27 EXPECT_NEAR(next_pose(0), 1, 0.000001);
28 EXPECT_NEAR(next_pose(1), 0, 0.000001);
29 EXPECT_NEAR(next_pose(2), 0, 0.000001);
30 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
31 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
32 EXPECT_FLOAT_EQ(pose_jacobian(0, 2), 0);
33 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
34 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
35 EXPECT_FLOAT_EQ(pose_jacobian(1, 2), 1);
36 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
37 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
38 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
39}
40
46TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_2) {
47 // Arrange
48 Eigen::Vector3d previous_pose(0, 0, M_PI / 4);
49 Eigen::Vector4d motion_data(2, 0, 0, 0);
50 double delta_t = 1;
51 ConstantAccelerationTurnrateModel model;
52
53 // Act
54 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
55 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
56
57 // Assert
58 EXPECT_NEAR(next_pose(0), 1.41, 0.01);
59 EXPECT_NEAR(next_pose(1), 1.41, 0.01);
60 EXPECT_NEAR(next_pose(2), M_PI / 4, 0.000001);
61 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
62 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
63 EXPECT_NEAR(pose_jacobian(0, 2), -1.41, 0.01);
64 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
65 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
66 EXPECT_NEAR(pose_jacobian(1, 2), 1.41, 0.01);
67 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
68 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
69 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
70}
71
76TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, BACKWARDS_MOVEMENT_TEST_1) {
77 // Arrange
78 Eigen::Vector3d previous_pose(0, 0, 0.0);
79 Eigen::Vector4d motion_data(-1, 0.0, 0, 0);
80 double delta_t = 1;
81 ConstantAccelerationTurnrateModel model;
82
83 // Act
84 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
85 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
86
87 // Assert
88 EXPECT_NEAR(next_pose(0), -1.0, 0.000001);
89 EXPECT_NEAR(next_pose(1), 0, 0.000001);
90 EXPECT_NEAR(abs(next_pose(2)), 0.0, 0.000001);
91 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
92 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
93 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
94 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
95 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
96 EXPECT_NEAR(pose_jacobian(1, 2), -1, 0.01);
97 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
98 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
99 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
100}
101
106TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, BACKWARDS_MOVEMENT_TEST_2) {
107 // Arrange
108 Eigen::Vector3d previous_pose(0, 0, M_PI);
109 Eigen::Vector4d motion_data(1, 0.0, 0, 0);
110 double delta_t = 1;
111 ConstantAccelerationTurnrateModel model;
112
113 // Act
114 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
115 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
116
117 // Assert
118 EXPECT_NEAR(next_pose(0), -1.00, 0.000001);
119 EXPECT_NEAR(next_pose(1), 0, 0.000001);
120 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
121 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
122 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
123 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
124 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
125 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
126 EXPECT_NEAR(pose_jacobian(1, 2), -1, 0.01);
127 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
128 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
129 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
130}
131
136TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, BACKWARDS_MOVEMENT_TEST_3) {
137 // Arrange
138 Eigen::Vector3d previous_pose(0, 0, -M_PI);
139 Eigen::Vector4d motion_data(-1, 0.0, 0, 0);
140 double delta_t = 1;
141 ConstantAccelerationTurnrateModel model;
142
143 // Act
144 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
145 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
146
147 // Assert
148 EXPECT_NEAR(next_pose(0), 1.00, 0.000001);
149 EXPECT_NEAR(next_pose(1), 0, 0.000001);
150 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
151 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
152 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
153 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
154 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
155 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
156 EXPECT_NEAR(pose_jacobian(1, 2), 1, 0.01);
157 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
158 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
159 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
160}
161
166TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_1) {
167 // Arrange
168 Eigen::Vector3d previous_pose(0, 0, 2 * M_PI);
169 Eigen::Vector4d motion_data(0.0, 0.0, 0, 0);
170 double delta_t = 1;
171 ConstantAccelerationTurnrateModel model;
172
173 // Act
174 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
175 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
176
177 // Assert
178 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
179 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
180 EXPECT_NEAR(next_pose(2), 0.0, 0.000001);
181 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
182 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
183 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
184 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
185 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
186 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
187 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
188 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
189 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
190}
191
196TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_2) {
197 // Arrange
198 Eigen::Vector3d previous_pose(0, 0, 3 * M_PI);
199 Eigen::Vector4d motion_data(0.0, 0.0, 0, 0);
200 double delta_t = 1;
201 ConstantAccelerationTurnrateModel model;
202
203 // Act
204 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
205 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
206
207 // Assert
208 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
209 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
210 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
211 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
212 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
213 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
214 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
215 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
216 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
217 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
218 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
219 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
220}
221
226TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_3) {
227 // Arrange
228 Eigen::Vector3d previous_pose(0, 0, -3 * M_PI);
229 Eigen::Vector4d motion_data(0.0, 0.0, 0, 0);
230 double delta_t = 1;
231 ConstantAccelerationTurnrateModel model;
232
233 // Act
234 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
235 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
236
237 // Assert
238 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
239 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
240 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
241 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
242 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
243 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
244 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
245 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
246 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
247 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
248 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
249 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
250}
251
256TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_4) {
257 // Arrange
258 Eigen::Vector3d previous_pose(0, 0, 1.5 * M_PI);
259 Eigen::Vector4d motion_data(0.0, 0.0, 0, 0);
260 double delta_t = 1;
261 ConstantAccelerationTurnrateModel model;
262
263 // Act
264 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
265 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
266
267 // Assert
268 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
269 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
270 EXPECT_NEAR(next_pose(2), -M_PI / 2, 0.000001);
271 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
272 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
273 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
274 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
275 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
276 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
277 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
278 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
279 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
280}
281
286TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_5) {
287 // Arrange
288 Eigen::Vector3d previous_pose(0, 0, -2.5 * M_PI);
289 Eigen::Vector4d motion_data(0.0, 0.0, 0, 0);
290 double delta_t = 1;
291 ConstantAccelerationTurnrateModel model;
292
293 // Act
294 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
295 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
296
297 // Assert
298 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
299 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
300 EXPECT_NEAR(next_pose(2), -M_PI / 2, 0.000001);
301 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
302 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
303 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
304 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
305 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
306 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
307 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
308 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
309 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
310}
311
316TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, CURVILINEAR_MOVEMENT_TEST_1) {
317 // Arrange
318 Eigen::Vector3d previous_pose(1, 2, M_PI / 4);
319 Eigen::Vector4d motion_data(3, 0, M_PI / 16, 0);
320 double delta_t = 1;
321 ConstantAccelerationTurnrateModel model;
322
323 // Act
324 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
325 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
326
327 // Assert
328 EXPECT_NEAR(next_pose(0), 2.9, 0.01);
329 EXPECT_NEAR(next_pose(1), 4.316, 0.01);
330 EXPECT_NEAR(next_pose(2), 0.98, 0.01);
331 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
332 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
333 EXPECT_NEAR(pose_jacobian(0, 2), -2.315, 0.01);
334 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
335 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
336 EXPECT_NEAR(pose_jacobian(1, 2), 1.9, 0.01);
337 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
338 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
339 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
340}
341
346TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, CURVILINEAR_MOVEMENT_TEST_2) {
347 // Arrange
348 Eigen::Vector3d previous_pose(1, 2, -M_PI / 4);
349 Eigen::Vector4d motion_data(3, 0, -M_PI / 8, 0);
350 double delta_t = 1;
351 ConstantAccelerationTurnrateModel model;
352
353 // Act
354 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
355 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
356
357 // Assert
358 EXPECT_NEAR(next_pose(0), 2.656, 0.01);
359 EXPECT_NEAR(next_pose(1), -0.48, 0.01);
360 EXPECT_NEAR(next_pose(2), -3 * M_PI / 8, 0.000001);
361 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
362 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
363 EXPECT_NEAR(pose_jacobian(0, 2), 2.48, 0.01);
364 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
365 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
366 EXPECT_NEAR(pose_jacobian(1, 2), 1.656, 0.01);
367 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
368 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
369 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
370}
371
376TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, CURVILINEAR_MOVEMENT_TEST_3) {
377 // Arrange
378 Eigen::Vector3d previous_pose(1, 2, -M_PI / 2);
379 Eigen::Vector4d motion_data(3, 0, M_PI / 4, 0);
380 double delta_t = 2;
381 ConstantAccelerationTurnrateModel model;
382
383 // Act
384 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t);
385 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t);
386
387 // Assert
388 EXPECT_NEAR(next_pose(0), 4.820, 0.01);
389 EXPECT_NEAR(next_pose(1), -1.820, 0.01);
390 EXPECT_NEAR(next_pose(2), 0.0, 0.01);
391 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
392 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
393 EXPECT_NEAR(pose_jacobian(0, 2), 3.820, 0.01);
394 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
395 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
396 EXPECT_NEAR(pose_jacobian(1, 2), 3.820, 0.01);
397 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
398 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
399 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
400}
ConstantAccelerationTurnrateModel
Motion model that predicts the pose of the robot given the velocities with a constant acceleration an...
Definition constant_acceleration_turnrate_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_acceleration_turnrate_model.hpp
TEST
TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_1)
Test the constant acceleration and turnrate model with a straight line movement in the x axis.
Definition constant_acceleration_turnrate_model_test.cpp:15
model
std::shared_ptr< IVehicleModel > model
Definition pacsim_main.cpp:61