regressor model update

CMakeLists.txt

@@ -41,7 +41,7 @@ include(CMakePrintHelpers)
 cmake_print_variables(CONFIG_FILE_PATH_NORMALIZED)
 
 # find dependencies
-find_package(Eigen3 3.4 REQUIRED NO_MODULE)
+find_package(Eigen3 REQUIRED NO_MODULE)
 # Eigen: Disable fast math be default.
 add_definitions(-DEIGEN_FAST_MATH=0)
 
@@ -98,7 +98,8 @@ if(BUILD_PYTHON)
 
   # Compile the nanobind module
   nanobind_add_module(_sdu_controllers
-                      python/sdu_controllers/_sdu_controllers.cpp)
+                      python/sdu_controllers/_sdu_controllers.cpp
+                      python/sdu_controllers/_robot_models.cpp)
   target_link_libraries(_sdu_controllers PUBLIC sdu_controllers)
 
   # Install the Python module shared library

include/sdu_controllers/kinematics/forward_kinematics.hpp

@@ -45,6 +45,16 @@ namespace sdu_controllers::kinematics
      */
     virtual Eigen::Matrix<double, 6, Eigen::Dynamic> geometric_jacobian(const Eigen::VectorXd& q) const;
 
+    /**
+     * @brief Compute the geometric Jacobian at the given joint configuration using precomputed forward kinematics matrices
+     * @param q Joint configuration
+     * @param fk_matrices Precomputed forward kinematics matrices for each joint
+     * @return The 6xDOF geometric Jacobian matrix
+     */
+    virtual Eigen::Matrix<double, 6, Eigen::Dynamic> geometric_jacobian(
+        const Eigen::VectorXd& q,
+        const std::vector<Eigen::Matrix4d>& fk_matrices) const;
+
     /**
      * @brief Get the degrees of freedom of the kinematic chain
      * @return Number of degrees of freedom

include/sdu_controllers/models/parameter_robot_model.hpp

@@ -122,8 +122,6 @@ namespace sdu_controllers::models
 
     std::vector<double> get_m() override;
 
-    std::vector<bool> get_is_joint_revolute() override;
-
     Eigen::Vector3d get_g0() override;
 
     Eigen::Matrix<double, Eigen::Dynamic, 3> get_CoM() override;

include/sdu_controllers/models/regressor_robot_model.hpp

@@ -10,13 +10,54 @@
 
 namespace sdu_controllers::models
 {
+  /**
+   * @brief Regressor-based robot model for dynamics computation.
+   * This class implements a robot model using regressor matrices to compute dynamics.
+   * It inherits from the RobotModel base class and provides implementations for inverse and forward dynamics,
+   * as well as methods to retrieve inertia, coriolis, gravity, jacobian, and jacobian dot matrices.
+   *
+   * This class is an abstract base class and requires derived classes to implement methods for obtaining
+   * the regressor matrices and friction regressor matrices, as well as other robot-specific parameters, such as joint
+   * limits.
+   */
   class RegressorRobotModel : public sdu_controllers::models::RobotModel
   {
    public:
+    /**
+     * @brief Construct a new Regressor Robot Model.
+     *
+     * @param fkModel shared pointer to forward kinematics solver
+     * @param g0 gravity vector in base frame
+     * @throws std::invalid_argument if fkModel is nullptr
+     */
     RegressorRobotModel(
         const std::shared_ptr<kinematics::ForwardKinematics>& fkModel,
         const Eigen::Vector3d& g0 = Eigen::Vector3d(0, 0, -9.81));
 
+    /**
+     * @brief Calculate the inverse dynamics.
+     * @param q robot joint positions.
+     * @param dq robot joint velocities.
+     * @param ddq robot joint accelerations.
+     * @param he end-effector wrench.
+     * @returns the computed torques for the joint actuators \f$ \tau \f$
+     */
+    virtual Eigen::VectorXd inverse_dynamics(
+        const Eigen::VectorXd& q,
+        const Eigen::VectorXd& dq,
+        const Eigen::VectorXd& ddq,
+        const Eigen::VectorXd& he) override;
+
+    /**
+     * @brief Calculate the forward dynamics.
+     * @param q robot joint positions.
+     * @param dq robot joint velocities.
+     * @param tau joint torques of the robot
+     * @returns the acceleration \f$ \ddot{q} \f$
+     */
+    virtual Eigen::VectorXd forward_dynamics(const Eigen::VectorXd& q, const Eigen::VectorXd& dq, const Eigen::VectorXd& tau)
+        override;
+
     /**
      * @brief Get inertia matrix \f$ \mathbf{B}(q) \f$
      * @param q the robot joint configuration.
@@ -51,6 +92,64 @@
      */
     virtual Eigen::MatrixXd get_jacobian_dot(const Eigen::VectorXd& q, const Eigen::VectorXd& dq) override;
 
+    /**
+     * @brief Get the degrees of freedom of the robot.
+     * @returns the number of degrees of freedom
+     */
+    virtual uint16_t get_dof() const override;
+
+    /**
+     * @brief Get the masses of the robot links.
+     * @note Assumes that the masses are part of the dynamic parameters used in the regressor.
+     * it will therefore just throw an error if called.
+     * @throws std::runtime_error if called.
+     * @returns vector containing the mass of each link
+     */
+    virtual std::vector<double> get_m() override;
+
+    /**
+     * @brief Get the gravity vector in base frame.
+     * @returns the 3D gravity vector
+     */
+    virtual Eigen::Vector3d get_g0() override;
+
+    /**
+     * @brief Get the center of mass positions for each link.
+     * @note Assumes that the CoM positions are part of the dynamic parameters used in the regressor.
+     * it will therefore just throw an error if called.
+     * @throws std::runtime_error if called.
+     * @returns matrix where each row contains the 3D center of mass position for a link
+     */
+    virtual Eigen::Matrix<double, Eigen::Dynamic, 3> get_CoM() override;
+
+    /**
+     * @brief Get the inertia tensors for each link.
+     * @note Assumes that the inertia tensors are part of the dynamic parameters used in the regressor.
+     * it will therefore just throw an error if called.
+     * @throws std::runtime_error if called.
+     * @returns vector of 3x3 inertia matrices for each link
+     */
+    virtual std::vector<Eigen::Matrix3d> get_link_inertia() override;
+
+    /**
+     * @brief Get the friction regressor matrix \f$ \mathbf{Y_f(dq)} \f$
+     * This function is alternative version taking std::vector<double> as input.
+     * @see get_friction_regressor(const Eigen::VectorXd& qd)
+     * @param qd [in] Joint velocities
+     * @return The friction regressor matrix
+     */
+    virtual Eigen::MatrixXd get_friction_regressor(const std::vector<double>& qd) const;
+
+    /**
+     * @brief Get the forward kinematics solver instance.
+     * @returns reference to the forward kinematics solver
+     */
+    virtual const kinematics::ForwardKinematics& get_fk_solver() const override;
+
+    // ################################################
+    // # Functions to be overridden by child classes  #
+    // ################################################
+
     /**
      * @brief Get joint position bounds.
      * @returns the joint position bounds
@@ -61,45 +160,54 @@
      * @brief Get maximum joint velocity.
      * @returns the maximum joint velocity
      */
-    virtual Eigen::VectorXd get_joint_max_vel() override;
+    virtual Eigen::VectorXd get_joint_max_vel() override = 0;
 
     /**
      * @brief Get maximum joint acceleration.
      * @returns the maximum joint acceleration
      */
-    virtual Eigen::VectorXd get_joint_max_acc() override;
+    virtual Eigen::VectorXd get_joint_max_acc() override = 0;
 
     /**
      * @brief Get maximum joint torque.
      * @returns the maximum joint torque
      */
-    virtual Eigen::VectorXd get_joint_max_torque() override;
+    virtual Eigen::VectorXd get_joint_max_torque() override = 0;
 
-    virtual uint16_t get_dof() const override;
-
-    virtual std::vector<double> get_m() override;
-
-    virtual Eigen::Vector3d get_g0() override;
-
-    virtual Eigen::Matrix<double, Eigen::Dynamic, 3> get_CoM() override;
-
-    virtual std::vector<Eigen::Matrix3d> get_link_inertia() override;
 
+    /**
+     * @brief Get the regressor matrix \f$ \mathbf{Y(q, \dot{q}, \ddot{q})} \f$
+     * Use this function to compute the regressor matrix for given joint positions, velocities and accelerations.
+     * Can be used in conjunction with the parameter vector to compute the inverse dynamics torques.
+     * @param q [in] Joint positions
+     * @param qd [in] Joint velocities
+     * @param qdd [in] Joint accelerations
+     * @return The regressor matrix
+     */
     virtual Eigen::MatrixXd get_regressor(const Eigen::VectorXd& q, const Eigen::VectorXd& qd, const Eigen::VectorXd& qdd)
         const = 0;
 
+    /**
+     * @brief Get the friction regressor matrix \f$ \mathbf{Y_f(dq)} \f$
+     * Use this function to compute the friction regressor matrix for given joint velocities.
+     * Can be used in conjunction with the friction parameter vector to compute the friction torques.
+     * @param qd [in] Joint velocities
+     * @return The friction regressor matrix
+     */
     virtual Eigen::MatrixXd get_friction_regressor(const Eigen::VectorXd& qd) const = 0;
 
-    virtual Eigen::MatrixXd get_friction_regressor(const std::vector<double>& qd) const;
-
+    /**
+     * @brief Get the dynamic parameters vector.
+     * @returns the dynamic parameters vector
+     */
     virtual Eigen::VectorXd get_parameters() const = 0;
 
+    /**
+     * @brief Get the friction parameters vector.
+     * @returns the friction parameters vector
+     */
     virtual Eigen::VectorXd get_friction_parameters() const = 0;
 
-    virtual Eigen::VectorXd get_tau(const Eigen::VectorXd& q, const Eigen::VectorXd& qd, const Eigen::VectorXd& qdd);
-
-    virtual const kinematics::ForwardKinematics& get_fk_solver() const override;
-
    protected:
     std::shared_ptr<kinematics::ForwardKinematics> fk_model_;
     Eigen::Vector3d gravity_;

include/sdu_controllers/models/robot_model.hpp

@@ -22,12 +22,10 @@ namespace sdu_controllers::models
 
     /**
      * @brief Calculate the inverse dynamics.
-     *
-     * Uses the recursive newton euler algorithm. (TODO: add reference to book)
-     *
-     * @param y auxiliary control input.
      * @param q robot joint positions.
      * @param dq robot joint velocities.
+     * @param ddq robot joint accelerations.
+     * @param he end-effector wrench.
      * @returns the computed torques for the joint actuators \f$ \tau \f$
      */
     virtual Eigen::VectorXd inverse_dynamics(const Eigen::VectorXd &q, const Eigen::VectorXd &dq,
@@ -36,9 +34,6 @@ namespace sdu_controllers::models
 
     /**
      * @brief Calculate the forward dynamics.
-     *
-     * Uses the recursive newton euler algorithm. (TODO: add reference to book)
-     *
      * @param q robot joint positions.
      * @param dq robot joint velocities.
      * @param tau joint torques of the robot
@@ -103,18 +98,40 @@ namespace sdu_controllers::models
      */
     virtual Eigen::VectorXd get_joint_max_torque() = 0;
 
+    /**
+     * @brief Get the degrees of freedom of the robot.
+     * @returns the number of degrees of freedom
+     */
     virtual uint16_t get_dof() const = 0;
 
+    /**
+     * @brief Get the masses of the robot links.
+     * @returns vector containing the mass of each link
+     */
     virtual std::vector<double> get_m() = 0;
-
-    virtual std::vector<bool> get_is_joint_revolute() = 0;
-
+
+    /**
+     * @brief Get the gravity vector in base frame.
+     * @returns the 3D gravity vector
+     */
     virtual Eigen::Vector3d get_g0() = 0;
 
+    /**
+     * @brief Get the center of mass positions for each link.
+     * @returns matrix where each row contains the 3D center of mass position for a link
+     */
     virtual Eigen::Matrix<double, Eigen::Dynamic, 3> get_CoM() = 0;
 
+    /**
+     * @brief Get the inertia tensors for each link.
+     * @returns vector of 3x3 inertia matrices for each link
+     */
     virtual std::vector<Eigen::Matrix3d> get_link_inertia() = 0;
 
+    /**
+     * @brief Get the forward kinematics solver instance.
+     * @returns reference to the forward kinematics solver
+     */
     virtual const kinematics::ForwardKinematics &get_fk_solver() const = 0;
   };
 

python/sdu_controllers/_robot_models.cpp

@@ -0,0 +1,83 @@
+#include <nanobind/eigen/dense.h>
+#include <nanobind/nanobind.h>
+#include <nanobind/stl/shared_ptr.h>
+#include <nanobind/stl/vector.h>
+
+#include <sdu_controllers/kinematics/forward_kinematics.hpp>
+
+// models
+#include <sdu_controllers/models/breeding_blanket_handling_robot_model.hpp>
+#include <sdu_controllers/models/parameter_robot_model.hpp>
+#include <sdu_controllers/models/regressor_robot_model.hpp>
+#include <sdu_controllers/models/robot_model.hpp>
+#include <sdu_controllers/models/ur_robot_model.hpp>
+
+namespace nb = nanobind;
+
+namespace sdu_controllers
+{
+
+  nb::module_ create_robot_models_module(nb::module_ &main_module)
+  {
+    nb::module_ m_models = main_module.def_submodule("models", "Submodule containing robot model definitions.");
+    // enums
+    nb::enum_<models::URRobotModel::RobotType>(m_models, "RobotType")
+        .value("ur3e", models::URRobotModel::RobotType::ur3e)
+        .value("ur5e", models::URRobotModel::RobotType::ur5e)
+        .value("ur10e", models::URRobotModel::RobotType::ur10e)
+        .export_values();
+
+    // models
+
+    nb::class_<models::RobotModel>(m_models, "RobotModel")
+        .def(
+            "inverse_dynamics",
+            &models::RobotModel::inverse_dynamics,
+            nb::arg("q"),
+            nb::arg("dq"),
+            nb::arg("ddq"),
+            nb::arg("he"))
+        .def("forward_dynamics", &models::RobotModel::forward_dynamics, nb::arg("q"), nb::arg("dq"), nb::arg("tau"))
+        .def("get_inertia_matrix", &models::RobotModel::get_inertia_matrix)
+        .def("get_coriolis", &models::RobotModel::get_coriolis)
+        .def("get_gravity", &models::RobotModel::get_gravity)
+        .def("get_jacobian", &models::RobotModel::get_jacobian, nb::arg("q"))
+        .def("get_jacobian_dot", &models::RobotModel::get_jacobian_dot, nb::arg("q"), nb::arg("dq"))
+        .def("get_joint_pos_bounds", &models::RobotModel::get_joint_pos_bounds)
+        .def("get_joint_max_vel", &models::RobotModel::get_joint_max_vel)
+        .def("get_joint_max_acc", &models::RobotModel::get_joint_max_acc)
+        .def("get_joint_max_torque", &models::RobotModel::get_joint_max_torque)
+        .def("get_dof", &models::RobotModel::get_dof)
+        .def("get_m", &models::RobotModel::get_m)
+        .def("get_g0", &models::RobotModel::get_g0)
+        .def("get_link_inertia", &models::RobotModel::get_link_inertia)
+        .def("get_fk_solver", &models::RobotModel::get_fk_solver);
+
+    nb::class_<models::RegressorRobotModel, models::RobotModel>(m_models, "RegressorRobotModel")
+        .def("get_regressor", &models::RegressorRobotModel::get_regressor, nb::arg("q"), nb::arg("qd"), nb::arg("qdd"))
+        .def(
+            "get_friction_regressor",
+            nb::overload_cast<const Eigen::VectorXd &>(&models::RegressorRobotModel::get_friction_regressor, nb::const_),
+            nb::arg("qd"))
+        .def(
+            "get_friction_regressor",
+            nb::overload_cast<const std::vector<double> &>(&models::RegressorRobotModel::get_friction_regressor, nb::const_),
+            nb::arg("qd"))
+        .def("get_parameters", &models::RegressorRobotModel::get_parameters)
+        .def("get_friction_parameters", &models::RegressorRobotModel::get_friction_parameters);
+
+    nb::class_<models::ParameterRobotModel>(m_models, "ParameterRobotModel");
+
+    nb::class_<models::URRobotModel, models::ParameterRobotModel>(m_models, "URRobotModel")
+        .def(nb::init<models::URRobotModel::RobotType>())
+        .def(nb::init<const std::string &>())
+        .def(nb::init<const models::RobotParameters &>());
+
+    nb::class_<models::BreedingBlanketHandlingRobotModel, models::ParameterRobotModel>(
+        m_models, "BreedingBlanketHandlingRobotModel")
+        .def(nb::init<>())
+        .def(nb::init<const std::string &>())
+        .def(nb::init<const models::RobotParameters &>());
+    return m_models;
+  }
+}  // namespace sdu_controllers