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

#include <chainexternalwrenchestimator.hpp>

Inheritance diagram for KDL::ChainExternalWrenchEstimator:

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. More...

void	getEstimatedJntTorque (JntArray &external_joint_torque)

int	JntToExtWrench (const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench)

int	setInitialMomentum (const JntArray &joint_position, const JntArray &joint_velocity)

void	setSVDEps (const double eps_in)

void	setSVDMaxIter (const int maxiter_in)

virtual const char *	strError (const int error) const

virtual void	updateInternalDataStructures ()

	~ChainExternalWrenchEstimator ()

Public Member Functions inherited from KDL::SolverI
virtual int	getError () const
	Return the latest error. More...

	SolverI ()
	Initialize latest error to E_NOERROR. More...

virtual	~SolverI ()

Static Public Attributes
static const int	E_DYNPARAMSOLVERCORIOLIS_FAILED = -103
	Internally-used Dynamics Parameters (Mass) solver failed. More...

static const int	E_DYNPARAMSOLVERGRAVITY_FAILED = -104
	Internally-used Dynamics Parameters (Coriolis) solver failed. More...

static const int	E_DYNPARAMSOLVERMASS_FAILED = -102
	Internally-used Jacobian solver failed. More...

static const int	E_FKSOLVERPOS_FAILED = -100

static const int	E_JACSOLVER_FAILED = -101
	Internally-used Forward Position Kinematics (Recursive) solver failed. More...

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

Private Attributes
const Chain &	CHAIN

JntArray	coriolis_torque

const double	DT_SEC

ChainDynParam	dynparam_solver

JntArray	estimated_ext_torque

JntArray	estimated_momentum_integral

Eigen::VectorXd	ESTIMATION_GAIN

const double	FILTER_CONST

JntArray	filtered_estimated_ext_torque

ChainFkSolverPos_recursive	fk_pos_solver

JntArray	gravity_torque

JntArray	initial_jnt_momentum

Jacobian	jacobian_end_eff

Eigen::MatrixXd	jacobian_end_eff_transpose

Eigen::MatrixXd	jacobian_end_eff_transpose_inv

ChainJntToJacSolver	jacobian_solver

JntSpaceInertiaMatrix	jnt_mass_matrix

JntSpaceInertiaMatrix	jnt_mass_matrix_dot

unsigned int	nj

unsigned int	ns

JntSpaceInertiaMatrix	previous_jnt_mass_matrix

Eigen::VectorXd	S

Eigen::VectorXd	S_inv

double	svd_eps

int	svd_maxiter

Eigen::VectorXd	tmp

JntArray	total_torque

Eigen::MatrixXd	U

Eigen::MatrixXd	V

Additional Inherited Members
Public Types inherited from KDL::SolverI
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 }

Protected Attributes inherited from KDL::SolverI
int	error
	Latest error, initialized to E_NOERROR in constructor. More...

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.

Definition at line 43 of file chainexternalwrenchestimator.hpp.

Member Typedef Documentation

◆ Vector6d

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

private

Definition at line 45 of file chainexternalwrenchestimator.hpp.

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

chain	The kinematic chain of the robot, an internal copy will be made.
gravity	The gravity-acceleration vector to use during the calculation.
sample_frequency	Frequency at which users updates it estimation loop (in Hz).
estimation_gain	Parameter used to control the estimator's convergence
filter_constant	Parameter 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.
eps	If a SVD-singular value is below this value, its inverse is set to zero. Default: 0.00001
maxiter	Maximum iterations for the SVD computations. Default: 150.

Definition at line 25 of file chainexternalwrenchestimator.cpp.

◆ ~ChainExternalWrenchEstimator()

KDL::ChainExternalWrenchEstimator::~ChainExternalWrenchEstimator ( )

inline

Definition at line 68 of file chainexternalwrenchestimator.hpp.

Member Function Documentation

◆ getEstimatedJntTorque()

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

Definition at line 198 of file chainexternalwrenchestimator.cpp.

◆ 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_position	The current (measured) joint positions.
joint_velocity	The current (measured) joint velocities.
joint_torque	The 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_wrench The 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

Definition at line 106 of file chainexternalwrenchestimator.cpp.

◆ setInitialMomentum()

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

Calculates robot's initial momentum in the joint space. Basically, 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.

Definition at line 74 of file chainexternalwrenchestimator.cpp.

◆ setSVDEps()

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

Definition at line 94 of file chainexternalwrenchestimator.cpp.

◆ setSVDMaxIter()

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

Definition at line 100 of file chainexternalwrenchestimator.cpp.

◆ 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.

Definition at line 204 of file chainexternalwrenchestimator.cpp.

◆ updateInternalDataStructures()

void KDL::ChainExternalWrenchEstimator::updateInternalDataStructures ( )

virtual

Update the internal data structures. This is required if the number of segments or number of joints of a chain/tree have changed. This provides a single point of contact for solver memory allocations.

Implements KDL::SolverI.

Definition at line 45 of file chainexternalwrenchestimator.cpp.