Dynamics calculations by constraints based on Vereshchagin 1989. for a chain. This class creates instance of hybrid dynamics solver. The solver calculates total joint space accelerations in a chain when a constraint force(s) is applied to the chain's end-effector (task space/cartesian space). More...

#include <chainidsolver_vereshchagin.hpp>

Inheritance diagram for KDL::ChainIdSolver_Vereshchagin:

[legend]

Classes
struct	segment_info

Public Member Functions
int	CartToJnt (const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, JntArray &torques)

	ChainIdSolver_Vereshchagin (const Chain &chain, Twist root_acc, unsigned int nc)

virtual void	updateInternalDataStructures ()

	~ChainIdSolver_Vereshchagin ()

Private Types
typedef std::vector< Frame >	Frames

typedef Eigen::Matrix< double, 6, 6 >	Matrix6d

typedef Eigen::Matrix< double, 6, Eigen::Dynamic >	Matrix6Xd

typedef std::vector< Twist >	Twists

typedef Eigen::Matrix< double, 6, 1 >	Vector6d

Private Types inherited from KDL::SolverI
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 }

Private Member Functions
void	constraint_calculation (const JntArray &beta)

void	downwards_sweep (const Jacobian &alfa, const JntArray &torques)

void	final_upwards_sweep (JntArray &q_dotdot, JntArray &torques)

void	initial_upwards_sweep (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext)

Private Member Functions inherited from KDL::SolverI
virtual int	getError () const
	Return the latest error. More...

	SolverI ()
	Initialize latest error to E_NOERROR. More...

virtual const char *	strError (const int error) const

virtual	~SolverI ()

Private Attributes
Twist	acc_root

Jacobian	alfa_N

Jacobian	alfa_N2

JntArray	beta_N

const Chain &	chain

Frame	F_total

Eigen::MatrixXd	M_0_inverse

unsigned int	nc

unsigned int	nj

unsigned int	ns

Eigen::VectorXd	nu

Eigen::VectorXd	nu_sum

Wrench	qdotdot_sum

std::vector< segment_info, Eigen::aligned_allocator< segment_info > >	results

Eigen::VectorXd	Sm

Eigen::VectorXd	tmpm

Eigen::MatrixXd	Um

Eigen::MatrixXd	Vm

Private Attributes inherited from KDL::SolverI
int	error
	Latest error, initialized to E_NOERROR in constructor. More...

Detailed Description

Dynamics calculations by constraints based on Vereshchagin 1989. for a chain. This class creates instance of hybrid dynamics solver. The solver calculates total joint space accelerations in a chain when a constraint force(s) is applied to the chain's end-effector (task space/cartesian space).

Definition at line 40 of file chainidsolver_vereshchagin.hpp.

Member Typedef Documentation

typedef std::vector<Frame> KDL::ChainIdSolver_Vereshchagin::Frames

private

Definition at line 43 of file chainidsolver_vereshchagin.hpp.

typedef Eigen::Matrix<double, 6, 6 > KDL::ChainIdSolver_Vereshchagin::Matrix6d

private

Definition at line 45 of file chainidsolver_vereshchagin.hpp.

typedef Eigen::Matrix<double, 6, Eigen::Dynamic> KDL::ChainIdSolver_Vereshchagin::Matrix6Xd

private

Definition at line 46 of file chainidsolver_vereshchagin.hpp.

typedef std::vector<Twist> KDL::ChainIdSolver_Vereshchagin::Twists

private

Definition at line 42 of file chainidsolver_vereshchagin.hpp.

typedef Eigen::Matrix<double, 6, 1 > KDL::ChainIdSolver_Vereshchagin::Vector6d

private

Definition at line 44 of file chainidsolver_vereshchagin.hpp.

Constructor & Destructor Documentation

KDL::ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin	(	const Chain &	chain,
		Twist	root_acc,
		unsigned int	nc
	)

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.
root_acc	The acceleration vector of the root to use during the calculation.(most likely contains gravity)

Definition at line 31 of file chainidsolver_vereshchagin.cpp.

KDL::ChainIdSolver_Vereshchagin::~ChainIdSolver_Vereshchagin ( )

inline

Definition at line 57 of file chainidsolver_vereshchagin.hpp.

Member Function Documentation

int KDL::ChainIdSolver_Vereshchagin::CartToJnt	(	const JntArray &	q,
		const JntArray &	q_dot,
		JntArray &	q_dotdot,
		const Jacobian &	alfa,
		const JntArray &	beta,
		const Wrenches &	f_ext,
		JntArray &	torques
	)

This method calculates joint space constraint torques and total joint space acceleration. It returns 0 when it succeeds, otherwise -1 or -2 for unmatching matrix and array sizes. Input parameters;

Parameters

q	The current joint positions
q_dot	The current joint velocities
f_ext	The external forces (no gravity, it is given in root acceleration) on the segments. Output parameters:
q_dotdot	The joint accelerations
torques	the resulting constraint torques for the joints

Returns: error/success code

Definition at line 51 of file chainidsolver_vereshchagin.cpp.

void KDL::ChainIdSolver_Vereshchagin::constraint_calculation ( const JntArray & beta )

private

This method calculates constraint force magnitudes.

Definition at line 249 of file chainidsolver_vereshchagin.cpp.

void KDL::ChainIdSolver_Vereshchagin::downwards_sweep	(	const Jacobian &	alfa,
		const JntArray &	torques
	)

private

This method is a force balance sweep. It calculates articulated body inertias and bias forces. Additionally, acceleration energies generated by bias forces and unit forces are calculated here.

Definition at line 133 of file chainidsolver_vereshchagin.cpp.

void KDL::ChainIdSolver_Vereshchagin::final_upwards_sweep	(	JntArray &	q_dotdot,
		JntArray &	torques
	)

private

This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together.

Definition at line 284 of file chainidsolver_vereshchagin.cpp.

void KDL::ChainIdSolver_Vereshchagin::initial_upwards_sweep	(	const JntArray &	q,
		const JntArray &	q_dot,
		const JntArray &	q_dotdot,
		const Wrenches &	f_ext
	)

private

This method calculates all cartesian space poses, twists, bias accelerations. External forces are also taken into account in this outward sweep.

Definition at line 72 of file chainidsolver_vereshchagin.cpp.

void KDL::ChainIdSolver_Vereshchagin::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 chainidsolver_vereshchagin.cpp.