From 22222465e35e83f520001e8a5de2c4a15f9f3463 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sat, 2 Jan 2021 23:18:46 +0100
Subject: [PATCH 01/26] External Wrench Estimation class created

First-order momentum observer
---
 .../src/chainexternalwrenchestimator.cpp      | 162 ++++++++++++++++++
 .../src/chainexternalwrenchestimator.hpp      |  97 +++++++++++
 2 files changed, 259 insertions(+)
 create mode 100644 orocos_kdl/src/chainexternalwrenchestimator.cpp
 create mode 100644 orocos_kdl/src/chainexternalwrenchestimator.hpp

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
new file mode 100644
index 000000000..d496076ba
--- /dev/null
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -0,0 +1,162 @@
+// Copyright  (C)  2021 Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
+
+// Version: 1.0
+// Author: Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#include "chainexternalwrenchestimator.hpp"
+namespace KDL {
+
+ChainExternalWrenchEstimator::ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant) :
+    CHAIN(chain),
+    DT_SEC(1.0 / sample_frequency), FILTER_CONST(filter_constant),
+    nj(CHAIN.getNrOfJoints()), ns(CHAIN.getNrOfSegments()),
+    jnt_mass_matrix(nj), previous_jnt_mass_matrix(nj),
+    initial_jnt_momentum(nj), estimated_momentum_integral(nj), filtered_estimated_ext_torque(nj),
+    ESTIMATION_GAIN(Eigen::VectorXd::Constant(nj, estimation_gain)),
+    dynparam_solver(CHAIN, gravity),
+    jacobian_solver(CHAIN),
+    fk_pos_solver(CHAIN)
+{
+}
+
+void ChainExternalWrenchEstimator::updateInternalDataStructures()
+{
+    nj = CHAIN.getNrOfJoints();
+    ns = CHAIN.getNrOfSegments();
+    jnt_mass_matrix.resize(nj);
+    previous_jnt_mass_matrix.resize(nj);
+    initial_jnt_momentum.resize(nj);
+    estimated_momentum_integral.resize(nj);
+    filtered_estimated_ext_torque.resize(nj);
+    ESTIMATION_GAIN.conservativeResizeLike(Eigen::VectorXd::Constant(nj, ESTIMATION_GAIN(0)));
+    dynparam_solver.updateInternalDataStructures();
+    jacobian_solver.updateInternalDataStructures();
+    fk_pos_solver.updateInternalDataStructures();
+}
+
+// Calculates robot's initial momentum in the joint space. If this method is not called by the user, zero values will be taken for the initial momentum.
+int ChainExternalWrenchEstimator::setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity)
+{
+    // Check sizes first
+    if (joint_position.rows() != nj || joint_velocity.rows() != nj) return (error = E_SIZE_MISMATCH);
+
+    // Calculate robot's inertia and momentum in the joint space
+    dynparam_solver.JntToMass(joint_position, jnt_mass_matrix);
+    initial_jnt_momentum.data = jnt_mass_matrix.data * joint_velocity.data;
+
+    // Reset data because of the new momentum offset
+    SetToZero(estimated_momentum_integral);
+    SetToZero(filtered_estimated_ext_torque);
+
+    return (error = E_NOERROR);
+}
+
+// This method calculates the external wrench that is applied on the robot's end-effector.
+int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench)
+{
+    /**
+     * ==========================================================================
+     * First-order momentum observer, an implementation based on:
+     * S. Haddadin, A. De Luca and A. Albu-Schäffer,
+     * "Robot Collisions: A Survey on Detection, Isolation, and Identification,"
+     * in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017.
+     * ==========================================================================
+     */
+
+    // Check sizes first
+    if (nj != CHAIN.getNrOfJoints() || ns != CHAIN.getNrOfSegments()) return (error = E_NOT_UP_TO_DATE);
+    if (joint_position.rows() != nj || joint_velocity.rows() != nj || joint_torque.rows() != nj) return (error = E_SIZE_MISMATCH);
+
+    /**
+     * =======================================================================================================================
+     * Part I: Estimation of the torques that are felt in joints as a result of the external wrench being applied on the robot
+     * =======================================================================================================================
+     */
+
+    // Calculate decomposed robot's dynamics
+    JntArray gravity_torque(nj), coriolis_torque(nj);
+    int solver_result = dynparam_solver.JntToMass(joint_position, jnt_mass_matrix);
+    if (solver_result != 0) return solver_result;
+    solver_result = dynparam_solver.JntToCoriolis(joint_position, joint_velocity, coriolis_torque);
+    if (solver_result != 0) return solver_result;
+    solver_result = dynparam_solver.JntToGravity(joint_position, gravity_torque);
+    if (solver_result != 0) return solver_result;
+
+    // Calculate the change of robot's inertia in the joint space
+    JntSpaceInertiaMatrix jnt_mass_matrix_dot(nj);
+    jnt_mass_matrix_dot.data = (jnt_mass_matrix.data - previous_jnt_mass_matrix.data) / DT_SEC;
+
+    // Save data for the next iteration
+    previous_jnt_mass_matrix.data = jnt_mass_matrix.data;
+
+    // Calculate total torque exerted on the joint
+    JntArray total_torque(nj);
+    total_torque.data = joint_torque.data - gravity_torque.data - coriolis_torque.data + jnt_mass_matrix_dot.data * joint_velocity.data;
+
+    // Accumulate main integral
+    estimated_momentum_integral.data += (total_torque.data + filtered_estimated_ext_torque.data) * DT_SEC;
+
+    // Estimate external joint torque
+    JntArray estimated_ext_torque(nj);
+    estimated_ext_torque.data = ESTIMATION_GAIN.asDiagonal() * (jnt_mass_matrix.data * joint_velocity.data - estimated_momentum_integral.data - initial_jnt_momentum.data);
+
+    // First order low-pass filter: filter out the noise from the estimated signal
+    // This filter can be turned off by setting FILTER_CONST value to 0
+    filtered_estimated_ext_torque.data = FILTER_CONST * filtered_estimated_ext_torque.data + (1.0 - FILTER_CONST) * estimated_ext_torque.data;
+
+    /**
+     * ==================================================================================================================
+     * Part II: Propagate above-estimated joint torques to Cartesian wrench using a pseudo-inverse of Jacobian-Transpose
+     * ==================================================================================================================
+     */
+    
+    // Compute robot's end-effector frame, expressed in the base frame
+    Frame end_eff_frame;
+    solver_result = fk_pos_solver.JntToCart(joint_position, end_eff_frame);
+    if (solver_result != 0) return solver_result;
+
+    // Compute robot's jacobian for the end-effector frame, expressed in the base frame
+    Jacobian jacobian_end_eff(nj);
+    solver_result = jacobian_solver.JntToJac(joint_position, jacobian_end_eff);
+    if (solver_result != 0) return solver_result;
+
+    // Transform the jacobian from the base frame to the end-effector frame. 
+    // This part can be commented out if the user wants its estimated wrench to be expressed w.r.t. base frame 
+    jacobian_end_eff.changeBase(end_eff_frame.M.Inverse()); // Jacobian is now expressed w.r.t. end-effector frame
+
+    // Compute SVD of the jacobian using Eigen functions
+    Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian_end_eff.data.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+    // Invert singular values
+    Eigen::VectorXd singular_inv(svd.singularValues());
+    for (int j = 0; j < singular_inv.size(); ++j)
+        singular_inv(j) = (singular_inv(j) < 1e-8) ? 0.0 : 1.0 / singular_inv(j);
+
+    // Compose SVD
+    Eigen::MatrixXd jacobian_end_eff_inv;
+    jacobian_end_eff_inv.noalias() = svd.matrixV() * singular_inv.matrix().asDiagonal() * svd.matrixU().adjoint();
+
+    // Compute end-effector's Cartesian wrench from the estimated joint torques
+    Eigen::VectorXd estimated_wrench = jacobian_end_eff_inv * filtered_estimated_ext_torque.data;
+    for (int i = 0; i < 6; i++) 
+        external_wrench(i) = estimated_wrench(i);
+
+    return (error = E_NOERROR);
+}
+
+} // namespace
diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp
new file mode 100644
index 000000000..8930c39fa
--- /dev/null
+++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp
@@ -0,0 +1,97 @@
+// Copyright  (C)  2021 Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
+
+// Version: 1.0
+// Author: Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+#ifndef KDL_EXTERNAL_WRENCH_ESTIMATOR_HPP
+#define KDL_EXTERNAL_WRENCH_ESTIMATOR_HPP
+
+#include <Eigen/Core>
+#include <Eigen/SVD> 
+#include "chaindynparam.hpp"
+#include "chainjnttojacsolver.hpp"
+#include "chainfksolverpos_recursive.hpp"
+#include <iostream>
+
+namespace KDL {
+
+    /**
+     * \brief First-order momentum observer for the estimation of external wrenches applied on the robot's end-effector. 
+     *
+     * Implementation based on:
+     * S. Haddadin, A. De Luca and A. Albu-Schäffer,
+     * "Robot Collisions: A Survey on Detection, Isolation, and Identification,"
+     * in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017.
+     */
+    class ChainExternalWrenchEstimator : public SolverI
+    {
+    public:
+        /**
+         * Constructor for the estimator, it will allocate all the necessary memory
+         * \param chain The kinematic chain of the robot, an internal copy will be made.
+         * \param gravity The gravity-acceleration vector to use during the calculation.
+         * \param sample_frequency Frequency at which users updates it estimation loop (in Hz).
+         * \param filter_constant Parameter defining how much the estimated signal should be filtered by the low-pass filter.
+         *                        This input value should be between 0 and 1. Higher the number means more noise needs to be filtered-out.
+         *                        The filter can be turned off by setting this value to 0.
+         */
+        ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant);
+        ~ChainExternalWrenchEstimator(){};
+
+        /**
+         * Calculates robot's initial momentum in the joint space. 
+         * Bassically, sets the offset for future estimation (momentum calculation).
+         * If this method is not called by the user, zero values will be taken for the initial momentum.
+         */
+        int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity);
+
+        /**
+         * This method calculates the external wrench that is applied on the robot's end-effector.
+         * Input parameters:
+         * \param joint_position The current (measured) joint positions.
+         * \param joint_velocity The current (measured) joint velocities.
+         * \param joint_torque The joint space torques. 
+         *                     Depending on the user's choice, this array can represent commanded or measured joint torques.
+         *                     A particular choice depends on the available sensors in robot's joint. 
+         *                     For more details see the above-referenced article.
+         *
+         * Output parameters:
+         * \param external_wrench The estimated external wrench applied on the robot's end-effector.
+         *                        The wrench will be expressed w.r.t. end-effector's frame.
+         *
+         * @return error/success code
+         */
+        int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench);
+
+        /// @copydoc KDL::SolverI::updateInternalDataStructures()
+        virtual void updateInternalDataStructures();
+
+    private:
+        const Chain &CHAIN;
+        const double DT_SEC, FILTER_CONST;
+        unsigned int nj, ns;
+        JntSpaceInertiaMatrix jnt_mass_matrix, previous_jnt_mass_matrix;
+        JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque;
+        Eigen::VectorXd ESTIMATION_GAIN;
+        ChainDynParam dynparam_solver;
+        ChainJntToJacSolver jacobian_solver;
+        ChainFkSolverPos_recursive fk_pos_solver;
+    };
+}
+
+#endif

From 8883ea04c60e746b6f1a4363d23a0f86e38058af Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sat, 2 Jan 2021 23:22:13 +0100
Subject: [PATCH 02/26] [Ext Wrench Estimator] Getter for the external joint
 torques added

---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 7 +++++++
 orocos_kdl/src/chainexternalwrenchestimator.hpp | 3 +++
 2 files changed, 10 insertions(+)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index d496076ba..8c002dd17 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -159,4 +159,11 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
     return (error = E_NOERROR);
 }
 
+// Getter for the torques felt in the robot's joints due to the external wrench being applied on the robot
+void ChainExternalWrenchEstimator::getEstimatedJntTorque(JntArray &external_joint_torque)
+{
+    assert(external_joint_torque.rows() == filtered_estimated_ext_torque.rows());
+    external_joint_torque = filtered_estimated_ext_torque;
+}
+
 } // namespace
diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp
index 8930c39fa..75ac23fea 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.hpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp
@@ -78,6 +78,9 @@ namespace KDL {
          */
         int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench);
 
+        // Returns the torques felt in the robot's joints as a result of the external wrench being applied on the robot.
+        void getEstimatedJntTorque(JntArray &external_joint_torque);
+
         /// @copydoc KDL::SolverI::updateInternalDataStructures()
         virtual void updateInternalDataStructures();
 

From ab00e8c567fc09e933b9df6cb336ec3254ac2ade Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sat, 2 Jan 2021 23:25:33 +0100
Subject: [PATCH 03/26] [Solver Tests] Test for internal data structures of the
 wrench estimator

---
 orocos_kdl/tests/solvertest.cpp | 8 ++++++++
 orocos_kdl/tests/solvertest.hpp | 1 +
 2 files changed, 9 insertions(+)

diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp
index 57b275463..5f64773ec 100644
--- a/orocos_kdl/tests/solvertest.cpp
+++ b/orocos_kdl/tests/solvertest.cpp
@@ -247,6 +247,7 @@ void SolverTest::UpdateChainTest()
     ChainIdSolver_RNE idsolver1(chain2, Vector::Zero());
     unsigned int nr_of_constraints = 4;
     ChainHdSolver_Vereshchagin hdsolver(chain2,Twist::Zero(),4);
