Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
constant_velocity_turnrate_model_test.cpp
Go to the documentation of this file.
1#include "motion_lib/v2p_models/constant_velocity_turnrate_model.hpp"
2
3#include <gtest/gtest.h>
4
5#include <cmath>
6
11TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_1) {
12 // Arrange
13 Eigen::Vector3d previous_pose(0, 0, 0);
14 Eigen::Vector3d velocities(1, 0, 0);
15 double delta_t = 1;
16 ConstantVelocityTurnrateModel model;
17
18 // Act
19 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
20 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
21
22 // Assert
23 EXPECT_NEAR(next_pose(0), 1, 0.000001);
24 EXPECT_NEAR(next_pose(1), 0, 0.000001);
25 EXPECT_NEAR(next_pose(2), 0, 0.000001);
26 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
27 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
28 EXPECT_FLOAT_EQ(pose_jacobian(0, 2), 0);
29 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
30 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
31 EXPECT_FLOAT_EQ(pose_jacobian(1, 2), 1);
32 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
33 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
34 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
35}
36
41TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_2) {
42 // Arrange
43 Eigen::Vector3d previous_pose(0, 0, M_PI / 4);
44 Eigen::Vector3d velocities(2, 0, 0);
45 double delta_t = 1;
46 ConstantVelocityTurnrateModel model;
47
48 // Act
49 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
50 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
51
52 // Assert
53 EXPECT_NEAR(next_pose(0), 1.41, 0.01);
54 EXPECT_NEAR(next_pose(1), 1.41, 0.01);
55 EXPECT_NEAR(next_pose(2), M_PI / 4, 0.000001);
56 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
57 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
58 EXPECT_NEAR(pose_jacobian(0, 2), -1.41, 0.01);
59 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
60 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
61 EXPECT_NEAR(pose_jacobian(1, 2), 1.41, 0.01);
62 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
63 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
64 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
65}
66
71TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, BACKWARDS_MOVEMENT_TEST_1) {
72 // Arrange
73 Eigen::Vector3d previous_pose(0, 0, 0.0);
74 Eigen::Vector3d velocities(-1, 0.0, 0);
75 double delta_t = 1;
76 ConstantVelocityTurnrateModel model;
77
78 // Act
79 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
80 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
81
82 // Assert
83 EXPECT_NEAR(next_pose(0), -1.0, 0.000001);
84 EXPECT_NEAR(next_pose(1), 0, 0.000001);
85 EXPECT_NEAR(abs(next_pose(2)), 0.0, 0.000001);
86 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
87 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
88 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
89 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
90 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
91 EXPECT_NEAR(pose_jacobian(1, 2), -1, 0.01);
92 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
93 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
94 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
95}
96
101TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, BACKWARDS_MOVEMENT_TEST_2) {
102 // Arrange
103 Eigen::Vector3d previous_pose(0, 0, M_PI);
104 Eigen::Vector3d velocities(1, 0.0, 0);
105 double delta_t = 1;
106 ConstantVelocityTurnrateModel model;
107
108 // Act
109 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
110 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
111
112 // Assert
113 EXPECT_NEAR(next_pose(0), -1.00, 0.000001);
114 EXPECT_NEAR(next_pose(1), 0, 0.000001);
115 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
116 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
117 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
118 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
119 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
120 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
121 EXPECT_NEAR(pose_jacobian(1, 2), -1, 0.01);
122 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
123 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
124 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
125}
126
131TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, BACKWARDS_MOVEMENT_TEST_3) {
132 // Arrange
133 Eigen::Vector3d previous_pose(0, 0, -M_PI);
134 Eigen::Vector3d velocities(-1, 0.0, 0);
135 double delta_t = 1;
136 ConstantVelocityTurnrateModel model;
137
138 // Act
139 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
140 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
141
142 // Assert
143 EXPECT_NEAR(next_pose(0), 1.00, 0.000001);
144 EXPECT_NEAR(next_pose(1), 0, 0.000001);
145 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
146 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
147 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
148 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
149 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
150 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
151 EXPECT_NEAR(pose_jacobian(1, 2), 1, 0.01);
152 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
153 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
154 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
155}
156
161TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_1) {
162 // Arrange
163 Eigen::Vector3d previous_pose(0, 0, 2 * M_PI);
164 Eigen::Vector3d velocities(0.0, 0.0, 0);
165 double delta_t = 1;
166 ConstantVelocityTurnrateModel model;
167
168 // Act
169 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
170 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
171
172 // Assert
173 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
174 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
175 EXPECT_NEAR(next_pose(2), 0.0, 0.000001);
176 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
177 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
178 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
179 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
180 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
181 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
182 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
183 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
184 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
185}
186
191TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_2) {
192 // Arrange
193 Eigen::Vector3d previous_pose(0, 0, 3 * M_PI);
194 Eigen::Vector3d velocities(0.0, 0.0, 0);
195 double delta_t = 1;
196 ConstantVelocityTurnrateModel model;
197
198 // Act
199 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
200 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
201
202 // Assert
203 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
204 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
205 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
206 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
207 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
208 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
209 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
210 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
211 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
212 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
213 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
214 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
215}
216
221TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_3) {
222 // Arrange
223 Eigen::Vector3d previous_pose(0, 0, -3 * M_PI);
224 Eigen::Vector3d velocities(0.0, 0.0, 0);
225 double delta_t = 1;
226 ConstantVelocityTurnrateModel model;
227
228 // Act
229 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
230 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
231
232 // Assert
233 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
234 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
235 EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001);
236 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
237 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
238 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
239 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
240 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
241 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
242 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
243 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
244 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
245}
246
251TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_4) {
252 // Arrange
253 Eigen::Vector3d previous_pose(0, 0, 1.5 * M_PI);
254 Eigen::Vector3d velocities(0.0, 0.0, 0);
255 double delta_t = 1;
256 ConstantVelocityTurnrateModel model;
257
258 // Act
259 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
260 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
261
262 // Assert
263 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
264 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
265 EXPECT_NEAR(next_pose(2), -M_PI / 2, 0.000001);
266 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
267 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
268 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
269 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
270 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
271 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
272 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
273 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
274 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
275}
276
281TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_5) {
282 // Arrange
283 Eigen::Vector3d previous_pose(0, 0, -2.5 * M_PI);
284 Eigen::Vector3d velocities(0.0, 0.0, 0);
285 double delta_t = 1;
286 ConstantVelocityTurnrateModel model;
287
288 // Act
289 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
290 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
291
292 // Assert
293 EXPECT_NEAR(next_pose(0), 0.0, 0.001);
294 EXPECT_NEAR(next_pose(1), 0.0, 0.000001);
295 EXPECT_NEAR(next_pose(2), -M_PI / 2, 0.000001);
296 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
297 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
298 EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01);
299 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
300 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
301 EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01);
302 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
303 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
304 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
305}
306
311TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, CURVILINEAR_MOVEMENT_TEST_1) {
312 // Arrange
313 Eigen::Vector3d previous_pose(1, 2, M_PI / 4);
314 Eigen::Vector3d velocities(3, 0, M_PI / 16);
315 double delta_t = 1;
316 ConstantVelocityTurnrateModel model;
317
318 // Act
319 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
320 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
321
322 // Assert
323 EXPECT_NEAR(next_pose(0), 2.9, 0.01);
324 EXPECT_NEAR(next_pose(1), 4.316, 0.01);
325 EXPECT_NEAR(next_pose(2), 0.98, 0.01);
326 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
327 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
328 EXPECT_NEAR(pose_jacobian(0, 2), -2.315, 0.01);
329 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
330 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
331 EXPECT_NEAR(pose_jacobian(1, 2), 1.9, 0.01);
332 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
333 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
334 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
335}
336
341TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, CURVILINEAR_MOVEMENT_TEST_2) {
342 // Arrange
343 Eigen::Vector3d previous_pose(1, 2, -M_PI / 4);
344 Eigen::Vector3d velocities(3, 0, -M_PI / 8);
345 double delta_t = 1;
346 ConstantVelocityTurnrateModel model;
347
348 // Act
349 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
350 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
351
352 // Assert
353 EXPECT_NEAR(next_pose(0), 2.656, 0.01);
354 EXPECT_NEAR(next_pose(1), -0.48, 0.01);
355 EXPECT_NEAR(next_pose(2), -3 * M_PI / 8, 0.000001);
356 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
357 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
358 EXPECT_NEAR(pose_jacobian(0, 2), 2.48, 0.01);
359 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
360 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
361 EXPECT_NEAR(pose_jacobian(1, 2), 1.656, 0.01);
362 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
363 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
364 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
365}
366
371TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, CURVILINEAR_MOVEMENT_TEST_3) {
372 // Arrange
373 Eigen::Vector3d previous_pose(1, 2, -M_PI / 2);
374 Eigen::Vector3d velocities(3, 0, M_PI / 4);
375 double delta_t = 2;
376 ConstantVelocityTurnrateModel model;
377
378 // Act
379 Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, velocities, delta_t);
380 Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, velocities, delta_t);
381
382 // Assert
383 EXPECT_NEAR(next_pose(0), 4.820, 0.01);
384 EXPECT_NEAR(next_pose(1), -1.820, 0.01);
385 EXPECT_NEAR(next_pose(2), 0.0, 0.01);
386 EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1);
387 EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0);
388 EXPECT_NEAR(pose_jacobian(0, 2), 3.820, 0.01);
389 EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0);
390 EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1);
391 EXPECT_NEAR(pose_jacobian(1, 2), 3.820, 0.01);
392 EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0);
393 EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0);
394 EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1);
395}
ConstantVelocityTurnrateModel
Motion model that predicts the pose of the robot given the velocities with a constant velocity model ...
Definition constant_velocity_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_velocity_turnrate_model.hpp
TEST
TEST(CONSTANT_VELOCITY_TURNRATE_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_1)
Test the constant velocity and turnrate model with a straight line movement in the x axis.
Definition constant_velocity_turnrate_model_test.cpp:11
model
std::shared_ptr< IVehicleModel > model
Definition pacsim_main.cpp:61