KDL 1.5.1
Loading...
Searching...
No Matches
KDL::ChainExternalWrenchEstimator Class Reference

First-order momentum observer for the estimation of external wrenches applied on the robot's end-effector. More...

#include <src/chainexternalwrenchestimator.hpp>

Inheritance diagram for KDL::ChainExternalWrenchEstimator:
Collaboration diagram for KDL::ChainExternalWrenchEstimator:

Public Types

enum  {
  E_DEGRADED = +1 , E_NOERROR = 0 , E_NO_CONVERGE = -1 , E_UNDEFINED = -2 ,
  E_NOT_UP_TO_DATE = -3 , E_SIZE_MISMATCH = -4 , E_MAX_ITERATIONS_EXCEEDED = -5 , E_OUT_OF_RANGE = -6 ,
  E_NOT_IMPLEMENTED = -7 , E_SVD_FAILED = -8
}
 

Public Member Functions

 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.
 
 ~ChainExternalWrenchEstimator ()
 
int setInitialMomentum (const JntArray &joint_position, const JntArray &joint_velocity)
 Calculates robot's initial momentum in the joint space.
 
void setSVDEps (const double eps_in)
 
void setSVDMaxIter (const int maxiter_in)
 
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.
 
void getEstimatedJntTorque (JntArray &external_joint_torque)
 
virtual void updateInternalDataStructures ()
 Update the internal data structures.
 
virtual const char * strError (const int error) const
 Return a description of the latest error.
 
virtual int getError () const
 Return the latest error.
 

Static Public Attributes

static const int E_FKSOLVERPOS_FAILED = -100
 
static const int E_JACSOLVER_FAILED = -101
 Internally-used Forward Position Kinematics (Recursive) solver failed.
 
static const int E_DYNPARAMSOLVERMASS_FAILED = -102
 Internally-used Jacobian solver failed.
 
static const int E_DYNPARAMSOLVERCORIOLIS_FAILED = -103
 Internally-used Dynamics Parameters (Mass) solver failed.
 
static const int E_DYNPARAMSOLVERGRAVITY_FAILED = -104
 Internally-used Dynamics Parameters (Coriolis) solver failed.
 

Protected Attributes

int error
 Latest error, initialized to E_NOERROR in constructor.
 

Private Types

typedef Eigen::Matrix< double, 6, 1 > Vector6d
 

Private Attributes

const ChainCHAIN
 
const double DT_SEC
 
const double FILTER_CONST
 
double svd_eps
 
int svd_maxiter
 
unsigned int nj
 
unsigned int ns
 
JntSpaceInertiaMatrix jnt_mass_matrix
 
JntSpaceInertiaMatrix previous_jnt_mass_matrix
 
JntSpaceInertiaMatrix jnt_mass_matrix_dot
 
JntArray initial_jnt_momentum
 
JntArray estimated_momentum_integral
 
JntArray filtered_estimated_ext_torque
 
JntArray gravity_torque
 
JntArray coriolis_torque
 
JntArray total_torque
 
JntArray estimated_ext_torque
 
Jacobian jacobian_end_eff
 
Eigen::MatrixXd jacobian_end_eff_transpose
 
Eigen::MatrixXd jacobian_end_eff_transpose_inv
 
Eigen::MatrixXd U
 
Eigen::MatrixXd V
 
Eigen::VectorXd S
 
Eigen::VectorXd S_inv
 
Eigen::VectorXd tmp
 
Eigen::VectorXd ESTIMATION_GAIN
 
ChainDynParam dynparam_solver
 
ChainJntToJacSolver jacobian_solver
 
ChainFkSolverPos_recursive fk_pos_solver
 

Detailed Description

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.

Note: This component assumes that the external wrench is applied on the end-effector (last) link of the robot's chain.

Member Typedef Documentation

◆ Vector6d

Eigen::Matrix<double, 6, 1 > KDL::ChainExternalWrenchEstimator::Vector6d
private

