orocos_kdl
chainexternalwrenchestimator.hpp
Go to the documentation of this file.
1 // Copyright (C) 2021 Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
2 
3 // Version: 1.0
4 // Author: Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
5 // URL: http://www.orocos.org/kdl
6 
7 // This library is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU Lesser General Public
9 // License as published by the Free Software Foundation; either
10 // version 2.1 of the License, or (at your option) any later version.
11 
12 // This library is distributed in the hope that it will be useful,
13 // but WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // Lesser General Public License for more details.
16 
17 // You should have received a copy of the GNU Lesser General Public
18 // License along with this library; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
20 
21 #ifndef KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
22 #define KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
23 
24 #include <Eigen/Core>
26 #include "chaindynparam.hpp"
27 #include "chainjnttojacsolver.hpp"
29 #include <iostream>
30 
31 namespace KDL {
32 
44  {
45  typedef Eigen::Matrix<double, 6, 1 > Vector6d;
46 
47  public:
48 
49  static const int E_FKSOLVERPOS_FAILED = -100;
50  static const int E_JACSOLVER_FAILED = -101;
51  static const int E_DYNPARAMSOLVERMASS_FAILED = -102;
52  static const int E_DYNPARAMSOLVERCORIOLIS_FAILED = -103;
53  static const int E_DYNPARAMSOLVERGRAVITY_FAILED = -104;
54 
67  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);
69 
75  int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity);
76 
77  // Sets singular-value eps parameter for the SVD calculation
78  void setSVDEps(const double eps_in);
79 
80  // Sets maximum iteration parameter for the SVD calculation
81  void setSVDMaxIter(const int maxiter_in);
82 
99  int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench);
100 
101  // Returns the torques felt in the robot's joints as a result of the external wrench being applied on the robot.
102  void getEstimatedJntTorque(JntArray &external_joint_torque);
103 
105  virtual void updateInternalDataStructures();
106 
108  virtual const char* strError(const int error) const;
109 
110  private:
111  const Chain &CHAIN;
112  const double DT_SEC, FILTER_CONST;
113  double svd_eps;
115  unsigned int nj, ns;
121  Eigen::VectorXd S, S_inv, tmp, ESTIMATION_GAIN;
125  };
126 }
127 
128 #endif
KDL::ChainExternalWrenchEstimator::E_DYNPARAMSOLVERCORIOLIS_FAILED
static const int E_DYNPARAMSOLVERCORIOLIS_FAILED
Internally-used Dynamics Parameters (Mass) solver failed.
Definition: chainexternalwrenchestimator.hpp:52
KDL::ChainExternalWrenchEstimator
First-order momentum observer for the estimation of external wrenches applied on the robot's end-effe...
Definition: chainexternalwrenchestimator.hpp:43
KDL::ChainExternalWrenchEstimator::coriolis_torque
JntArray coriolis_torque
Definition: chainexternalwrenchestimator.hpp:118
KDL::ChainExternalWrenchEstimator::E_JACSOLVER_FAILED
static const int E_JACSOLVER_FAILED
Internally-used Forward Position Kinematics (Recursive) solver failed.
Definition: chainexternalwrenchestimator.hpp:50
KDL::JntArray
Definition: jntarray.hpp:69
KDL::ChainExternalWrenchEstimator::svd_eps
double svd_eps
Definition: chainexternalwrenchestimator.hpp:113
KDL::ChainExternalWrenchEstimator::V
Eigen::MatrixXd V
Definition: chainexternalwrenchestimator.hpp:120
KDL::ChainExternalWrenchEstimator::CHAIN
const Chain & CHAIN
Definition: chainexternalwrenchestimator.hpp:111
KDL::ChainExternalWrenchEstimator::initial_jnt_momentum
JntArray initial_jnt_momentum
Definition: chainexternalwrenchestimator.hpp:117
KDL::ChainExternalWrenchEstimator::estimated_ext_torque
JntArray estimated_ext_torque
Definition: chainexternalwrenchestimator.hpp:118
chainfksolverpos_recursive.hpp
KDL::ChainExternalWrenchEstimator::jacobian_end_eff
Jacobian jacobian_end_eff
Definition: chainexternalwrenchestimator.hpp:119
KDL::ChainExternalWrenchEstimator::gravity_torque
JntArray gravity_torque
Definition: chainexternalwrenchestimator.hpp:118
KDL::ChainExternalWrenchEstimator::jacobian_solver
ChainJntToJacSolver jacobian_solver
Definition: chainexternalwrenchestimator.hpp:123
KDL::ChainExternalWrenchEstimator::~ChainExternalWrenchEstimator
~ChainExternalWrenchEstimator()
Definition: chainexternalwrenchestimator.hpp:68
iostream
KDL::ChainDynParam
Definition: chaindynparam.hpp:47
KDL::ChainExternalWrenchEstimator::JntToExtWrench
int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench)
Definition: chainexternalwrenchestimator.cpp:106
KDL
Definition: kukaLWR_DHnew.cpp:25
chaindynparam.hpp
KDL::ChainExternalWrenchEstimator::jacobian_end_eff_transpose_inv
Eigen::MatrixXd jacobian_end_eff_transpose_inv
Definition: chainexternalwrenchestimator.hpp:120
KDL::ChainExternalWrenchEstimator::fk_pos_solver
ChainFkSolverPos_recursive fk_pos_solver
Definition: chainexternalwrenchestimator.hpp:124
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::ChainExternalWrenchEstimator::S
Eigen::VectorXd S
Definition: chainexternalwrenchestimator.hpp:121
KDL::ChainExternalWrenchEstimator::setSVDEps
void setSVDEps(const double eps_in)
Definition: chainexternalwrenchestimator.cpp:94
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:881
KDL::ChainExternalWrenchEstimator::svd_maxiter
int svd_maxiter
Definition: chainexternalwrenchestimator.hpp:114
KDL::ChainExternalWrenchEstimator::E_DYNPARAMSOLVERMASS_FAILED
static const int E_DYNPARAMSOLVERMASS_FAILED
Internally-used Jacobian solver failed.
Definition: chainexternalwrenchestimator.hpp:51
KDL::ChainExternalWrenchEstimator::total_torque
JntArray total_torque
Definition: chainexternalwrenchestimator.hpp:118
KDL::ChainExternalWrenchEstimator::filtered_estimated_ext_torque
JntArray filtered_estimated_ext_torque
Definition: chainexternalwrenchestimator.hpp:117
KDL::ChainExternalWrenchEstimator::FILTER_CONST
const double FILTER_CONST
Definition: chainexternalwrenchestimator.hpp:112
KDL::ChainExternalWrenchEstimator::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainexternalwrenchestimator.cpp:45
KDL::ChainExternalWrenchEstimator::dynparam_solver
ChainDynParam dynparam_solver
Definition: chainexternalwrenchestimator.hpp:122
KDL::ChainExternalWrenchEstimator::strError
virtual const char * strError(const int error) const
Definition: chainexternalwrenchestimator.cpp:204
KDL::ChainExternalWrenchEstimator::ns
unsigned int ns
Definition: chainexternalwrenchestimator.hpp:115
KDL::ChainExternalWrenchEstimator::jnt_mass_matrix
JntSpaceInertiaMatrix jnt_mass_matrix
Definition: chainexternalwrenchestimator.hpp:116
KDL::ChainExternalWrenchEstimator::E_DYNPARAMSOLVERGRAVITY_FAILED
static const int E_DYNPARAMSOLVERGRAVITY_FAILED
Internally-used Dynamics Parameters (Coriolis) solver failed.
Definition: chainexternalwrenchestimator.hpp:53
svd_eigen_HH.hpp
KDL::ChainExternalWrenchEstimator::previous_jnt_mass_matrix
JntSpaceInertiaMatrix previous_jnt_mass_matrix
Definition: chainexternalwrenchestimator.hpp:116
KDL::ChainExternalWrenchEstimator::tmp
Eigen::VectorXd tmp
Definition: chainexternalwrenchestimator.hpp:121
KDL::ChainExternalWrenchEstimator::setInitialMomentum
int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity)
Definition: chainexternalwrenchestimator.cpp:74
KDL::ChainExternalWrenchEstimator::jnt_mass_matrix_dot
JntSpaceInertiaMatrix jnt_mass_matrix_dot
Definition: chainexternalwrenchestimator.hpp:116
KDL::ChainExternalWrenchEstimator::ChainExternalWrenchEstimator
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)
Internally-used Dynamics Parameters (Gravity) solver failed.
Definition: chainexternalwrenchestimator.cpp:25
KDL::ChainExternalWrenchEstimator::S_inv
Eigen::VectorXd S_inv
Definition: chainexternalwrenchestimator.hpp:121
KDL::SolverI
Definition: solveri.hpp:84
KDL::ChainExternalWrenchEstimator::DT_SEC
const double DT_SEC
Definition: chainexternalwrenchestimator.hpp:112
KDL::ChainExternalWrenchEstimator::U
Eigen::MatrixXd U
Definition: chainexternalwrenchestimator.hpp:120
KDL::ChainExternalWrenchEstimator::getEstimatedJntTorque
void getEstimatedJntTorque(JntArray &external_joint_torque)
Definition: chainexternalwrenchestimator.cpp:198
KDL::ChainExternalWrenchEstimator::Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: chainexternalwrenchestimator.hpp:45
KDL::ChainExternalWrenchEstimator::nj
unsigned int nj
Definition: chainexternalwrenchestimator.hpp:115
chainjnttojacsolver.hpp
KDL::ChainExternalWrenchEstimator::estimated_momentum_integral
JntArray estimated_momentum_integral
Definition: chainexternalwrenchestimator.hpp:117
KDL::ChainJntToJacSolver
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition: chainjnttojacsolver.hpp:38
KDL::ChainFkSolverPos_recursive
Definition: chainfksolverpos_recursive.hpp:36
KDL::ChainExternalWrenchEstimator::E_FKSOLVERPOS_FAILED
static const int E_FKSOLVERPOS_FAILED
Definition: chainexternalwrenchestimator.hpp:49
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::ChainExternalWrenchEstimator::ESTIMATION_GAIN
Eigen::VectorXd ESTIMATION_GAIN
Definition: chainexternalwrenchestimator.hpp:121
KDL::ChainExternalWrenchEstimator::jacobian_end_eff_transpose
Eigen::MatrixXd jacobian_end_eff_transpose
Definition: chainexternalwrenchestimator.hpp:120
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::Jacobian
Definition: jacobian.hpp:36
KDL::ChainExternalWrenchEstimator::setSVDMaxIter
void setSVDMaxIter(const int maxiter_in)
Definition: chainexternalwrenchestimator.cpp:100