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