KDL  1.3.0
Public Member Functions | Private Attributes | List of all members
KDL::ChainIdSolver_RNE Class Reference

Recursive newton euler inverse dynamics solver. More...

#include <src/chainidsolver_recursive_newton_euler.hpp>

Inheritance diagram for KDL::ChainIdSolver_RNE:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainIdSolver_RNE:
Collaboration graph
[legend]

Public Member Functions

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

Private Attributes

Chain chain
 
unsigned int nj
 
unsigned int ns
 
std::vector< FrameX
 
std::vector< TwistS
 
std::vector< Twistv
 
std::vector< Twista
 
std::vector< Wrenchf
 
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.

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
chainThe kinematic chain to calculate the inverse dynamics for, an internal copy will be made.
gravThe gravity vector to use during the calculation.

References ag, and KDL::Vector::Zero().

◆ ~ChainIdSolver_RNE()

KDL::ChainIdSolver_RNE::~ChainIdSolver_RNE ( )
inline

References CartToJnt().

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
qThe current joint positions
q_dotThe current joint velocities
q_dotdotThe current joint accelerations
f_extThe external forces (no gravity) on the segments Output parameters:
torquesthe resulting torques for the joints

Implements KDL::ChainIdSolver.

References a, ag, chain, KDL::dot(), f, KDL::Segment::getInertia(), KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), nj, KDL::Joint::None, ns, KDL::Segment::pose(), KDL::JntArray::rows(), S, KDL::Segment::twist(), v, and X.

Referenced by KDL::ChainDynParam::JntToCoriolis(), KDL::ChainDynParam::JntToGravity(), and ~ChainIdSolver_RNE().

Member Data Documentation

◆ a

std::vector<Twist> KDL::ChainIdSolver_RNE::a
private

Referenced by CartToJnt().

◆ ag

Twist KDL::ChainIdSolver_RNE::ag
private

Referenced by CartToJnt(), and ChainIdSolver_RNE().

◆ chain

Chain KDL::ChainIdSolver_RNE::chain
private

Referenced by CartToJnt().

◆ f

std::vector<Wrench> KDL::ChainIdSolver_RNE::f
private

Referenced by CartToJnt().

◆ nj

unsigned int KDL::ChainIdSolver_RNE::nj
private

Referenced by CartToJnt().

◆ ns

unsigned int KDL::ChainIdSolver_RNE::ns
private

Referenced by CartToJnt().

◆ S

std::vector<Twist> KDL::ChainIdSolver_RNE::S
private

Referenced by CartToJnt().

◆ v

std::vector<Twist> KDL::ChainIdSolver_RNE::v
private

Referenced by CartToJnt().

◆ X

std::vector<Frame> KDL::ChainIdSolver_RNE::X
private

Referenced by CartToJnt().


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