diff --git a/CMakeLists.txt b/CMakeLists.txt index ea39fce..3aa9493 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,7 @@ set( source/Common/EliteException.cpp source/Common/SshUtils.cpp source/Common/RtUtils.cpp + source/Common/MathUtils.cpp source/Primary/PrimaryPort.cpp source/Primary/PrimaryPortInterface.cpp source/Primary/RobotConfPackage.cpp @@ -99,6 +100,7 @@ set( Common/Utils.hpp Common/EndianUtils.hpp Common/StringUtils.hpp + Common/MathUtils.hpp ) # Set output library default name diff --git a/include/Common/MathUtils.hpp b/include/Common/MathUtils.hpp new file mode 100644 index 0000000..9563247 --- /dev/null +++ b/include/Common/MathUtils.hpp @@ -0,0 +1,76 @@ +#ifndef __ELITE__MATH_UTILS_HPP__ +#define __ELITE__MATH_UTILS_HPP__ + +#include +#include + +namespace ELITE +{ + +namespace MATH_UTILS { + +/** + * @brief Check if a double value is zero within a minimum threshold + * + * @param value The double value to check + * @param minVal The minimum threshold to consider as zero (default is 1.0e-9) + * @return true if the value is considered zero + * @return false if the value is not zero + */ +inline bool isZero(double value, double minVal = 1.0e-9) { + return std::fabs(value) < minVal; +} + +/** + * @brief Convert a 6D vector (position + RPY) to a 4x4 pose matrix + * + * @param vt 6D vector (position + RPY) + * @return PoseMatrix 4x4 pose matrix + */ +PoseMatrix vector2Matrix(const vector6d_t& vt); + +/** + * @brief Convert a 4x4 pose matrix to a 6D vector (position + RPY) + * + * @param pm The 4x4 pose matrix + * @return vector6d_t The 6D vector (position + RPY) + */ +vector6d_t matrix2Vector(const PoseMatrix& pm); + +/** + * @brief Calculate the Euclidean distance between the positions of two poses + * + * @param pose1 pose 1 + * @param pose2 pose 2 + * @return double The Euclidean distance between the positions of the two poses + */ +double poseDistance(const vector6d_t& pose1, const vector6d_t& pose2); + +/** + * @brief Transform a local frame pose to a world frame pose + * + * T_world_target = T_world_local * T_local_target + * + * @param wp Local coordinate system values in the world coordinate system. + * @param lp Values of the target coordinate system in the local coordinate system. + * @return PoseMatrix/vector6d_t Transformed pose in the world coordinate system. + */ +PoseMatrix transLocalFrameToWorld(const PoseMatrix& wp, const PoseMatrix& lp); +vector6d_t transLocalFrameToWorld(const vector6d_t& wp, const vector6d_t& lp); + +/** + * @brief Transform a world frame pose to a local frame pose + * + * T_local_target = T_world_local_inv * T_world_target + * + * @param wp Local coordinate system values in the world coordinate system. + * @param lp Values of the target coordinate system in the world coordinate system. + * @return PoseMatrix/vector6d_t Transformed pose in the local coordinate system. + */ +PoseMatrix transWorldFrameToLocal(const PoseMatrix& wp, const PoseMatrix& lp); +vector6d_t transWorldFrameToLocal(const vector6d_t& wp, const vector6d_t& lp); + +} // namespace MATH_UTILS +} // namespace ELITE + +#endif // __ELITE__MATH_UTILS_HPP__ \ No newline at end of file diff --git a/include/Common/Utils.hpp b/include/Common/Utils.hpp index 0a50a91..2520359 100644 --- a/include/Common/Utils.hpp +++ b/include/Common/Utils.hpp @@ -5,5 +5,6 @@ #include #include #include +#include #endif \ No newline at end of file diff --git a/include/Elite/DataType.hpp b/include/Elite/DataType.hpp index de4db82..174ffc7 100644 --- a/include/Elite/DataType.hpp +++ b/include/Elite/DataType.hpp @@ -152,6 +152,19 @@ enum class FreedriveAction : int { FREEDRIVE_START = 1 }; +// 4x4 Pose Matrix +struct PoseMatrix { + std::array, 4> data{{ + {1.0, 0.0, 0.0, 0.0}, + {0.0, 1.0, 0.0, 0.0}, + {0.0, 0.0, 1.0, 0.0}, + {0.0, 0.0, 0.0, 1.0} + }}; + PoseMatrix operator*(const PoseMatrix& other) const; + PoseMatrix& operator*=(const PoseMatrix& other); + PoseMatrix inverse() const; +}; + using vector3d_t = std::array; using vector6d_t = std::array; using vector6int32_t = std::array; diff --git a/source/Common/MathUtils.cpp b/source/Common/MathUtils.cpp new file mode 100644 index 0000000..9f6898d --- /dev/null +++ b/source/Common/MathUtils.cpp @@ -0,0 +1,152 @@ +#include "MathUtils.hpp" +#include "DataType.hpp" +#include + +namespace ELITE { + +PoseMatrix PoseMatrix::operator*(const PoseMatrix& other) const { + PoseMatrix result; + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + result.data[i][j] = 0.0; + for (int k = 0; k < 4; ++k) { + result.data[i][j] += data[i][k] * other.data[k][j]; + } + } + } + return result; +} + +PoseMatrix& PoseMatrix::operator*=(const PoseMatrix& other) { + *this = *this * other; + return *this; +} + +PoseMatrix PoseMatrix::inverse() const { + PoseMatrix inv{}; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + inv.data[i][j] = data[j][i]; + } + } + inv.data[0][3] = -data[0][3] * data[0][0] - data[1][3] * data[1][0] - data[2][3] * data[2][0]; + inv.data[1][3] = -data[0][3] * data[0][1] - data[1][3] * data[1][1] - data[2][3] * data[2][1]; + inv.data[2][3] = -data[0][3] * data[0][2] - data[1][3] * data[1][2] - data[2][3] * data[2][2]; + inv.data[3][0] = 0.0; + inv.data[3][1] = 0.0; + inv.data[3][2] = 0.0; + inv.data[3][3] = 1.0; + return inv; +} + +namespace MATH_UTILS +{ + + +PoseMatrix vector2Matrix(const vector6d_t& vt) { + PoseMatrix matrix{}; + double s[3] = {0}; + double c[3] = {0}; + c[0] = std::cos(vt[3]); + c[1] = std::cos(vt[4]); + c[2] = std::cos(vt[5]); + s[0] = std::sin(vt[3]); + s[1] = std::sin(vt[4]); + s[2] = std::sin(vt[5]); + + matrix.data[0][0] = c[2] * c[1]; + matrix.data[0][1] = c[2] * s[1] * s[0] - s[2] * c[0]; + matrix.data[0][2] = c[2] * s[1] * c[0] + s[2] * s[0]; + matrix.data[0][3] = vt[0]; + + matrix.data[1][0] = s[2] * c[1]; + matrix.data[1][1] = s[2] * s[1] * s[0] + c[2] * c[0]; + matrix.data[1][2] = s[2] * s[1] * c[0] - c[2] * s[0]; + matrix.data[1][3] = vt[1]; + + matrix.data[2][0] = -s[1]; + matrix.data[2][1] = c[1] * s[0]; + matrix.data[2][2] = c[1] * c[0]; + matrix.data[2][3] = vt[2]; + + matrix.data[3][0] = 0; + matrix.data[3][1] = 0; + matrix.data[3][2] = 0; + matrix.data[3][3] = 1; + return matrix; +} + +vector6d_t matrix2Vector(const PoseMatrix& pm) { + double nx = 0.0, ny = 0.0, nz = 0.0, ox = 0.0, oy = 0.0, oz = 0.0, az = 0.0, px = 0.0, py = 0.0, pz = 0.0; + double r_y = 0.0; + double r_x = 0.0; + double r_z = 0.0; + + nx = pm.data[0][0]; + ny = pm.data[1][0]; + nz = pm.data[2][0]; + + ox = pm.data[0][1]; + oy = pm.data[1][1]; + oz = pm.data[2][1]; + + az = pm.data[2][2]; + + px = pm.data[0][3]; + py = pm.data[1][3]; + pz = pm.data[2][3]; + r_y = atan2(-nz, sqrt(nx * nx + ny * ny)); // beta + if (isZero(cos(r_y)) == 0) { + r_z = atan2(ny, nx); // alfa + r_x = atan2(oz, az); // gama + } else { + if (r_y > 0) { + r_z = 0; + r_x = atan2(ox, oy); + } else { + r_z = 0; + r_x = -atan2(ox, oy); + } + } + vector6d_t rpy{}; + rpy[0] = px; + rpy[1] = py; + rpy[2] = pz; + rpy[3] = r_x; + rpy[4] = r_y; + rpy[5] = r_z; + return rpy; +} + + +double poseDistance(const vector6d_t& pose1, const vector6d_t& pose2) { + double dx = pose1[0] - pose2[0]; + double dy = pose1[1] - pose2[1]; + double dz = pose1[2] - pose2[2]; + return std::sqrt(dx * dx + dy * dy + dz * dz); +} + +PoseMatrix transLocalFrameToWorld(const PoseMatrix& wp, const PoseMatrix& lp) { + return wp * lp; +} + +vector6d_t transLocalFrameToWorld(const vector6d_t& wp, const vector6d_t& lp) { + auto wp_mat = vector2Matrix(wp); + auto lp_mat = vector2Matrix(lp); + auto result_mat = wp_mat * lp_mat; + return matrix2Vector(result_mat); +} + +PoseMatrix transWorldFrameToLocal(const PoseMatrix& wp, const PoseMatrix& lp) { + return wp.inverse() * lp; +} + +vector6d_t transWorldFrameToLocal(const vector6d_t& wp, const vector6d_t& lp) { + auto wp_mat = vector2Matrix(wp); + auto lp_mat = vector2Matrix(lp); + auto result_mat = wp_mat.inverse() * lp_mat; + return matrix2Vector(result_mat); +} + +} // namespace MATH_UTILS +} // namespace ELITE diff --git a/source/Common/StringUtils.cpp b/source/Common/StringUtils.cpp index aa3820b..4af75de 100644 --- a/source/Common/StringUtils.cpp +++ b/source/Common/StringUtils.cpp @@ -1,4 +1,4 @@ -#include "Utils.hpp" +#include "StringUtils.hpp" #include #include