diff --git a/doc/pages/how_to_guides/installation.rst b/doc/pages/how_to_guides/installation.rst index cb0bc3a..0ffa9ca 100644 --- a/doc/pages/how_to_guides/installation.rst +++ b/doc/pages/how_to_guides/installation.rst @@ -45,6 +45,9 @@ Building sdu_controllers requires the following software installed: Optional dependencies --------------------- +.. role:: bash(code) + :language: bash + These dependencies are only required for specific features or examples: * Doxygen: Build the documentation (skipped automatically if missing). @@ -56,6 +59,11 @@ These dependencies are only required for specific features or examples: * typing_extensions (Python package): Required for stub generation when `BUILD_PYTHON_STUBS=ON`. * ur_rtde: Required for UR hardware examples (`BUILD_UR_EXAMPLES=ON`). + * Ubuntu install with: + * :bash:`sudo add-apt-repository ppa:sdurobotics/ur-rtde` + * :bash:`sudo apt update` + * :bash:`sudo apt install librtde librtde-dev`. + * Other platforms read documentation at https://sdurobotics.gitlab.io/ur_rtde/installation/installation.html * Python example requirements: Required for running Python hardware examples * install with `pip install -r hardware_examples/python/ur/requirements.txt`. diff --git a/include/sdu_controllers/kinematics/dh_kinematics.hpp b/include/sdu_controllers/kinematics/dh_kinematics.hpp index b8bf1fd..71c4e82 100644 --- a/include/sdu_controllers/kinematics/dh_kinematics.hpp +++ b/include/sdu_controllers/kinematics/dh_kinematics.hpp @@ -15,12 +15,20 @@ namespace sdu_controllers::kinematics bool is_joint_revolute; // true if the joint is revolute, false if prismatic }; + + /** + * @class DHKinematics + * @brief Forward kinematics computation using classical Denavit-Hartenberg (DH) parameters + * + * This class implements forward kinematics using the classical (standard) Denavit-Hartenberg + * parameter convention, as presented in standard robotics textbooks (Spong, Murray, Sastry). + */ class DHKinematics : public ForwardKinematics { public: /** - * @brief Construct a empty DHKinematics + * @brief Construct an empty DHKinematics */ DHKinematics(); diff --git a/include/sdu_controllers/math/pose.hpp b/include/sdu_controllers/math/pose.hpp index 3f48812..81e0562 100644 --- a/include/sdu_controllers/math/pose.hpp +++ b/include/sdu_controllers/math/pose.hpp @@ -247,6 +247,14 @@ namespace sdu_controllers::math orientation_.normalize(); } + + Pose inverse() const + { + Eigen::Quaterniond inv_orientation = orientation_.conjugate(); + Eigen::Vector3d inv_position = inv_orientation * (-position_); + return Pose(inv_position, inv_orientation); + } + /** * @brief Convert the orientation to Euler angles in the specified order. * @@ -412,6 +420,121 @@ namespace sdu_controllers::math return *this; } + /** + * @brief Compose this pose with another pose (transformation composition). + * + * Performs the transformation composition T_result = T_this * T_other. + * This is equivalent to applying this pose's transformation first, then the other pose's. + * + * @param other The pose to compose with + * @return The resulting composed pose + */ + Pose operator*(const Pose& other) const + { + // Compose directly using quaternions for efficiency + Eigen::Vector3d new_position = orientation_ * other.position_ + position_; + Eigen::Quaterniond new_orientation = orientation_ * other.orientation_; + return Pose(new_position, new_orientation); + } + + /** + * @brief Compose this pose with a transformation matrix. + * + * Performs the transformation composition T_result = T_this * T_transform. + * + * @param transform The Affine3d transformation to compose with + * @return The resulting composed pose + */ + Pose operator*(const Eigen::Affine3d& transform) const + { + Eigen::Affine3d this_transform = to_transform(); + Eigen::Affine3d result_transform = this_transform * transform; + return Pose(result_transform); + } + + /** + * @brief Compose this pose with a 4x4 transformation matrix. + * + * Performs the transformation composition T_result = T_this * T_matrix. + * + * @param matrix The 4x4 transformation matrix to compose with + * @return The resulting composed pose + */ + Pose operator*(const Eigen::Matrix4d& matrix) const + { + Eigen::Affine3d this_transform = to_transform(); + Eigen::Affine3d other_transform(matrix); + Eigen::Affine3d result_transform = this_transform * other_transform; + return Pose(result_transform); + } + + /** + * @brief Transform a 3D point using this pose. + * + * Applies the rotation and translation of this pose to transform a point + * from the local frame to the global frame. + * + * @param point The 3D point to transform + * @return The transformed point + */ + Eigen::Vector3d operator*(const Eigen::Vector3d& point) const + { + return orientation_ * point + position_; + } + + /** + * @brief Apply a rotation matrix to this pose (left multiplication). + * + * Creates a new pose by rotating this pose's orientation and position by the given rotation matrix. + * This performs R * T where R is the rotation and T is this pose. + * + * @param rotation The 3x3 rotation matrix + * @param pose The pose to rotate + * @return The resulting rotated pose + */ + friend Pose operator*(const Eigen::Matrix3d& rotation, const Pose& pose) + { + Eigen::Vector3d new_position = rotation * pose.position_; + Eigen::Quaterniond rotation_quat(rotation); + Eigen::Quaterniond new_orientation = rotation_quat * pose.orientation_; + return Pose(new_position, new_orientation); + } + + /** + * @brief Apply a transformation matrix to a pose (left multiplication). + * + * Creates a new pose by composing the transformation with this pose. + * This performs T1 * T2 where T1 is the transform and T2 is this pose. + * + * @param transform The Affine3d transformation + * @param pose The pose to transform + * @return The resulting transformed pose + */ + friend Pose operator*(const Eigen::Affine3d& transform, const Pose& pose) + { + Eigen::Affine3d pose_transform = pose.to_transform(); + Eigen::Affine3d result_transform = transform * pose_transform; + return Pose(result_transform); + } + + /** + * @brief Apply a 4x4 transformation matrix to a pose (left multiplication). + * + * Creates a new pose by composing the transformation with this pose. + * This performs T1 * T2 where T1 is the matrix and T2 is this pose. + * + * @param matrix The 4x4 transformation matrix + * @param pose The pose to transform + * @return The resulting transformed pose + */ + friend Pose operator*(const Eigen::Matrix4d& matrix, const Pose& pose) + { + Eigen::Affine3d transform(matrix); + Eigen::Affine3d pose_transform = pose.to_transform(); + Eigen::Affine3d result_transform = transform * pose_transform; + return Pose(result_transform); + } + /** * @brief Format the pose as a string according to the specified format. * diff --git a/python/generate_stubs.py b/python/generate_stubs.py index 4c9d87e..456b83e 100755 --- a/python/generate_stubs.py +++ b/python/generate_stubs.py @@ -9,7 +9,7 @@ # Generate stubs for main module sg_main = StubGen(_sdu_controllers) sg_main.put(_sdu_controllers) -main_stubs = sg_main.get().replace("_sdu_robotics", "sdu_robotics") +main_stubs = sg_main.get().replace("_sdu_controllers", "sdu_controllers") # Try to generate stubs for submodules submodules = {} @@ -23,7 +23,7 @@ try: sg = StubGen(attr) sg.put(attr) - submodules[attr_name] = sg.get().replace("_sdu_robotics", "sdu_robotics") + submodules[attr_name] = sg.get().replace("_sdu_controllers", "sdu_controllers") except Exception as e: print(f" Warning: Could not generate stubs for {attr_name}: {e}") except Exception as e: diff --git a/python/sdu_controllers/_math.cpp b/python/sdu_controllers/_math.cpp index a608da5..7d0a673 100644 --- a/python/sdu_controllers/_math.cpp +++ b/python/sdu_controllers/_math.cpp @@ -25,6 +25,15 @@ namespace sdu_controllers nb::module_ m = main_module.def_submodule("math", "Submodule containing math utilities."); m.doc() = "Python bindings for sdu_controllers math utilities."; + // PoseFormat enum + nb::enum_(m, "PoseFormat") + .value("Default", math::PoseFormat::Default, "Basic format: Position [x, y, z], Orientation [w, x, y, z]") + .value("Compact", math::PoseFormat::Compact, "Compact format: [x, y, z, qw, qx, qy, qz]") + .value("Verbose", math::PoseFormat::Verbose, "Verbose format with labels and separate lines") + .value("Euler", math::PoseFormat::Euler, "Position and Euler angles (default ZYZ)") + .value("AngleAxis", math::PoseFormat::AngleAxis, "Position and angle-axis representation") + .value("Matrix", math::PoseFormat::Matrix, "Full 4x4 transformation matrix"); + // Pose utilities nb::class_(m, "Pose") .def(nb::init<>(), "Default pose (zero position, identity rotation).") @@ -48,6 +57,7 @@ namespace sdu_controllers .def("set_position", &math::Pose::set_position, nb::arg("position"), "Set position.") .def("get_orientation", &math::Pose::get_orientation, "Get orientation as quaternion (w, x, y, z).") .def("set_orientation", &math::Pose::set_orientation, nb::arg("orientation"), "Set orientation quaternion.") + .def("inverse", &math::Pose::inverse, "Compute the inverse of this pose.") .def( "to_euler_angles", &math::Pose::to_euler_angles, @@ -59,9 +69,37 @@ namespace sdu_controllers "Return orientation as angle-axis vector (axis * angle).") .def("to_vector7d", &math::Pose::to_vector7d, "Return [x, y, z, qw, qx, qy, qz].") .def("to_std_vector", &math::Pose::to_std_vector, "Return pose as std::vector of length 7.") - .def("to_transform", &math::Pose::to_transform, "Return pose as homogeneous transform Affine3d.") + .def("to_transform", [](const math::Pose& p) { return p.to_transform().matrix(); }, "Return pose as homogeneous transform 4x4 matrix.") .def("to_pose6d_std", &math::Pose::to_pose6d_std, "Return pose as 6D std::vector [X, Y, Z, Rx, Ry, Rz] with angle-axis orientation.") - .def("to_pose6d_eigen", &math::Pose::to_pose6d_eigen, "Return pose as 6D Eigen::VectorXd [X, Y, Z, Rx, Ry, Rz] with angle-axis orientation."); + .def("to_pose6d_eigen", &math::Pose::to_pose6d_eigen, "Return pose as 6D Eigen::VectorXd [X, Y, Z, Rx, Ry, Rz] with angle-axis orientation.") + .def( + "to_string", + &math::Pose::to_string, + nb::arg("format") = math::PoseFormat::Default, + nb::arg("precision") = 6, + "Format the pose as a string.") + .def("__repr__", [](const math::Pose& p) { return p.to_string(math::PoseFormat::Compact); }) + .def("__str__", [](const math::Pose& p) { return p.to_string(math::PoseFormat::Default); }) + .def( + "__mul__", + [](const math::Pose& self, const math::Pose& other) { return self * other; }, + nb::arg("other"), + "Compose this pose with another pose.") + .def( + "__mul__", + [](const math::Pose& self, const Eigen::Affine3d& transform) { return self * transform; }, + nb::arg("transform"), + "Compose this pose with an Affine3d transformation.") + .def( + "__mul__", + [](const math::Pose& self, const Eigen::Matrix4d& matrix) { return self * matrix; }, + nb::arg("matrix"), + "Compose this pose with a 4x4 transformation matrix.") + .def( + "__mul__", + [](const math::Pose& self, const Eigen::Vector3d& point) { return self * point; }, + nb::arg("point"), + "Transform a 3D point using this pose."); // Core dynamics helpers nb::class_(m, "InverseDynamics") diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index c46e408..d61f5fc 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -8,5 +8,10 @@ target_link_libraries(rnea_test PUBLIC sdu_controllers Catch2::Catch2) add_executable(ur5e_inv_dyn_comparison ur5e_inv_dyn_comparison.cpp) target_link_libraries(ur5e_inv_dyn_comparison PUBLIC sdu_controllers Catch2::Catch2) +add_executable(pose_test pose_test.cpp) +target_link_libraries(pose_test PUBLIC sdu_controllers Catch2::Catch2) +target_compile_definitions(pose_test PRIVATE PROJECT_SOURCE_DIR="${CMAKE_SOURCE_DIR}") + # allow user to run tests with `make test` or `ctest` catch_discover_tests(tests) +catch_discover_tests(pose_test) diff --git a/tests/pose_test.cpp b/tests/pose_test.cpp new file mode 100644 index 0000000..9bbe2f2 --- /dev/null +++ b/tests/pose_test.cpp @@ -0,0 +1,570 @@ +#define CATCH_CONFIG_MAIN +#include +#include +#include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + + +using namespace sdu_controllers::math; + +// Helper function to compare doubles with tolerance +bool approx_equal(double a, double b, double tolerance = 1e-10) +{ + return std::abs(a - b) < tolerance; +} + +// Helper function to compare Vector3d with tolerance +bool approx_equal(const Eigen::Vector3d& a, const Eigen::Vector3d& b, double tolerance = 1e-10) +{ + return (a - b).norm() < tolerance; +} + +// Helper function to compare Quaternions with tolerance +bool approx_equal(const Eigen::Quaterniond& a, const Eigen::Quaterniond& b, double tolerance = 1e-10) +{ + // Quaternions q and -q represent the same rotation + double dot = a.w() * b.w() + a.x() * b.x() + a.y() * b.y() + a.z() * b.z(); + return std::abs(std::abs(dot) - 1.0) < tolerance; +} + +TEST_CASE("Pose: Default constructor creates identity pose", "[pose][constructor]") +{ + Pose pose; + + REQUIRE(approx_equal(pose.get_position(), Eigen::Vector3d::Zero())); + REQUIRE(approx_equal(pose.get_orientation(), Eigen::Quaterniond::Identity())); +} + +TEST_CASE("Pose: Constructor from position and quaternion", "[pose][constructor]") +{ + Eigen::Vector3d pos(1.0, 2.0, 3.0); + Eigen::Quaterniond quat(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + + Pose pose(pos, quat); + + REQUIRE(approx_equal(pose.get_position(), pos)); + REQUIRE(approx_equal(pose.get_orientation(), quat)); +} + +TEST_CASE("Pose: Constructor from position and angle-axis vector", "[pose][constructor]") +{ + Eigen::Vector3d pos(1.0, 2.0, 3.0); + Eigen::Vector3d angle_axis(0.0, 0.0, M_PI / 4); + + Pose pose(pos, angle_axis); + + Eigen::Quaterniond expected_quat(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + + REQUIRE(approx_equal(pose.get_position(), pos)); + REQUIRE(approx_equal(pose.get_orientation(), expected_quat)); +} + +TEST_CASE("Pose: Constructor from 6D vector (rotation vector)", "[pose][constructor]") +{ + std::vector pose_vec = {1.0, 2.0, 3.0, 0.0, 0.0, M_PI / 4}; + + Pose pose(pose_vec); + + Eigen::Vector3d expected_pos(1.0, 2.0, 3.0); + Eigen::Quaterniond expected_quat(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + + REQUIRE(approx_equal(pose.get_position(), expected_pos)); + REQUIRE(approx_equal(pose.get_orientation(), expected_quat)); +} + +TEST_CASE("Pose: Constructor from 7D vector (quaternion)", "[pose][constructor]") +{ + Eigen::Quaterniond quat(Eigen::AngleAxisd(M_PI / 3, Eigen::Vector3d::UnitX())); + std::vector pose_vec = {1.0, 2.0, 3.0, quat.w(), quat.x(), quat.y(), quat.z()}; + + Pose pose(pose_vec); + + Eigen::Vector3d expected_pos(1.0, 2.0, 3.0); + + REQUIRE(approx_equal(pose.get_position(), expected_pos)); + REQUIRE(approx_equal(pose.get_orientation(), quat)); +} + +TEST_CASE("Pose: Constructor from Eigen::VectorXd", "[pose][constructor]") +{ + Eigen::VectorXd pose_vec(6); + pose_vec << 1.0, 2.0, 3.0, 0.0, M_PI / 4, 0.0; + + Pose pose(pose_vec); + + Eigen::Vector3d expected_pos(1.0, 2.0, 3.0); + Eigen::Quaterniond expected_quat(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY())); + + REQUIRE(approx_equal(pose.get_position(), expected_pos)); + REQUIRE(approx_equal(pose.get_orientation(), expected_quat)); +} + +TEST_CASE("Pose: Constructor from std::array", "[pose][constructor]") +{ + Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); + std::array pose_array = {1.0, 2.0, 3.0, quat.w(), quat.x(), quat.y(), quat.z()}; + + Pose pose(pose_array); + + REQUIRE(approx_equal(pose.get_position(), Eigen::Vector3d(1.0, 2.0, 3.0))); + REQUIRE(approx_equal(pose.get_orientation(), quat)); +} + +TEST_CASE("Pose: Constructor from Affine3d transformation", "[pose][constructor]") +{ + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + transform.translate(Eigen::Vector3d(1.0, 2.0, 3.0)); + transform.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + + Pose pose(transform); + + REQUIRE(approx_equal(pose.get_position(), Eigen::Vector3d(1.0, 2.0, 3.0))); + REQUIRE(approx_equal(pose.get_orientation(), Eigen::Quaterniond(transform.rotation()))); +} + +TEST_CASE("Pose: Constructor from Matrix4d", "[pose][constructor]") +{ + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + transform.translate(Eigen::Vector3d(1.0, 2.0, 3.0)); + transform.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + + Eigen::Matrix4d matrix = transform.matrix(); + Pose pose(matrix); + + REQUIRE(approx_equal(pose.get_position(), Eigen::Vector3d(1.0, 2.0, 3.0))); + REQUIRE(approx_equal(pose.get_orientation(), Eigen::Quaterniond(transform.rotation()))); +} + +TEST_CASE("Pose: Copy constructor", "[pose][constructor]") +{ + Pose original(Eigen::Vector3d(1.0, 2.0, 3.0), + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ()))); + + Pose copy(original); + + REQUIRE(approx_equal(copy.get_position(), original.get_position())); + REQUIRE(approx_equal(copy.get_orientation(), original.get_orientation())); +} + +TEST_CASE("Pose: Setters modify pose correctly", "[pose][setters]") +{ + Pose pose; + + Eigen::Vector3d new_pos(5.0, 6.0, 7.0); + pose.set_position(new_pos); + REQUIRE(approx_equal(pose.get_position(), new_pos)); + + Eigen::Quaterniond new_quat(Eigen::AngleAxisd(M_PI / 3, Eigen::Vector3d::UnitX())); + pose.set_orientation(new_quat); + REQUIRE(approx_equal(pose.get_orientation(), new_quat)); +} + +TEST_CASE("Pose: to_vector7d conversion", "[pose][conversion]") +{ + Eigen::Vector3d pos(1.0, 2.0, 3.0); + Eigen::Quaterniond quat(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + Pose pose(pos, quat); + + Eigen::Matrix vec = pose.to_vector7d(); + + REQUIRE(approx_equal(vec(0), pos.x())); + REQUIRE(approx_equal(vec(1), pos.y())); + REQUIRE(approx_equal(vec(2), pos.z())); + REQUIRE(approx_equal(vec(3), quat.w())); + REQUIRE(approx_equal(vec(4), quat.x())); + REQUIRE(approx_equal(vec(5), quat.y())); + REQUIRE(approx_equal(vec(6), quat.z())); +} + +TEST_CASE("Pose: to_std_vector conversion", "[pose][conversion]") +{ + Eigen::Vector3d pos(1.0, 2.0, 3.0); + Eigen::Quaterniond quat(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + Pose pose(pos, quat); + + std::vector vec = pose.to_std_vector(); + + REQUIRE(vec.size() == 7); + REQUIRE(approx_equal(vec[0], pos.x())); + REQUIRE(approx_equal(vec[1], pos.y())); + REQUIRE(approx_equal(vec[2], pos.z())); + REQUIRE(approx_equal(vec[3], quat.w())); + REQUIRE(approx_equal(vec[4], quat.x())); + REQUIRE(approx_equal(vec[5], quat.y())); + REQUIRE(approx_equal(vec[6], quat.z())); +} + +TEST_CASE("Pose: to_transform conversion", "[pose][conversion]") +{ + Eigen::Vector3d pos(1.0, 2.0, 3.0); + Eigen::Quaterniond quat(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + Pose pose(pos, quat); + + Eigen::Affine3d transform = pose.to_transform(); + + REQUIRE(approx_equal(transform.translation(), pos)); + REQUIRE(approx_equal(Eigen::Quaterniond(transform.rotation()), quat)); +} + +TEST_CASE("Pose: to_angle_axis_vector conversion", "[pose][conversion]") +{ + double angle = M_PI / 4; + Eigen::Vector3d axis = Eigen::Vector3d::UnitZ(); + Eigen::Quaterniond quat(Eigen::AngleAxisd(angle, axis)); + Pose pose(Eigen::Vector3d::Zero(), quat); + + Eigen::Vector3d aa_vec = pose.to_angle_axis_vector(); + + REQUIRE(approx_equal(aa_vec.norm(), angle, 1e-6)); + REQUIRE(approx_equal(aa_vec.normalized(), axis, 1e-6)); +} + +TEST_CASE("Pose: to_pose6d conversions", "[pose][conversion]") +{ + Eigen::Vector3d pos(1.0, 2.0, 3.0); + double angle = M_PI / 4; + Eigen::Vector3d axis = Eigen::Vector3d::UnitZ(); + Eigen::Quaterniond quat(Eigen::AngleAxisd(angle, axis)); + Pose pose(pos, quat); + + SECTION("to_pose6d_std") + { + std::vector vec = pose.to_pose6d_std(); + REQUIRE(vec.size() == 6); + REQUIRE(approx_equal(vec[0], pos.x())); + REQUIRE(approx_equal(vec[1], pos.y())); + REQUIRE(approx_equal(vec[2], pos.z())); + + Eigen::Vector3d rotation_vec(vec[3], vec[4], vec[5]); + REQUIRE(approx_equal(rotation_vec.norm(), angle, 1e-6)); + } + + SECTION("to_pose6d_eigen") + { + Eigen::VectorXd vec = pose.to_pose6d_eigen(); + REQUIRE(vec.size() == 6); + REQUIRE(approx_equal(vec(0), pos.x())); + REQUIRE(approx_equal(vec(1), pos.y())); + REQUIRE(approx_equal(vec(2), pos.z())); + + Eigen::Vector3d rotation_vec = vec.tail<3>(); + REQUIRE(approx_equal(rotation_vec.norm(), angle, 1e-6)); + } +} + +TEST_CASE("Pose: Implicit conversion to Affine3d", "[pose][conversion]") +{ + Eigen::Vector3d pos(1.0, 2.0, 3.0); + Eigen::Quaterniond quat(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + Pose pose(pos, quat); + + Eigen::Affine3d transform = pose; // implicit conversion + + REQUIRE(approx_equal(transform.translation(), pos)); + REQUIRE(approx_equal(Eigen::Quaterniond(transform.rotation()), quat)); +} + +TEST_CASE("Pose: Assignment operator from Pose", "[pose][operators]") +{ + Pose pose1(Eigen::Vector3d(1.0, 2.0, 3.0), + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ()))); + Pose pose2; + + pose2 = pose1; + + REQUIRE(approx_equal(pose2.get_position(), pose1.get_position())); + REQUIRE(approx_equal(pose2.get_orientation(), pose1.get_orientation())); +} + +TEST_CASE("Pose: Assignment operator from vector", "[pose][operators]") +{ + Pose pose; + std::vector vec = {1.0, 2.0, 3.0, 0.0, 0.0, M_PI / 4}; + + pose = vec; + + Eigen::Vector3d expected_pos(1.0, 2.0, 3.0); + Eigen::Quaterniond expected_quat(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + + REQUIRE(approx_equal(pose.get_position(), expected_pos)); + REQUIRE(approx_equal(pose.get_orientation(), expected_quat)); +} + +TEST_CASE("Pose: Multiplication operator Pose * Pose", "[pose][operators][multiplication]") +{ + // Create two poses + Pose pose1(Eigen::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()))); + Pose pose2(Eigen::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()))); + + Pose result = pose1 * pose2; + + // After 90-degree rotation, (1,0,0) becomes (0,1,0), then add (1,0,0) + Eigen::Vector3d expected_pos(1.0, 1.0, 0.0); + Eigen::Quaterniond expected_quat(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())); + + REQUIRE(approx_equal(result.get_position(), expected_pos, 1e-6)); + REQUIRE(approx_equal(result.get_orientation(), expected_quat, 1e-6)); +} + +TEST_CASE("Pose: Multiplication operator Pose * Vector3d", "[pose][operators][multiplication]") +{ + // 90-degree rotation around Z-axis and translation + Pose pose(Eigen::Vector3d(1.0, 2.0, 3.0), + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()))); + + Eigen::Vector3d point(1.0, 0.0, 0.0); + Eigen::Vector3d transformed = pose * point; + + // (1,0,0) rotated 90° around Z becomes (0,1,0), then add translation + Eigen::Vector3d expected(1.0, 3.0, 3.0); + + REQUIRE(approx_equal(transformed, expected, 1e-6)); +} + +TEST_CASE("Pose: Multiplication operator Pose * Affine3d", "[pose][operators][multiplication]") +{ + Pose pose(Eigen::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()))); + + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + transform.translate(Eigen::Vector3d(1.0, 0.0, 0.0)); + transform.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + + Pose result = pose * transform; + + Eigen::Vector3d expected_pos(1.0, 1.0, 0.0); + Eigen::Quaterniond expected_quat(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())); + + REQUIRE(approx_equal(result.get_position(), expected_pos, 1e-6)); + REQUIRE(approx_equal(result.get_orientation(), expected_quat, 1e-6)); +} + +TEST_CASE("Pose: Multiplication operator Pose * Matrix4d", "[pose][operators][multiplication]") +{ + Pose pose(Eigen::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()))); + + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + transform.translate(Eigen::Vector3d(1.0, 0.0, 0.0)); + transform.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + + Eigen::Matrix4d matrix = transform.matrix(); + Pose result = pose * matrix; + + Eigen::Vector3d expected_pos(1.0, 1.0, 0.0); + Eigen::Quaterniond expected_quat(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())); + + REQUIRE(approx_equal(result.get_position(), expected_pos, 1e-6)); + REQUIRE(approx_equal(result.get_orientation(), expected_quat, 1e-6)); +} + +TEST_CASE("Pose: Left multiplication Matrix3d * Pose", "[pose][operators][multiplication]") +{ + Pose pose(Eigen::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond::Identity()); + + Eigen::Matrix3d rotation = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + + Pose result = rotation * pose; + + // Rotation should affect both position and orientation + Eigen::Vector3d expected_pos(0.0, 1.0, 0.0); + Eigen::Quaterniond expected_quat(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + + REQUIRE(approx_equal(result.get_position(), expected_pos, 1e-6)); + REQUIRE(approx_equal(result.get_orientation(), expected_quat, 1e-6)); +} + +TEST_CASE("Pose: Left multiplication Affine3d * Pose", "[pose][operators][multiplication]") +{ + Pose pose(Eigen::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond::Identity()); + + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + transform.translate(Eigen::Vector3d(1.0, 2.0, 3.0)); + transform.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + + Pose result = transform * pose; + + // (1,0,0) rotated 90° becomes (0,1,0), then add (1,2,3) + Eigen::Vector3d expected_pos(1.0, 3.0, 3.0); + Eigen::Quaterniond expected_quat(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + + REQUIRE(approx_equal(result.get_position(), expected_pos, 1e-6)); + REQUIRE(approx_equal(result.get_orientation(), expected_quat, 1e-6)); +} + +TEST_CASE("Pose: Left multiplication Matrix4d * Pose", "[pose][operators][multiplication]") +{ + Pose pose(Eigen::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond::Identity()); + + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + transform.translate(Eigen::Vector3d(1.0, 2.0, 3.0)); + transform.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + + Eigen::Matrix4d matrix = transform.matrix(); + Pose result = matrix * pose; + + Eigen::Vector3d expected_pos(1.0, 3.0, 3.0); + Eigen::Quaterniond expected_quat(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + + REQUIRE(approx_equal(result.get_position(), expected_pos, 1e-6)); + REQUIRE(approx_equal(result.get_orientation(), expected_quat, 1e-6)); +} + +TEST_CASE("Pose: Composition with identity pose", "[pose][operators]") +{ + Pose pose(Eigen::Vector3d(1.0, 2.0, 3.0), + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ()))); + Pose identity; + + Pose result1 = pose * identity; + Pose result2 = identity * pose; + + REQUIRE(approx_equal(result1.get_position(), pose.get_position(), 1e-6)); + REQUIRE(approx_equal(result1.get_orientation(), pose.get_orientation(), 1e-6)); + + REQUIRE(approx_equal(result2.get_position(), pose.get_position(), 1e-6)); + REQUIRE(approx_equal(result2.get_orientation(), pose.get_orientation(), 1e-6)); +} + +TEST_CASE("Pose: Round-trip conversions", "[pose][conversion]") +{ + Eigen::Vector3d original_pos(1.0, 2.0, 3.0); + Eigen::Quaterniond original_quat(Eigen::AngleAxisd(M_PI / 3, Eigen::Vector3d(1, 1, 1).normalized())); + Pose original(original_pos, original_quat); + + SECTION("Through to_transform") + { + Eigen::Affine3d transform = original.to_transform(); + Pose reconstructed(transform); + + REQUIRE(approx_equal(reconstructed.get_position(), original_pos, 1e-6)); + REQUIRE(approx_equal(reconstructed.get_orientation(), original_quat, 1e-6)); + } + + SECTION("Through to_std_vector") + { + std::vector vec = original.to_std_vector(); + Pose reconstructed(vec); + + REQUIRE(approx_equal(reconstructed.get_position(), original_pos, 1e-6)); + REQUIRE(approx_equal(reconstructed.get_orientation(), original_quat, 1e-6)); + } + + SECTION("Through to_pose6d_std") + { + std::vector vec = original.to_pose6d_std(); + Pose reconstructed(vec); + + REQUIRE(approx_equal(reconstructed.get_position(), original_pos, 1e-6)); + REQUIRE(approx_equal(reconstructed.get_orientation(), original_quat, 1e-6)); + } +} + +TEST_CASE("Pose: Euler angle conversion", "[pose][euler]") +{ + // Create a pose with a known rotation + double angle = M_PI / 4; + Eigen::Quaterniond quat(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ())); + Pose pose(Eigen::Vector3d::Zero(), quat); + + Eigen::Vector3d euler_zyz = pose.to_euler_angles("ZYZ"); + + // Reconstruct quaternion from Euler angles + Eigen::Matrix3d rotation = (Eigen::AngleAxisd(euler_zyz(0), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(euler_zyz(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(euler_zyz(2), Eigen::Vector3d::UnitZ())).toRotationMatrix(); + Eigen::Quaterniond reconstructed_quat(rotation); + + REQUIRE(approx_equal(quat, reconstructed_quat, 1e-6)); +} + +TEST_CASE("Pose: String representation formats", "[pose][string]") +{ + Eigen::Vector3d pos(1.0, 2.0, 3.0); + Eigen::Quaterniond quat(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + Pose pose(pos, quat); + + SECTION("Default format") + { + std::string str = pose.to_string(PoseFormat::Default); + REQUIRE(str.find("Position") != std::string::npos); + REQUIRE(str.find("Orientation") != std::string::npos); + } + + SECTION("Compact format") + { + std::string str = pose.to_string(PoseFormat::Compact); + REQUIRE(str.find("[") != std::string::npos); + REQUIRE(str.find("]") != std::string::npos); + } + + SECTION("Verbose format") + { + std::string str = pose.to_string(PoseFormat::Verbose); + REQUIRE(str.find("Pose:") != std::string::npos); + REQUIRE(str.find("x:") != std::string::npos); + } + + SECTION("Euler format") + { + std::string str = pose.to_string(PoseFormat::Euler); + REQUIRE(str.find("Euler") != std::string::npos); + } + + SECTION("AngleAxis format") + { + std::string str = pose.to_string(PoseFormat::AngleAxis); + REQUIRE(str.find("Angle-Axis") != std::string::npos); + } + + SECTION("Matrix format") + { + std::string str = pose.to_string(PoseFormat::Matrix); + REQUIRE(str.find("Transform Matrix") != std::string::npos); + } +} + +TEST_CASE("Pose: Edge case - zero rotation vector", "[pose][edge_cases]") +{ + std::vector pose_vec = {1.0, 2.0, 3.0, 0.0, 0.0, 0.0}; + Pose pose(pose_vec); + + REQUIRE(approx_equal(pose.get_position(), Eigen::Vector3d(1.0, 2.0, 3.0))); + REQUIRE(approx_equal(pose.get_orientation(), Eigen::Quaterniond::Identity())); +} + +TEST_CASE("Pose: Edge case - very small rotation vector", "[pose][edge_cases]") +{ + std::vector pose_vec = {1.0, 2.0, 3.0, 1e-12, 1e-12, 1e-12}; + Pose pose(pose_vec); + + REQUIRE(approx_equal(pose.get_position(), Eigen::Vector3d(1.0, 2.0, 3.0))); + REQUIRE(approx_equal(pose.get_orientation(), Eigen::Quaterniond::Identity())); +} + +TEST_CASE("Pose: Chain of transformations", "[pose][integration]") +{ + // Create a chain: translate(1,0,0) -> rotate(90°,Z) -> translate(0,1,0) -> rotate(90°,Z) + Pose pose1(Eigen::Vector3d(1.0, 0.0, 0.0), Eigen::Quaterniond::Identity()); + Pose pose2(Eigen::Vector3d::Zero(), + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()))); + Pose pose3(Eigen::Vector3d(0.0, 1.0, 0.0), Eigen::Quaterniond::Identity()); + Pose pose4(Eigen::Vector3d::Zero(), + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()))); + + Pose result = pose1 * pose2 * pose3 * pose4; + + // Verify the result makes sense + REQUIRE(result.get_position().norm() > 0); + REQUIRE(approx_equal(result.get_orientation().norm(), 1.0, 1e-6)); +}