KDL 1.5.1
Public Types | Public Member Functions | Protected Attributes | Private Attributes | List of all members
KDL::ChainFdSolver_RNE Class Reference

Recursive newton euler forward dynamics solver. More...

#include <src/chainfdsolver_recursive_newton_euler.hpp>

Inheritance diagram for KDL::ChainFdSolver_RNE:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainFdSolver_RNE:
Collaboration graph
[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

 ChainFdSolver_RNE (const Chain &chain, Vector grav)
 Constructor for the solver, it will allocate all the necessary memory. More...
 
 ~ChainFdSolver_RNE ()
 
int CartToJnt (const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches &f_ext, JntArray &q_dotdot)
 Function to calculate from Cartesian forces to joint torques. More...
 
virtual void updateInternalDataStructures ()
 Update the internal data structures. More...
 
void RK4Integrator (unsigned int &nj, const double &t, double &dt, KDL::JntArray &q, KDL::JntArray &q_dot, KDL::JntArray &torques, KDL::Wrenches &f_ext, KDL::ChainFdSolver_RNE &fdsolver, KDL::JntArray &q_dotdot, KDL::JntArray &dq, KDL::JntArray &dq_dot, KDL::JntArray &q_temp, KDL::JntArray &q_dot_temp)
 Function to integrate the joint accelerations resulting from the forward dynamics solver. More...
 
virtual int getError () const
 Return the latest error. More...
 
virtual const char * strError (const int error) const
 Return a description of the latest error. More...
 

Protected Attributes

int error
 Latest error, initialized to E_NOERROR in constructor. More...
 

Private Attributes

const Chainchain
 
ChainDynParam DynSolver
 
ChainIdSolver_RNE IdSolver
 
unsigned int nj
 
unsigned int ns
 
JntSpaceInertiaMatrix H
 
JntArray Tzeroacc
 
Eigen::MatrixXd H_eig
 
Eigen::VectorXd Tzeroacc_eig
 
Eigen::MatrixXd L_eig
 
Eigen::VectorXd D_eig
 
Eigen::VectorXd r_eig
 
Eigen::VectorXd acc_eig
 

Detailed Description

Recursive newton euler forward 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 Chapter 6 for basic algorithm.

It calculates the accelerations for the joints (qdotdot), given the position and velocity 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

◆ ChainFdSolver_RNE()

KDL::ChainFdSolver_RNE::ChainFdSolver_RNE ( const Chain chain,
Vector  grav 
)

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

Parameters
chainThe kinematic chain to calculate the forward dynamics for, an internal copy will be made.
gravThe gravity vector to use during the calculation.

◆ ~ChainFdSolver_RNE()

KDL::ChainFdSolver_RNE::~ChainFdSolver_RNE ( )
inline

Member Function Documentation

◆ CartToJnt()

int KDL::ChainFdSolver_RNE::CartToJnt ( const JntArray q,
const JntArray q_dot,
const JntArray torques,
const Wrenches f_ext,
JntArray q_dotdot 
)
virtual

Function to calculate from Cartesian forces to joint torques.

Input parameters;

Parameters
qThe current joint positions
q_dotThe current joint velocities
torquesThe current joint torques (applied by controller)
f_extThe external forces (no gravity) on the segments Output parameters:
q_dotdotThe resulting joint accelerations

Implements KDL::ChainFdSolver.

References acc_eig, KDL::ChainIdSolver_RNE::CartToJnt(), chain, D_eig, DynSolver, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), H, H_eig, IdSolver, KDL::ChainDynParam::JntToMass(), L_eig, nj, ns, r_eig, KDL::JntArray::rows(), Tzeroacc, and Tzeroacc_eig.

Referenced by RK4Integrator().

◆ getError()

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

Return the latest error.

References KDL::SolverI::error.

◆ RK4Integrator()

void KDL::ChainFdSolver_RNE::RK4Integrator ( unsigned int &  nj,
const double &  t,
double &  dt,
KDL::JntArray q,
KDL::JntArray q_dot,
KDL::JntArray torques,
KDL::Wrenches f_ext,
KDL::ChainFdSolver_RNE fdsolver,
KDL::JntArray q_dotdot,
KDL::JntArray dq,
KDL::JntArray dq_dot,
KDL::JntArray q_temp,
KDL::JntArray q_dot_temp 
)

Function to integrate the joint accelerations resulting from the forward dynamics solver.

Input parameters;

Parameters
njThe number of joints
tThe current time
dtThe integration period
qThe current joint positions
q_dotThe current joint velocities
torquesThe current joint torques (applied by controller)
f_extThe external forces (no gravity) on the segments
fdsolverThe forward dynamics solver Output parameters:
tThe updated time
qThe updated joint positions
q_dotThe updated joint velocities
q_dotdotThe current joint accelerations
dqThe joint position increment
dq_dotThe joint velocity increment Temporary parameters:
qtempIntermediate joint positions
qdtempIntermediate joint velocities

References CartToJnt(), and nj.

◆ strError()

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

◆ updateInternalDataStructures()

void KDL::ChainFdSolver_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 chain, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), nj, and ns.

Member Data Documentation

◆ acc_eig

Eigen::VectorXd KDL::ChainFdSolver_RNE::acc_eig
private

Referenced by CartToJnt().

◆ chain

const Chain& KDL::ChainFdSolver_RNE::chain
private

◆ D_eig

Eigen::VectorXd KDL::ChainFdSolver_RNE::D_eig
private

Referenced by CartToJnt().

◆ DynSolver

ChainDynParam KDL::ChainFdSolver_RNE::DynSolver
private

Referenced by CartToJnt().

◆ error

int KDL::SolverI::error
protectedinherited

Latest error, initialized to E_NOERROR in constructor.

Referenced by KDL::ChainIdSolver_RNE::CartToJnt(), KDL::TreeIdSolver_RNE::CartToJnt(), CartToJnt(), KDL::ChainHdSolver_Vereshchagin::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_wdls::getSigma(), KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainFkSolverVel_recursive::JntToCart(), KDL::ChainExternalWrenchEstimator::JntToExtWrench(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainDynParam::JntToMass(), KDL::ChainExternalWrenchEstimator::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(), KDL::ChainExternalWrenchEstimator::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().

◆ H

JntSpaceInertiaMatrix KDL::ChainFdSolver_RNE::H
private

Referenced by CartToJnt().

◆ H_eig

Eigen::MatrixXd KDL::ChainFdSolver_RNE::H_eig
private

Referenced by CartToJnt().

◆ IdSolver

ChainIdSolver_RNE KDL::ChainFdSolver_RNE::IdSolver
private

Referenced by CartToJnt().

◆ L_eig

Eigen::MatrixXd KDL::ChainFdSolver_RNE::L_eig
private

Referenced by CartToJnt().

◆ nj

unsigned int KDL::ChainFdSolver_RNE::nj
private

◆ ns

unsigned int KDL::ChainFdSolver_RNE::ns
private

◆ r_eig

Eigen::VectorXd KDL::ChainFdSolver_RNE::r_eig
private

Referenced by CartToJnt().

◆ Tzeroacc

JntArray KDL::ChainFdSolver_RNE::Tzeroacc
private

Referenced by CartToJnt().

◆ Tzeroacc_eig

Eigen::VectorXd KDL::ChainFdSolver_RNE::Tzeroacc_eig
private

Referenced by CartToJnt().


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