+    ChainExternalWrenchEstimator extwrench_estimator(chain2,Vector::Zero(), 100.0, 30.0, 0.5);
 
     JntArray q_in(chain2.getNrOfJoints());
     JntArray q_in2(chain2.getNrOfJoints());
@@ -266,6 +267,7 @@ void SolverTest::UpdateChainTest()
     FrameVel T2;
     Wrenches wrenches(chain2.getNrOfSegments());
     JntSpaceInertiaMatrix m(chain2.getNrOfJoints());
+    Wrench wrench_out;
 
     Jacobian alpha(nr_of_constraints - 1);
     JntArray beta(nr_of_constraints - 1);
@@ -291,6 +293,7 @@ void SolverTest::UpdateChainTest()
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToCoriolis(q_in, q_in2, q_out));
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToGravity(q_in, q_out));
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToMass(q_in, m));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out));
 
     jacsolver1.updateInternalDataStructures();
     jacdotsolver1.updateInternalDataStructures();
@@ -304,6 +307,7 @@ void SolverTest::UpdateChainTest()
     idsolver1.updateInternalDataStructures();
     hdsolver.updateInternalDataStructures();
     dynparam.updateInternalDataStructures();
+    extwrench_estimator.updateInternalDataStructures();
 
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments()));
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments()));
@@ -322,6 +326,7 @@ void SolverTest::UpdateChainTest()
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out));
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out));
 
     q_in.resize(chain2.getNrOfJoints());
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments()));
@@ -339,10 +344,12 @@ void SolverTest::UpdateChainTest()
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out));
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out));
     q_in2.resize(chain2.getNrOfJoints());
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out));
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
+    CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out));
     wrenches.resize(chain2.getNrOfSegments());
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2));
     CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau));
@@ -379,6 +386,7 @@ void SolverTest::UpdateChainTest()
     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out));
     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out));
     CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m));
+    CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out));
 }
 void SolverTest::FkPosAndJacTest()
 {
diff --git a/orocos_kdl/tests/solvertest.hpp b/orocos_kdl/tests/solvertest.hpp
index 699c0cba0..6d59f5d67 100644
--- a/orocos_kdl/tests/solvertest.hpp
+++ b/orocos_kdl/tests/solvertest.hpp
@@ -20,6 +20,7 @@
 #include <chaindynparam.hpp>
 #include <chainidsolver_recursive_newton_euler.hpp>
 #include <chainfdsolver_recursive_newton_euler.hpp>
+#include <chainexternalwrenchestimator.hpp>
 #include <utilities/ldl_solver_eigen.hpp>
 
 

From 406facb18a3dab6791a106eb5dbc885e7d626275 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sat, 2 Jan 2021 23:29:51 +0100
Subject: [PATCH 04/26] [Solver Tests] Main accuracy-test for the wrench
 estimator added

---
 orocos_kdl/tests/solvertest.cpp | 176 ++++++++++++++++++++++++++++++++
 orocos_kdl/tests/solvertest.hpp |   2 +
 2 files changed, 178 insertions(+)

diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp
index 5f64773ec..c730f4b2b 100644
--- a/orocos_kdl/tests/solvertest.cpp
+++ b/orocos_kdl/tests/solvertest.cpp
@@ -1558,3 +1558,179 @@ void SolverTest::FdAndVereshchaginSolversConsistencyTest()
 
     return;
 }
+
+void SolverTest::ExternalWrenchEstimatorTest()
+{
+    /**
+     * Closed-loop test for the external wrench estimator class:
+     * Simple controlled behaviour of the robot subjected to an external force is simulated.
+     * The external wrench estimator is called in each iteration of the control loop 
+     * so to converge to final wrench value.
+     * In the end, estimated wrench is compared to the ground-truth values of the simulated wrench. 
+     */
+    std::cout << "KDL External Wrench Estimator Test" << std::endl;
+
+    int ret;
+    double eps = 0.85;
+    unsigned int nj = kukaLWR.getNrOfJoints();
+    unsigned int ns = kukaLWR.getNrOfSegments();
+    CPPUNIT_ASSERT(Equal(nj, ns)); // Current implementation of the Vereshchagin solver can only work with chains that have equal number of joints and segments
+
+    // Joint position, velocity, acceleration and torques
+    KDL::JntArray q(nj); // Joint state
+    KDL::JntArray qd(nj); // Joint state
+    KDL::JntArray q_desired(nj); // Joint state
+    KDL::JntArray qd_desired(nj); // Joint state
+    KDL::JntArray qdd(nj); // Resultant joint accelerations
+    KDL::JntArray qdd_command(nj); // Joint acceleration commands for RNE
+    KDL::JntArray command_torque(nj); // Control torque to actuate the robot
+    KDL::JntArray constraint_tau(nj); // It will result in zero in Vereshchagin for this test
+    KDL::JntArray ext_torque_reference(nj); // Ground-truth joint torques due to the external force applied on the end-effector
+    KDL::JntArray ext_torque_estimated(nj); // Estimated joint torques
+
+    // Random initial state
+    q(0) = 1.0;
+    q(1) = 0.0;
+    q(2) = 0.0;
+    q(3) = -1.57;
+    q(4) = 0.0;
+    q(5) = 1.57;
+    q(6) = -0.8;
+    q_desired = q; // desired state is the same as the initial
+
+    qd(0) = 0.2;
+    qd(1) = -0.1;
+    qd(2) = 0.3;
+    qd(3) = 0.5;
+    qd(4) = -0.1;
+    qd(5) = -0.15;
+    qd(6) = 0.9;
+
+    KDL::Wrenches f_ext_base (ns); // External Wrenches acting on the end-effector, expressed in base link coordinates
+    KDL::Wrenches f_ext_zero(ns); // Zero Wrenches
+
+    // Ground-truth external wrench acting on the end-effector
+    KDL::Vector f(10.0, -20.0, 30.0);
+    KDL::Vector n(0.0, 0.0, 0.0);
+    KDL::Wrench f_tool_reference(f, n); // expressed in local end-effector's frame
+    KDL::Wrench f_tool_estimated;
+
+    ChainFkSolverPos_recursive fksolverpos(kukaLWR);
+    ChainJntToJacSolver jacobian_solver(kukaLWR);
+    Frame end_effector_pose;
+    Jacobian jacobian_end_eff(nj);
+
+    fksolverpos.JntToCart(q, end_effector_pose);
+    f_ext_base[ns - 1] = end_effector_pose.M * f_tool_reference;
+
+    // Compute robot's jacobian for the end-effector frame, expressed in the base frame
+    jacobian_solver.JntToJac(q, jacobian_end_eff);
+
+    // Inverse Force Kinematics
+    Eigen::Matrix<double, 6, 1> wrench;
+    wrench(0) = f_ext_base[ns - 1](0);
+    wrench(1) = f_ext_base[ns - 1](1);
+    wrench(2) = f_ext_base[ns - 1](2);
+    wrench(3) = f_ext_base[ns - 1](3);
+    wrench(4) = f_ext_base[ns - 1](4);
+    wrench(5) = f_ext_base[ns - 1](5);
+    ext_torque_reference.data = jacobian_end_eff.data.transpose() * wrench;
+
+    // Arm root acceleration (robot's base mounted on an even surface)
+    // Note: Vereshchagin solver takes root acc. with opposite sign comparead to the above FD and RNE solvers
+    Vector linearAcc(0.0, 0.0, 9.81); Vector angularAcc(0.0, 0.0, 0.0);
+    Twist root_Acc(linearAcc, angularAcc);
+
+    // RNE ID solver for control purposes
+    KDL::ChainIdSolver_RNE IdSolver(kukaLWR, -linearAcc);
+
+    // Vereshchagin Hybrid Dynamics solver for simulation purposes
+    int numberOfConstraints = 6;
+    Jacobian alpha(numberOfConstraints); // Constraint Unit forces at the end-effector
+    JntArray beta(numberOfConstraints); // Acceleration energy at the end-effector
+    KDL::SetToZero(alpha); // Set to zero to deactivate all constraints
+    KDL::SetToZero(beta); // Set to zero to deactivate all constraints
+    ChainHdSolver_Vereshchagin constraintSolver(kukaLWR, root_Acc, numberOfConstraints);
+
+    // External Wrench Estimator
+    double sample_frequency = 10000.0; // Hz
+    double estimation_gain  = 30.0;
+    double filter_constant  = 0.5;
+    ChainExternalWrenchEstimator extwrench_estimator(kukaLWR, -linearAcc, sample_frequency, estimation_gain, filter_constant);
+    extwrench_estimator.setInitialMomentum(q, qd); // sets the offset for future estimation (momentum calculation)
+
+    // ##########################################################################################
+    // Control and simulation
+    // ##########################################################################################
+
+    // Control gains for a simple PID controller
+    double k_p = 0.01; // Proportional
+    double k_i = 0.01; // Integral
+    double k_d = 0.01; // Derivative
+
+    // Time required to complete the task (keep joints static -> not moving)
+    double simulationTime = 0.23; // in seconds
+    double timeDelta = 1.0 / sample_frequency; // unit of seconds
+
+    // Iteration-limited control loop
+    JntArray position_error(nj);
+    JntArray integral_error(nj); // accumulated position error
+    JntArray velocity_error(nj);
+    for (double t = 0.0; t <= simulationTime; t = t + timeDelta)
+    {
+        // Compute errors
+        position_error.data = q_desired.data - q.data;
+        integral_error.data += timeDelta * position_error.data;
+        velocity_error.data = qd_desired.data - qd.data;
+
+        // Compute joint commands (simple PID controller)
+        qdd_command.data = k_p * position_error.data + k_i * integral_error.data + k_d * velocity_error.data;
+
+        // Compute necessary joint torques to maintain desired state, given command accelerations (hide external wrench from this dynamics calculation)
+        ret = IdSolver.CartToJnt(q, qd, qdd_command, f_ext_zero, command_torque);
+        if (ret < 0)
+        {
+            std::cout << "KDL RNE solver ERROR: " << ret << std::endl;
+            break;
+        }
+
+        // Compute resultant joint accelerations that simulate robot's behaviour, given the command torques (add external wrench in this dynamics calculation)
+        ret = constraintSolver.CartToJnt(q, qd, qdd, alpha, beta, f_ext_base, command_torque, constraint_tau);
+        if (ret < 0)
+        {
+            std::cout << "KDL Vereshchagin solver ERROR: " << ret << std::endl;
+            break;
+        }
+        
+        // State integration: integrate from model accelerations to next joint state (positions and velocities)
+        qd.data = qd.data + qdd.data * timeDelta; // Euler Forward
+        q.data = q.data + (qd.data - qdd.data * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
+        
+        // Estimate external wrench
+        extwrench_estimator.JntToExtWrench(q, qd, command_torque, f_tool_estimated);
+        extwrench_estimator.getEstimatedJntTorque(ext_torque_estimated);
+    }
+
+    // ##################################################################################
+    // Final comparison
+    // ##################################################################################
+    
+    // Estimated Wrench
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(0), f_tool_reference(0), eps));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(1), f_tool_reference(1), eps));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(2), f_tool_reference(2), eps));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(3), f_tool_reference(3), eps));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(4), f_tool_reference(4), eps));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(5), f_tool_reference(5), eps));
+
+    // Estimated Joint torques
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(0), ext_torque_reference(0), eps));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(1), ext_torque_reference(1), eps));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(2), ext_torque_reference(2), eps));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(3), ext_torque_reference(3), eps));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(4), ext_torque_reference(4), eps));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(5), ext_torque_reference(5), eps));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(6), ext_torque_reference(6), eps));
+
+    return;
+}
diff --git a/orocos_kdl/tests/solvertest.hpp b/orocos_kdl/tests/solvertest.hpp
index 6d59f5d67..8da18319f 100644
--- a/orocos_kdl/tests/solvertest.hpp
+++ b/orocos_kdl/tests/solvertest.hpp
@@ -34,6 +34,7 @@ class SolverTest : public CppUnit::TestFixture
     CPPUNIT_TEST(FkVelAndIkVelTest );
     CPPUNIT_TEST(FkPosAndIkPosTest );
     CPPUNIT_TEST(VereshchaginTest );
+    CPPUNIT_TEST(ExternalWrenchEstimatorTest );
     CPPUNIT_TEST(IkSingularValueTest );
     CPPUNIT_TEST(IkVelSolverWDLSTest );
     CPPUNIT_TEST(FkPosVectTest );
@@ -54,6 +55,7 @@ class SolverTest : public CppUnit::TestFixture
     void FkVelAndIkVelTest();
     void FkPosAndIkPosTest();
     void VereshchaginTest();
+    void ExternalWrenchEstimatorTest();
     void IkSingularValueTest() ;
     void IkVelSolverWDLSTest();
     void FkPosVectTest();

From 9993196186451ce42740590e5efc47cefe3fd233 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sun, 3 Jan 2021 12:27:32 +0100
Subject: [PATCH 05/26] [Ext Wrench Estimator Test] Integrator updated (more
 stable version)

---
 orocos_kdl/tests/solvertest.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp
index c730f4b2b..a736816e1 100644
--- a/orocos_kdl/tests/solvertest.cpp
+++ b/orocos_kdl/tests/solvertest.cpp
@@ -1704,7 +1704,7 @@ void SolverTest::ExternalWrenchEstimatorTest()
         
         // State integration: integrate from model accelerations to next joint state (positions and velocities)
         qd.data = qd.data + qdd.data * timeDelta; // Euler Forward
