From f06a53a00e124d9fed3570784b8331daaacef8df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mads=20Elkj=C3=A6r=20Larsen?= Date: Thu, 13 Aug 2026 09:35:18 +0200 Subject: [PATCH 1/7] Potential fix --- src/kinematics/forward_kinematics.cpp | 4 ++-- src/math/rnea.cpp | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/kinematics/forward_kinematics.cpp b/src/kinematics/forward_kinematics.cpp index dcb5624..539b7a6 100644 --- a/src/kinematics/forward_kinematics.cpp +++ b/src/kinematics/forward_kinematics.cpp @@ -86,8 +86,8 @@ size_t ForwardKinematics::get_dof() const { size_t count = 0; for (const auto& jt : joint_type_) - if (jt != ForwardKinematics::FIXED) - ++count; + //if (jt != ForwardKinematics::FIXED) + ++count; return count; } diff --git a/src/math/rnea.cpp b/src/math/rnea.cpp index 9f5431d..2f9d875 100644 --- a/src/math/rnea.cpp +++ b/src/math/rnea.cpp @@ -217,6 +217,7 @@ namespace sdu_controllers::math ddp_(Eigen::all, i) = ddp_(Eigen::all, i - 1) + domega_(Eigen::all, i).cross(r_) + omega_(Eigen::all, i).cross(omega_(Eigen::all, i).cross(r_)); } + ++q_idx; } else if (joint_type.at(i) == kinematics::ForwardKinematics::JointType::REVOLUTE) { // revolute joint @@ -312,8 +313,11 @@ namespace sdu_controllers::math if (joint_type.at(i) == kinematics::ForwardKinematics::JointType::FIXED) { // Fixed joints carry no torque output, skip tau assignment - continue; + tau_(q_idx) = 0; + + --q_idx; } + else if (joint_type.at(i) == kinematics::ForwardKinematics::JointType::REVOLUTE) { // Revolute if (i == 0) From 223d3056df084f02eafc36d9d768a34739b441e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mads=20Elkj=C3=A6r=20Larsen?= Date: Thu, 13 Aug 2026 12:37:06 +0200 Subject: [PATCH 2/7] Checked that it gives the same as simulink on a 3 joint robot --- src/math/rnea.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/math/rnea.cpp b/src/math/rnea.cpp index 2f9d875..c3821d8 100644 --- a/src/math/rnea.cpp +++ b/src/math/rnea.cpp @@ -314,7 +314,6 @@ namespace sdu_controllers::math { // Fixed joints carry no torque output, skip tau assignment tau_(q_idx) = 0; - --q_idx; } From 7d74b5c860f8fb4dbef0ede7daca1f85374d4a8e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mads=20Elkj=C3=A6r=20Larsen?= Date: Thu, 13 Aug 2026 13:01:39 +0200 Subject: [PATCH 3/7] Added getter function for the reaction forces. Have been validated against simulink --- include/sdu_controllers/math/rnea.hpp | 4 ++++ python/sdu_controllers/_math.cpp | 12 +++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/include/sdu_controllers/math/rnea.hpp b/include/sdu_controllers/math/rnea.hpp index 686d18b..0c1a5ec 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 joint dynamics of every frame component + const Eigen::Matrix3Xd& get_joint_forces() const { return f_; } + const Eigen::Matrix3Xd& get_joint_moments() 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..5c932db 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_joint_forces", + &math::RecursiveNewtonEuler::get_joint_forces, + "Get reaction forces from the most recent inverse dynamics call" + ) + .def( + "get_joint_moments", + &math::RecursiveNewtonEuler::get_joint_moments, + "Get reaction moments from the most recent inverse dynamics call" + ); // Linear algebra utilities m.def( From e707a3118503d77c1265ce66352cf4d419ab51d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mads=20Elkj=C3=A6r=20Larsen?= Date: Thu, 13 Aug 2026 13:39:54 +0200 Subject: [PATCH 4/7] Reverted to original code - but fixed that number of links and Dof can be different --- src/kinematics/forward_kinematics.cpp | 2 +- src/math/rnea.cpp | 4 +--- src/models/parameter_robot_model.cpp | 7 ------- 3 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/kinematics/forward_kinematics.cpp b/src/kinematics/forward_kinematics.cpp index 539b7a6..8bb3c8d 100644 --- a/src/kinematics/forward_kinematics.cpp +++ b/src/kinematics/forward_kinematics.cpp @@ -86,7 +86,7 @@ size_t ForwardKinematics::get_dof() const { size_t count = 0; for (const auto& jt : joint_type_) - //if (jt != ForwardKinematics::FIXED) + if (jt != ForwardKinematics::FIXED) ++count; return count; } diff --git a/src/math/rnea.cpp b/src/math/rnea.cpp index c3821d8..04b36e4 100644 --- a/src/math/rnea.cpp +++ b/src/math/rnea.cpp @@ -217,7 +217,6 @@ namespace sdu_controllers::math ddp_(Eigen::all, i) = ddp_(Eigen::all, i - 1) + domega_(Eigen::all, i).cross(r_) + omega_(Eigen::all, i).cross(omega_(Eigen::all, i).cross(r_)); } - ++q_idx; } else if (joint_type.at(i) == kinematics::ForwardKinematics::JointType::REVOLUTE) { // revolute joint @@ -313,8 +312,7 @@ namespace sdu_controllers::math if (joint_type.at(i) == kinematics::ForwardKinematics::JointType::FIXED) { // Fixed joints carry no torque output, skip tau assignment - tau_(q_idx) = 0; - --q_idx; + continue; } else if (joint_type.at(i) == kinematics::ForwardKinematics::JointType::REVOLUTE) 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); From ea3fd78430c6f5d166278f39466d08c4bf305c15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mads=20Elkj=C3=A6r=20Larsen?= Date: Thu, 13 Aug 2026 13:43:04 +0200 Subject: [PATCH 5/7] Revereted small changes --- src/kinematics/forward_kinematics.cpp | 2 +- src/math/rnea.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/kinematics/forward_kinematics.cpp b/src/kinematics/forward_kinematics.cpp index 8bb3c8d..dcb5624 100644 --- a/src/kinematics/forward_kinematics.cpp +++ b/src/kinematics/forward_kinematics.cpp @@ -87,7 +87,7 @@ size_t ForwardKinematics::get_dof() const size_t count = 0; for (const auto& jt : joint_type_) if (jt != ForwardKinematics::FIXED) - ++count; + ++count; return count; } diff --git a/src/math/rnea.cpp b/src/math/rnea.cpp index 04b36e4..9f5431d 100644 --- a/src/math/rnea.cpp +++ b/src/math/rnea.cpp @@ -314,7 +314,6 @@ namespace sdu_controllers::math // Fixed joints carry no torque output, skip tau assignment continue; } - else if (joint_type.at(i) == kinematics::ForwardKinematics::JointType::REVOLUTE) { // Revolute if (i == 0) From 481369b717e83fbacb982354a8da621f9c992c5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mads=20Elkj=C3=A6r=20Larsen?= Date: Thu, 13 Aug 2026 13:56:14 +0200 Subject: [PATCH 6/7] Changed namings for the reaction forces functions --- python/sdu_controllers/_math.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/python/sdu_controllers/_math.cpp b/python/sdu_controllers/_math.cpp index 5c932db..21f7d49 100644 --- a/python/sdu_controllers/_math.cpp +++ b/python/sdu_controllers/_math.cpp @@ -173,13 +173,13 @@ namespace sdu_controllers "Set the z-axis of the base frame", nb::arg("z0")) .def( - "get_joint_forces", - &math::RecursiveNewtonEuler::get_joint_forces, + "get_reaction_forces_frames", + &math::RecursiveNewtonEuler::get_reaction_forces_frames, "Get reaction forces from the most recent inverse dynamics call" ) .def( - "get_joint_moments", - &math::RecursiveNewtonEuler::get_joint_moments, + "get_reaction_moments_frames", + &math::RecursiveNewtonEuler::get_reaction_moments_frames, "Get reaction moments from the most recent inverse dynamics call" ); From abfa178eddc378bf4550bc160479f6f61d6dd611 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mads=20Elkj=C3=A6r=20Larsen?= Date: Thu, 13 Aug 2026 13:59:13 +0200 Subject: [PATCH 7/7] fixed header --- include/sdu_controllers/math/rnea.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/sdu_controllers/math/rnea.hpp b/include/sdu_controllers/math/rnea.hpp index 0c1a5ec..96d394f 100644 --- a/include/sdu_controllers/math/rnea.hpp +++ b/include/sdu_controllers/math/rnea.hpp @@ -30,9 +30,9 @@ namespace sdu_controllers::math Eigen::VectorXd velocity_product(const Eigen::VectorXd &q, const Eigen::VectorXd &dq); Eigen::VectorXd gravity(const Eigen::VectorXd &q); - // Get complete joint dynamics of every frame component - const Eigen::Matrix3Xd& get_joint_forces() const { return f_; } - const Eigen::Matrix3Xd& get_joint_moments() const { return mu_; } + // 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$.