diff --git a/CMakeLists.txt b/CMakeLists.txt index 800a0c4..e3d4f05 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/sdu_controllers/kinematics/forward_kinematics.hpp b/include/sdu_controllers/kinematics/forward_kinematics.hpp index 7d448f5..2476a80 100644 --- a/include/sdu_controllers/kinematics/forward_kinematics.hpp +++ b/include/sdu_controllers/kinematics/forward_kinematics.hpp @@ -45,6 +45,16 @@ namespace sdu_controllers::kinematics */ virtual Eigen::Matrix 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 geometric_jacobian( + const Eigen::VectorXd& q, + const std::vector& fk_matrices) const; + /** * @brief Get the degrees of freedom of the kinematic chain * @return Number of degrees of freedom diff --git a/include/sdu_controllers/models/parameter_robot_model.hpp b/include/sdu_controllers/models/parameter_robot_model.hpp index 69ba29e..df7d01a 100644 --- a/include/sdu_controllers/models/parameter_robot_model.hpp +++ b/include/sdu_controllers/models/parameter_robot_model.hpp @@ -122,8 +122,6 @@ namespace sdu_controllers::models std::vector get_m() override; - std::vector get_is_joint_revolute() override; - Eigen::Vector3d get_g0() override; Eigen::Matrix get_CoM() override; diff --git a/include/sdu_controllers/models/regressor_robot_model.hpp b/include/sdu_controllers/models/regressor_robot_model.hpp index 51c96e6..c2f1b3f 100644 --- a/include/sdu_controllers/models/regressor_robot_model.hpp +++ b/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& 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 @@ namespace sdu_controllers::models */ 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 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 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 get_link_inertia() override; + + /** + * @brief Get the friction regressor matrix \f$ \mathbf{Y_f(dq)} \f$ + * This function is alternative version taking std::vector 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& 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 @@ namespace sdu_controllers::models * @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 get_m() override; - - virtual Eigen::Vector3d get_g0() override; - - virtual Eigen::Matrix get_CoM() override; - - virtual std::vector 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& 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 fk_model_; Eigen::Vector3d gravity_; diff --git a/include/sdu_controllers/models/robot_model.hpp b/include/sdu_controllers/models/robot_model.hpp index a1adcb2..34e627a 100644 --- a/include/sdu_controllers/models/robot_model.hpp +++ b/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 get_m() = 0; - - virtual std::vector 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 get_CoM() = 0; + /** + * @brief Get the inertia tensors for each link. + * @returns vector of 3x3 inertia matrices for each link + */ virtual std::vector 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; }; diff --git a/python/sdu_controllers/_robot_models.cpp b/python/sdu_controllers/_robot_models.cpp new file mode 100644 index 0000000..4629445 --- /dev/null +++ b/python/sdu_controllers/_robot_models.cpp @@ -0,0 +1,83 @@ +#include +#include +#include +#include + +#include + +// models +#include +#include +#include +#include +#include + +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_(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_(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_(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(&models::RegressorRobotModel::get_friction_regressor, nb::const_), + nb::arg("qd")) + .def( + "get_friction_regressor", + nb::overload_cast &>(&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_(m_models, "ParameterRobotModel"); + + nb::class_(m_models, "URRobotModel") + .def(nb::init()) + .def(nb::init()) + .def(nb::init()); + + nb::class_( + m_models, "BreedingBlanketHandlingRobotModel") + .def(nb::init<>()) + .def(nb::init()) + .def(nb::init()); + return m_models; + } +} // namespace sdu_controllers \ No newline at end of file diff --git a/python/sdu_controllers/_sdu_controllers.cpp b/python/sdu_controllers/_sdu_controllers.cpp index a441b13..76ce318 100644 --- a/python/sdu_controllers/_sdu_controllers.cpp +++ b/python/sdu_controllers/_sdu_controllers.cpp @@ -14,65 +14,26 @@ #include #include -// models -#include -#include -#include -#include - // kinematics -#include #include +#include #include namespace nb = nanobind; namespace sdu_controllers { + nb::module_ create_robot_models_module(nb::module_ &main_module); + NB_MODULE(_sdu_controllers, m) { m.doc() = "Python Bindings for sdu_controllers"; - m.def("add_one", &add_one, "Increments an integer value"); - nb::module_ m_models = m.def_submodule("models", "Submodule containing robot model definitions."); + nb::module_ m_models = create_robot_models_module(m); nb::module_ m_controllers = m.def_submodule("controllers", "Submodule containing robot control methods."); nb::module_ m_math = m.def_submodule("math", "Submodule containing math utility functions."); nb::module_ m_kinematics = m.def_submodule("kinematics", "Submodule containing functions for calculating kinematics."); - // enums - nb::enum_(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_(m_models, "ParameterRobotModel"); - - nb::class_(m_models, "URRobotModel") - .def(nb::init()) - .def(nb::init()) - .def(nb::init()) - .def("get_inertia_matrix", &models::URRobotModel::get_inertia_matrix) - .def("get_coriolis", &models::URRobotModel::get_coriolis) - .def("get_gravity", &models::URRobotModel::get_gravity) - .def("get_dof", &models::URRobotModel::get_dof) - .def("get_joint_pos_bounds", &models::URRobotModel::get_joint_pos_bounds) - .def("get_joint_max_vel", &models::URRobotModel::get_joint_max_vel) - .def("get_joint_max_acc", &models::URRobotModel::get_joint_max_acc) - .def("get_joint_max_torque", &models::URRobotModel::get_joint_max_torque); - - nb::class_(m_models, "BreedingBlanketHandlingRobotModel") - .def(nb::init<>()) - .def(nb::init()) - .def(nb::init()) - .def("get_inertia_matrix", &models::BreedingBlanketHandlingRobotModel::get_inertia_matrix) - .def("get_coriolis", &models::BreedingBlanketHandlingRobotModel::get_coriolis) - .def("get_gravity", &models::BreedingBlanketHandlingRobotModel::get_gravity) - .def("get_dof", &models::BreedingBlanketHandlingRobotModel::get_dof) - .def("set_tcp_mass", &models::BreedingBlanketHandlingRobotModel::set_tcp_mass) - .def("get_CoM", &models::BreedingBlanketHandlingRobotModel::get_CoM); - // controllers nb::class_(m_controllers, "PIDController") .def( @@ -150,7 +111,7 @@ namespace sdu_controllers nb::module_ eigen = nb::module_::import_("numpy"); nb::class_(m_math, "RecursiveNewtonEuler") .def( - nb::init(), + nb::init(), "Initialize the RecursiveNewtonEuler algorithm with a robot model", nb::arg("robot_model")) .def( @@ -212,9 +173,17 @@ namespace sdu_controllers "Get the type of each joint in the kinematic chain") .def( "geometric_jacobian", - &sdu_controllers::kinematics::ForwardKinematics::geometric_jacobian, + nb::overload_cast( + &sdu_controllers::kinematics::ForwardKinematics::geometric_jacobian, nb::const_), "Compute the geometric Jacobian at the given joint configuration", nb::arg("q")) + .def( + "geometric_jacobian", + nb::overload_cast &>( + &sdu_controllers::kinematics::ForwardKinematics::geometric_jacobian, nb::const_), + "Compute the geometric Jacobian at the given joint configuration using precomputed forward kinematics matrices", + nb::arg("q"), + nb::arg("fk_matrices")) .def( "get_dof", &sdu_controllers::kinematics::ForwardKinematics::get_dof, diff --git a/src/kinematics/forward_kinematics.cpp b/src/kinematics/forward_kinematics.cpp index 77e0e21..e4eb32c 100644 --- a/src/kinematics/forward_kinematics.cpp +++ b/src/kinematics/forward_kinematics.cpp @@ -24,24 +24,31 @@ Eigen::Matrix ForwardKinematics::geometric_jacobian(c { std::vector T_chain = forward_kinematics_all(q); + return geometric_jacobian(q, T_chain); +} + +Eigen::Matrix ForwardKinematics::geometric_jacobian( + const Eigen::VectorXd& q, + const std::vector& fk_matrices) const +{ Eigen::Vector3d z_im1, o_im1, o_n; Eigen::Matrix J(6, joint_type_.size()); // Get position of end effector - o_n = T_chain.back().template block<3, 1>(0, 3); + o_n = fk_matrices.back().template block<3, 1>(0, 3); // Loop through joints for (int i = 0; i < joint_type_.size(); i++) { - Eigen::Matrix4d T = T_chain[i]; + Eigen::Matrix4d T = fk_matrices[i]; // Get z_{i-1}, o_{i-1} // z_{i-1} is the unit vector along the z-axis of the previous joint // o_{i-1} is the position of the center of the previous joint frame if (i > 0) { - z_im1 = T_chain[i - 1].block<3, 1>(0, 2); - o_im1 = T_chain[i - 1].block<3, 1>(0, 3); + z_im1 = fk_matrices[i - 1].block<3, 1>(0, 2); + o_im1 = fk_matrices[i - 1].block<3, 1>(0, 3); } else { diff --git a/src/math/rnea.cpp b/src/math/rnea.cpp index 02b0fd4..30f0c4a 100644 --- a/src/math/rnea.cpp +++ b/src/math/rnea.cpp @@ -2,6 +2,13 @@ #include #include +namespace Eigen { + namespace indexing { + + } + using Eigen::indexing::all; +} + namespace sdu_controllers::math { RecursiveNewtonEuler::RecursiveNewtonEuler(models::RobotModel &robot_model) : robot_model_(robot_model) diff --git a/src/models/parameter_robot_model.cpp b/src/models/parameter_robot_model.cpp index 8a3064e..c0b3c48 100644 --- a/src/models/parameter_robot_model.cpp +++ b/src/models/parameter_robot_model.cpp @@ -177,7 +177,7 @@ namespace sdu_controllers::models std::cout << "Found the DH parameters using these instead."; } } - else if (kinematics_type == "DH" or tried_to_use_frames) + else if (kinematics_type == "DH" || tried_to_use_frames) { params.dof = static_cast(root["dof"].as()); auto a_opt = utils::yaml_node_to_eigen_vector(root["kinematics"]["dh_params"]["a"]); @@ -426,11 +426,6 @@ namespace sdu_controllers::models return mass_; } - std::vector ParameterRobotModel::get_is_joint_revolute() - { - return is_joint_revolute_; - } - Eigen::Vector3d ParameterRobotModel::get_g0() { return g0_; diff --git a/src/models/regressor_robot_model.cpp b/src/models/regressor_robot_model.cpp index b255ff6..8bcda17 100644 --- a/src/models/regressor_robot_model.cpp +++ b/src/models/regressor_robot_model.cpp @@ -4,6 +4,15 @@ using namespace sdu_controllers::models; using namespace sdu_controllers; +namespace Eigen +{ + namespace indexing + { + + } + using Eigen::indexing::all; +} // namespace Eigen + RegressorRobotModel::RegressorRobotModel( const std::shared_ptr& fkModel, const Eigen::Vector3d& g0) @@ -12,10 +21,36 @@ RegressorRobotModel::RegressorRobotModel( { if (!fk_model_) { - throw std::runtime_error("RegressorRobotModel: No FK model provided"); + throw std::invalid_argument("RegressorRobotModel: No FK model provided"); } } +Eigen::VectorXd RegressorRobotModel::inverse_dynamics( + const Eigen::VectorXd& q, + const Eigen::VectorXd& dq, + const Eigen::VectorXd& ddq, + const Eigen::VectorXd& he) +{ + Eigen::VectorXd tau_he = Eigen::VectorXd::Zero(q.rows()); + if (he.size() == 6) + { + Eigen::MatrixXd J = fk_model_->geometric_jacobian(q); + tau_he = J.transpose() * he; + } + return get_regressor(q, dq, ddq) * get_parameters() + get_friction_regressor(dq) * get_friction_parameters() + tau_he; +} + +Eigen::VectorXd +RegressorRobotModel::forward_dynamics(const Eigen::VectorXd& q, const Eigen::VectorXd& dq, const Eigen::VectorXd& tau) +{ + Eigen::VectorXd zero_vec = Eigen::VectorXd::Zero(q.rows()); + + // Get C(q, dq) dq + g(q) + Eigen::VectorXd tau_bar = inverse_dynamics(q, dq, zero_vec, zero_vec); + Eigen::MatrixXd B = get_inertia_matrix(q); + return B.lu().solve(tau - tau_bar); +} + Eigen::MatrixXd RegressorRobotModel::get_inertia_matrix(const Eigen::VectorXd& q) { Eigen::VectorXd zero_vec = Eigen::VectorXd::Zero(q.rows()), he0 = Eigen::VectorXd::Zero(6), @@ -88,7 +123,7 @@ Eigen::MatrixXd RegressorRobotModel::get_gravity(const Eigen::VectorXd& q) Eigen::VectorXd zero_vec = Eigen::VectorXd::Zero(q.rows()), he0 = Eigen::VectorXd::Zero(6); Eigen::VectorXd grav = Eigen::VectorXd::Zero(q.rows()); - grav = get_tau(q, zero_vec, zero_vec); + grav = get_regressor(q, zero_vec, zero_vec) * get_parameters(); return grav; } @@ -105,7 +140,21 @@ Eigen::MatrixXd RegressorRobotModel::get_jacobian(const Eigen::VectorXd& q) Eigen::MatrixXd RegressorRobotModel::get_jacobian_dot(const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { - throw std::runtime_error("RegressorRobotModel: get_jacobian_dot not implemented"); + const double dt = 1e-6; + + // Calculate the Jacobian at the current configuration q + Eigen::MatrixXd J_current = get_jacobian(q); + + // Estimate the configuration at the next time step (q_next) using Euler integration + Eigen::VectorXd q_next = q + (dq * dt); + + // Calculate the Jacobian at the predicted next configuration + Eigen::MatrixXd J_next = get_jacobian(q_next); + + // Compute the numerical derivative + Eigen::MatrixXd J_dot = (J_next - J_current) / dt; + + return J_dot; } uint16_t RegressorRobotModel::get_dof() const @@ -133,11 +182,6 @@ std::vector RegressorRobotModel::get_link_inertia() throw std::runtime_error("RegressorRobotModel: get_link_inertia not implemented"); } -Eigen::VectorXd RegressorRobotModel::get_tau(const Eigen::VectorXd& q, const Eigen::VectorXd& qd, const Eigen::VectorXd& qdd) -{ - return get_regressor(q, qd, qdd) * get_parameters() + get_friction_regressor(qd) * get_friction_parameters(); -} - Eigen::MatrixXd RegressorRobotModel::get_friction_regressor(const std::vector& qd) const { Eigen::VectorXd qd_eigen = Eigen::Map(qd.data(), qd.size());