Skip to content

Commit 152f8a2

Browse files
AndeshogAndeshog
authored
add eta quat struct and utilities for quaternion pid (#17)
* feat/refactor: add eta quat struct, refactor types and update docs * test: update old tests and add new tests for new additions * doc: improve documentation on math functions * codecov: ignore test folders * codecov: ignore test folders v2 * codecov: ignore test folders v3 istg * codecov: ignore only cpp tests (bug with gtest that messes up coverage)
1 parent 0b38732 commit 152f8a2

7 files changed

Lines changed: 437 additions & 124 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
cmake_minimum_required(VERSION 3.8)
22
project(vortex_utils)
33

4-
set(CMAKE_CXX_STANDARD 17)
4+
set(CMAKE_CXX_STANDARD 20)
55
add_compile_options(-Wall -Wextra -Wpedantic)
66

77
find_package(ament_cmake REQUIRED)

‎codecov.yml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,3 +13,6 @@ fixes:
1313
comment:
1414
layout: "diff, flags, files"
1515
behavior: default
16+
17+
ignore:
18+
- "cpp_test/**"

‎cpp_test/test_math.cpp‎

Lines changed: 91 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,6 @@
11
#include <gtest/gtest.h>
2+
#include <eigen3/Eigen/Dense>
3+
#include <eigen3/Eigen/Geometry>
24
#include "vortex/utils/math.hpp"
35

46
namespace vortex::utils::math {
@@ -30,7 +32,7 @@ TEST(get_skew_symmetric_matrix, test_skew_symmetric) {
3032
expected << 0, -3, 2, 3, 0, -1, -2, 1, 0;
3133

3234
Eigen::Matrix3d result = get_skew_symmetric_matrix(vector);
33-
EXPECT_EQ(expected, result);
35+
EXPECT_TRUE(result.isApprox(expected, 1e-12));
3436
}
3537

3638
// Test that rotation matrix is correctly constructed
@@ -42,14 +44,14 @@ TEST(get_rotation_matrix, test_rotation_matrix) {
4244
expected << 0.41198225, -0.83373765, -0.36763046, -0.05872664, -0.42691762,
4345
0.90238159, -0.90929743, -0.35017549, -0.2248451;
4446
Eigen::Matrix3d result = get_rotation_matrix(roll, pitch, yaw);
45-
EXPECT_NEAR(0.0, matrix_norm_diff(expected, result), 0.01);
47+
EXPECT_TRUE(result.isApprox(expected, 1e-8));
4648
}
4749

4850
TEST(get_transformation_matrix_attitude, test_transformation_matrix_zeros) {
4951
Eigen::Matrix3d transformation_matrix{
5052
get_transformation_matrix_attitude(0.0, 0.0)};
5153
Eigen::Matrix3d expected{Eigen::Matrix3d::Identity()};
52-
EXPECT_NEAR(0.0, matrix_norm_diff(expected, transformation_matrix), 0.01);
54+
EXPECT_TRUE(transformation_matrix.isApprox(expected, 1e-12));
5355
}
5456

5557
TEST(get_transformation_matrix_attitude_quat,
@@ -59,19 +61,15 @@ TEST(get_transformation_matrix_attitude_quat,
5961
get_transformation_matrix_attitude_quat(quat)};
6062
Eigen::Matrix<double, 4, 3> expected = Eigen::Matrix<double, 4, 3>::Zero();
6163
expected.bottomRightCorner<3, 3>() = 0.5 * Eigen::Matrix3d::Identity();
62-
EXPECT_NEAR(0.0, matrix_norm_diff(expected, transformation_matrix), 0.01);
64+
EXPECT_TRUE(transformation_matrix.isApprox(expected, 1e-12));
6365
}
6466

6567
TEST(eigen_vector3d_to_quaternion, zero_vector_returns_identity) {
6668
const Eigen::Vector3d v = Eigen::Vector3d::Zero();
6769

6870
const Eigen::Quaterniond q = eigen_vector3d_to_quaternion(v);
6971

70-
EXPECT_NEAR(q.w(), 1.0, 1e-15);
71-
EXPECT_NEAR(q.x(), 0.0, 1e-15);
72-
EXPECT_NEAR(q.y(), 0.0, 1e-15);
73-
EXPECT_NEAR(q.z(), 0.0, 1e-15);
74-
EXPECT_NEAR(q.norm(), 1.0, 1e-15);
72+
EXPECT_TRUE(q.isApprox(Eigen::Quaterniond::Identity(), 1e-12));
7573
}
7674

7775
TEST(eigen_vector3d_to_quaternion, general_vector_matches_axis_angle_formula) {
@@ -87,51 +85,39 @@ TEST(eigen_vector3d_to_quaternion, general_vector_matches_axis_angle_formula) {
8785

8886
const Eigen::Quaterniond q = eigen_vector3d_to_quaternion(v);
8987

90-
EXPECT_NEAR(q.w(), expected.w(), 1e-12);
91-
EXPECT_NEAR(q.x(), expected.x(), 1e-12);
92-
EXPECT_NEAR(q.y(), expected.y(), 1e-12);
93-
EXPECT_NEAR(q.z(), expected.z(), 1e-12);
94-
EXPECT_NEAR(q.norm(), 1.0, 1e-12);
88+
EXPECT_TRUE(q.isApprox(expected, 1e-12));
9589
}
9690

9791
// Test that the identity quaternion correctly maps to 0, 0, 0
9892
TEST(quat_to_euler, test_quat_to_euler_1) {
9993
Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0);
10094
Eigen::Vector3d expected(0.0, 0.0, 0.0);
10195
Eigen::Vector3d result = quat_to_euler(q);
102-
for (int i = 0; i < 3; ++i) {
103-
EXPECT_NEAR(expected[i], result[i], 0.01);
104-
}
96+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
10597
}
10698

10799
// Test that only changing w and x in the quat only affects roll
108100
TEST(quat_to_euler, test_quat_to_euler_2) {
109101
Eigen::Quaterniond q2(0.707, 0.707, 0.0, 0.0);
110102
Eigen::Vector3d expected2(1.57, 0.0, 0.0);
111103
Eigen::Vector3d result2 = quat_to_euler(q2);
112-
for (int i = 0; i < 3; ++i) {
113-
EXPECT_NEAR(expected2[i], result2[i], 0.01);
114-
}
104+
EXPECT_TRUE(result2.isApprox(expected2, 1e-3));
115105
}
116106

117107
// Test that only changing w and z in the quat only affects yaw
118108
TEST(quat_to_euler, test_quat_to_euler_3) {
119-
Eigen::Quaterniond q4(0.707, 0.0, 0.0, 0.707);
120-
Eigen::Vector3d expected4(0.0, 0.0, 1.57);
121-
Eigen::Vector3d result4 = quat_to_euler(q4);
122-
for (int i = 0; i < 3; ++i) {
123-
EXPECT_NEAR(expected4[i], result4[i], 0.01);
124-
}
109+
Eigen::Quaterniond q3(0.707, 0.0, 0.0, 0.707);
110+
Eigen::Vector3d expected3(0.0, 0.0, 1.57);
111+
Eigen::Vector3d result3 = quat_to_euler(q3);
112+
EXPECT_TRUE(result3.isApprox(expected3, 1e-3));
125113
}
126114

127115
// Test that a quaternion is correctly converted to euler angles
128116
TEST(quat_to_euler, test_quat_to_euler_4) {
129-
Eigen::Quaterniond q5(0.770, 0.4207, -0.4207, -0.229);
130-
Eigen::Vector3d expected5(1.237, -0.4729, -0.9179);
131-
Eigen::Vector3d result5 = quat_to_euler(q5);
132-
for (int i = 0; i < 3; ++i) {
133-
EXPECT_NEAR(expected5[i], result5[i], 0.01);
134-
}
117+
Eigen::Quaterniond q4(0.770, 0.4207, -0.4207, -0.229);
118+
Eigen::Vector3d expected4(1.237, -0.4729, -0.9179);
119+
Eigen::Vector3d result4 = quat_to_euler(q4);
120+
EXPECT_TRUE(result4.isApprox(expected4, 1e-3));
135121
}
136122

137123
// Test that a quaternion with flipped signs is correctly converted to euler
@@ -140,9 +126,8 @@ TEST(quat_to_euler, test_quat_to_euler_5) {
140126
Eigen::Quaterniond q5(0.770, 0.4207, 0.4207, 0.229);
141127
Eigen::Vector3d expected5(1.237, 0.4729, 0.9179);
142128
Eigen::Vector3d result5 = quat_to_euler(q5);
143-
for (int i = 0; i < 3; ++i) {
144-
EXPECT_NEAR(expected5[i], result5[i], 0.01);
145-
}
129+
130+
EXPECT_TRUE(result5.isApprox(expected5, 1e-3));
146131
}
147132

148133
// Test that zero euler angles construct the correct quaternion
@@ -153,9 +138,8 @@ TEST(euler_to_quat, test_euler_to_quat_1) {
153138
Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
154139
Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
155140
Eigen::Vector4d expected{0.0, 0.0, 0.0, 1.0};
156-
for (int i = 0; i < 4; ++i) {
157-
EXPECT_NEAR(expected[i], result[i], 0.01);
158-
}
141+
142+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
159143
}
160144

161145
// Test that non-zero roll constructs the correct quaternion
@@ -166,9 +150,8 @@ TEST(euler_to_quat, test_euler_to_quat_2) {
166150
Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
167151
Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
168152
Eigen::Vector4d expected{0.479, 0.0, 0.0, 0.877};
169-
for (int i = 0; i < 4; ++i) {
170-
EXPECT_NEAR(expected[i], result[i], 0.01);
171-
}
153+
154+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
172155
}
173156

174157
// Test that non-zero pitch constructs the correct quaternion
@@ -179,9 +162,8 @@ TEST(euler_to_quat, test_euler_to_quat_3) {
179162
Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
180163
Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
181164
Eigen::Vector4d expected{0.0, 0.479, 0.0, 0.877};
182-
for (int i = 0; i < 4; ++i) {
183-
EXPECT_NEAR(expected[i], result[i], 0.01);
184-
}
165+
166+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
185167
}
186168

187169
// Test that non-zero yaw constructs the correct quaternion
@@ -192,9 +174,8 @@ TEST(euler_to_quat, test_euler_to_quat_4) {
192174
Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
193175
Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
194176
Eigen::Vector4d expected{0.0, 0.0, 0.479, 0.877};
195-
for (int i = 0; i < 4; ++i) {
196-
EXPECT_NEAR(expected[i], result[i], 0.01);
197-
}
177+
178+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
198179
}
199180

200181
// Test that non-zero euler angles constructs the correct quaternion
@@ -205,9 +186,8 @@ TEST(euler_to_quat, test_euler_to_quat_5) {
205186
Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
206187
Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
207188
Eigen::Vector4d expected{0.1675, 0.5709, 0.1675, 0.786};
208-
for (int i = 0; i < 4; ++i) {
209-
EXPECT_NEAR(expected[i], result[i], 0.01);
210-
}
189+
190+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
211191
}
212192

213193
// Test that zero euler angles construct the correct quaternion
@@ -218,9 +198,7 @@ TEST(euler_to_quat_vec3, zero_angles_identity_quaternion) {
218198
const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
219199
const Eigen::Vector4d expected(0.0, 0.0, 0.0, 1.0);
220200

221-
for (int i = 0; i < 4; ++i) {
222-
EXPECT_NEAR(expected[i], result[i], 0.01);
223-
}
201+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
224202
}
225203

226204
// Test that non-zero roll constructs the correct quaternion
@@ -231,9 +209,7 @@ TEST(euler_to_quat_vec3, roll_only) {
231209
const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
232210
const Eigen::Vector4d expected(0.479, 0.0, 0.0, 0.877);
233211

234-
for (int i = 0; i < 4; ++i) {
235-
EXPECT_NEAR(expected[i], result[i], 0.01);
236-
}
212+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
237213
}
238214

239215
// Test that non-zero pitch constructs the correct quaternion
@@ -244,9 +220,7 @@ TEST(euler_to_quat_vec3, pitch_only) {
244220
const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
245221
const Eigen::Vector4d expected(0.0, 0.479, 0.0, 0.877);
246222

247-
for (int i = 0; i < 4; ++i) {
248-
EXPECT_NEAR(expected[i], result[i], 0.01);
249-
}
223+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
250224
}
251225

252226
// Test that non-zero yaw constructs the correct quaternion
@@ -257,9 +231,7 @@ TEST(euler_to_quat_vec3, yaw_only) {
257231
const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
258232
const Eigen::Vector4d expected(0.0, 0.0, 0.479, 0.877);
259233

260-
for (int i = 0; i < 4; ++i) {
261-
EXPECT_NEAR(expected[i], result[i], 0.01);
262-
}
234+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
263235
}
264236

265237
// Test that non-zero euler angles construct the correct quaternion
@@ -270,9 +242,63 @@ TEST(euler_to_quat_vec3, roll_pitch_yaw_all_nonzero) {
270242
const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
271243
const Eigen::Vector4d expected(0.1675, 0.5709, 0.1675, 0.786);
272244

273-
for (int i = 0; i < 4; ++i) {
274-
EXPECT_NEAR(expected[i], result[i], 0.01);
275-
}
245+
EXPECT_TRUE(result.isApprox(expected, 1e-3));
246+
}
247+
248+
// Test pseudo inverse, results from
249+
// https://www.emathhelp.net/calculators/linear-algebra/pseudoinverse-calculator
250+
TEST(pseudo_inverse, pseudo_inverse_of_square_matrix_same_as_inverse) {
251+
Eigen::MatrixXd matrix(2, 2);
252+
matrix << 4, 7, 2, 6;
253+
Eigen::MatrixXd expected = matrix.inverse();
254+
Eigen::MatrixXd result = pseudo_inverse(matrix);
255+
EXPECT_TRUE(expected.isApprox(result, 1e-10));
256+
}
257+
258+
TEST(pseudo_inverse, pseudo_inverse_of_2_by_3_matrix) {
259+
Eigen::MatrixXd matrix(2, 3);
260+
matrix << 1, 2, 3, 4, 5, 6;
261+
Eigen::MatrixXd expected(3, 2);
262+
expected << -17.0 / 18.0, 4.0 / 9.0, -1.0 / 9.0, 1.0 / 9.0, 13.0 / 18.0,
263+
-2.0 / 9.0;
264+
Eigen::MatrixXd result = pseudo_inverse(matrix);
265+
EXPECT_TRUE(expected.isApprox(result, 1e-10));
266+
}
267+
268+
TEST(pseudo_inverse, pseudo_inverse_of_3_by_2_matrix) {
269+
Eigen::MatrixXd matrix(3, 2);
270+
matrix << 1, 2, 3, 4, 5, 6;
271+
Eigen::MatrixXd expected(2, 3);
272+
expected << -4.0 / 3.0, -1.0 / 3.0, 2.0 / 3.0, 13.0 / 12.0, 1.0 / 3.0,
273+
-5.0 / 12.0;
274+
Eigen::MatrixXd result = pseudo_inverse(matrix);
275+
EXPECT_TRUE(expected.isApprox(result, 1e-10));
276+
}
277+
278+
TEST(clamp_values, test_clamp_values) {
279+
Eigen::Vector<double, 5> values;
280+
values << -10.0, -1.0, 0.0, 1.0, 10.0;
281+
double min_val = -5.0;
282+
double max_val = 5.0;
283+
Eigen::Vector<double, 5> expected;
284+
expected << -5.0, -1.0, 0.0, 1.0, 5.0;
285+
Eigen::Vector<double, 5> result = clamp_values(values, min_val, max_val);
286+
EXPECT_TRUE(expected.isApprox(result, 1e-10));
287+
}
288+
289+
TEST(anti_windup, test_anti_windup) {
290+
double dt = 0.1;
291+
Eigen::Vector<double, 3> error;
292+
error << 1.0, -2.0, 3.0;
293+
Eigen::Vector<double, 3> integral;
294+
integral << 0.5, -0.5, 0.5;
295+
double min_val = -0.5;
296+
double max_val = 0.7;
297+
Eigen::Vector<double, 3> expected;
298+
expected << 0.6, -0.5, 0.7; // Last value clamped
299+
Eigen::Vector<double, 3> result =
300+
anti_windup(dt, error, integral, min_val, max_val);
301+
EXPECT_TRUE(expected.isApprox(result, 1e-10));
276302
}
277303

278304
} // namespace vortex::utils::math

0 commit comments

Comments
 (0)