Recursive newton euler forward dynamics solver. More...

#include <chainfdsolver_recursive_newton_euler.hpp>

Inheritance diagram for KDL::ChainFdSolver_RNE:

Public Member Functions
int	CartToJnt (const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches &f_ext, JntArray &q_dotdot)
	ChainFdSolver_RNE (const Chain &chain, Vector grav)
void	RK4Integrator (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)
virtual void	updateInternalDataStructures ()
	~ChainFdSolver_RNE ()
Private Attributes
Eigen::VectorXd	acc_eig
const Chain &	chain
Eigen::VectorXd	D_eig
ChainDynParam	DynSolver
JntSpaceInertiaMatrix	H
Eigen::MatrixXd	H_eig
ChainIdSolver_RNE	IdSolver
Eigen::MatrixXd	L_eig
unsigned int	nj
unsigned int	ns
Eigen::VectorXd	r_eig
JntArray	Tzeroacc
Eigen::VectorXd	Tzeroacc_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.

Definition at line 42 of file chainfdsolver_recursive_newton_euler.hpp.

Constructor & Destructor Documentation

KDL::ChainFdSolver_RNE::ChainFdSolver_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 forward dynamics for, an internal copy will be made.
grav	The gravity vector to use during the calculation.

Definition at line 29 of file chainfdsolver_recursive_newton_euler.cpp.

KDL::ChainFdSolver_RNE::~ChainFdSolver_RNE ( ) [inline]

Definition at line 50 of file chainfdsolver_recursive_newton_euler.hpp.

Member Function Documentation

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:

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

Implements KDL::ChainFdSolver.

Definition at line 51 of file chainfdsolver_recursive_newton_euler.cpp.

void KDL::ChainFdSolver_RNE::RK4Integrator	(	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:

nj	The number of joints
t	The current time
dt	The integration period
q	The current joint positions
q_dot	The current joint velocities
torques	The current joint torques (applied by controller)
f_ext	The external forces (no gravity) on the segments
fdsolver	The forward dynamics solver Output parameters:
t	The updated time
q	The updated joint positions
q_dot	The updated joint velocities
q_dotdot	The current joint accelerations
dq	The joint position increment
dq_dot	The joint velocity increment Temporary parameters:
qtemp	Intermediate joint positions
qdtemp	Intermediate joint velocities

Definition at line 95 of file chainfdsolver_recursive_newton_euler.cpp.

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.

Definition at line 46 of file chainfdsolver_recursive_newton_euler.cpp.