chainexternalwrenchestimator.hpp

// 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_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
#define KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
 
#include <Eigen/Core>
#include "utilities/svd_eigen_HH.hpp"
#include "chaindynparam.hpp"
#include "chainjnttojacsolver.hpp"
#include "chainfksolverpos_recursive.hpp"
#include <iostream>
 
namespace KDL {

    class ChainExternalWrenchEstimator : public SolverI
    {
        typedef Eigen::Matrix<double, 6, 1 > Vector6d;
 
    public:
 
        static const int E_FKSOLVERPOS_FAILED = -100; 
        static const int E_JACSOLVER_FAILED = -101; 
        static const int E_DYNPARAMSOLVERMASS_FAILED = -102; 
        static const int E_DYNPARAMSOLVERCORIOLIS_FAILED = -103; 
        static const int E_DYNPARAMSOLVERGRAVITY_FAILED = -104; 

        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(){};

        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);

        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);

        virtual void updateInternalDataStructures();

        virtual const char* strError(const int error) const;
 
    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, 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;
        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;
        ChainFkSolverPos_recursive fk_pos_solver;
    };
}
 
#endif