diff --git a/include/sdu_controllers/math/rnea.hpp b/include/sdu_controllers/math/rnea.hpp index 686d18b..96d394f 100644 --- a/include/sdu_controllers/math/rnea.hpp +++ b/include/sdu_controllers/math/rnea.hpp @@ -29,6 +29,10 @@ namespace sdu_controllers::math Eigen::MatrixXd coriolis(const Eigen::VectorXd &q, const Eigen::VectorXd &dq); Eigen::VectorXd velocity_product(const Eigen::VectorXd &q, const Eigen::VectorXd &dq); Eigen::VectorXd gravity(const Eigen::VectorXd &q); + + // Get complete reaction dynamics of every joint/frame + const Eigen::Matrix3Xd& get_reaction_forces_frames() const { return f_; } + const Eigen::Matrix3Xd& get_reaction_moments_frames() const { return mu_; } /** * @brief Classical joint friction torque \f$ \tau_{f} \f$. diff --git a/python/sdu_controllers/_math.cpp b/python/sdu_controllers/_math.cpp index 7d0a673..21f7d49 100644 --- a/python/sdu_controllers/_math.cpp +++ b/python/sdu_controllers/_math.cpp @@ -171,7 +171,17 @@ namespace sdu_controllers "set_z0", &math::RecursiveNewtonEuler::set_z0, "Set the z-axis of the base frame", - nb::arg("z0")); + nb::arg("z0")) + .def( + "get_reaction_forces_frames", + &math::RecursiveNewtonEuler::get_reaction_forces_frames, + "Get reaction forces from the most recent inverse dynamics call" + ) + .def( + "get_reaction_moments_frames", + &math::RecursiveNewtonEuler::get_reaction_moments_frames, + "Get reaction moments from the most recent inverse dynamics call" + ); // Linear algebra utilities m.def( diff --git a/src/models/parameter_robot_model.cpp b/src/models/parameter_robot_model.cpp index e75ab2d..9e41de7 100644 --- a/src/models/parameter_robot_model.cpp +++ b/src/models/parameter_robot_model.cpp @@ -289,13 +289,6 @@ namespace sdu_controllers::models params.g0 = Eigen::Vector3d(0.0, 0.0, -9.81); } - // Basic validation: sizes must match dof if provided - if (params.dof != 0 && params.dof != static_cast(n_links)) - { - std::cerr << "YAML validation error: declared dof (" << params.dof << ") " - << "does not match number of inertial entries (" << n_links << ") in " << filepath << std::endl; - return std::nullopt; - } // set dof if not set if (params.dof == 0) params.dof = static_cast(n_links);