-        q.data = q.data + (qd.data - qdd.data * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
+        q.data = q.data + qd.data * timeDelta; // Symplectic Euler
         
         // Estimate external wrench
         extwrench_estimator.JntToExtWrench(q, qd, command_torque, f_tool_estimated);

From 0d2e65e3c9bb067987666c08a2cf328e2de41843 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sun, 3 Jan 2021 22:11:51 +0100
Subject: [PATCH 06/26] [Ext Wrench Estimator] Eigen SVD replaced with KDL's
 internal SVD

Now svd_eigen_HH solver is used.
---
 .../src/chainexternalwrenchestimator.cpp      | 39 ++++++++++++-------
 .../src/chainexternalwrenchestimator.hpp      | 17 ++++++--
 2 files changed, 39 insertions(+), 17 deletions(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index 8c002dd17..c2c409dda 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -21,12 +21,18 @@
 #include "chainexternalwrenchestimator.hpp"
 namespace KDL {
 
-ChainExternalWrenchEstimator::ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant) :
+ChainExternalWrenchEstimator::ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps, const int maxiter) :
     CHAIN(chain),
     DT_SEC(1.0 / sample_frequency), FILTER_CONST(filter_constant),
+    svd_eps(eps),
+    svd_maxiter(maxiter),
     nj(CHAIN.getNrOfJoints()), ns(CHAIN.getNrOfSegments()),
     jnt_mass_matrix(nj), previous_jnt_mass_matrix(nj),
     initial_jnt_momentum(nj), estimated_momentum_integral(nj), filtered_estimated_ext_torque(nj),
+    jacobian_end_eff(nj),
+    jacobian_end_eff_t(Eigen::MatrixXd::Zero(nj, 6)), jacobian_end_eff_t_inv(Eigen::MatrixXd::Zero(6, nj)), 
+    U(Eigen::MatrixXd::Zero(nj, 6)), V(Eigen::MatrixXd::Zero(6, 6)),
+    S(Eigen::VectorXd::Zero(6)), S_inv(Eigen::VectorXd::Zero(6)), tmp(Eigen::VectorXd::Zero(6)),
     ESTIMATION_GAIN(Eigen::VectorXd::Constant(nj, estimation_gain)),
     dynparam_solver(CHAIN, gravity),
     jacobian_solver(CHAIN),
@@ -43,6 +49,14 @@ void ChainExternalWrenchEstimator::updateInternalDataStructures()
     initial_jnt_momentum.resize(nj);
     estimated_momentum_integral.resize(nj);
     filtered_estimated_ext_torque.resize(nj);
+    jacobian_end_eff.resize(nj);
+    jacobian_end_eff_t.conservativeResizeLike(MatrixXd::Zero(nj, 6));
+    jacobian_end_eff_t_inv.conservativeResizeLike(MatrixXd::Zero(6, nj));
+    U.conservativeResizeLike(MatrixXd::Zero(nj, 6));
+    V.conservativeResizeLike(MatrixXd::Zero(6, 6));
+    S.conservativeResizeLike(VectorXd::Zero(6));
+    S_inv.conservativeResizeLike(VectorXd::Zero(6));
+    tmp.conservativeResizeLike(VectorXd::Zero(6));
     ESTIMATION_GAIN.conservativeResizeLike(Eigen::VectorXd::Constant(nj, ESTIMATION_GAIN(0)));
     dynparam_solver.updateInternalDataStructures();
     jacobian_solver.updateInternalDataStructures();
@@ -131,7 +145,6 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
     if (solver_result != 0) return solver_result;
 
     // Compute robot's jacobian for the end-effector frame, expressed in the base frame
-    Jacobian jacobian_end_eff(nj);
     solver_result = jacobian_solver.JntToJac(joint_position, jacobian_end_eff);
     if (solver_result != 0) return solver_result;
 
@@ -139,20 +152,20 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
     // This part can be commented out if the user wants its estimated wrench to be expressed w.r.t. base frame 
     jacobian_end_eff.changeBase(end_eff_frame.M.Inverse()); // Jacobian is now expressed w.r.t. end-effector frame
 
-    // Compute SVD of the jacobian using Eigen functions
-    Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian_end_eff.data.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV);
+    // SVD of "Jac^T" with maximum iterations "maxiter": Jac^T = U * S^-1 * V^T
+    jacobian_end_eff_t = jacobian_end_eff.data.transpose();
+    solver_result = svd_eigen_HH(jacobian_end_eff_t, U, S, V, tmp, svd_maxiter);
+    if (solver_result != 0) return (error = E_SVD_FAILED);
 
-    // Invert singular values
-    Eigen::VectorXd singular_inv(svd.singularValues());
-    for (int j = 0; j < singular_inv.size(); ++j)
-        singular_inv(j) = (singular_inv(j) < 1e-8) ? 0.0 : 1.0 / singular_inv(j);
+    // Invert singular values: S^-1
+    for (int i = 0; i < S.size(); ++i)
+        S_inv(i) = (std::fabs(S(i)) < svd_eps) ? 0.0 : 1.0 / S(i);
 
-    // Compose SVD
-    Eigen::MatrixXd jacobian_end_eff_inv;
-    jacobian_end_eff_inv.noalias() = svd.matrixV() * singular_inv.matrix().asDiagonal() * svd.matrixU().adjoint();
+    // Compose the inverse: (Jac^T)^-1 = V * S^-1 * U^T
+    jacobian_end_eff_t_inv = V * S_inv.asDiagonal() * U.adjoint();
 
-    // Compute end-effector's Cartesian wrench from the estimated joint torques
-    Eigen::VectorXd estimated_wrench = jacobian_end_eff_inv * filtered_estimated_ext_torque.data;
+    // Compute end-effector's Cartesian wrench from the estimated joint torques: (Jac^T)^-1 * ext_tau
+    Vector6d estimated_wrench = jacobian_end_eff_t_inv * filtered_estimated_ext_torque.data;
     for (int i = 0; i < 6; i++) 
         external_wrench(i) = estimated_wrench(i);
 
diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp
index 75ac23fea..5a429409c 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.hpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp
@@ -22,7 +22,7 @@
 #define KDL_EXTERNAL_WRENCH_ESTIMATOR_HPP
 
 #include <Eigen/Core>
-#include <Eigen/SVD> 
+#include "utilities/svd_eigen_HH.hpp"
 #include "chaindynparam.hpp"
 #include "chainjnttojacsolver.hpp"
 #include "chainfksolverpos_recursive.hpp"
@@ -40,6 +40,8 @@ namespace KDL {
      */
     class ChainExternalWrenchEstimator : public SolverI
     {
+        typedef Eigen::Matrix<double, 6, 1 > Vector6d;
+
     public:
         /**
          * Constructor for the estimator, it will allocate all the necessary memory
@@ -49,8 +51,10 @@ namespace KDL {
          * \param filter_constant Parameter defining how much the estimated signal should be filtered by the low-pass filter.
          *                        This input value should be between 0 and 1. Higher the number means more noise needs to be filtered-out.
          *                        The filter can be turned off by setting this value to 0.
+         * \param eps If a SVD-singular value is below this value, its inverse is set to zero. Default: 0.00001
+         * \param maxiter Maximum iterations for the SVD computations. Default: 150.
          */
-        ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant);
+        ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps = 0.00001, const int maxiter = 150);
         ~ChainExternalWrenchEstimator(){};
 
         /**
@@ -87,10 +91,15 @@ namespace KDL {
     private:
         const Chain &CHAIN;
         const double DT_SEC, FILTER_CONST;
+        double svd_eps;
+        int svd_maxiter;
         unsigned int nj, ns;
         JntSpaceInertiaMatrix jnt_mass_matrix, previous_jnt_mass_matrix;
-        JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque;
-        Eigen::VectorXd ESTIMATION_GAIN;
+        JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque, 
+                 gravity_torque, coriolis_torque, total_torque, estimated_ext_torque;
+        Jacobian jacobian_end_eff;
+        Eigen::MatrixXd jacobian_end_eff_t, jacobian_end_eff_t_inv, U, V;
+        Eigen::VectorXd S, S_inv, tmp, ESTIMATION_GAIN;
         ChainDynParam dynparam_solver;
         ChainJntToJacSolver jacobian_solver;
         ChainFkSolverPos_recursive fk_pos_solver;

From e975d3016c892985c70d7988a3ac5b4535aa57f0 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sun, 3 Jan 2021 22:12:44 +0100
Subject: [PATCH 07/26] [Ext Wrench Estimator] Setters for SVD- eps and maxiter
 parameters added

---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 12 ++++++++++++
 orocos_kdl/src/chainexternalwrenchestimator.hpp |  6 ++++++
 2 files changed, 18 insertions(+)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index c2c409dda..6056ae45e 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -80,6 +80,18 @@ int ChainExternalWrenchEstimator::setInitialMomentum(const JntArray &joint_posit
     return (error = E_NOERROR);
 }
 
+// Sets singular-value eps parameter for the SVD calculation
+void ChainExternalWrenchEstimator::setSVDEps(const double eps_in)
+{
+    svd_eps = eps_in;
+}
+
+// Sets maximum iteration parameter for the SVD calculation
+void ChainExternalWrenchEstimator::setSVDMaxIter(const int maxiter_in)
+{
+    svd_maxiter = maxiter_in;
+}
+
 // This method calculates the external wrench that is applied on the robot's end-effector.
 int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench)
 {
diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp
index 5a429409c..ec3a4005b 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.hpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp
@@ -64,6 +64,12 @@ namespace KDL {
          */
         int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity);
 
+        // Sets singular-value eps parameter for the SVD calculation
+        void setSVDEps(const double eps_in);
+
+        // Sets maximum iteration parameter for the SVD calculation
+        void setSVDMaxIter(const int maxiter_in);
+
         /**
          * This method calculates the external wrench that is applied on the robot's end-effector.
          * Input parameters:

From c4d02acc8c8e54a444bc3511e0e17c1321f09118 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sun, 3 Jan 2021 22:15:07 +0100
Subject: [PATCH 08/26] [Ext Wrench Estimator] Some variables predefined in
 constructor

To avoid run-time memory allocation
---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 12 +++++++-----
 orocos_kdl/src/chainexternalwrenchestimator.hpp |  2 +-
 2 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index 6056ae45e..0a0952548 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -27,8 +27,9 @@ ChainExternalWrenchEstimator::ChainExternalWrenchEstimator(const Chain &chain, c
     svd_eps(eps),
     svd_maxiter(maxiter),
     nj(CHAIN.getNrOfJoints()), ns(CHAIN.getNrOfSegments()),
-    jnt_mass_matrix(nj), previous_jnt_mass_matrix(nj),
+    jnt_mass_matrix(nj), previous_jnt_mass_matrix(nj), jnt_mass_matrix_dot(nj),
     initial_jnt_momentum(nj), estimated_momentum_integral(nj), filtered_estimated_ext_torque(nj),
+    gravity_torque(nj), coriolis_torque(nj), total_torque(nj), estimated_ext_torque(nj),
     jacobian_end_eff(nj),
     jacobian_end_eff_t(Eigen::MatrixXd::Zero(nj, 6)), jacobian_end_eff_t_inv(Eigen::MatrixXd::Zero(6, nj)), 
     U(Eigen::MatrixXd::Zero(nj, 6)), V(Eigen::MatrixXd::Zero(6, 6)),
@@ -46,9 +47,14 @@ void ChainExternalWrenchEstimator::updateInternalDataStructures()
     ns = CHAIN.getNrOfSegments();
     jnt_mass_matrix.resize(nj);
     previous_jnt_mass_matrix.resize(nj);
+    jnt_mass_matrix_dot.resize(nj);
     initial_jnt_momentum.resize(nj);
     estimated_momentum_integral.resize(nj);
     filtered_estimated_ext_torque.resize(nj);
+    gravity_torque.resize(nj);
+    coriolis_torque.resize(nj);
+    total_torque.resize(nj);
+    estimated_ext_torque.resize(nj);
     jacobian_end_eff.resize(nj);
     jacobian_end_eff_t.conservativeResizeLike(MatrixXd::Zero(nj, 6));
     jacobian_end_eff_t_inv.conservativeResizeLike(MatrixXd::Zero(6, nj));
@@ -115,7 +121,6 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
      */
 
     // Calculate decomposed robot's dynamics
-    JntArray gravity_torque(nj), coriolis_torque(nj);
     int solver_result = dynparam_solver.JntToMass(joint_position, jnt_mass_matrix);
     if (solver_result != 0) return solver_result;
     solver_result = dynparam_solver.JntToCoriolis(joint_position, joint_velocity, coriolis_torque);
@@ -124,21 +129,18 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
     if (solver_result != 0) return solver_result;
 
     // Calculate the change of robot's inertia in the joint space
-    JntSpaceInertiaMatrix jnt_mass_matrix_dot(nj);
     jnt_mass_matrix_dot.data = (jnt_mass_matrix.data - previous_jnt_mass_matrix.data) / DT_SEC;
 
     // Save data for the next iteration
     previous_jnt_mass_matrix.data = jnt_mass_matrix.data;
 
     // Calculate total torque exerted on the joint
-    JntArray total_torque(nj);
     total_torque.data = joint_torque.data - gravity_torque.data - coriolis_torque.data + jnt_mass_matrix_dot.data * joint_velocity.data;
 
     // Accumulate main integral
     estimated_momentum_integral.data += (total_torque.data + filtered_estimated_ext_torque.data) * DT_SEC;
 
     // Estimate external joint torque
-    JntArray estimated_ext_torque(nj);
     estimated_ext_torque.data = ESTIMATION_GAIN.asDiagonal() * (jnt_mass_matrix.data * joint_velocity.data - estimated_momentum_integral.data - initial_jnt_momentum.data);
 
     // First order low-pass filter: filter out the noise from the estimated signal
diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp
index ec3a4005b..bf049112f 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.hpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp
@@ -100,7 +100,7 @@ namespace KDL {
         double svd_eps;
         int svd_maxiter;
         unsigned int nj, ns;
-        JntSpaceInertiaMatrix jnt_mass_matrix, previous_jnt_mass_matrix;
+        JntSpaceInertiaMatrix jnt_mass_matrix, previous_jnt_mass_matrix, jnt_mass_matrix_dot;
         JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque, 
                  gravity_torque, coriolis_torque, total_torque, estimated_ext_torque;
         Jacobian jacobian_end_eff;

From 60709fca3d426a2166541565b75e8ceeb825bf3b Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sun, 3 Jan 2021 22:40:14 +0100
Subject: [PATCH 09/26] [Ext Wrench Estimator] Brief description updated

---
 orocos_kdl/src/chainexternalwrenchestimator.hpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp
index bf049112f..2219f8640 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.hpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp
@@ -37,6 +37,8 @@ namespace KDL {
      * S. Haddadin, A. De Luca and A. Albu-Schäffer,
      * "Robot Collisions: A Survey on Detection, Isolation, and Identification,"
      * in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017.
+     * 
+     * Note: This component assumes that the external wrench is applied on the end-effector (last) link of the robot's chain.
      */
     class ChainExternalWrenchEstimator : public SolverI
     {

From 0df471f69742823202fe51803f3406f309533e6f Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sun, 3 Jan 2021 23:58:40 +0100
Subject: [PATCH 10/26] [Ext Wrench Estimator Test] Ext torque comparison
 removed

Since the robot's null space is moving throughtout simulation, this comparison cannot always give us proper test
---
 orocos_kdl/tests/solvertest.cpp | 12 ------------
 1 file changed, 12 deletions(-)

diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp
index a736816e1..43517d81e 100644
--- a/orocos_kdl/tests/solvertest.cpp
+++ b/orocos_kdl/tests/solvertest.cpp
@@ -1708,14 +1708,11 @@ void SolverTest::ExternalWrenchEstimatorTest()
         
         // Estimate external wrench
         extwrench_estimator.JntToExtWrench(q, qd, command_torque, f_tool_estimated);
-        extwrench_estimator.getEstimatedJntTorque(ext_torque_estimated);
     }
 
     // ##################################################################################
     // Final comparison
     // ##################################################################################
-    
-    // Estimated Wrench
     CPPUNIT_ASSERT(Equal(f_tool_estimated(0), f_tool_reference(0), eps));
     CPPUNIT_ASSERT(Equal(f_tool_estimated(1), f_tool_reference(1), eps));
     CPPUNIT_ASSERT(Equal(f_tool_estimated(2), f_tool_reference(2), eps));
@@ -1723,14 +1720,5 @@ void SolverTest::ExternalWrenchEstimatorTest()
     CPPUNIT_ASSERT(Equal(f_tool_estimated(4), f_tool_reference(4), eps));
     CPPUNIT_ASSERT(Equal(f_tool_estimated(5), f_tool_reference(5), eps));
 
-    // Estimated Joint torques
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(0), ext_torque_reference(0), eps));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(1), ext_torque_reference(1), eps));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(2), ext_torque_reference(2), eps));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(3), ext_torque_reference(3), eps));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(4), ext_torque_reference(4), eps));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(5), ext_torque_reference(5), eps));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(6), ext_torque_reference(6), eps));
-
     return;
 }

From 2f2ac0894bc93182a817ce58e0342a72a05e9397 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <djordje.vukcevic@h-brs.de>
Date: Sat, 9 Jan 2021 13:34:10 +0100
Subject: [PATCH 11/26] [Ext Wrench Estimator] Format inline returns

Co-authored-by: Matthijs van der Burgh <matthijs.vander.burgh@live.nl>
---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index 0a0952548..c40f442da 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -122,11 +122,14 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
 
     // Calculate decomposed robot's dynamics
     int solver_result = dynparam_solver.JntToMass(joint_position, jnt_mass_matrix);
-    if (solver_result != 0) return solver_result;
+    if (solver_result != 0)
+        return solver_result;
     solver_result = dynparam_solver.JntToCoriolis(joint_position, joint_velocity, coriolis_torque);
-    if (solver_result != 0) return solver_result;
+    if (solver_result != 0)
+        return solver_result;
     solver_result = dynparam_solver.JntToGravity(joint_position, gravity_torque);
-    if (solver_result != 0) return solver_result;
+    if (solver_result != 0)
+        return solver_result;
 
     // Calculate the change of robot's inertia in the joint space
     jnt_mass_matrix_dot.data = (jnt_mass_matrix.data - previous_jnt_mass_matrix.data) / DT_SEC;

From 662d472544307f611361123af87e4ed7f331128f Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <djordje.vukcevic@h-brs.de>
Date: Sat, 9 Jan 2021 13:34:34 +0100
Subject: [PATCH 12/26] [Ext Wrench Estimator] Format inline returns

Co-authored-by: Matthijs van der Burgh <matthijs.vander.burgh@live.nl>
---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index c40f442da..3ac710e29 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -159,7 +159,8 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
     // Compute robot's end-effector frame, expressed in the base frame
     Frame end_eff_frame;
     solver_result = fk_pos_solver.JntToCart(joint_position, end_eff_frame);
-    if (solver_result != 0) return solver_result;
+    if (solver_result != 0)
+        return solver_result;
 
     // Compute robot's jacobian for the end-effector frame, expressed in the base frame
     solver_result = jacobian_solver.JntToJac(joint_position, jacobian_end_eff);

From 0c0984d75d7edd7aa6e7f142f5d3d96ad395e3b1 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <djordje.vukcevic@h-brs.de>
Date: Sat, 9 Jan 2021 13:34:45 +0100
Subject: [PATCH 13/26] [Ext Wrench Estimator] Format inline returns

Co-authored-by: Matthijs van der Burgh <matthijs.vander.burgh@live.nl>
---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index 3ac710e29..f21632575 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -164,7 +164,8 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
 
     // Compute robot's jacobian for the end-effector frame, expressed in the base frame
     solver_result = jacobian_solver.JntToJac(joint_position, jacobian_end_eff);
-    if (solver_result != 0) return solver_result;
+    if (solver_result != 0)
+        return solver_result;
 
     // Transform the jacobian from the base frame to the end-effector frame. 
     // This part can be commented out if the user wants its estimated wrench to be expressed w.r.t. base frame 

From 9e92dd0008f0ad2d22d8daf9b01991ccb6915ed3 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <djordje.vukcevic@h-brs.de>
Date: Sat, 9 Jan 2021 13:35:00 +0100
Subject: [PATCH 14/26] [Ext Wrench Estimator] Format inline returns

Co-authored-by: Matthijs van der Burgh <matthijs.vander.burgh@live.nl>
---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index f21632575..68d0ecf98 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -174,7 +174,8 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
     // SVD of "Jac^T" with maximum iterations "maxiter": Jac^T = U * S^-1 * V^T
     jacobian_end_eff_t = jacobian_end_eff.data.transpose();
     solver_result = svd_eigen_HH(jacobian_end_eff_t, U, S, V, tmp, svd_maxiter);
-    if (solver_result != 0) return (error = E_SVD_FAILED);
+    if (solver_result != 0)
+        return (error = E_SVD_FAILED);
 
     // Invert singular values: S^-1
     for (int i = 0; i < S.size(); ++i)

From f06808d237acdaab5ab7b4f15c2dae1257785090 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <djordje.vukcevic@h-brs.de>
Date: Sat, 9 Jan 2021 13:36:21 +0100
Subject: [PATCH 15/26] [Ext Wrench Estimator] Update hpp flags

Co-authored-by: Matthijs van der Burgh <matthijs.vander.burgh@live.nl>
---
 orocos_kdl/src/chainexternalwrenchestimator.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp
index 2219f8640..1c42e74f8 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.hpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp
@@ -18,8 +18,8 @@
 // License along with this library; if not, write to the Free Software
 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
-#ifndef KDL_EXTERNAL_WRENCH_ESTIMATOR_HPP
-#define KDL_EXTERNAL_WRENCH_ESTIMATOR_HPP
+#ifndef KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
+#define KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
 
 #include <Eigen/Core>
 #include "utilities/svd_eigen_HH.hpp"

From 7a5343705c83754fdea4ce9a9e54f2e328f3922c Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <djordje.vukcevic@h-brs.de>
Date: Sat, 9 Jan 2021 13:36:45 +0100
Subject: [PATCH 16/26] [Ext Wrench Estimator] Format inline returns

Co-authored-by: Matthijs van der Burgh <matthijs.vander.burgh@live.nl>
---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index 68d0ecf98..6b4a699b8 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -111,8 +111,10 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
      */
 
     // Check sizes first
-    if (nj != CHAIN.getNrOfJoints() || ns != CHAIN.getNrOfSegments()) return (error = E_NOT_UP_TO_DATE);
-    if (joint_position.rows() != nj || joint_velocity.rows() != nj || joint_torque.rows() != nj) return (error = E_SIZE_MISMATCH);
+    if (nj != CHAIN.getNrOfJoints() || ns != CHAIN.getNrOfSegments())
+        return (error = E_NOT_UP_TO_DATE);
+    if (joint_position.rows() != nj || joint_velocity.rows() != nj || joint_torque.rows() != nj)
+        return (error = E_SIZE_MISMATCH);
 
     /**
      * =======================================================================================================================

From 7c5ca3efd9e220b081de037eea96d7be71ee2939 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <djordje.vukcevic@h-brs.de>
Date: Sat, 9 Jan 2021 13:37:01 +0100
Subject: [PATCH 17/26] [Ext Wrench Estimator] Format inline returns

Co-authored-by: Matthijs van der Burgh <matthijs.vander.burgh@live.nl>
---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index 6b4a699b8..80e2a7d25 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -73,7 +73,8 @@ void ChainExternalWrenchEstimator::updateInternalDataStructures()
 int ChainExternalWrenchEstimator::setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity)
 {
     // Check sizes first
-    if (joint_position.rows() != nj || joint_velocity.rows() != nj) return (error = E_SIZE_MISMATCH);
+    if (joint_position.rows() != nj || joint_velocity.rows() != nj)
+        return (error = E_SIZE_MISMATCH);
 
     // Calculate robot's inertia and momentum in the joint space
     dynparam_solver.JntToMass(joint_position, jnt_mass_matrix);

From d57b003306e1be1abfdffb1abd2a669242b1b901 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <djordje.vukcevic@h-brs.de>
Date: Sat, 9 Jan 2021 13:38:21 +0100
Subject: [PATCH 18/26] [Ext Wrench Estimator] fix line space

Co-authored-by: Matthijs van der Burgh <matthijs.vander.burgh@live.nl>
---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index 80e2a7d25..e79c0358a 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -19,6 +19,7 @@
 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
 #include "chainexternalwrenchestimator.hpp"
+
 namespace KDL {
 
 ChainExternalWrenchEstimator::ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps, const int maxiter) :

From f31ca111b0cb92f05d81c49a87975d00f650721c Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sat, 9 Jan 2021 13:52:44 +0100
Subject: [PATCH 19/26] [Solver Test] Gravity acc. sign updated in
 ExtWrenchEstimator test

---
 orocos_kdl/tests/solvertest.cpp | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp
index 43517d81e..0c1c4d5d7 100644
--- a/orocos_kdl/tests/solvertest.cpp
+++ b/orocos_kdl/tests/solvertest.cpp
@@ -1637,12 +1637,10 @@ void SolverTest::ExternalWrenchEstimatorTest()
     ext_torque_reference.data = jacobian_end_eff.data.transpose() * wrench;
 
     // Arm root acceleration (robot's base mounted on an even surface)
-    // Note: Vereshchagin solver takes root acc. with opposite sign comparead to the above FD and RNE solvers
-    Vector linearAcc(0.0, 0.0, 9.81); Vector angularAcc(0.0, 0.0, 0.0);
-    Twist root_Acc(linearAcc, angularAcc);
+    Vector linearAcc(0.0, 0.0, -9.81); Vector angularAcc(0.0, 0.0, 0.0);
 
     // RNE ID solver for control purposes
-    KDL::ChainIdSolver_RNE IdSolver(kukaLWR, -linearAcc);
+    KDL::ChainIdSolver_RNE IdSolver(kukaLWR, linearAcc);
 
     // Vereshchagin Hybrid Dynamics solver for simulation purposes
     int numberOfConstraints = 6;
@@ -1650,7 +1648,8 @@ void SolverTest::ExternalWrenchEstimatorTest()
     JntArray beta(numberOfConstraints); // Acceleration energy at the end-effector
     KDL::SetToZero(alpha); // Set to zero to deactivate all constraints
     KDL::SetToZero(beta); // Set to zero to deactivate all constraints
-    ChainHdSolver_Vereshchagin constraintSolver(kukaLWR, root_Acc, numberOfConstraints);
+    Twist vereshchagin_root_Acc(-linearAcc, angularAcc); // Note: Vereshchagin solver takes root acc. with opposite sign comparead to the above FD and RNE solvers
+    ChainHdSolver_Vereshchagin constraintSolver(kukaLWR, vereshchagin_root_Acc, numberOfConstraints);
 
     // External Wrench Estimator
     double sample_frequency = 10000.0; // Hz

From bcaf74d0bcdae5f2662bd7cb346e1f9c6a06292e Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sat, 9 Jan 2021 14:19:29 +0100
Subject: [PATCH 20/26] [Solver Test] Gravity acc. sign fixed in
 ExtWrenchEstimator test

---
 orocos_kdl/tests/solvertest.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp
index 0c1c4d5d7..6a812afe2 100644
--- a/orocos_kdl/tests/solvertest.cpp
+++ b/orocos_kdl/tests/solvertest.cpp
@@ -1571,7 +1571,7 @@ void SolverTest::ExternalWrenchEstimatorTest()
     std::cout << "KDL External Wrench Estimator Test" << std::endl;
 
     int ret;
-    double eps = 0.85;
+    double eps = 0.7;
     unsigned int nj = kukaLWR.getNrOfJoints();
     unsigned int ns = kukaLWR.getNrOfSegments();
     CPPUNIT_ASSERT(Equal(nj, ns)); // Current implementation of the Vereshchagin solver can only work with chains that have equal number of joints and segments
@@ -1655,7 +1655,7 @@ void SolverTest::ExternalWrenchEstimatorTest()
     double sample_frequency = 10000.0; // Hz
     double estimation_gain  = 30.0;
     double filter_constant  = 0.5;
-    ChainExternalWrenchEstimator extwrench_estimator(kukaLWR, -linearAcc, sample_frequency, estimation_gain, filter_constant);
+    ChainExternalWrenchEstimator extwrench_estimator(kukaLWR, linearAcc, sample_frequency, estimation_gain, filter_constant);
     extwrench_estimator.setInitialMomentum(q, qd); // sets the offset for future estimation (momentum calculation)
 
     // ##########################################################################################

From 4e61210a4c685182bf8359f6d91d8112b7c3e926 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sat, 9 Jan 2021 14:30:53 +0100
Subject: [PATCH 21/26] [ExtWrench Estimator] naming for Jac matrix updated

---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 14 +++++++-------
 orocos_kdl/src/chainexternalwrenchestimator.hpp |  2 +-
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index e79c0358a..068b68b69 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -32,7 +32,7 @@ ChainExternalWrenchEstimator::ChainExternalWrenchEstimator(const Chain &chain, c
     initial_jnt_momentum(nj), estimated_momentum_integral(nj), filtered_estimated_ext_torque(nj),
     gravity_torque(nj), coriolis_torque(nj), total_torque(nj), estimated_ext_torque(nj),
     jacobian_end_eff(nj),
-    jacobian_end_eff_t(Eigen::MatrixXd::Zero(nj, 6)), jacobian_end_eff_t_inv(Eigen::MatrixXd::Zero(6, nj)), 
+    jacobian_end_eff_transpose(Eigen::MatrixXd::Zero(nj, 6)), jacobian_end_eff_transpose_inv(Eigen::MatrixXd::Zero(6, nj)), 
     U(Eigen::MatrixXd::Zero(nj, 6)), V(Eigen::MatrixXd::Zero(6, 6)),
     S(Eigen::VectorXd::Zero(6)), S_inv(Eigen::VectorXd::Zero(6)), tmp(Eigen::VectorXd::Zero(6)),
     ESTIMATION_GAIN(Eigen::VectorXd::Constant(nj, estimation_gain)),
@@ -57,8 +57,8 @@ void ChainExternalWrenchEstimator::updateInternalDataStructures()
     total_torque.resize(nj);
     estimated_ext_torque.resize(nj);
     jacobian_end_eff.resize(nj);
-    jacobian_end_eff_t.conservativeResizeLike(MatrixXd::Zero(nj, 6));
-    jacobian_end_eff_t_inv.conservativeResizeLike(MatrixXd::Zero(6, nj));
+    jacobian_end_eff_transpose.conservativeResizeLike(MatrixXd::Zero(nj, 6));
+    jacobian_end_eff_transpose_inv.conservativeResizeLike(MatrixXd::Zero(6, nj));
     U.conservativeResizeLike(MatrixXd::Zero(nj, 6));
     V.conservativeResizeLike(MatrixXd::Zero(6, 6));
     S.conservativeResizeLike(VectorXd::Zero(6));
@@ -176,8 +176,8 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
     jacobian_end_eff.changeBase(end_eff_frame.M.Inverse()); // Jacobian is now expressed w.r.t. end-effector frame
 
     // SVD of "Jac^T" with maximum iterations "maxiter": Jac^T = U * S^-1 * V^T
-    jacobian_end_eff_t = jacobian_end_eff.data.transpose();
-    solver_result = svd_eigen_HH(jacobian_end_eff_t, U, S, V, tmp, svd_maxiter);
+    jacobian_end_eff_transpose = jacobian_end_eff.data.transpose();
+    solver_result = svd_eigen_HH(jacobian_end_eff_transpose, U, S, V, tmp, svd_maxiter);
     if (solver_result != 0)
         return (error = E_SVD_FAILED);
 
@@ -186,10 +186,10 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
         S_inv(i) = (std::fabs(S(i)) < svd_eps) ? 0.0 : 1.0 / S(i);
 
     // Compose the inverse: (Jac^T)^-1 = V * S^-1 * U^T
-    jacobian_end_eff_t_inv = V * S_inv.asDiagonal() * U.adjoint();
+    jacobian_end_eff_transpose_inv = V * S_inv.asDiagonal() * U.adjoint();
 
     // Compute end-effector's Cartesian wrench from the estimated joint torques: (Jac^T)^-1 * ext_tau
-    Vector6d estimated_wrench = jacobian_end_eff_t_inv * filtered_estimated_ext_torque.data;
+    Vector6d estimated_wrench = jacobian_end_eff_transpose_inv * filtered_estimated_ext_torque.data;
     for (int i = 0; i < 6; i++) 
         external_wrench(i) = estimated_wrench(i);
 
diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp
index 1c42e74f8..ae87e0c4b 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.hpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp
@@ -106,7 +106,7 @@ namespace KDL {
         JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque, 
                  gravity_torque, coriolis_torque, total_torque, estimated_ext_torque;
         Jacobian jacobian_end_eff;
-        Eigen::MatrixXd jacobian_end_eff_t, jacobian_end_eff_t_inv, U, V;
+        Eigen::MatrixXd jacobian_end_eff_transpose, jacobian_end_eff_transpose_inv, U, V;
         Eigen::VectorXd S, S_inv, tmp, ESTIMATION_GAIN;
         ChainDynParam dynparam_solver;
         ChainJntToJacSolver jacobian_solver;

From a0451363e88ecdbcc3688c28c7795a305a68f35f Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Sun, 17 Jan 2021 23:03:37 +0100
Subject: [PATCH 22/26] [Ext Wrench Estimator] Unique error-return flags added

---
 .../src/chainexternalwrenchestimator.cpp      | 44 +++++++++++--------
 .../src/chainexternalwrenchestimator.hpp      | 10 +++++
 2 files changed, 36 insertions(+), 18 deletions(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index 068b68b69..bced16d2f 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -78,7 +78,9 @@ int ChainExternalWrenchEstimator::setInitialMomentum(const JntArray &joint_posit
         return (error = E_SIZE_MISMATCH);
 
     // Calculate robot's inertia and momentum in the joint space
-    dynparam_solver.JntToMass(joint_position, jnt_mass_matrix);
+    if (E_NOERROR != dynparam_solver.JntToMass(joint_position, jnt_mass_matrix))
+        return (error = E_DYNPARAMSOLVERMASS_FAILED);
+
     initial_jnt_momentum.data = jnt_mass_matrix.data * joint_velocity.data;
 
     // Reset data because of the new momentum offset
@@ -125,15 +127,14 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
      */
 
     // Calculate decomposed robot's dynamics
-    int solver_result = dynparam_solver.JntToMass(joint_position, jnt_mass_matrix);
-    if (solver_result != 0)
-        return solver_result;
-    solver_result = dynparam_solver.JntToCoriolis(joint_position, joint_velocity, coriolis_torque);
-    if (solver_result != 0)
-        return solver_result;
-    solver_result = dynparam_solver.JntToGravity(joint_position, gravity_torque);
-    if (solver_result != 0)
-        return solver_result;
+    if (E_NOERROR != dynparam_solver.JntToMass(joint_position, jnt_mass_matrix))
+        return (error = E_DYNPARAMSOLVERMASS_FAILED);
+
+    if (E_NOERROR != dynparam_solver.JntToCoriolis(joint_position, joint_velocity, coriolis_torque))
+        return (error = E_DYNPARAMSOLVERCORIOLIS_FAILED);
+
+    if (E_NOERROR != dynparam_solver.JntToGravity(joint_position, gravity_torque))
+        return (error = E_DYNPARAMSOLVERGRAVITY_FAILED);
 
     // Calculate the change of robot's inertia in the joint space
     jnt_mass_matrix_dot.data = (jnt_mass_matrix.data - previous_jnt_mass_matrix.data) / DT_SEC;
@@ -162,14 +163,12 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
     
     // Compute robot's end-effector frame, expressed in the base frame
     Frame end_eff_frame;
-    solver_result = fk_pos_solver.JntToCart(joint_position, end_eff_frame);
-    if (solver_result != 0)
-        return solver_result;
+    if (E_NOERROR != fk_pos_solver.JntToCart(joint_position, end_eff_frame))
+        return (error = E_FKSOLVERPOS_FAILED);
 
     // Compute robot's jacobian for the end-effector frame, expressed in the base frame
-    solver_result = jacobian_solver.JntToJac(joint_position, jacobian_end_eff);
-    if (solver_result != 0)
-        return solver_result;
+    if (E_NOERROR != jacobian_solver.JntToJac(joint_position, jacobian_end_eff))
+        return (error = E_JACSOLVER_FAILED);
 
     // Transform the jacobian from the base frame to the end-effector frame. 
     // This part can be commented out if the user wants its estimated wrench to be expressed w.r.t. base frame 
@@ -177,8 +176,7 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
 
     // SVD of "Jac^T" with maximum iterations "maxiter": Jac^T = U * S^-1 * V^T
     jacobian_end_eff_transpose = jacobian_end_eff.data.transpose();
-    solver_result = svd_eigen_HH(jacobian_end_eff_transpose, U, S, V, tmp, svd_maxiter);
-    if (solver_result != 0)
+    if (E_NOERROR != svd_eigen_HH(jacobian_end_eff_transpose, U, S, V, tmp, svd_maxiter))
         return (error = E_SVD_FAILED);
 
     // Invert singular values: S^-1
@@ -203,4 +201,14 @@ void ChainExternalWrenchEstimator::getEstimatedJntTorque(JntArray &external_join
     external_joint_torque = filtered_estimated_ext_torque;
 }
 
+const char* ChainExternalWrenchEstimator::strError(const int error) const
+{
+    if (E_FKSOLVERPOS_FAILED == error) return "Internally-used Forward Position Kinematics (Recursive) solver failed";
+    else if (E_JACSOLVER_FAILED == error) return "Internally-used Jacobian solver failed";
+    else if (E_DYNPARAMSOLVERMASS_FAILED == error) return "Internally-used Dynamics Parameters (Mass) solver failed";
+    else if (E_DYNPARAMSOLVERCORIOLIS_FAILED == error) return "Internally-used Dynamics Parameters (Coriolis) solver failed";
+    else if (E_DYNPARAMSOLVERGRAVITY_FAILED == error) return "Internally-used Dynamics Parameters (Gravity) solver failed";
+    else return SolverI::strError(error);
+}
+
 } // namespace
diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp
index ae87e0c4b..1a321b18e 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.hpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp
@@ -45,6 +45,13 @@ namespace KDL {
         typedef Eigen::Matrix<double, 6, 1 > Vector6d;
 
     public:
+
+        static const int E_FKSOLVERPOS_FAILED = -100; //! Internally-used Forward Position Kinematics (Recursive) solver failed
+        static const int E_JACSOLVER_FAILED = -101; //! Internally-used Jacobian solver failed
+        static const int E_DYNPARAMSOLVERMASS_FAILED = -102; //! Internally-used Dynamics Parameters (Mass) solver failed
+        static const int E_DYNPARAMSOLVERCORIOLIS_FAILED = -103; //! Internally-used Dynamics Parameters (Coriolis) solver failed
+        static const int E_DYNPARAMSOLVERGRAVITY_FAILED = -104; //! Internally-used Dynamics Parameters (Gravity) solver failed
+
         /**
          * Constructor for the estimator, it will allocate all the necessary memory
          * \param chain The kinematic chain of the robot, an internal copy will be made.
@@ -96,6 +103,9 @@ namespace KDL {
         /// @copydoc KDL::SolverI::updateInternalDataStructures()
         virtual void updateInternalDataStructures();
 
+        /// @copydoc KDL::SolverI::strError()
+        virtual const char* strError(const int error) const;
+
     private:
         const Chain &CHAIN;
         const double DT_SEC, FILTER_CONST;

From 891047a6199752998b2533f332dd4002c34a19df Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Fri, 14 May 2021 14:25:50 +0200
Subject: [PATCH 23/26] [Ext wrench estimator] comments updated

---
 orocos_kdl/src/chainexternalwrenchestimator.cpp | 8 ++++----
 orocos_kdl/src/chainexternalwrenchestimator.hpp | 1 +
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp
index bced16d2f..c20f5c53c 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.cpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp
@@ -121,9 +121,9 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
         return (error = E_SIZE_MISMATCH);
 
     /**
-     * =======================================================================================================================
-     * Part I: Estimation of the torques that are felt in joints as a result of the external wrench being applied on the robot
-     * =======================================================================================================================
+     * ================================================================================================================
+     * Part I: Estimation of the torques felt in joints as a result of the external wrench being applied on the robot
+     * ================================================================================================================
      */
 
     // Calculate decomposed robot's dynamics
@@ -171,7 +171,7 @@ int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position,
         return (error = E_JACSOLVER_FAILED);
 
     // Transform the jacobian from the base frame to the end-effector frame. 
-    // This part can be commented out if the user wants its estimated wrench to be expressed w.r.t. base frame 
+    // This part can be commented out if the user wants estimated wrench to be expressed w.r.t. base frame 
     jacobian_end_eff.changeBase(end_eff_frame.M.Inverse()); // Jacobian is now expressed w.r.t. end-effector frame
 
     // SVD of "Jac^T" with maximum iterations "maxiter": Jac^T = U * S^-1 * V^T
diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp
index 1a321b18e..c60b882d4 100644
--- a/orocos_kdl/src/chainexternalwrenchestimator.hpp
+++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp
@@ -57,6 +57,7 @@ namespace KDL {
          * \param chain The kinematic chain of the robot, an internal copy will be made.
          * \param gravity The gravity-acceleration vector to use during the calculation.
          * \param sample_frequency Frequency at which users updates it estimation loop (in Hz).
+         * \param estimation_gain Parameter used to control the estimator's convergence
          * \param filter_constant Parameter defining how much the estimated signal should be filtered by the low-pass filter.
          *                        This input value should be between 0 and 1. Higher the number means more noise needs to be filtered-out.
          *                        The filter can be turned off by setting this value to 0.

From 2ddf32c61595864eb0d3c7853e7befa3328f9e08 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Fri, 14 May 2021 14:30:04 +0200
Subject: [PATCH 24/26] [Solver test] control and simulation updated in the
 estimator's test

Simple PD controller implemented for controlling end-effector's position and velocity

Simulation updated with full-joint-circle crossing saturation and start-time for ext. wrench simulation
---
 orocos_kdl/tests/solvertest.cpp | 235 ++++++++++++++++++++------------
 1 file changed, 151 insertions(+), 84 deletions(-)

diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp
index 6a812afe2..c346b3bbe 100644
--- a/orocos_kdl/tests/solvertest.cpp
+++ b/orocos_kdl/tests/solvertest.cpp
@@ -1570,74 +1570,49 @@ void SolverTest::ExternalWrenchEstimatorTest()
      */
     std::cout << "KDL External Wrench Estimator Test" << std::endl;
 
+    /**
+     * This EPS has a slightly different purpose than the EPSes of the other solver-tests. While other EPSes are taking care of the differences that
+     * originate from e.g. floating-number imprecisions, different compilers (or same compiler but different flags) used between different machines (OS), etc. 
+     * The EPS specified bellow is there to cover those imperfections as well but, it's also there to
+     * take into account the noise in estimated signals (the differences between estimated and ground-truth wrenches), caused by other computations in this test
+     * (ones coming from the implemented controller and the dynamics simulator) not just those coming from the estimator itself.
+     */
+    double eps_wrench = 0.5, eps_torque = 0.2;
     int ret;
-    double eps = 0.7;
     unsigned int nj = kukaLWR.getNrOfJoints();
     unsigned int ns = kukaLWR.getNrOfSegments();
     CPPUNIT_ASSERT(Equal(nj, ns)); // Current implementation of the Vereshchagin solver can only work with chains that have equal number of joints and segments
 
-    // Joint position, velocity, acceleration and torques
-    KDL::JntArray q(nj); // Joint state
-    KDL::JntArray qd(nj); // Joint state
-    KDL::JntArray q_desired(nj); // Joint state
-    KDL::JntArray qd_desired(nj); // Joint state
-    KDL::JntArray qdd(nj); // Resultant joint accelerations
-    KDL::JntArray qdd_command(nj); // Joint acceleration commands for RNE
-    KDL::JntArray command_torque(nj); // Control torque to actuate the robot
-    KDL::JntArray constraint_tau(nj); // It will result in zero in Vereshchagin for this test
-    KDL::JntArray ext_torque_reference(nj); // Ground-truth joint torques due to the external force applied on the end-effector
-    KDL::JntArray ext_torque_estimated(nj); // Estimated joint torques
-
-    // Random initial state
-    q(0) = 1.0;
-    q(1) = 0.0;
-    q(2) = 0.0;
-    q(3) = -1.57;
-    q(4) = 0.0;
-    q(5) = 1.57;
-    q(6) = -0.8;
-    q_desired = q; // desired state is the same as the initial
-
-    qd(0) = 0.2;
-    qd(1) = -0.1;
-    qd(2) = 0.3;
-    qd(3) = 0.5;
-    qd(4) = -0.1;
-    qd(5) = -0.15;
-    qd(6) = 0.9;
-
-    KDL::Wrenches f_ext_base (ns); // External Wrenches acting on the end-effector, expressed in base link coordinates
-    KDL::Wrenches f_ext_zero(ns); // Zero Wrenches
-
-    // Ground-truth external wrench acting on the end-effector
-    KDL::Vector f(10.0, -20.0, 30.0);
-    KDL::Vector n(0.0, 0.0, 0.0);
-    KDL::Wrench f_tool_reference(f, n); // expressed in local end-effector's frame
-    KDL::Wrench f_tool_estimated;
-
-    ChainFkSolverPos_recursive fksolverpos(kukaLWR);
-    ChainJntToJacSolver jacobian_solver(kukaLWR);
+    // Initialize state and control variables
+    JntArray q(nj); // Current joint position
+    JntArray qd(nj); // Current joint velocity
+    JntArray qdd(nj); // Resultant joint accelerations
+    JntArrayVel jnt_position_velocity(nj); // variable necessary for the FK vel solver
+    JntArray jnt_array_zero(nj); // Zero joint input for RNE
+    JntArray command_torque(nj); // Control torque to actuate the robot
+    JntArray constraint_tau(nj); // It will result in zero in Vereshchagin for this test
+    JntArray gravity_torque(nj); // Gravity torque computed by RNE
+    JntArray ext_torque_reference(nj); // Ground-truth joint torques due to the external force applied on the end-effector
+    JntArray ext_torque_estimated(nj); // Estimated joint torques
+    Wrenches f_ext_base(ns); // External Wrenches acting on the end-effector, expressed in base-link coordinates
+    Wrenches f_ext_zero(ns); // Zero Wrenches
+    Wrench f_tool_estimated; // External Wrenches estimated by the momentum-observer
     Frame end_effector_pose;
+    Frame desired_end_eff_pose;
     Jacobian jacobian_end_eff(nj);
-
-    fksolverpos.JntToCart(q, end_effector_pose);
-    f_ext_base[ns - 1] = end_effector_pose.M * f_tool_reference;
-
-    // Compute robot's jacobian for the end-effector frame, expressed in the base frame
-    jacobian_solver.JntToJac(q, jacobian_end_eff);
-
-    // Inverse Force Kinematics
-    Eigen::Matrix<double, 6, 1> wrench;
-    wrench(0) = f_ext_base[ns - 1](0);
-    wrench(1) = f_ext_base[ns - 1](1);
-    wrench(2) = f_ext_base[ns - 1](2);
-    wrench(3) = f_ext_base[ns - 1](3);
-    wrench(4) = f_ext_base[ns - 1](4);
-    wrench(5) = f_ext_base[ns - 1](5);
-    ext_torque_reference.data = jacobian_end_eff.data.transpose() * wrench;
+    FrameVel end_eff_twist;
+    FrameVel desired_end_eff_twist;
+    Eigen::Matrix<double, 6, 1> end_eff_force; // variable necessary for the control
+    Eigen::Matrix<double, 6, 1> end_eff_pos_error; // variable necessary for the control
+    Eigen::Matrix<double, 6, 1> end_eff_vel_error; // variable necessary for the control
 
     // Arm root acceleration (robot's base mounted on an even surface)
     Vector linearAcc(0.0, 0.0, -9.81); Vector angularAcc(0.0, 0.0, 0.0);
+    
+    // Initialize kinematics solvers
+    ChainFkSolverPos_recursive fksolverpos(kukaLWR);
+    ChainFkSolverVel_recursive fksolvervel(kukaLWR);
+    ChainJntToJacSolver jacobian_solver(kukaLWR);
 
     // RNE ID solver for control purposes
     KDL::ChainIdSolver_RNE IdSolver(kukaLWR, linearAcc);
@@ -1652,47 +1627,111 @@ void SolverTest::ExternalWrenchEstimatorTest()
     ChainHdSolver_Vereshchagin constraintSolver(kukaLWR, vereshchagin_root_Acc, numberOfConstraints);
 
     // External Wrench Estimator
-    double sample_frequency = 10000.0; // Hz
+    double sample_frequency = 1000.0; // Hz
     double estimation_gain  = 30.0;
     double filter_constant  = 0.5;
     ChainExternalWrenchEstimator extwrench_estimator(kukaLWR, linearAcc, sample_frequency, estimation_gain, filter_constant);
-    extwrench_estimator.setInitialMomentum(q, qd); // sets the offset for future estimation (momentum calculation)
+    
+    // Initial test state
+    q(0) = 1.0;
+    q(1) = 0.0;
+    q(2) = 0.0;
+    q(3) = 4.71;
+    q(4) = 0.0;
+    q(5) = 1.57;
+    q(6) = 5.48;
+
+    qd(0) = 0.2;
+    qd(1) = -0.1;
+    qd(2) = 0.3;
+    qd(3) = 0.1;
+    qd(4) = -0.1;
+    qd(5) = -0.15;
+    qd(6) = 0.3;
+
+    // Ground-truth external wrench acting on the end-effector
+    KDL::Vector f(10.0, -20.0, 30.0);
+    KDL::Vector n(0.3, -0.7, 0.0);
+    KDL::Wrench f_tool_reference(f, n); // expressed in local end-effector's frame
+
+    end_eff_force.setZero();
+    end_eff_pos_error.setZero();
+    end_eff_vel_error.setZero();
 
     // ##########################################################################################
     // Control and simulation
     // ##########################################################################################
 
-    // Control gains for a simple PID controller
-    double k_p = 0.01; // Proportional
-    double k_i = 0.01; // Integral
-    double k_d = 0.01; // Derivative
+    // Initialize the estimator
+    extwrench_estimator.setInitialMomentum(q, qd); // sets the offset for future estimation (momentum calculation)
+
+    // Control gains for a simple PD controller
+    double k_p = 1500.0; // Proportional
+    double k_d = 300.0; // Derivative
 
-    // Time required to complete the task (keep joints static -> not moving)
-    double simulationTime = 0.23; // in seconds
+    // Time required to complete the task
+    double simulationTime = 0.4; // in seconds
     double timeDelta = 1.0 / sample_frequency; // unit of seconds
 
-    // Iteration-limited control loop
-    JntArray position_error(nj);
-    JntArray integral_error(nj); // accumulated position error
-    JntArray velocity_error(nj);
+    // Set the desired Cartesian state
+    fksolverpos.JntToCart(q, end_effector_pose);
+    desired_end_eff_pose.p(0) = end_effector_pose.p(0) + 0.02;
+    desired_end_eff_pose.p(1) = end_effector_pose.p(1) + 0.02;
+    desired_end_eff_pose.p(2) = end_effector_pose.p(2) + 0.02;
+    desired_end_eff_twist.p.v(0) = 0.0;
+    desired_end_eff_twist.p.v(1) = 0.0;
+    desired_end_eff_twist.p.v(2) = 0.0;
+
     for (double t = 0.0; t <= simulationTime; t = t + timeDelta)
     {
-        // Compute errors
-        position_error.data = q_desired.data - q.data;
-        integral_error.data += timeDelta * position_error.data;
-        velocity_error.data = qd_desired.data - qd.data;
+        ret = jacobian_solver.JntToJac(q, jacobian_end_eff);
+        if (ret < 0)
+        {
+            std::cout << "Jacobian solver ERROR: " << ret << std::endl;
+            break;
+        }
 
-        // Compute joint commands (simple PID controller)
-        qdd_command.data = k_p * position_error.data + k_i * integral_error.data + k_d * velocity_error.data;
+        ret = fksolverpos.JntToCart(q, end_effector_pose);
+        if (ret < 0)
+        {
+            std::cout << "FK pos solver ERROR: " << ret << std::endl;
+            break;
+        }
 
-        // Compute necessary joint torques to maintain desired state, given command accelerations (hide external wrench from this dynamics calculation)
-        ret = IdSolver.CartToJnt(q, qd, qdd_command, f_ext_zero, command_torque);
+        jnt_position_velocity.q = q;
+        jnt_position_velocity.qdot = qd;
+        ret = fksolvervel.JntToCart(jnt_position_velocity, end_eff_twist);
+        if (ret < 0)
+        {
+            std::cout << "FK vel solver ERROR: " << ret << std::endl;
+            break;
+        }
+
+        end_eff_pos_error(0) = end_effector_pose.p(0) - desired_end_eff_pose.p(0);
+        end_eff_pos_error(1) = end_effector_pose.p(1) - desired_end_eff_pose.p(1);
+        end_eff_pos_error(2) = end_effector_pose.p(2) - desired_end_eff_pose.p(2);
+
+        end_eff_vel_error(0) = end_eff_twist.p.v(0) - desired_end_eff_twist.p.v(0);
+        end_eff_vel_error(1) = end_eff_twist.p.v(1) - desired_end_eff_twist.p.v(1);
+        end_eff_vel_error(2) = end_eff_twist.p.v(2) - desired_end_eff_twist.p.v(2);
+
+        end_eff_force = -end_eff_pos_error * k_p - end_eff_vel_error * k_d;
+
+        // Compute necessary joint torques to maintain desired state, given accelerations (hide external wrench from this dynamics calculation)
+        ret = IdSolver.CartToJnt(q, jnt_array_zero, jnt_array_zero, f_ext_zero, gravity_torque);
         if (ret < 0)
         {
             std::cout << "KDL RNE solver ERROR: " << ret << std::endl;
             break;
         }
 
+        // Compute control command
+        command_torque.data =  jacobian_end_eff.data.transpose() * end_eff_force;
+        command_torque.data += gravity_torque.data;
+
+        // Start simulating the external force
+        if (t > 0.1) f_ext_base[ns - 1] = end_effector_pose.M * f_tool_reference;
+
         // Compute resultant joint accelerations that simulate robot's behaviour, given the command torques (add external wrench in this dynamics calculation)
         ret = constraintSolver.CartToJnt(q, qd, qdd, alpha, beta, f_ext_base, command_torque, constraint_tau);
         if (ret < 0)
@@ -1704,20 +1743,48 @@ void SolverTest::ExternalWrenchEstimatorTest()
         // State integration: integrate from model accelerations to next joint state (positions and velocities)
         qd.data = qd.data + qdd.data * timeDelta; // Euler Forward
         q.data = q.data + qd.data * timeDelta; // Symplectic Euler
+
+        // Saturate integrated joint position for full circle crossing
+        for (unsigned int j = 0; j < nj; j++)
+        {
+            q(j) = std::fmod(q(j), 360 * deg2rad);
+            if (q(j) < 0.0) q(j) += 360 * deg2rad;
+        }
         
         // Estimate external wrench
         extwrench_estimator.JntToExtWrench(q, qd, command_torque, f_tool_estimated);
     }
 
+    // Inverse Force Kinematics
+    Eigen::Matrix<double, 6, 1> wrench;
+    wrench(0) = f_ext_base[ns - 1](0);
+    wrench(1) = f_ext_base[ns - 1](1);
+    wrench(2) = f_ext_base[ns - 1](2);
+    wrench(3) = f_ext_base[ns - 1](3);
+    wrench(4) = f_ext_base[ns - 1](4);
+    wrench(5) = f_ext_base[ns - 1](5);
+    ext_torque_reference.data = jacobian_end_eff.data.transpose() * wrench;
+
+    // Get estimated joint torque 
+    extwrench_estimator.getEstimatedJntTorque(ext_torque_estimated);
+
     // ##################################################################################
     // Final comparison
     // ##################################################################################
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(0), f_tool_reference(0), eps));
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(1), f_tool_reference(1), eps));
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(2), f_tool_reference(2), eps));
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(3), f_tool_reference(3), eps));
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(4), f_tool_reference(4), eps));
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(5), f_tool_reference(5), eps));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(0), f_tool_reference(0), eps_wrench));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(1), f_tool_reference(1), eps_wrench));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(2), f_tool_reference(2), eps_wrench));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(3), f_tool_reference(3), eps_wrench));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(4), f_tool_reference(4), eps_wrench));
+    CPPUNIT_ASSERT(Equal(f_tool_estimated(5), f_tool_reference(5), eps_wrench));
+
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(0), ext_torque_reference(0), eps_torque));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(1), ext_torque_reference(1), eps_torque));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(2), ext_torque_reference(2), eps_torque));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(3), ext_torque_reference(3), eps_torque));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(4), ext_torque_reference(4), eps_torque));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(5), ext_torque_reference(5), eps_torque));
+    CPPUNIT_ASSERT(Equal(ext_torque_estimated(6), ext_torque_reference(6), eps_torque));
 
     return;
 }