Member Enumeration Documentation

◆ anonymous enum

anonymous enum
inherited
Enumerator
E_DEGRADED 

Converged but degraded solution (e.g. WDLS with psuedo-inverse singular)

E_NOERROR 

No error.

E_NO_CONVERGE 

Failed to converge.

E_UNDEFINED 

Undefined value (e.g. computed a NAN, or tan(90 degrees) )

E_NOT_UP_TO_DATE 

Chain size changed.

E_SIZE_MISMATCH 

Input size does not match internal state.

E_MAX_ITERATIONS_EXCEEDED 

Maximum number of iterations exceeded.

E_OUT_OF_RANGE 

Requested index out of range.

E_NOT_IMPLEMENTED 

Not yet implemented.

E_SVD_FAILED 

Internal svd calculation failed.

Constructor & Destructor Documentation

◆ ChainExternalWrenchEstimator()

KDL::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.

Constructor for the estimator, it will allocate all the necessary memory

Parameters
chainThe kinematic chain of the robot, an internal copy will be made.
gravityThe gravity-acceleration vector to use during the calculation.
sample_frequencyFrequency at which users updates it estimation loop (in Hz).
estimation_gainParameter used to control the estimator's convergence
filter_constantParameter 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.
epsIf a SVD-singular value is below this value, its inverse is set to zero. Default: 0.00001
maxiterMaximum iterations for the SVD computations. Default: 150.

◆ ~ChainExternalWrenchEstimator()

KDL::ChainExternalWrenchEstimator::~ChainExternalWrenchEstimator ( )
inline

Member Function Documentation

◆ getError()

virtual int KDL::SolverI::getError ( ) const
inlinevirtualinherited

Return the latest error.

References KDL::SolverI::error.

◆ getEstimatedJntTorque()

void KDL::ChainExternalWrenchEstimator::getEstimatedJntTorque ( JntArray & external_joint_torque)

◆ JntToExtWrench()

int KDL::ChainExternalWrenchEstimator::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.

Input parameters:

Parameters
joint_positionThe current (measured) joint positions.
joint_velocityThe current (measured) joint velocities.
joint_torqueThe 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:

Parameters
external_wrenchThe estimated external wrench applied on the robot's end-effector. The wrench will be expressed w.r.t. end-effector's frame.
Returns
error/success code

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.


Part I: Estimation of the torques felt in joints as a result of the external wrench being applied on the robot


Part II: Propagate above-estimated joint torques to Cartesian wrench using a pseudo-inverse of Jacobian-Transpose

