#include <src/chainidsolver_vereshchagin.hpp>

Inheritance diagram for KDL::ChainIdSolver_Vereshchagin:

[legend]

Collaboration diagram for KDL::ChainIdSolver_Vereshchagin:

[legend]

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

int	CartToJnt (const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, const JntArray &ff_torques, JntArray &constraint_torques)
	This method calculates joint space constraint torques and accelerations.

virtual void	updateInternalDataStructures ()
	Update the internal data structures.

void	getTransformedLinkAcceleration (Twists &x_dotdot)

void	getTotalTorque (JntArray &total_tau)

void	getContraintForceMagnitude (Eigen::VectorXd &nu_)

Private Types
typedef std::vector< Twist >	Twists

typedef std::vector< Frame >	Frames

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

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

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

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	initial_upwards_sweep (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext)
	This method calculates all cartesian space poses, twists, bias accelerations.

void	downwards_sweep (const Jacobian &alfa, const JntArray &ff_torques)
	This method is a force balance sweep.

void	constraint_calculation (const JntArray &beta)
	This method calculates constraint force magnitudes.

void	final_upwards_sweep (JntArray &q_dotdot, JntArray &constraint_torques)
	This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together.

virtual int	getError () const
	Return the latest error.

virtual const char *	strError (const int error) const
	Return a description of the latest error.

Private Attributes
const Chain &	chain

unsigned int	nj

unsigned int	ns

unsigned int	nc

Twist	acc_root

Jacobian	alfa_N

Jacobian	alfa_N2

Eigen::MatrixXd	M_0_inverse

Eigen::MatrixXd	Um

Eigen::MatrixXd	Vm

JntArray	beta_N

Eigen::VectorXd	nu

Eigen::VectorXd	nu_sum

Eigen::VectorXd	Sm

Eigen::VectorXd	tmpm

Eigen::VectorXd	total_torques

Wrench	qdotdot_sum

Frame	F_total

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

int	error
	Latest error, initialized to E_NOERROR in constructor.

Member Typedef Documentation

◆ Frames

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

privateinherited

◆ Matrix6d

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

privateinherited

◆ Matrix6Xd

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

privateinherited

◆ Twists

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

privateinherited

◆ Vector6d

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

privateinherited

Member Enumeration Documentation

◆ anonymous enum

anonymous enum

inherited

Enumerator
E_DEGRADED	Converged but degraded solution (e.g. WDLS with psuedo-inverse singular)
E_NOERROR	No error.
E_NO_CONVERGE	Failed to converge.
E_UNDEFINED	Undefined value (e.g. computed a NAN, or tan(90 degrees) )
E_NOT_UP_TO_DATE	Chain size changed.
E_SIZE_MISMATCH	Input size does not match internal state.
E_MAX_ITERATIONS_EXCEEDED	Maximum number of iterations exceeded.
E_OUT_OF_RANGE	Requested index out of range.
E_NOT_IMPLEMENTED	Not yet implemented.
E_SVD_FAILED	Internal svd calculation failed.

Constructor & Destructor Documentation

◆ ChainIdSolver_Vereshchagin()

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

References KDL::ChainHdSolver_Vereshchagin::ChainHdSolver_Vereshchagin().

Member Function Documentation

◆ CartToJnt()

int KDL::ChainHdSolver_Vereshchagin::CartToJnt	(	const JntArray &	q,
		const JntArray &	q_dot,
		JntArray &	q_dotdot,
		const Jacobian &	alfa,
		const JntArray &	beta,
		const Wrenches &	f_ext,
		const JntArray &	ff_torques,
		JntArray &	constraint_torques )

inherited

This method calculates joint space constraint torques and accelerations.

It returns 0 when it succeeds, otherwise -1 or -2 for nonmatching matrix and array sizes. Input parameters:

Parameters

q	The current joint positions
q_dot	The current joint velocities
alpha	The active constraint directions (unit constraint forces expressed w.r.t. robot's base frame)
beta	The acceleration energy setpoints (expressed w.r.t. above-defined unit constraint forces)
f_ext	The external forces (no gravity, it is given in root acceleration) on the segments
ff_torques	The feed-forward joint space torques

Output parameters:

Parameters

q_dotdot	The resulting joint accelerations
constraint_torques	The resulting joint constraint torques (what each joint feels due to the constraint forces acting on the end-effector)

Returns: error/success code

References chain, KDL::Jacobian::columns(), constraint_calculation(), downwards_sweep(), KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, final_upwards_sweep(), initial_upwards_sweep(), nc, nj, ns, and KDL::JntArray::rows().

◆ constraint_calculation()

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

privateinherited

This method calculates constraint force magnitudes.

References acc_root, KDL::JntArray::data, M_0_inverse, nc, nu, nu_sum, results, Sm, tmpm, Um, and Vm.

Referenced by CartToJnt().

◆ downwards_sweep()

void KDL::ChainHdSolver_Vereshchagin::downwards_sweep	(	const Jacobian &	alfa,
		const JntArray &	ff_torques )

privateinherited

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.

Additionally adding joint inertia to s.D, see:

equation a) in Vereshchagin89
equation 9.28, page 188, Featherstone book 2008

Referenced by CartToJnt().

◆ final_upwards_sweep()

void KDL::ChainHdSolver_Vereshchagin::final_upwards_sweep	(	JntArray &	q_dotdot,
		JntArray &	constraint_torques )

privateinherited

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

Referenced by CartToJnt().

◆ getContraintForceMagnitude()

void KDL::ChainHdSolver_Vereshchagin::getContraintForceMagnitude ( Eigen::VectorXd & nu_ )

inherited

References nu.

◆ getError()

virtual int KDL::SolverI::getError ( ) const

inlinevirtualinherited

Return the latest error.

References error.

◆ getTotalTorque()

void KDL::ChainHdSolver_Vereshchagin::getTotalTorque ( JntArray & total_tau )

inherited

References KDL::JntArray::data, and total_torques.

◆ getTransformedLinkAcceleration()

void KDL::ChainHdSolver_Vereshchagin::getTransformedLinkAcceleration ( Twists & x_dotdot )

inherited

References acc_root, ns, and results.

◆ initial_upwards_sweep()

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

privateinherited

This method calculates all cartesian space poses, twists, bias accelerations.

External forces are also taken into account in this outward sweep.

Referenced by CartToJnt().

◆ strError()

virtual const char * KDL::SolverI::strError ( const int error ) const

inlinevirtualinherited

Return a description of the latest error.

Returns: if error is known then a description of error, otherwise "UNKNOWN ERROR"

Reimplemented in KDL::ChainExternalWrenchEstimator, KDL::ChainIkSolverPos_LMA, KDL::ChainIkSolverPos_NR, KDL::ChainIkSolverPos_NR_JL, KDL::ChainIkSolverVel_pinv, KDL::ChainIkSolverVel_wdls, and KDL::ChainJntToJacDotSolver.

References E_DEGRADED, E_MAX_ITERATIONS_EXCEEDED, E_NO_CONVERGE, E_NOERROR, E_NOT_IMPLEMENTED, E_NOT_UP_TO_DATE, E_OUT_OF_RANGE, E_SIZE_MISMATCH, E_SVD_FAILED, E_UNDEFINED, and error.

Referenced by KDL::ChainExternalWrenchEstimator::strError(), KDL::ChainIkSolverPos_LMA::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverVel_wdls::strError(), and KDL::ChainJntToJacDotSolver::strError().

◆ updateInternalDataStructures()

void KDL::ChainHdSolver_Vereshchagin::updateInternalDataStructures ( )

virtualinherited

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.

References chain, nc, nj, ns, results, and total_torques.