From 30fb9eaf61016f494d077fe967a26b843c62e751 Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Fri, 14 May 2021 17:15:59 +0200
Subject: [PATCH 25/26] [Solver test] Ext. Wrench Estimator test extended

Additional test cases added: different initial joint configurations and reference ext. wrenches considered during the test
---
 orocos_kdl/tests/solvertest.cpp | 290 ++++++++++++++++++--------------
 1 file changed, 160 insertions(+), 130 deletions(-)

diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp
index c346b3bbe..32fa54f35 100644
--- a/orocos_kdl/tests/solvertest.cpp
+++ b/orocos_kdl/tests/solvertest.cpp
@@ -1577,7 +1577,7 @@ void SolverTest::ExternalWrenchEstimatorTest()
      * take into account the noise in estimated signals (the differences between estimated and ground-truth wrenches), caused by other computations in this test
      * (ones coming from the implemented controller and the dynamics simulator) not just those coming from the estimator itself.
      */
-    double eps_wrench = 0.5, eps_torque = 0.2;
+    double eps_wrench = 0.6, eps_torque = 0.3;
     int ret;
     unsigned int nj = kukaLWR.getNrOfJoints();
     unsigned int ns = kukaLWR.getNrOfSegments();
@@ -1628,11 +1628,14 @@ void SolverTest::ExternalWrenchEstimatorTest()
 
     // External Wrench Estimator
     double sample_frequency = 1000.0; // Hz
-    double estimation_gain  = 30.0;
+    double estimation_gain  = 45.0;
     double filter_constant  = 0.5;
     ChainExternalWrenchEstimator extwrench_estimator(kukaLWR, linearAcc, sample_frequency, estimation_gain, filter_constant);
     
-    // Initial test state
+    std::vector<KDL::JntArray> jnt_pos;
+    std::vector<KDL::Wrench> wrench_reference;
+
+    // Set first test case
     q(0) = 1.0;
     q(1) = 0.0;
     q(2) = 0.0;
@@ -1640,31 +1643,35 @@ void SolverTest::ExternalWrenchEstimatorTest()
     q(4) = 0.0;
     q(5) = 1.57;
     q(6) = 5.48;
-
-    qd(0) = 0.2;
-    qd(1) = -0.1;
-    qd(2) = 0.3;
-    qd(3) = 0.1;
-    qd(4) = -0.1;
-    qd(5) = -0.15;
-    qd(6) = 0.3;
-
-    // Ground-truth external wrench acting on the end-effector
-    KDL::Vector f(10.0, -20.0, 30.0);
-    KDL::Vector n(0.3, -0.7, 0.0);
-    KDL::Wrench f_tool_reference(f, n); // expressed in local end-effector's frame
-
-    end_eff_force.setZero();
-    end_eff_pos_error.setZero();
-    end_eff_vel_error.setZero();
+    jnt_pos.push_back(q);
+    wrench_reference.push_back(Wrench(Vector(10.0, -20.0, 30.0), Vector(0.0, 0.0, 0.0))); // Ground-truth external wrench acting on the end-effector expressed in local end-effector's frame
+
+    // Set second test case
+    q(0) = 2.96;
+    q(1) = 1.02;
+    q(2) = 6.15;
+    q(3) = 1.61;
+    q(4) = 0.22;
+    q(5) = 0.17;
+    q(6) = 0.01;
+    jnt_pos.push_back(q);
+    wrench_reference.push_back(Wrench(Vector(0.0, 0.0, 0.0), Vector(0.3, -0.7, 0.0))); // expressed in local end-effector's frame
+
+    // Set third test case
+    q(0) = 1.12;
+    q(1) = 0.66;
+    q(2) = 6.15;
+    q(3) = 4.09;
+    q(4) = 1.64;
+    q(5) = 0.12;
+    q(6) = 0.01;
+    jnt_pos.push_back(q);
+    wrench_reference.push_back(Wrench(Vector(13.0, -0.5, 7.0), Vector(-0.4, 0.0, 0.9))); // expressed in local end-effector's frame
 
     // ##########################################################################################
     // Control and simulation
     // ##########################################################################################
 
-    // Initialize the estimator
-    extwrench_estimator.setInitialMomentum(q, qd); // sets the offset for future estimation (momentum calculation)
-
     // Control gains for a simple PD controller
     double k_p = 1500.0; // Proportional
     double k_d = 300.0; // Derivative
@@ -1673,118 +1680,141 @@ void SolverTest::ExternalWrenchEstimatorTest()
     double simulationTime = 0.4; // in seconds
     double timeDelta = 1.0 / sample_frequency; // unit of seconds
 
-    // Set the desired Cartesian state
-    fksolverpos.JntToCart(q, end_effector_pose);
-    desired_end_eff_pose.p(0) = end_effector_pose.p(0) + 0.02;
-    desired_end_eff_pose.p(1) = end_effector_pose.p(1) + 0.02;
-    desired_end_eff_pose.p(2) = end_effector_pose.p(2) + 0.02;
-    desired_end_eff_twist.p.v(0) = 0.0;
-    desired_end_eff_twist.p.v(1) = 0.0;
-    desired_end_eff_twist.p.v(2) = 0.0;
-
-    for (double t = 0.0; t <= simulationTime; t = t + timeDelta)
+    // Iterate over test cases
+    for (unsigned int i = 0; i < jnt_pos.size(); i++)
     {
-        ret = jacobian_solver.JntToJac(q, jacobian_end_eff);
-        if (ret < 0)
-        {
-            std::cout << "Jacobian solver ERROR: " << ret << std::endl;
-            break;
-        }
-
-        ret = fksolverpos.JntToCart(q, end_effector_pose);
-        if (ret < 0)
-        {
-            std::cout << "FK pos solver ERROR: " << ret << std::endl;
-            break;
-        }
-
-        jnt_position_velocity.q = q;
-        jnt_position_velocity.qdot = qd;
-        ret = fksolvervel.JntToCart(jnt_position_velocity, end_eff_twist);
-        if (ret < 0)
-        {
-            std::cout << "FK vel solver ERROR: " << ret << std::endl;
-            break;
-        }
-
-        end_eff_pos_error(0) = end_effector_pose.p(0) - desired_end_eff_pose.p(0);
-        end_eff_pos_error(1) = end_effector_pose.p(1) - desired_end_eff_pose.p(1);
-        end_eff_pos_error(2) = end_effector_pose.p(2) - desired_end_eff_pose.p(2);
-
-        end_eff_vel_error(0) = end_eff_twist.p.v(0) - desired_end_eff_twist.p.v(0);
-        end_eff_vel_error(1) = end_eff_twist.p.v(1) - desired_end_eff_twist.p.v(1);
-        end_eff_vel_error(2) = end_eff_twist.p.v(2) - desired_end_eff_twist.p.v(2);
-
-        end_eff_force = -end_eff_pos_error * k_p - end_eff_vel_error * k_d;
-
-        // Compute necessary joint torques to maintain desired state, given accelerations (hide external wrench from this dynamics calculation)
-        ret = IdSolver.CartToJnt(q, jnt_array_zero, jnt_array_zero, f_ext_zero, gravity_torque);
-        if (ret < 0)
+        // Re-set control and simulation variables
+        q = jnt_pos[i];
+        qd(0) = 0.2;
+        qd(1) = -0.1;
+        qd(2) = 0.3;
+        qd(3) = 0.5;
+        qd(4) = -0.1;
+        qd(5) = -0.15;
+        qd(6) = 0.9;
+
+        end_eff_force.setZero();
+        end_eff_pos_error.setZero();
+        end_eff_vel_error.setZero();
+        f_ext_base = f_ext_zero;
+
+        // Initialize the estimator
+        extwrench_estimator.updateInternalDataStructures();
+        extwrench_estimator.setInitialMomentum(q, qd); // sets the offset for future estimation (momentum calculation)
+
+        // Set the desired Cartesian state
+        fksolverpos.JntToCart(q, end_effector_pose);
+        desired_end_eff_pose.p(0) = end_effector_pose.p(0) + 0.02;
+        desired_end_eff_pose.p(1) = end_effector_pose.p(1) + 0.02;
+        desired_end_eff_pose.p(2) = end_effector_pose.p(2) + 0.02;
+        desired_end_eff_twist.p.v(0) = 0.0;
+        desired_end_eff_twist.p.v(1) = 0.0;
+        desired_end_eff_twist.p.v(2) = 0.0;
+
+        for (double t = 0.0; t <= simulationTime; t = t + timeDelta)
         {
-            std::cout << "KDL RNE solver ERROR: " << ret << std::endl;
-            break;
+            ret = jacobian_solver.JntToJac(q, jacobian_end_eff);
+            if (ret < 0)
+            {
+                std::cout << "Jacobian solver ERROR: " << ret << std::endl;
+                break;
+            }
+
+            ret = fksolverpos.JntToCart(q, end_effector_pose);
+            if (ret < 0)
+            {
+                std::cout << "FK pos solver ERROR: " << ret << std::endl;
+                break;
+            }
+
+            jnt_position_velocity.q = q;
+            jnt_position_velocity.qdot = qd;
+            ret = fksolvervel.JntToCart(jnt_position_velocity, end_eff_twist);
+            if (ret < 0)
+            {
+                std::cout << "FK vel solver ERROR: " << ret << std::endl;
+                break;
+            }
+
+            end_eff_pos_error(0) = end_effector_pose.p(0) - desired_end_eff_pose.p(0);
+            end_eff_pos_error(1) = end_effector_pose.p(1) - desired_end_eff_pose.p(1);
+            end_eff_pos_error(2) = end_effector_pose.p(2) - desired_end_eff_pose.p(2);
+
+            end_eff_vel_error(0) = end_eff_twist.p.v(0) - desired_end_eff_twist.p.v(0);
+            end_eff_vel_error(1) = end_eff_twist.p.v(1) - desired_end_eff_twist.p.v(1);
+            end_eff_vel_error(2) = end_eff_twist.p.v(2) - desired_end_eff_twist.p.v(2);
+
+            end_eff_force = -end_eff_pos_error * k_p - end_eff_vel_error * k_d;
+
+            // Compute gravity joint torques (hide external wrench from this dynamics calculation)
+            ret = IdSolver.CartToJnt(q, jnt_array_zero, jnt_array_zero, f_ext_zero, gravity_torque);
+            if (ret < 0)
+            {
+                std::cout << "KDL RNE solver ERROR: " << ret << std::endl;
+                break;
+            }
+
+            // Compute joint control commands
+            command_torque.data =  jacobian_end_eff.data.transpose() * end_eff_force;
+            command_torque.data += gravity_torque.data;
+
+            // Start simulating the external force
+            if (t > 0.2) f_ext_base[ns - 1] = end_effector_pose.M * wrench_reference[i];
+
+            // Compute resultant joint accelerations that simulate robot's behaviour, given the command torques (add external wrench in this dynamics calculation)
+            ret = constraintSolver.CartToJnt(q, qd, qdd, alpha, beta, f_ext_base, command_torque, constraint_tau);
+            if (ret < 0)
+            {
+                std::cout << "KDL Vereshchagin solver ERROR: " << ret << std::endl;
+                break;
+            }
+            
+            // State integration: integrate from model accelerations to next joint state (positions and velocities)
+            qd.data = qd.data + qdd.data * timeDelta; // Euler Forward
+            q.data = q.data + qd.data * timeDelta; // Symplectic Euler
+
+            // Saturate integrated joint position for full circle crossing
+            for (unsigned int j = 0; j < nj; j++)
+            {
+                q(j) = std::fmod(q(j), 360 * deg2rad);
+                if (q(j) < 0.0) q(j) += 360 * deg2rad;
+            }
+            
+            // Estimate external wrench
+            extwrench_estimator.JntToExtWrench(q, qd, command_torque, f_tool_estimated);
         }
 
-        // Compute control command
-        command_torque.data =  jacobian_end_eff.data.transpose() * end_eff_force;
-        command_torque.data += gravity_torque.data;
-
-        // Start simulating the external force
-        if (t > 0.1) f_ext_base[ns - 1] = end_effector_pose.M * f_tool_reference;
-
-        // Compute resultant joint accelerations that simulate robot's behaviour, given the command torques (add external wrench in this dynamics calculation)
-        ret = constraintSolver.CartToJnt(q, qd, qdd, alpha, beta, f_ext_base, command_torque, constraint_tau);
-        if (ret < 0)
-        {
-            std::cout << "KDL Vereshchagin solver ERROR: " << ret << std::endl;
-            break;
-        }
-        
-        // State integration: integrate from model accelerations to next joint state (positions and velocities)
-        qd.data = qd.data + qdd.data * timeDelta; // Euler Forward
-        q.data = q.data + qd.data * timeDelta; // Symplectic Euler
-
-        // Saturate integrated joint position for full circle crossing
-        for (unsigned int j = 0; j < nj; j++)
-        {
-            q(j) = std::fmod(q(j), 360 * deg2rad);
-            if (q(j) < 0.0) q(j) += 360 * deg2rad;
-        }
-        
-        // Estimate external wrench
-        extwrench_estimator.JntToExtWrench(q, qd, command_torque, f_tool_estimated);
+        // Inverse Force Kinematics
+        Eigen::Matrix<double, 6, 1> wrench;
+        wrench(0) = f_ext_base[ns - 1](0);
+        wrench(1) = f_ext_base[ns - 1](1);
+        wrench(2) = f_ext_base[ns - 1](2);
+        wrench(3) = f_ext_base[ns - 1](3);
+        wrench(4) = f_ext_base[ns - 1](4);
+        wrench(5) = f_ext_base[ns - 1](5);
+        ext_torque_reference.data = jacobian_end_eff.data.transpose() * wrench;
+
+        // Get estimated joint torque 
+        extwrench_estimator.getEstimatedJntTorque(ext_torque_estimated);
+
+        // ##################################################################################
+        // Final comparison
+        // ##################################################################################
+        CPPUNIT_ASSERT(Equal(f_tool_estimated(0), wrench_reference[i](0), eps_wrench));
+        CPPUNIT_ASSERT(Equal(f_tool_estimated(1), wrench_reference[i](1), eps_wrench));
+        CPPUNIT_ASSERT(Equal(f_tool_estimated(2), wrench_reference[i](2), eps_wrench));
+        CPPUNIT_ASSERT(Equal(f_tool_estimated(3), wrench_reference[i](3), eps_wrench));
+        CPPUNIT_ASSERT(Equal(f_tool_estimated(4), wrench_reference[i](4), eps_wrench));
+        CPPUNIT_ASSERT(Equal(f_tool_estimated(5), wrench_reference[i](5), eps_wrench));
+
+        CPPUNIT_ASSERT(Equal(ext_torque_estimated(0), ext_torque_reference(0), eps_torque));
+        CPPUNIT_ASSERT(Equal(ext_torque_estimated(1), ext_torque_reference(1), eps_torque));
+        CPPUNIT_ASSERT(Equal(ext_torque_estimated(2), ext_torque_reference(2), eps_torque));
+        CPPUNIT_ASSERT(Equal(ext_torque_estimated(3), ext_torque_reference(3), eps_torque));
+        CPPUNIT_ASSERT(Equal(ext_torque_estimated(4), ext_torque_reference(4), eps_torque));
+        CPPUNIT_ASSERT(Equal(ext_torque_estimated(5), ext_torque_reference(5), eps_torque));
+        CPPUNIT_ASSERT(Equal(ext_torque_estimated(6), ext_torque_reference(6), eps_torque));
     }
 
-    // Inverse Force Kinematics
-    Eigen::Matrix<double, 6, 1> wrench;
-    wrench(0) = f_ext_base[ns - 1](0);
-    wrench(1) = f_ext_base[ns - 1](1);
-    wrench(2) = f_ext_base[ns - 1](2);
-    wrench(3) = f_ext_base[ns - 1](3);
-    wrench(4) = f_ext_base[ns - 1](4);
-    wrench(5) = f_ext_base[ns - 1](5);
-    ext_torque_reference.data = jacobian_end_eff.data.transpose() * wrench;
-
-    // Get estimated joint torque 
-    extwrench_estimator.getEstimatedJntTorque(ext_torque_estimated);
-
-    // ##################################################################################
-    // Final comparison
-    // ##################################################################################
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(0), f_tool_reference(0), eps_wrench));
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(1), f_tool_reference(1), eps_wrench));
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(2), f_tool_reference(2), eps_wrench));
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(3), f_tool_reference(3), eps_wrench));
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(4), f_tool_reference(4), eps_wrench));
-    CPPUNIT_ASSERT(Equal(f_tool_estimated(5), f_tool_reference(5), eps_wrench));
-
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(0), ext_torque_reference(0), eps_torque));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(1), ext_torque_reference(1), eps_torque));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(2), ext_torque_reference(2), eps_torque));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(3), ext_torque_reference(3), eps_torque));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(4), ext_torque_reference(4), eps_torque));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(5), ext_torque_reference(5), eps_torque));
-    CPPUNIT_ASSERT(Equal(ext_torque_estimated(6), ext_torque_reference(6), eps_torque));
-
     return;
 }

From 2bd28d8943346fe15e81e0d3949cf9982182894c Mon Sep 17 00:00:00 2001
From: Djordje Vukcevic <vukcevicdjordjemne@gmail.com>
Date: Fri, 14 May 2021 17:36:55 +0200
Subject: [PATCH 26/26] [Solver test] Ext. Wrench estimator tested with random
 values

---
 orocos_kdl/tests/solvertest.cpp | 33 +++++++++++++++++++++------------
 1 file changed, 21 insertions(+), 12 deletions(-)

diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp
index 32fa54f35..24bde309b 100644
--- a/orocos_kdl/tests/solvertest.cpp
+++ b/orocos_kdl/tests/solvertest.cpp
@@ -2,6 +2,7 @@
 #include <frames_io.hpp>
 #include <framevel_io.hpp>
 #include <kinfam_io.hpp>
+#include <random>
 #include <time.h>
 #include <utilities/utility.h>
 
@@ -1577,7 +1578,7 @@ void SolverTest::ExternalWrenchEstimatorTest()
      * take into account the noise in estimated signals (the differences between estimated and ground-truth wrenches), caused by other computations in this test
      * (ones coming from the implemented controller and the dynamics simulator) not just those coming from the estimator itself.
      */
-    double eps_wrench = 0.6, eps_torque = 0.3;
+    double eps_wrench = 0.5, eps_torque = 0.3;
     int ret;
     unsigned int nj = kukaLWR.getNrOfJoints();
     unsigned int ns = kukaLWR.getNrOfSegments();
@@ -1631,10 +1632,18 @@ void SolverTest::ExternalWrenchEstimatorTest()
     double estimation_gain  = 45.0;
     double filter_constant  = 0.5;
     ChainExternalWrenchEstimator extwrench_estimator(kukaLWR, linearAcc, sample_frequency, estimation_gain, filter_constant);
-    
+
+    // Prepare test cases
     std::vector<KDL::JntArray> jnt_pos;
     std::vector<KDL::Wrench> wrench_reference;
 
+    // Intialize random generator
+    std::random_device rd;  //Will be used to obtain a seed for the random number engine
+    std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
+    std::uniform_real_distribution<> dis_force(-15.0, 15.0);
+    std::uniform_real_distribution<> dis_moment(-0.9, 0.9);
+    std::uniform_real_distribution<> dis_jnt_vel(-0.5, 0.5);
+
     // Set first test case
     q(0) = 1.0;
     q(1) = 0.0;
@@ -1644,7 +1653,7 @@ void SolverTest::ExternalWrenchEstimatorTest()
     q(5) = 1.57;
     q(6) = 5.48;
     jnt_pos.push_back(q);
-    wrench_reference.push_back(Wrench(Vector(10.0, -20.0, 30.0), Vector(0.0, 0.0, 0.0))); // Ground-truth external wrench acting on the end-effector expressed in local end-effector's frame
+    wrench_reference.push_back(Wrench(Vector(dis_force(gen), dis_force(gen), dis_force(gen)), Vector(0.0, 0.0, 0.0))); // Ground-truth external wrench acting on the end-effector expressed in local end-effector's frame
 
     // Set second test case
     q(0) = 2.96;
@@ -1655,7 +1664,7 @@ void SolverTest::ExternalWrenchEstimatorTest()
     q(5) = 0.17;
     q(6) = 0.01;
     jnt_pos.push_back(q);
-    wrench_reference.push_back(Wrench(Vector(0.0, 0.0, 0.0), Vector(0.3, -0.7, 0.0))); // expressed in local end-effector's frame
+    wrench_reference.push_back(Wrench(Vector(0.0, 0.0, 0.0), Vector(dis_moment(gen), dis_moment(gen), 0.0))); // expressed in local end-effector's frame
 
     // Set third test case
     q(0) = 1.12;
@@ -1666,7 +1675,7 @@ void SolverTest::ExternalWrenchEstimatorTest()
     q(5) = 0.12;
     q(6) = 0.01;
     jnt_pos.push_back(q);
-    wrench_reference.push_back(Wrench(Vector(13.0, -0.5, 7.0), Vector(-0.4, 0.0, 0.9))); // expressed in local end-effector's frame
+    wrench_reference.push_back(Wrench(Vector(dis_force(gen), dis_force(gen), dis_force(gen)), Vector(dis_moment(gen), 0.0, dis_moment(gen)))); // expressed in local end-effector's frame
 
     // ##########################################################################################
     // Control and simulation
@@ -1685,13 +1694,13 @@ void SolverTest::ExternalWrenchEstimatorTest()
     {
         // Re-set control and simulation variables
         q = jnt_pos[i];
-        qd(0) = 0.2;
-        qd(1) = -0.1;
-        qd(2) = 0.3;
-        qd(3) = 0.5;
-        qd(4) = -0.1;
-        qd(5) = -0.15;
-        qd(6) = 0.9;
+        qd(0) = dis_jnt_vel(gen);
+        qd(1) = dis_jnt_vel(gen);
+        qd(2) = dis_jnt_vel(gen);
+        qd(3) = dis_jnt_vel(gen);
+        qd(4) = dis_jnt_vel(gen);
+        qd(5) = dis_jnt_vel(gen);
+        qd(6) = dis_jnt_vel(gen);
 
         end_eff_force.setZero();
         end_eff_pos_error.setZero();