References CHAIN, KDL::Jacobian::changeBase, coriolis_torque, KDL::Jacobian::data, KDL::JntArray::data, DT_SEC, dynparam_solver, E_DYNPARAMSOLVERCORIOLIS_FAILED, E_DYNPARAMSOLVERGRAVITY_FAILED, E_DYNPARAMSOLVERMASS_FAILED, E_FKSOLVERPOS_FAILED, E_JACSOLVER_FAILED, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::E_SVD_FAILED, KDL::SolverI::error, estimated_ext_torque, estimated_momentum_integral, ESTIMATION_GAIN, FILTER_CONST, filtered_estimated_ext_torque, fk_pos_solver, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), gravity_torque, initial_jnt_momentum, KDL::Rotation::Inverse(), jacobian_end_eff, jacobian_end_eff_transpose, jacobian_end_eff_transpose_inv, jacobian_solver, jnt_mass_matrix, jnt_mass_matrix_dot, KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainDynParam::JntToCoriolis(), KDL::ChainDynParam::JntToGravity(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainDynParam::JntToMass(), KDL::Frame::M, nj, ns, previous_jnt_mass_matrix, KDL::JntArray::rows(), S, S_inv, svd_eps, svd_maxiter, tmp, total_torque, U, and V.

◆ setInitialMomentum()

int KDL::ChainExternalWrenchEstimator::setInitialMomentum ( const JntArray & joint_position,
const JntArray & joint_velocity )

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.

References KDL::JntArray::data, dynparam_solver, E_DYNPARAMSOLVERMASS_FAILED, KDL::SolverI::E_NOERROR, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, estimated_momentum_integral, filtered_estimated_ext_torque, initial_jnt_momentum, jnt_mass_matrix, KDL::ChainDynParam::JntToMass(), nj, KDL::JntArray::rows(), and KDL::SetToZero().

◆ setSVDEps()

void KDL::ChainExternalWrenchEstimator::setSVDEps ( const double eps_in)

References svd_eps.

◆ setSVDMaxIter()

void KDL::ChainExternalWrenchEstimator::setSVDMaxIter ( const int maxiter_in)

References svd_maxiter.

◆ strError()

const char * KDL::ChainExternalWrenchEstimator::strError ( const int error) const
virtual

Return a description of the latest error.

Returns
if error is known then a description of error, otherwise "UNKNOWN ERROR"

Reimplemented from KDL::SolverI.

References E_DYNPARAMSOLVERCORIOLIS_FAILED, E_DYNPARAMSOLVERGRAVITY_FAILED, E_DYNPARAMSOLVERMASS_FAILED, E_FKSOLVERPOS_FAILED, E_JACSOLVER_FAILED, KDL::SolverI::error, and KDL::SolverI::strError().

◆ updateInternalDataStructures()

Member Data Documentation

◆ CHAIN

const Chain& KDL::ChainExternalWrenchEstimator::CHAIN
private

◆ coriolis_torque

JntArray KDL::ChainExternalWrenchEstimator::coriolis_torque
private

◆ DT_SEC

const double KDL::ChainExternalWrenchEstimator::DT_SEC
private

Referenced by JntToExtWrench().

◆ dynparam_solver

ChainDynParam KDL::ChainExternalWrenchEstimator::dynparam_solver
private

◆ E_DYNPARAMSOLVERCORIOLIS_FAILED

const int KDL::ChainExternalWrenchEstimator::E_DYNPARAMSOLVERCORIOLIS_FAILED = -103
static

Internally-used Dynamics Parameters (Mass) solver failed.

Referenced by JntToExtWrench(), and strError().

◆ E_DYNPARAMSOLVERGRAVITY_FAILED

const int KDL::ChainExternalWrenchEstimator::E_DYNPARAMSOLVERGRAVITY_FAILED = -104
static

Internally-used Dynamics Parameters (Coriolis) solver failed.

Referenced by JntToExtWrench(), and strError().

◆ E_DYNPARAMSOLVERMASS_FAILED

const int KDL::ChainExternalWrenchEstimator::E_DYNPARAMSOLVERMASS_FAILED = -102
static

Internally-used Jacobian solver failed.

Referenced by JntToExtWrench(), setInitialMomentum(), and strError().

◆ E_FKSOLVERPOS_FAILED

const int KDL::ChainExternalWrenchEstimator::E_FKSOLVERPOS_FAILED = -100
static

Referenced by JntToExtWrench(), and strError().

◆ E_JACSOLVER_FAILED

const int KDL::ChainExternalWrenchEstimator::E_JACSOLVER_FAILED = -101
static

Internally-used Forward Position Kinematics (Recursive) solver failed.

Referenced by JntToExtWrench(), and strError().

◆ error

int KDL::SolverI::error
protectedinherited

Latest error, initialized to E_NOERROR in constructor.

Referenced by KDL::ChainFdSolver_RNE::CartToJnt(), KDL::ChainHdSolver_Vereshchagin::CartToJnt(), KDL::ChainIdSolver_RNE::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::TreeIdSolver_RNE::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_wdls::getSigma(), KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainFkSolverVel_recursive::JntToCart(), JntToExtWrench(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainDynParam::JntToMass(), setInitialMomentum(), KDL::ChainIkSolverPos_NR_JL::setJointLimits(), KDL::ChainJntToJacDotSolver::setLockedJoints(), KDL::ChainJntToJacSolver::setLockedJoints(), KDL::ChainIkSolverVel_pinv_nso::setOptPos(), KDL::ChainIkSolverVel_wdls::setWeightJS(), KDL::ChainIkSolverVel_pinv_nso::setWeights(), KDL::ChainIkSolverVel_wdls::setWeightTS(), strError(), KDL::ChainIkSolverPos_LMA::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverVel_wdls::strError(), KDL::ChainJntToJacDotSolver::strError(), and KDL::SolverI::strError().

◆ estimated_ext_torque

JntArray KDL::ChainExternalWrenchEstimator::estimated_ext_torque
private

◆ estimated_momentum_integral

JntArray KDL::ChainExternalWrenchEstimator::estimated_momentum_integral
private

◆ ESTIMATION_GAIN

Eigen::VectorXd KDL::ChainExternalWrenchEstimator::ESTIMATION_GAIN
private

◆ FILTER_CONST

const double KDL::ChainExternalWrenchEstimator::FILTER_CONST
private

Referenced by JntToExtWrench().

◆ filtered_estimated_ext_torque

JntArray KDL::ChainExternalWrenchEstimator::filtered_estimated_ext_torque
private

◆ fk_pos_solver

ChainFkSolverPos_recursive KDL::ChainExternalWrenchEstimator::fk_pos_solver
private

◆ gravity_torque

JntArray KDL::ChainExternalWrenchEstimator::gravity_torque
private

◆ initial_jnt_momentum

JntArray KDL::ChainExternalWrenchEstimator::initial_jnt_momentum
private

◆ jacobian_end_eff

Jacobian KDL::ChainExternalWrenchEstimator::jacobian_end_eff
private

◆ jacobian_end_eff_transpose

Eigen::MatrixXd KDL::ChainExternalWrenchEstimator::jacobian_end_eff_transpose
private

◆ jacobian_end_eff_transpose_inv

Eigen::MatrixXd KDL::ChainExternalWrenchEstimator::jacobian_end_eff_transpose_inv
private

◆ jacobian_solver

ChainJntToJacSolver KDL::ChainExternalWrenchEstimator::jacobian_solver
private

◆ jnt_mass_matrix

JntSpaceInertiaMatrix KDL::ChainExternalWrenchEstimator::jnt_mass_matrix
private

◆ jnt_mass_matrix_dot

JntSpaceInertiaMatrix KDL::ChainExternalWrenchEstimator::jnt_mass_matrix_dot
private

◆ nj

unsigned int KDL::ChainExternalWrenchEstimator::nj
private

◆ ns

unsigned int KDL::ChainExternalWrenchEstimator::ns
private

◆ previous_jnt_mass_matrix

JntSpaceInertiaMatrix KDL::ChainExternalWrenchEstimator::previous_jnt_mass_matrix
private

◆ S

Eigen::VectorXd KDL::ChainExternalWrenchEstimator::S
private

◆ S_inv

Eigen::VectorXd KDL::ChainExternalWrenchEstimator::S_inv
private

◆ svd_eps

double KDL::ChainExternalWrenchEstimator::svd_eps
private

Referenced by JntToExtWrench(), and setSVDEps().

◆ svd_maxiter

int KDL::ChainExternalWrenchEstimator::svd_maxiter
private

Referenced by JntToExtWrench(), and setSVDMaxIter().

◆ tmp

Eigen::VectorXd KDL::ChainExternalWrenchEstimator::tmp
private

◆ total_torque

JntArray KDL::ChainExternalWrenchEstimator::total_torque
private

◆ U

Eigen::MatrixXd KDL::ChainExternalWrenchEstimator::U
private

◆ V

Eigen::MatrixXd KDL::ChainExternalWrenchEstimator::V
private

The documentation for this class was generated from the following files: