KDL 1.5.1
Loading...
Searching...
No Matches
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>
25#include "utilities/svd_eigen_HH.hpp"
26#include "chaindynparam.hpp"
29#include <iostream>
30
31namespace 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
Implementation of a method to calculate the matrices H (inertia),C(coriolis) and G(gravitation) for t...
Definition chaindynparam.hpp:48
First-order momentum observer for the estimation of external wrenches applied on the robot's end-effe...
Definition chainexternalwrenchestimator.hpp:44
JntArray estimated_ext_torque
Definition chainexternalwrenchestimator.hpp:118
JntSpaceInertiaMatrix jnt_mass_matrix
Definition chainexternalwrenchestimator.hpp:116
const Chain & CHAIN
Definition chainexternalwrenchestimator.hpp:111
Eigen::MatrixXd jacobian_end_eff_transpose_inv
Definition chainexternalwrenchestimator.hpp:120
static const int E_DYNPARAMSOLVERCORIOLIS_FAILED
Internally-used Dynamics Parameters (Mass) solver failed.
Definition chainexternalwrenchestimator.hpp:52
const double FILTER_CONST
Definition chainexternalwrenchestimator.hpp:112
JntArray initial_jnt_momentum
Definition chainexternalwrenchestimator.hpp:117
static const int E_DYNPARAMSOLVERGRAVITY_FAILED
Internally-used Dynamics Parameters (Coriolis) solver failed.
Definition chainexternalwrenchestimator.hpp:53
void setSVDMaxIter(const int maxiter_in)
Definition chainexternalwrenchestimator.cpp:100
const double DT_SEC
Definition chainexternalwrenchestimator.hpp:112
Eigen::MatrixXd U
Definition chainexternalwrenchestimator.hpp:120
Jacobian jacobian_end_eff
Definition chainexternalwrenchestimator.hpp:119
Eigen::VectorXd S
Definition chainexternalwrenchestimator.hpp:121
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition chainexternalwrenchestimator.cpp:204
int svd_maxiter
Definition chainexternalwrenchestimator.hpp:114
double svd_eps
Definition chainexternalwrenchestimator.hpp:113
void getEstimatedJntTorque(JntArray &external_joint_torque)
Definition chainexternalwrenchestimator.cpp:198
JntArray filtered_estimated_ext_torque
Definition chainexternalwrenchestimator.hpp:117
ChainDynParam dynparam_solver
Definition chainexternalwrenchestimator.hpp:122
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition chainexternalwrenchestimator.cpp:45
JntArray gravity_torque
Definition chainexternalwrenchestimator.hpp:118
JntArray coriolis_torque
Definition chainexternalwrenchestimator.hpp:118
Eigen::VectorXd S_inv
Definition chainexternalwrenchestimator.hpp:121
JntSpaceInertiaMatrix jnt_mass_matrix_dot
Definition chainexternalwrenchestimator.hpp:116
Eigen::MatrixXd V
Definition chainexternalwrenchestimator.hpp:120
int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench)
This method calculates the external wrench that is applied on the robot's end-effector.
Definition chainexternalwrenchestimator.cpp:106
static const int E_FKSOLVERPOS_FAILED
Definition chainexternalwrenchestimator.hpp:49
void setSVDEps(const double eps_in)
Definition chainexternalwrenchestimator.cpp:94
unsigned int nj
Definition chainexternalwrenchestimator.hpp:115
static const int E_DYNPARAMSOLVERMASS_FAILED
Internally-used Jacobian solver failed.
Definition chainexternalwrenchestimator.hpp:51
Eigen::Matrix< double, 6, 1 > Vector6d
Definition chainexternalwrenchestimator.hpp:45
JntArray estimated_momentum_integral
Definition chainexternalwrenchestimator.hpp:117
unsigned int ns
Definition chainexternalwrenchestimator.hpp:115
JntSpaceInertiaMatrix previous_jnt_mass_matrix
Definition chainexternalwrenchestimator.hpp:116
static const int E_JACSOLVER_FAILED
Internally-used Forward Position Kinematics (Recursive) solver failed.
Definition chainexternalwrenchestimator.hpp:50
Eigen::VectorXd ESTIMATION_GAIN
Definition chainexternalwrenchestimator.hpp:121
int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity)
Calculates robot's initial momentum in the joint space.
Definition chainexternalwrenchestimator.cpp:74
~ChainExternalWrenchEstimator()
Definition chainexternalwrenchestimator.hpp:68
JntArray total_torque
Definition chainexternalwrenchestimator.hpp:118
ChainFkSolverPos_recursive fk_pos_solver
Definition chainexternalwrenchestimator.hpp:124
ChainJntToJacSolver jacobian_solver
Definition chainexternalwrenchestimator.hpp:123
Eigen::MatrixXd jacobian_end_eff_transpose
Definition chainexternalwrenchestimator.hpp:120
Eigen::VectorXd tmp
Definition chainexternalwrenchestimator.hpp:121
Implementation of a recursive forward position kinematics algorithm to calculate the position transfo...
Definition chainfksolverpos_recursive.hpp:37
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition chainjnttojacsolver.hpp:39
Definition chain.hpp:35
Definition jacobian.hpp:37
Definition jntarray.hpp:70
Solver interface supporting storage and description of the latest error.
Definition solveri.hpp:85
int error
Latest error, initialized to E_NOERROR in constructor.
Definition solveri.hpp:149
A concrete implementation of a 3 dimensional vector class.
Definition frames.hpp:161
represents both translational and rotational acceleration.
Definition frames.hpp:879
Definition articulatedbodyinertia.cpp:26