Recursive Newton Euler ID solver for chains in a tree. More...

#include <KdlChainIdRne.h>

Public Member Functions
int	DynamicsPass (const KDL::Wrenches &f_ext, KDL::JntArray &torques, KDL::Wrench &f_base, std::vector< KDL::Wrench > &f, const KDL::RigidBodyInertia &I_ext, const KDL::RigidBodyInertia &I_seg, KDL::JntArray &Hv, KDL::RigidBodyInertia &I_base)
	KdlChainIdRne (const KDL::Chain &chain, const KDL::Twist &v_base, const KDL::Twist &a_base)
	Constructor for KdlChainIdRne (empty)
int	KinematicsPass (const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, KDL::Twist &v_out, KDL::Twist &a_out)
	~KdlChainIdRne ()
Private Attributes
std::vector< KDL::Twist >	a
KDL::Twist	ab
KDL::Chain	chain
KDL::Wrench	Fis
std::vector < KDL::RigidBodyInertia >	Ii
unsigned int	nj
unsigned int	ns
std::vector< KDL::Twist >	S
std::vector< KDL::Twist >	v
KDL::Twist	vb
std::vector< KDL::Frame >	X

Detailed Description

Recursive Newton Euler ID solver for chains in a tree.

The algorithm implementation is based on the book "Rigid Body Dynamics Algorithms" of Roy Featherstone, 2008 (ISBN:978-0-387-74314-1) See pages 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.

This function assumes that the contributions of the base joint are calculated in the calling function.

Definition at line 27 of file KdlChainIdRne.h.

Constructor & Destructor Documentation

KdlChainIdRne::KdlChainIdRne	(	const KDL::Chain &	chain,
		const KDL::Twist &	v_base,
		const KDL::Twist &	a_base
	)

Constructor for KdlChainIdRne (empty)

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.
v_base	The velocity of the base frame in world/parent frame
a_base	The acceleration of the base frame due to prior joints and/or gravity, in world/parent frame.
chain_
v_base
a_base

Definition at line 12 of file KdlChainIdRne.cpp.

KdlChainIdRne::~KdlChainIdRne ( ) [inline]

Definition at line 36 of file KdlChainIdRne.h.

Member Function Documentation

int KdlChainIdRne::DynamicsPass	(	const KDL::Wrenches &	f_ext,
		KDL::JntArray &	torques,
		KDL::Wrench &	f_base,
		std::vector< KDL::Wrench > &	f,
		const KDL::RigidBodyInertia &	I_ext,
		const KDL::RigidBodyInertia &	I_seg,
		KDL::JntArray &	Hv,
		KDL::RigidBodyInertia &	I_base
	)

Function to calculate from Cartesian forces to joint torques. Must call KinematicsPass prior to this function. Input Parameters:

Parameters:

f_ext	The external forces (no gravity) on the segments in that segments frame (different than Featherstone algorithm) Output parameters:
torques	the resulting torques for the joints
f_base	The wrench on the base frame for tree ID
torques
forceBase
inertiaExt
inertiaSeg
Hv
inertiaBase

Definition at line 95 of file KdlChainIdRne.cpp.

int KdlChainIdRne::KinematicsPass	(	const KDL::JntArray &	q,
		const KDL::JntArray &	q_dot,
		const KDL::JntArray &	q_dotdot,
		KDL::Twist &	v_out,
		KDL::Twist &	a_out
	)

Function to calculate all necessary kinematic parameters. Must be called prior to DynamicsPass Input parameters:

Parameters:

q	The current joint positions
q_dot	The current joint velocities
q_dotdot	The current joint accelerations
q	Position
q_dot	Velocity
q_dotdot	Acceleration
velOut
accelOut