KDL  1.3.0
Classes | Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
KDL::ChainIdSolver_Vereshchagin Class Reference

Dynamics calculations by constraints based on Vereshchagin 1989. More...

#include <src/chainidsolver_vereshchagin.hpp>

Collaboration diagram for KDL::ChainIdSolver_Vereshchagin:
Collaboration graph
[legend]

Classes

struct  segment_info
 

Public Member Functions

 ChainIdSolver_Vereshchagin (const Chain &chain, Twist root_acc, unsigned int nc)
 Constructor for the solver, it will allocate all the necessary memory. More...
 
 ~ChainIdSolver_Vereshchagin ()
 
int 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. More...
 

Private Types

typedef std::vector< TwistTwists
 
typedef std::vector< FrameFrames
 
typedef Eigen::Matrix< double, 6, 1 > Vector6d
 
typedef Eigen::Matrix< double, 6, 6 > Matrix6d
 
typedef Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd
 

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. More...
 
void downwards_sweep (const Jacobian &alfa, const JntArray &torques)
 This method is a force balance sweep. More...
 
void constraint_calculation (const JntArray &beta)
 This method calculates constraint force magnitudes. More...
 
void final_upwards_sweep (JntArray &q_dotdot, JntArray &torques)
 This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together. More...
 

Private Attributes

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
 
Wrench qdotdot_sum
 
Frame F_total
 
std::vector< segment_inforesults
 

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

Member Typedef Documentation

typedef std::vector<Frame> KDL::ChainIdSolver_Vereshchagin::Frames
private
typedef Eigen::Matrix<double, 6, 6 > KDL::ChainIdSolver_Vereshchagin::Matrix6d
private
typedef Eigen::Matrix<double, 6, Eigen::Dynamic> KDL::ChainIdSolver_Vereshchagin::Matrix6Xd
private
typedef std::vector<Twist> KDL::ChainIdSolver_Vereshchagin::Twists
private
typedef Eigen::Matrix<double, 6, 1 > KDL::ChainIdSolver_Vereshchagin::Vector6d
private

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
chainThe kinematic chain to calculate the inverse dynamics for, an internal copy will be made.
root_accThe acceleration vector of the root to use during the calculation.(most likely contains gravity)

References acc_root, M_0_inverse, nc, nu_sum, Sm, tmpm, Um, and Vm.

KDL::ChainIdSolver_Vereshchagin::~ChainIdSolver_Vereshchagin ( )
inline

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
qThe current joint positions
q_dotThe current joint velocities
f_extThe external forces (no gravity, it is given in root acceleration) on the segments. Output parameters:
q_dotdotThe joint accelerations
torquesthe resulting constraint torques for the joints

References KDL::Jacobian::columns(), constraint_calculation(), downwards_sweep(), final_upwards_sweep(), initial_upwards_sweep(), nc, nj, and ns.

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

This method calculates constraint force magnitudes.

References acc_root, KDL::Vector::data, M_0_inverse, nc, nu, nu_sum, results, KDL::Twist::rot, Sm, tmpm, Um, KDL::Twist::vel, and Vm.

Referenced by CartToJnt().

void KDL::ChainIdSolver_Vereshchagin::downwards_sweep ( const Jacobian alfa,
const JntArray &  torques 
)
private
void KDL::ChainIdSolver_Vereshchagin::final_upwards_sweep ( JntArray &  q_dotdot,
JntArray &  torques 
)
private
void KDL::ChainIdSolver_Vereshchagin::initial_upwards_sweep ( const JntArray &  q,
const JntArray &  q_dot,
const JntArray &  q_dotdot,
const Wrenches f_ext 
)
private

Member Data Documentation

Twist KDL::ChainIdSolver_Vereshchagin::acc_root
private
Jacobian KDL::ChainIdSolver_Vereshchagin::alfa_N
private
Jacobian KDL::ChainIdSolver_Vereshchagin::alfa_N2
private
JntArray KDL::ChainIdSolver_Vereshchagin::beta_N
private
Chain KDL::ChainIdSolver_Vereshchagin::chain
private
Frame KDL::ChainIdSolver_Vereshchagin::F_total
private
Eigen::MatrixXd KDL::ChainIdSolver_Vereshchagin::M_0_inverse
private
unsigned int KDL::ChainIdSolver_Vereshchagin::nc
private
unsigned int KDL::ChainIdSolver_Vereshchagin::nj
private

Referenced by CartToJnt(), and downwards_sweep().

unsigned int KDL::ChainIdSolver_Vereshchagin::ns
private
Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::nu
private
Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::nu_sum
private
Wrench KDL::ChainIdSolver_Vereshchagin::qdotdot_sum
private
std::vector<segment_info> KDL::ChainIdSolver_Vereshchagin::results
private
Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::Sm
private
Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::tmpm
private
Eigen::MatrixXd KDL::ChainIdSolver_Vereshchagin::Um
private
Eigen::MatrixXd KDL::ChainIdSolver_Vereshchagin::Vm
private

The documentation for this class was generated from the following files: