Recursive newton euler inverse dynamics solver. More...

#include <src/chainidsolver_recursive_newton_euler.hpp>

Inheritance diagram for KDL::ChainIdSolver_RNE:

[legend]

Collaboration diagram for KDL::ChainIdSolver_RNE:

[legend]

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
	ChainIdSolver_RNE (const Chain &chain, Vector grav)
	Constructor for the solver, it will allocate all the necessary memory.

	~ChainIdSolver_RNE ()

int	CartToJnt (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
	Function to calculate from Cartesian forces to joint torques.

virtual void	updateInternalDataStructures ()
	Update the internal data structures.

virtual int	getError () const
	Return the latest error.

virtual const char *	strError (const int error) const
	Return a description of the latest error.

Protected Attributes
int	error
	Latest error, initialized to E_NOERROR in constructor.

Private Attributes
const Chain &	chain

unsigned int	nj

unsigned int	ns

std::vector< Frame >	X

std::vector< Twist >	S

std::vector< Twist >	v

std::vector< Twist >	a

std::vector< Wrench >	f

Twist	ag

Detailed Description

Recursive newton euler inverse dynamics solver.

The algorithm implementation is based on the book "Rigid Body Dynamics Algorithms" of Roy Featherstone, 2008 (ISBN:978-0-387-74314-1) See page 96 for the pseudo-code.

It calculates the torques for the joints, given the motion of the joints (q,qdot,qdotdot), external forces on the segments (expressed in the segments reference frame) and the dynamical parameters of the segments.

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

◆ ChainIdSolver_RNE()

KDL::ChainIdSolver_RNE::ChainIdSolver_RNE	(	const Chain &	chain,
		Vector	grav )

Constructor for the solver, it will allocate all the necessary memory.

Parameters

chain	The kinematic chain to calculate the inverse dynamics for, an internal copy will be made.
grav	The gravity vector to use during the calculation.

References a, ag, chain, f, nj, ns, S, v, X, and KDL::Vector::Zero().

◆ ~ChainIdSolver_RNE()

KDL::ChainIdSolver_RNE::~ChainIdSolver_RNE ( )

inline

Member Function Documentation

◆ CartToJnt()

int KDL::ChainIdSolver_RNE::CartToJnt	(	const JntArray &	q,
		const JntArray &	q_dot,
		const JntArray &	q_dotdot,
		const Wrenches &	f_ext,
		JntArray &	torques )

virtual

Function to calculate from Cartesian forces to joint torques.

Input parameters;

Parameters

q	The current joint positions
q_dot	The current joint velocities
q_dotdot	The current joint accelerations
f_ext	The external forces (no gravity) on the segments Output parameters:
torques	the resulting torques for the joints

Implements KDL::ChainIdSolver.

References a, ag, chain, KDL::dot(), KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, f, KDL::Joint::Fixed, nj, ns, KDL::JntArray::rows(), S, v, and X.

◆ getError()

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

inlinevirtualinherited

Return the latest error.

References error.

◆ strError()

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

inlinevirtualinherited

Return a description of the latest error.

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

Reimplemented in KDL::ChainExternalWrenchEstimator, KDL::ChainIkSolverPos_LMA, KDL::ChainIkSolverPos_NR, KDL::ChainIkSolverPos_NR_JL, KDL::ChainIkSolverVel_pinv, KDL::ChainIkSolverVel_wdls, and KDL::ChainJntToJacDotSolver.

References E_DEGRADED, E_MAX_ITERATIONS_EXCEEDED, E_NO_CONVERGE, E_NOERROR, E_NOT_IMPLEMENTED, E_NOT_UP_TO_DATE, E_OUT_OF_RANGE, E_SIZE_MISMATCH, E_SVD_FAILED, E_UNDEFINED, and error.

Referenced by KDL::ChainExternalWrenchEstimator::strError(), KDL::ChainIkSolverPos_LMA::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverVel_wdls::strError(), and KDL::ChainJntToJacDotSolver::strError().

◆ updateInternalDataStructures()

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

References a, chain, f, nj, ns, S, v, and X.