Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
bicycle_model_test.cpp
Go to the documentation of this file.
1#include "motion_lib/s2v_model/bicycle_model.hpp"
2
3#include <gtest/gtest.h>
4
5#include <cmath>
6
7/* ----------------------- ODOMETRY MODEL -------------------------*/
8
14TEST(ODOMETRY_SUBSCRIBER, CONVERSION_TEST) {
15 // Straight Line
16 BicycleModel bicycle_model =
17 BicycleModel(common_lib::car_parameters::CarParameters(0.516, 1.6, 0.79, 1.2, 0.791, 4.0));
18 double rl_speed = 60;
19 double rr_speed = 60;
20 double fl_speed = 60;
21 double fr_speed = 60;
22 double steering_angle = 0;
23 std::pair<double, double> velocity_data =
24 bicycle_model.wheels_velocities_to_cg(rl_speed, rr_speed, fl_speed, fr_speed, steering_angle);
25 EXPECT_NEAR(velocity_data.first, 1.6210, 0.0001);
26 EXPECT_DOUBLE_EQ(velocity_data.second, 0);
27
28 // Curving left
29 rl_speed = 60;
30 rr_speed = 60;
31 fl_speed = 60;
32 fr_speed = 60;
33 steering_angle = M_PI / 8;
34 velocity_data =
35 bicycle_model.wheels_velocities_to_cg(rl_speed, rr_speed, fl_speed, fr_speed, steering_angle);
36 EXPECT_GE(velocity_data.first, 1.6210);
37 EXPECT_LE(velocity_data.first, 1.6210 * 2);
38 EXPECT_LE(velocity_data.second, M_PI);
39 EXPECT_GE(velocity_data.second, M_PI / 8);
40
41 // Curving right
42 rl_speed = 60;
43 rr_speed = 60;
44 fl_speed = 60;
45 fr_speed = 60;
46 steering_angle = -M_PI / 8;
47 velocity_data =
48 bicycle_model.wheels_velocities_to_cg(rl_speed, rr_speed, fl_speed, fr_speed, steering_angle);
49 EXPECT_GE(velocity_data.first, 1.6210);
50 EXPECT_LE(velocity_data.first, 1.6210 * 2);
51 EXPECT_GE(velocity_data.second, -M_PI);
52 EXPECT_LE(velocity_data.second, -M_PI / 8);
53}
54
59TEST(BicycleModelTest, TestCgVelocityToWheels) {
60 common_lib::car_parameters::CarParameters car_parameters_;
61 car_parameters_.dist_cg_2_rear_axis = 1.0;
62 car_parameters_.wheelbase = 2.5;
63 car_parameters_.wheel_diameter = 0.6;
64 car_parameters_.gear_ratio = 4.0;
65 BicycleModel bicycle_model = BicycleModel(car_parameters_);
66 Eigen::Vector3d cg_velocities(10.0, 2.0, 0.1); // Example velocities
67 Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(cg_velocities);
68
69 ASSERT_EQ(observations.size(), 6);
70
71 EXPECT_NEAR(observations(0), 325.584, 0.01); // front_wheels_rpm
72 EXPECT_NEAR(observations(1), 325.584, 0.01); // front_wheels_rpm
73 EXPECT_NEAR(observations(2), 324.004, 0.01); // rear_wheels_rpm
74 EXPECT_NEAR(observations(3), 324.004, 0.01); // rear_wheels_rpm
75 EXPECT_NEAR(observations(4), 0.02450, 0.001); // steering_angle
76 EXPECT_NEAR(observations(5), 1296.02, 0.01); // motor_rpm
77}
78
84TEST(BicycleModelTest, TestCgVelocityToWheelsNegativeVx) {
85 common_lib::car_parameters::CarParameters car_parameters_;
86 car_parameters_.dist_cg_2_rear_axis = 1.0;
87 car_parameters_.wheelbase = 2.5;
88 car_parameters_.wheel_diameter = 0.6;
89 car_parameters_.gear_ratio = 4.0;
90 BicycleModel bicycle_model = BicycleModel(car_parameters_);
91 Eigen::Vector3d cg_velocities(-10.0, 2.0, 0.1); // Example velocities
92 Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(cg_velocities);
93
94 // Check the size of the observations vector
95 ASSERT_EQ(observations.size(), 6);
96
97 EXPECT_NEAR(observations(0), -325.584, 0.01); // front_wheels_rpm
98 EXPECT_NEAR(observations(1), -325.584, 0.01); // front_wheels_rpm
99 EXPECT_NEAR(observations(2), -324.004, 0.01); // rear_wheels_rpm
100 EXPECT_NEAR(observations(3), -324.004, 0.01); // rear_wheels_rpm
101 EXPECT_NEAR(observations(4), 0.02450, 0.001); // steering_angle
102 EXPECT_NEAR(observations(5), -1296.02, 0.01); // motor_rpm
103}
104
110TEST(BicycleModelTest, TestCgVelocityToWheelsZeroVx) {
111 common_lib::car_parameters::CarParameters car_parameters_;
112 car_parameters_.dist_cg_2_rear_axis = 1.0;
113 car_parameters_.wheelbase = 2.5;
114 car_parameters_.wheel_diameter = 0.6;
115 car_parameters_.gear_ratio = 4.0;
116 BicycleModel bicycle_model = BicycleModel(car_parameters_);
117 Eigen::Vector3d cg_velocities(0.0, -2.0, -0.1); // Example velocities
118 Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(cg_velocities);
119
120 // Check the size of the observations vector
121 ASSERT_EQ(observations.size(), 6);
122
123 EXPECT_NEAR(observations(0), 68.4366, 0.01); // front_wheels_rpm
124 EXPECT_NEAR(observations(1), 68.4366, 0.01); // front_wheels_rpm
125 EXPECT_NEAR(observations(2), 60.47888, 0.01); // rear_wheels_rpm
126 EXPECT_NEAR(observations(3), 60.47888, 0.01); // rear_wheels_rpm
127 EXPECT_NEAR(observations(4), -0.1243, 0.001); // steering_angle
128 EXPECT_NEAR(observations(5), 241.91552, 0.01); // motor_rpm
129}
130
136TEST(BicycleModelTest, TestCgVelocityToWheelsZeroVy) {
137 common_lib::car_parameters::CarParameters car_parameters_;
138 car_parameters_.dist_cg_2_rear_axis = 1.0;
139 car_parameters_.wheelbase = 2.5;
140 car_parameters_.wheel_diameter = 0.6;
141 car_parameters_.gear_ratio = 4.0;
142 BicycleModel bicycle_model = BicycleModel(car_parameters_);
143 Eigen::Vector3d cg_velocities(0.0, 0.0, 0.0); // Example velocities
144 Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(cg_velocities);
145
146 // Check the size of the observations vector
147 ASSERT_EQ(observations.size(), 6);
148
149 EXPECT_NEAR(observations(0), 0, 0.01); // front_wheels_rpm
150 EXPECT_NEAR(observations(1), 0, 0.01); // front_wheels_rpm
151 EXPECT_NEAR(observations(2), 0, 0.01); // rear_wheels_rpm
152 EXPECT_NEAR(observations(3), 0, 0.01); // rear_wheels_rpm
153 EXPECT_NEAR(observations(4), 0, 0.001); // steering_angle
154 EXPECT_NEAR(observations(5), 0, 0.01); // motor_rpm
155}
156
162TEST(BicycleModelTest, TestJacobianCgVelocityToWheels) {
163 // Initialize car parameters
164 common_lib::car_parameters::CarParameters car_parameters;
165 car_parameters.dist_cg_2_rear_axis = 1.0;
166 car_parameters.wheelbase = 2.5;
167 car_parameters.wheel_diameter = 0.6;
168 car_parameters.gear_ratio = 4.0;
169
170 // Initialize BicycleModel with car parameters
171 BicycleModel bicycle_model(car_parameters);
172
173 // Example velocities
174 Eigen::Vector3d cg_velocities(10.0, 2.0, 0.1);
175
176 // Calculate the Jacobian
177 Eigen::MatrixXd jacobian = bicycle_model.jacobian_cg_velocity_to_wheels(cg_velocities);
178
179 // Check the size of the Jacobian matrix
180 ASSERT_EQ(jacobian.rows(), 6);
181 ASSERT_EQ(jacobian.cols(), 3);
182
183 EXPECT_NEAR(jacobian(0, 0), 31.1199, 0.01);
184 EXPECT_NEAR(jacobian(0, 1), 6.69077, 0.01);
185 EXPECT_NEAR(jacobian(0, 2), 10.0362, 0.01);
186 EXPECT_NEAR(jacobian(1, 0), 31.1199, 0.01);
187 EXPECT_NEAR(jacobian(1, 1), 6.69077, 0.01);
188 EXPECT_NEAR(jacobian(1, 2), 10.0362, 0.01);
189 EXPECT_NEAR(jacobian(2, 0), 31.2715, 0.01);
190 EXPECT_NEAR(jacobian(2, 1), 5.94159, 0.01);
191 EXPECT_NEAR(jacobian(2, 2), -5.9416, 0.01);
192 EXPECT_NEAR(jacobian(3, 0), 31.2715, 0.01);
193 EXPECT_NEAR(jacobian(3, 1), 5.94159, 0.01);
194 EXPECT_NEAR(jacobian(3, 2), -5.9416, 0.01);
195 EXPECT_NEAR(jacobian(4, 0), -0.002355, 0.01);
196 EXPECT_NEAR(jacobian(4, 1), -0.00047, 0.01);
197 EXPECT_NEAR(jacobian(4, 2), 0.24499, 0.01);
198 EXPECT_NEAR(jacobian(5, 0), 125.086, 0.01);
199 EXPECT_NEAR(jacobian(5, 1), 23.7664, 0.01);
200 EXPECT_NEAR(jacobian(5, 2), -23.7664, 0.01);
201}
202
209TEST(BicycleModelTest, TestJacobianCgVelocityToWheelsZeroVy) {
210 // Initialize car parameters
211 common_lib::car_parameters::CarParameters car_parameters;
212 car_parameters.dist_cg_2_rear_axis = 1.0;
213 car_parameters.wheelbase = 2.5;
214 car_parameters.wheel_diameter = 0.6;
215 car_parameters.gear_ratio = 4.0;
216
217 // Initialize BicycleModel with car parameters
218 BicycleModel bicycle_model(car_parameters);
219
220 // Example velocities
221 Eigen::Vector3d cg_velocities(-10.0, 0, -0.1);
222
223 // Calculate the Jacobian
224 Eigen::MatrixXd jacobian = bicycle_model.jacobian_cg_velocity_to_wheels(cg_velocities);
225
226 // Check the size of the Jacobian matrix
227 ASSERT_EQ(jacobian.rows(), 6);
228 ASSERT_EQ(jacobian.cols(), 3);
229
230 EXPECT_NEAR(jacobian(0, 0), 31.8274082, 0.01);
231 EXPECT_NEAR(jacobian(0, 1), 0.47741112, 0.01);
232 EXPECT_NEAR(jacobian(0, 2), 0.71611668, 0.01);
233 EXPECT_NEAR(jacobian(1, 0), 31.8274082, 0.01);
234 EXPECT_NEAR(jacobian(1, 1), 0.47741112, 0.01);
235 EXPECT_NEAR(jacobian(1, 2), 0.71611668, 0.01);
236 EXPECT_NEAR(jacobian(2, 0), 31.8293971, 0.01);
237 EXPECT_NEAR(jacobian(2, 1), -0.3182939, 0.01);
238 EXPECT_NEAR(jacobian(2, 2), 0.31829397, 0.01);
239 EXPECT_NEAR(jacobian(3, 0), 31.8293971, 0.01);
240 EXPECT_NEAR(jacobian(3, 1), -0.3182939, 0.01);
241 EXPECT_NEAR(jacobian(3, 2), 0.31829397, 0.01);
242 EXPECT_NEAR(jacobian(4, 0), -0.00149966, 0.01);
243 EXPECT_NEAR(jacobian(4, 1), 0.0, 0.01);
244 EXPECT_NEAR(jacobian(4, 2), 0.24984, 0.01);
245 EXPECT_NEAR(jacobian(5, 0), 127.3175884, 0.01);
246 EXPECT_NEAR(jacobian(5, 1), -1.27316, 0.01);
247 EXPECT_NEAR(jacobian(5, 2), 1.27316, 0.01);
248}
TEST
TEST(ODOMETRY_SUBSCRIBER, CONVERSION_TEST)
Tests the conversion from wheel revolutions to velocities using the bycicle model.
Definition bicycle_model_test.cpp:14
BicycleModel
Simple bicycle model structure.
Definition bicycle_model.hpp:13
BicycleModel::wheels_velocities_to_cg
std::pair< double, double > wheels_velocities_to_cg(double rl_rpm, double fl_rpm, double rr_rpm, double fr_rpm, double steering_angle) override
assumes a bicycle model and a set of parameters about the vehicle to calculate the velocities of the ...
BicycleModel::jacobian_cg_velocity_to_wheels
Eigen::MatrixXd jacobian_cg_velocity_to_wheels(const Eigen::Vector3d &cg_velocities) override
Calculates the jacobian of the function cg_velocity_to_wheels with respect to the center of mass velo...
BicycleModel::cg_velocity_to_wheels
Eigen::VectorXd cg_velocity_to_wheels(const Eigen::Vector3d &cg_velocities) override
Assumes a bicycle model and a set of parameters about the vehicle to calculate the velocities of the ...
bicycle_model.hpp
common_lib::car_parameters::CarParameters
Definition car_parameters.hpp:15
common_lib::car_parameters::CarParameters::wheel_diameter
double wheel_diameter
Definition car_parameters.hpp:16
common_lib::car_parameters::CarParameters::gear_ratio
double gear_ratio
Definition car_parameters.hpp:21
common_lib::car_parameters::CarParameters::wheelbase
double wheelbase
Definition car_parameters.hpp:17
common_lib::car_parameters::CarParameters::dist_cg_2_rear_axis
double dist_cg_2_rear_axis
Definition car_parameters.hpp:20