Pose math upgrade

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`.
 

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();
 

include/sdu_controllers/math/pose.hpp

@@ -247,6 +247,14 @@
       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 @@
       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.
      *

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:

python/sdu_controllers/_math.cpp

@@ -25,6 +25,15 @@
     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_<math::PoseFormat>(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_<math::Pose>(m, "Pose")
         .def(nb::init<>(), "Default pose (zero position, identity rotation).")
@@ -48,6 +57,7 @@
         .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 @@
             "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_<math::InverseDynamics>(m, "InverseDynamics")

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)