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]

List of all members.

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 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 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

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.