diff --git a/CMakeLists.txt b/CMakeLists.txt index acd52ef..490e425 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.8) project(vortex_utils) -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 20) add_compile_options(-Wall -Wextra -Wpedantic) find_package(ament_cmake REQUIRED) diff --git a/codecov.yml b/codecov.yml index 9a9c84b..fd624ce 100644 --- a/codecov.yml +++ b/codecov.yml @@ -13,3 +13,6 @@ fixes: comment: layout: "diff, flags, files" behavior: default + +ignore: + - "cpp_test/**" diff --git a/cpp_test/test_math.cpp b/cpp_test/test_math.cpp index f06c8ce..a18caac 100644 --- a/cpp_test/test_math.cpp +++ b/cpp_test/test_math.cpp @@ -1,4 +1,6 @@ #include +#include +#include #include "vortex/utils/math.hpp" namespace vortex::utils::math { @@ -30,7 +32,7 @@ TEST(get_skew_symmetric_matrix, test_skew_symmetric) { expected << 0, -3, 2, 3, 0, -1, -2, 1, 0; Eigen::Matrix3d result = get_skew_symmetric_matrix(vector); - EXPECT_EQ(expected, result); + EXPECT_TRUE(result.isApprox(expected, 1e-12)); } // Test that rotation matrix is correctly constructed @@ -42,14 +44,14 @@ TEST(get_rotation_matrix, test_rotation_matrix) { expected << 0.41198225, -0.83373765, -0.36763046, -0.05872664, -0.42691762, 0.90238159, -0.90929743, -0.35017549, -0.2248451; Eigen::Matrix3d result = get_rotation_matrix(roll, pitch, yaw); - EXPECT_NEAR(0.0, matrix_norm_diff(expected, result), 0.01); + EXPECT_TRUE(result.isApprox(expected, 1e-8)); } TEST(get_transformation_matrix_attitude, test_transformation_matrix_zeros) { Eigen::Matrix3d transformation_matrix{ get_transformation_matrix_attitude(0.0, 0.0)}; Eigen::Matrix3d expected{Eigen::Matrix3d::Identity()}; - EXPECT_NEAR(0.0, matrix_norm_diff(expected, transformation_matrix), 0.01); + EXPECT_TRUE(transformation_matrix.isApprox(expected, 1e-12)); } TEST(get_transformation_matrix_attitude_quat, @@ -59,7 +61,7 @@ TEST(get_transformation_matrix_attitude_quat, get_transformation_matrix_attitude_quat(quat)}; Eigen::Matrix expected = Eigen::Matrix::Zero(); expected.bottomRightCorner<3, 3>() = 0.5 * Eigen::Matrix3d::Identity(); - EXPECT_NEAR(0.0, matrix_norm_diff(expected, transformation_matrix), 0.01); + EXPECT_TRUE(transformation_matrix.isApprox(expected, 1e-12)); } TEST(eigen_vector3d_to_quaternion, zero_vector_returns_identity) { @@ -67,11 +69,7 @@ TEST(eigen_vector3d_to_quaternion, zero_vector_returns_identity) { const Eigen::Quaterniond q = eigen_vector3d_to_quaternion(v); - EXPECT_NEAR(q.w(), 1.0, 1e-15); - EXPECT_NEAR(q.x(), 0.0, 1e-15); - EXPECT_NEAR(q.y(), 0.0, 1e-15); - EXPECT_NEAR(q.z(), 0.0, 1e-15); - EXPECT_NEAR(q.norm(), 1.0, 1e-15); + EXPECT_TRUE(q.isApprox(Eigen::Quaterniond::Identity(), 1e-12)); } TEST(eigen_vector3d_to_quaternion, general_vector_matches_axis_angle_formula) { @@ -87,11 +85,7 @@ TEST(eigen_vector3d_to_quaternion, general_vector_matches_axis_angle_formula) { const Eigen::Quaterniond q = eigen_vector3d_to_quaternion(v); - EXPECT_NEAR(q.w(), expected.w(), 1e-12); - EXPECT_NEAR(q.x(), expected.x(), 1e-12); - EXPECT_NEAR(q.y(), expected.y(), 1e-12); - EXPECT_NEAR(q.z(), expected.z(), 1e-12); - EXPECT_NEAR(q.norm(), 1.0, 1e-12); + EXPECT_TRUE(q.isApprox(expected, 1e-12)); } // Test that the identity quaternion correctly maps to 0, 0, 0 @@ -99,9 +93,7 @@ TEST(quat_to_euler, test_quat_to_euler_1) { Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0); Eigen::Vector3d expected(0.0, 0.0, 0.0); Eigen::Vector3d result = quat_to_euler(q); - for (int i = 0; i < 3; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + EXPECT_TRUE(result.isApprox(expected, 1e-3)); } // Test that only changing w and x in the quat only affects roll @@ -109,29 +101,23 @@ TEST(quat_to_euler, test_quat_to_euler_2) { Eigen::Quaterniond q2(0.707, 0.707, 0.0, 0.0); Eigen::Vector3d expected2(1.57, 0.0, 0.0); Eigen::Vector3d result2 = quat_to_euler(q2); - for (int i = 0; i < 3; ++i) { - EXPECT_NEAR(expected2[i], result2[i], 0.01); - } + EXPECT_TRUE(result2.isApprox(expected2, 1e-3)); } // Test that only changing w and z in the quat only affects yaw TEST(quat_to_euler, test_quat_to_euler_3) { - Eigen::Quaterniond q4(0.707, 0.0, 0.0, 0.707); - Eigen::Vector3d expected4(0.0, 0.0, 1.57); - Eigen::Vector3d result4 = quat_to_euler(q4); - for (int i = 0; i < 3; ++i) { - EXPECT_NEAR(expected4[i], result4[i], 0.01); - } + Eigen::Quaterniond q3(0.707, 0.0, 0.0, 0.707); + Eigen::Vector3d expected3(0.0, 0.0, 1.57); + Eigen::Vector3d result3 = quat_to_euler(q3); + EXPECT_TRUE(result3.isApprox(expected3, 1e-3)); } // Test that a quaternion is correctly converted to euler angles TEST(quat_to_euler, test_quat_to_euler_4) { - Eigen::Quaterniond q5(0.770, 0.4207, -0.4207, -0.229); - Eigen::Vector3d expected5(1.237, -0.4729, -0.9179); - Eigen::Vector3d result5 = quat_to_euler(q5); - for (int i = 0; i < 3; ++i) { - EXPECT_NEAR(expected5[i], result5[i], 0.01); - } + Eigen::Quaterniond q4(0.770, 0.4207, -0.4207, -0.229); + Eigen::Vector3d expected4(1.237, -0.4729, -0.9179); + Eigen::Vector3d result4 = quat_to_euler(q4); + EXPECT_TRUE(result4.isApprox(expected4, 1e-3)); } // 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) { Eigen::Quaterniond q5(0.770, 0.4207, 0.4207, 0.229); Eigen::Vector3d expected5(1.237, 0.4729, 0.9179); Eigen::Vector3d result5 = quat_to_euler(q5); - for (int i = 0; i < 3; ++i) { - EXPECT_NEAR(expected5[i], result5[i], 0.01); - } + + EXPECT_TRUE(result5.isApprox(expected5, 1e-3)); } // Test that zero euler angles construct the correct quaternion @@ -153,9 +138,8 @@ TEST(euler_to_quat, test_euler_to_quat_1) { Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)}; Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; Eigen::Vector4d expected{0.0, 0.0, 0.0, 1.0}; - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + + EXPECT_TRUE(result.isApprox(expected, 1e-3)); } // Test that non-zero roll constructs the correct quaternion @@ -166,9 +150,8 @@ TEST(euler_to_quat, test_euler_to_quat_2) { Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)}; Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; Eigen::Vector4d expected{0.479, 0.0, 0.0, 0.877}; - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + + EXPECT_TRUE(result.isApprox(expected, 1e-3)); } // Test that non-zero pitch constructs the correct quaternion @@ -179,9 +162,8 @@ TEST(euler_to_quat, test_euler_to_quat_3) { Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)}; Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; Eigen::Vector4d expected{0.0, 0.479, 0.0, 0.877}; - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + + EXPECT_TRUE(result.isApprox(expected, 1e-3)); } // Test that non-zero yaw constructs the correct quaternion @@ -192,9 +174,8 @@ TEST(euler_to_quat, test_euler_to_quat_4) { Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)}; Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; Eigen::Vector4d expected{0.0, 0.0, 0.479, 0.877}; - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + + EXPECT_TRUE(result.isApprox(expected, 1e-3)); } // Test that non-zero euler angles constructs the correct quaternion @@ -205,9 +186,8 @@ TEST(euler_to_quat, test_euler_to_quat_5) { Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)}; Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; Eigen::Vector4d expected{0.1675, 0.5709, 0.1675, 0.786}; - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + + EXPECT_TRUE(result.isApprox(expected, 1e-3)); } // Test that zero euler angles construct the correct quaternion @@ -218,9 +198,7 @@ TEST(euler_to_quat_vec3, zero_angles_identity_quaternion) { const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; const Eigen::Vector4d expected(0.0, 0.0, 0.0, 1.0); - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + EXPECT_TRUE(result.isApprox(expected, 1e-3)); } // Test that non-zero roll constructs the correct quaternion @@ -231,9 +209,7 @@ TEST(euler_to_quat_vec3, roll_only) { const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; const Eigen::Vector4d expected(0.479, 0.0, 0.0, 0.877); - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + EXPECT_TRUE(result.isApprox(expected, 1e-3)); } // Test that non-zero pitch constructs the correct quaternion @@ -244,9 +220,7 @@ TEST(euler_to_quat_vec3, pitch_only) { const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; const Eigen::Vector4d expected(0.0, 0.479, 0.0, 0.877); - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + EXPECT_TRUE(result.isApprox(expected, 1e-3)); } // Test that non-zero yaw constructs the correct quaternion @@ -257,9 +231,7 @@ TEST(euler_to_quat_vec3, yaw_only) { const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; const Eigen::Vector4d expected(0.0, 0.0, 0.479, 0.877); - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + EXPECT_TRUE(result.isApprox(expected, 1e-3)); } // Test that non-zero euler angles construct the correct quaternion @@ -270,9 +242,63 @@ TEST(euler_to_quat_vec3, roll_pitch_yaw_all_nonzero) { const Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()}; const Eigen::Vector4d expected(0.1675, 0.5709, 0.1675, 0.786); - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(expected[i], result[i], 0.01); - } + EXPECT_TRUE(result.isApprox(expected, 1e-3)); +} + +// Test pseudo inverse, results from +// https://www.emathhelp.net/calculators/linear-algebra/pseudoinverse-calculator +TEST(pseudo_inverse, pseudo_inverse_of_square_matrix_same_as_inverse) { + Eigen::MatrixXd matrix(2, 2); + matrix << 4, 7, 2, 6; + Eigen::MatrixXd expected = matrix.inverse(); + Eigen::MatrixXd result = pseudo_inverse(matrix); + EXPECT_TRUE(expected.isApprox(result, 1e-10)); +} + +TEST(pseudo_inverse, pseudo_inverse_of_2_by_3_matrix) { + Eigen::MatrixXd matrix(2, 3); + matrix << 1, 2, 3, 4, 5, 6; + Eigen::MatrixXd expected(3, 2); + expected << -17.0 / 18.0, 4.0 / 9.0, -1.0 / 9.0, 1.0 / 9.0, 13.0 / 18.0, + -2.0 / 9.0; + Eigen::MatrixXd result = pseudo_inverse(matrix); + EXPECT_TRUE(expected.isApprox(result, 1e-10)); +} + +TEST(pseudo_inverse, pseudo_inverse_of_3_by_2_matrix) { + Eigen::MatrixXd matrix(3, 2); + matrix << 1, 2, 3, 4, 5, 6; + Eigen::MatrixXd expected(2, 3); + expected << -4.0 / 3.0, -1.0 / 3.0, 2.0 / 3.0, 13.0 / 12.0, 1.0 / 3.0, + -5.0 / 12.0; + Eigen::MatrixXd result = pseudo_inverse(matrix); + EXPECT_TRUE(expected.isApprox(result, 1e-10)); +} + +TEST(clamp_values, test_clamp_values) { + Eigen::Vector values; + values << -10.0, -1.0, 0.0, 1.0, 10.0; + double min_val = -5.0; + double max_val = 5.0; + Eigen::Vector expected; + expected << -5.0, -1.0, 0.0, 1.0, 5.0; + Eigen::Vector result = clamp_values(values, min_val, max_val); + EXPECT_TRUE(expected.isApprox(result, 1e-10)); +} + +TEST(anti_windup, test_anti_windup) { + double dt = 0.1; + Eigen::Vector error; + error << 1.0, -2.0, 3.0; + Eigen::Vector integral; + integral << 0.5, -0.5, 0.5; + double min_val = -0.5; + double max_val = 0.7; + Eigen::Vector expected; + expected << 0.6, -0.5, 0.7; // Last value clamped + Eigen::Vector result = + anti_windup(dt, error, integral, min_val, max_val); + EXPECT_TRUE(expected.isApprox(result, 1e-10)); } } // namespace vortex::utils::math diff --git a/cpp_test/test_types.cpp b/cpp_test/test_types.cpp index ab700e1..a8537ed 100644 --- a/cpp_test/test_types.cpp +++ b/cpp_test/test_types.cpp @@ -26,26 +26,20 @@ TEST_F(TypesTests, test_eta) { Eigen::Matrix3d expected_rm{ vortex::utils::math::get_rotation_matrix(1.0, 0.5, 1.7)}; Eigen::Matrix3d result_rm{eta.as_rotation_matrix()}; - double diff_rm{ - vortex::utils::math::matrix_norm_diff(expected_rm, result_rm)}; - EXPECT_NEAR(diff_rm, 0.0, 1e-12); + EXPECT_TRUE(result_rm.isApprox(expected_rm, 1e-12)); Eigen::Matrix3d expected_tm{ vortex::utils::math::get_transformation_matrix_attitude(1.0, 0.5)}; Eigen::Matrix3d result_tm{eta.as_transformation_matrix()}; - double diff_tm{ - vortex::utils::math::matrix_norm_diff(expected_tm, result_tm)}; - EXPECT_NEAR(diff_tm, 0.0, 1e-12); + EXPECT_TRUE(result_tm.isApprox(expected_tm, 1e-12)); // Test to_vector eta.x = 5.0; eta.y = -4.0; eta.z = 2.1; - Eigen::Vector6d result_v{eta.to_vector()}; - Eigen::Vector6d expected_v{5.0, -4.0, 2.1, 1.0, 0.5, 1.7}; - for (int i = 0; i < 6; ++i) { - EXPECT_NEAR(expected_v[i], result_v[i], 1e-12); - } + Eigen::Vector result_v{eta.to_vector()}; + Eigen::Vector expected_v{5.0, -4.0, 2.1, 1.0, 0.5, 1.7}; + EXPECT_TRUE(result_v.isApprox(expected_v, 1e-12)); // Test operator- vortex::utils::types::Eta other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3}; @@ -58,6 +52,41 @@ TEST_F(TypesTests, test_eta) { EXPECT_NEAR(diff.yaw, 1.4, 1e-12); } +TEST_F(TypesTests, test_eta_quat) { + vortex::utils::types::EtaQuat eta_quat; + // Test correct zero initialization + EXPECT_EQ(eta_quat.x, 0.0); + EXPECT_EQ(eta_quat.y, 0.0); + EXPECT_EQ(eta_quat.z, 0.0); + EXPECT_EQ(eta_quat.qw, 1.0); + EXPECT_EQ(eta_quat.qx, 0.0); + EXPECT_EQ(eta_quat.qy, 0.0); + EXPECT_EQ(eta_quat.qz, 0.0); + + // Test to_vector + eta_quat.x = 5.0; + eta_quat.y = -4.0; + eta_quat.z = 2.1; + eta_quat.qw = 1.0; + eta_quat.qx = 0.5; + eta_quat.qy = -0.5; + eta_quat.qz = 0.25; + Eigen::Vector result_v{eta_quat.to_vector()}; + Eigen::Vector expected_v{5.0, -4.0, 2.1, 1.0, 0.5, -0.5, 0.25}; + EXPECT_TRUE(result_v.isApprox(expected_v, 1e-12)); + + // Test operator- + vortex::utils::types::EtaQuat other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.4}; + vortex::utils::types::EtaQuat diff{eta_quat - other}; + auto pos = diff.pos_vector(); + EXPECT_TRUE(pos.isApprox(Eigen::Vector3d(4.0, -6.0, -0.9), 1e-12)); + auto q = diff.ori_quaternion(); + EXPECT_TRUE(q.isApprox( + Eigen::Quaterniond(0.21908902300206642, -0.6207522318391883, + -0.7302967433402215, -0.18257418583505536), + 1e-12)); +} + TEST_F(TypesTests, test_nu) { vortex::utils::types::Nu nu; // Test correct zero initialization @@ -75,11 +104,9 @@ TEST_F(TypesTests, test_nu) { nu.p = 1.0; nu.q = 0.5; nu.r = 1.7; - Eigen::Vector6d result_v{nu.to_vector()}; - Eigen::Vector6d expected_v{5.0, -4.0, 2.1, 1.0, 0.5, 1.7}; - for (int i = 0; i < 6; ++i) { - EXPECT_NEAR(expected_v[i], result_v[i], 1e-12); - } + Eigen::Vector result_v{nu.to_vector()}; + Eigen::Vector expected_v{5.0, -4.0, 2.1, 1.0, 0.5, 1.7}; + EXPECT_TRUE(result_v.isApprox(expected_v, 1e-12)); // Test operator- vortex::utils::types::Nu other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3}; diff --git a/include/vortex/utils/math.hpp b/include/vortex/utils/math.hpp index 8f46257..5ed0818 100644 --- a/include/vortex/utils/math.hpp +++ b/include/vortex/utils/math.hpp @@ -7,45 +7,114 @@ namespace vortex::utils::math { -// @brief Function to calculate the smallest signed angle between two angles. -// Maps the angle to the interval [-pi, pi]. +/** + * @brief Function to calculate the smallest signed angle between two angles. + * Maps the angle to the interval [-pi, pi]. + * @param angle The input angle in radians. + * @return The smallest signed angle in radians. + */ double ssa(const double angle); -// @brief Helper to calculate error between two matrices -inline double matrix_norm_diff(Eigen::MatrixXd m1, Eigen::MatrixXd m2) { - return (m1 - m2).norm(); -} - -// @brief Calculates the skew-symmetric matrix from a 3D vector. +/** + * @brief Calculates the skew-symmetric matrix from a 3D vector. + * @param vector Eigen::Vector3d + * @return Eigen::Matrix3d skew-symmetric matrix + */ Eigen::Matrix3d get_skew_symmetric_matrix(const Eigen::Vector3d& vector); -// @brief Rotation matrix from Eigen quat +/** + * @brief Rotation matrix from euler angles + * @param roll Roll angle in radians. + * @param pitch Pitch angle in radians. + * @param yaw Yaw angle in radians. + * @return Eigen::Matrix3d rotation matrix + */ Eigen::Matrix3d get_rotation_matrix(const double roll, const double pitch, const double yaw); -// @brief Fossen, 2021 eq. 2.41 +/** + * @brief Fossen, 2021 eq. 2.41 + * @param roll Roll angle in radians. + * @param pitch Pitch angle in radians. + * @return Eigen::Matrix3d transformation matrix + */ Eigen::Matrix3d get_transformation_matrix_attitude(const double roll, const double pitch); -// @brief Fossen, 2021 eq. 2.78 +/** + * @brief Fossen, 2021 eq. 2.78 + * @param quat Eigen::Quaterniond + * @return Eigen::Matrix transformation matrix + */ Eigen::Matrix get_transformation_matrix_attitude_quat( const Eigen::Quaterniond& quat); -// @brief Converts an Eigen::Vector3d with euler angles to Eigen::Quaterniond +/** + * @brief Converts an Eigen::Vector3d with euler angles to Eigen::Quaterniond + * using the axis-angle representation. + * @param vector Eigen::Vector3d with roll, pitch, yaw + * @return Eigen::Quaterniond + */ Eigen::Quaterniond eigen_vector3d_to_quaternion(const Eigen::Vector3d& vector); -// @brief Converts a quaternion to Euler angles. +/** + * @brief Converts a quaternion to Euler angles. + * @param q Eigen::Quaterniond + * @return Eigen::Vector3d with roll, pitch, yaw + */ Eigen::Vector3d quat_to_euler(const Eigen::Quaterniond& q); -// @brief Converts Euler angles to quaternion +/** + * @brief Converts Euler angles to quaternion. + * @param roll Roll angle in radians. + * @param pitch Pitch angle in radians. + * @param yaw Yaw angle in radians. + */ Eigen::Quaterniond euler_to_quat(const double roll, const double pitch, const double yaw); -// @brief Converts Eigen::Vector3d with euler angles to quaternion +/** + * @brief Converts Eigen::Vector3d with euler angles to quaternion. + * @param euler Eigen::Vector3d with roll, pitch, yaw + * @return Eigen::Quaterniond + */ Eigen::Quaterniond euler_to_quat(const Eigen::Vector3d& euler); +/** + * @brief Computes the Moore-Penrose pseudo-inverse of a matrix. + * @param matrix The input matrix to be inverted. + * @return The pseudo-inverse of the input matrix. + */ +Eigen::MatrixXd pseudo_inverse(const Eigen::MatrixXd& matrix); + +/** + * @brief Clamps the values of a vector between min_val and max_val. + * @param values The input vector. + * @param min_val The minimum value. + * @param max_val The maximum value. + * @return The clamped vector. + */ + +Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, + const double min_val, + const double max_val); + +/** + * @brief Anti-windup for integral term in PID controller. + * @param dt Time step. + * @param error The current error vector. + * @param integral The current integral vector. + * @param min_val Minimum value for clamping. + * @param max_val Maximum value for clamping. + * @return The updated integral vector after applying anti-windup. + */ +Eigen::VectorXd anti_windup(const double dt, + const Eigen::VectorXd& error, + const Eigen::VectorXd& integral, + const double min_val, + const double max_val); } // namespace vortex::utils::math #endif // VORTEX_UTILS_MATH_HPP diff --git a/include/vortex/utils/types.hpp b/include/vortex/utils/types.hpp index 559cad7..664af6a 100644 --- a/include/vortex/utils/types.hpp +++ b/include/vortex/utils/types.hpp @@ -1,17 +1,24 @@ #ifndef VORTEX_UTILS_TYPES_HPP #define VORTEX_UTILS_TYPES_HPP +#include #include - -namespace Eigen { -typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Vector6d; -} // namespace Eigen +#include "math.hpp" namespace vortex::utils::types { -// @brief Struct to represent the state vector eta, -// containing the position and orientation. +/** + * @brief Struct to represent the state vector eta according to eq. 2.3 in + * Fossen, 2021, containing the position and orientation. + */ +struct Eta; + +/** + * @brief Struct to represent the state vector eta according to eq. 2.3 in + * Fossen, 2021, containing the position and orientation as quaternion. + */ +struct EtaQuat; + struct Eta { double x{}; double y{}; @@ -31,24 +38,40 @@ struct Eta { return eta; } - Eigen::Vector6d to_vector() const { - return Eigen::Vector6d{x, y, z, roll, pitch, yaw}; - } - - // @brief Make the rotation matrix according to eq. 2.31 in Fossen, 2021 - Eigen::Matrix3d as_rotation_matrix() const { - Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); - Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); - Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); + /** + * @brief Get the position vector (x, y, z). + * @return Eigen::Vector3d{x, y, z} + */ + Eigen::Vector3d pos_vector() const { return Eigen::Vector3d{x, y, z}; } - Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle; - Eigen::Matrix3d rotation_matrix = q.toRotationMatrix(); + /** + * @brief Get the orientation vector (roll, pitch, yaw). + * @return Eigen::Vector3d{roll, pitch, yaw} + */ + Eigen::Vector3d ori_vector() const { + return Eigen::Vector3d{roll, pitch, yaw}; + } - return rotation_matrix; + /** + * @brief Convert to Eigen::Vector6d + * @return Eigen::Vector6d{x, y, z, roll, pitch, yaw} + */ + Eigen::Vector to_vector() const { + return Eigen::Vector{x, y, z, roll, pitch, yaw}; } - // @brief Make the transformation matrix according to eq. 2.41 in Fossen, - // 2021 + /** + * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021 + * @return Eigen::Matrix3d rotation matrix + */ + Eigen::Matrix3d as_rotation_matrix() const { + return vortex::utils::math::get_rotation_matrix(roll, pitch, yaw); + } + /** + * @brief Make the transformation matrix according to eq. 2.41 in Fossen, + * 2021 + * @return Eigen::Matrix3d transformation matrix + */ Eigen::Matrix3d as_transformation_matrix() const { double cphi = cos(roll); double sphi = sin(roll); @@ -77,18 +100,124 @@ struct Eta { return transformation_matrix; } - Eigen::Matrix6d as_j_matrix() const { + /** + * @brief Make the J matrix according to eq. 2.53 in Fossen, 2021 + * @return Eigen::Matrix6d J matrix + */ + Eigen::Matrix as_j_matrix() const { Eigen::Matrix3d rotation_matrix = as_rotation_matrix(); Eigen::Matrix3d transformation_matrix = as_transformation_matrix(); - Eigen::Matrix6d j_matrix = Eigen::Matrix6d::Zero(); - j_matrix.block<3, 3>(0, 0) = rotation_matrix; - j_matrix.block<3, 3>(3, 3) = transformation_matrix; + Eigen::Matrix j_matrix = + Eigen::Matrix::Zero(); + j_matrix.topLeftCorner<3, 3>() = rotation_matrix; + j_matrix.bottomRightCorner<3, 3>() = transformation_matrix; return j_matrix; } + /** + * @brief Convert to EtaQuat + * @return EtaQuat + */ + EtaQuat as_eta_quat() const; }; +struct EtaQuat { + double x{}; + double y{}; + double z{}; + double qw{1.0}; + double qx{}; + double qy{}; + double qz{}; + + /** + * @brief Get the position vector (x, y, z). + * @return Eigen::Vector3d{x, y, z} + */ + Eigen::Vector3d pos_vector() const { return Eigen::Vector3d{x, y, z}; } + + /** + * @brief Get the orientation as Eigen::Quaterniond. + * @return Eigen::Quaterniond + */ + Eigen::Quaterniond ori_quaternion() const { + Eigen::Quaterniond quat; + quat.w() = qw; + quat.x() = qx; + quat.y() = qy; + quat.z() = qz; + return quat.normalized(); + } + + /** + * @brief Convert to Eigen::Vector7d + * @return Eigen::Vector7d{x, y, z, qw, qx, qy, qz} + */ + Eigen::Vector to_vector() const { + return Eigen::Vector{x, y, z, qw, qx, qy, qz}; + } + + EtaQuat operator-(const EtaQuat& other) const { + EtaQuat eta; + eta.x = x - other.x; + eta.y = y - other.y; + eta.z = z - other.z; + Eigen::Quaterniond q1 = ori_quaternion(); + Eigen::Quaterniond q2 = other.ori_quaternion(); + Eigen::Quaterniond q_error = q2.conjugate() * q1; + q_error.normalize(); + eta.qw = q_error.w(); + eta.qx = q_error.x(); + eta.qy = q_error.y(); + eta.qz = q_error.z(); + return eta; + } + + /** + * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021 + * @return Eigen::Matrix3d rotation matrix + */ + Eigen::Matrix3d as_rotation_matrix() const { + return ori_quaternion().toRotationMatrix(); + } + + /** + * @brief Make the transformation matrix according to eq. 2.78 in Fossen, + * 2021 + * @return Eigen::Matrix transformation matrix + */ + Eigen::Matrix as_transformation_matrix() const { + return vortex::utils::math::get_transformation_matrix_attitude_quat( + ori_quaternion()); + } + + /** + * @brief Make the J matrix according to eq. 2.83 in Fossen, 2021 + * @return Eigen::Matrix J matrix + */ + Eigen::Matrix as_j_matrix() const { + Eigen::Matrix3d R = as_rotation_matrix(); + Eigen::Matrix T = as_transformation_matrix(); + + Eigen::Matrix j_matrix = + Eigen::Matrix::Zero(); + j_matrix.topLeftCorner<3, 3>() = R; + j_matrix.bottomRightCorner<4, 3>() = T; + + return j_matrix; + } + /** + * @brief Convert to Eta with Euler angles + * @return Eta + */ + Eta as_eta_euler() const; +}; + +/** + * @brief Struct to represent the state vector nu according to eq. 2.5 in + * Fossen, 2021, containing the linear and angular velocities. + */ struct Nu { double u{}; double v{}; @@ -108,11 +237,42 @@ struct Nu { return nu; } - Eigen::Vector6d to_vector() const { - return Eigen::Vector6d{u, v, w, p, q, r}; + /** + * @brief Convert to Eigen::Vector6d + * @return Eigen::Vector6d{u, v, w, p, q, r} + */ + Eigen::Vector to_vector() const { + return Eigen::Vector{u, v, w, p, q, r}; } }; +inline EtaQuat Eta::as_eta_quat() const { + Eigen::Quaterniond quat = + vortex::utils::math::euler_to_quat(roll, pitch, yaw); + + EtaQuat eta_quat{.x = x, + .y = y, + .z = z, + .qw = quat.w(), + .qx = quat.x(), + .qy = quat.y(), + .qz = quat.z()}; + return eta_quat; +} + +inline Eta EtaQuat::as_eta_euler() const { + Eigen::Vector3d euler_angles = + vortex::utils::math::quat_to_euler(ori_quaternion()); + + Eta eta{.x = x, + .y = y, + .z = z, + .roll = euler_angles(0), + .pitch = euler_angles(1), + .yaw = euler_angles(2)}; + return eta; +} + } // namespace vortex::utils::types #endif // VORTEX_UTILS_TYPES_HPP diff --git a/src/math.cpp b/src/math.cpp index 7a6786e..284d930 100644 --- a/src/math.cpp +++ b/src/math.cpp @@ -103,4 +103,32 @@ Eigen::Quaterniond euler_to_quat(const Eigen::Vector3d& euler) { return q.normalized(); } +Eigen::MatrixXd pseudo_inverse(const Eigen::MatrixXd& matrix) { + if (matrix.rows() >= matrix.cols()) { + return (matrix.transpose() * matrix).ldlt().solve(matrix.transpose()); + } else { + return matrix.transpose() * (matrix * matrix.transpose()) + .ldlt() + .solve(Eigen::MatrixXd::Identity( + matrix.rows(), matrix.rows())); + } +} + +Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, + const double min_val, + const double max_val) { + return values.cwiseMax(min_val).cwiseMin(max_val); +} + +Eigen::VectorXd anti_windup(const double dt, + const Eigen::VectorXd& error, + const Eigen::VectorXd& integral, + const double min_val, + const double max_val) { + Eigen::VectorXd integral_anti_windup = integral + (error * dt); + + integral_anti_windup = clamp_values(integral_anti_windup, min_val, max_val); + return integral_anti_windup; +} + } // namespace vortex::utils::math