KDL  1.4.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>

Inheritance diagram for KDL::ChainIdSolver_Vereshchagin:
Inheritance graph
[legend]
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...
 
virtual void updateInternalDataStructures ()
 Update the internal data structures. 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
 
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. 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...
 
virtual int getError () const
 Return the latest error. More...
 
virtual const char * strError (const int error) const
 Return a description of the latest error. More...
 

Private Attributes

const Chainchain
 
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_info, Eigen::aligned_allocator< segment_info > > results
 
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).

Member Typedef Documentation

§ Frames

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

§ Matrix6d

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

§ Matrix6Xd

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

§ Twists

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

§ Vector6d

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

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

§ ~ChainIdSolver_Vereshchagin()

KDL::ChainIdSolver_Vereshchagin::~ChainIdSolver_Vereshchagin ( )
inline

Member Function Documentation

§ CartToJnt()

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
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(), KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), initial_upwards_sweep(), nc, nj, ns, and KDL::JntArray::rows().

Referenced by ~ChainIdSolver_Vereshchagin().

§ constraint_calculation()

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

This method calculates constraint force magnitudes.

References acc_root, KDL::JntArray::data, 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(), and ~ChainIdSolver_Vereshchagin().

§ downwards_sweep()

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.

References KDL::ChainIdSolver_Vereshchagin::segment_info::C, chain, KDL::ChainIdSolver_Vereshchagin::segment_info::D, KDL::Vector::data, KDL::dot(), KDL::ChainIdSolver_Vereshchagin::segment_info::E, KDL::ChainIdSolver_Vereshchagin::segment_info::E_tilde, KDL::ChainIdSolver_Vereshchagin::segment_info::EZ, KDL::ChainIdSolver_Vereshchagin::segment_info::F, F_total, KDL::Wrench::force, KDL::ChainIdSolver_Vereshchagin::segment_info::G, KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::ChainIdSolver_Vereshchagin::segment_info::H, KDL::Rotation::Inverse(), KDL::ChainIdSolver_Vereshchagin::segment_info::M, KDL::Frame::M, nc, nj, KDL::Joint::None, ns, KDL::ChainIdSolver_Vereshchagin::segment_info::P, KDL::ChainIdSolver_Vereshchagin::segment_info::P_tilde, KDL::ChainIdSolver_Vereshchagin::segment_info::PC, KDL::ChainIdSolver_Vereshchagin::segment_info::PZ, KDL::ChainIdSolver_Vereshchagin::segment_info::R, KDL::ChainIdSolver_Vereshchagin::segment_info::R_tilde, results, KDL::Twist::rot, KDL::Wrench::torque, KDL::ChainIdSolver_Vereshchagin::segment_info::totalBias, KDL::ChainIdSolver_Vereshchagin::segment_info::U, KDL::ChainIdSolver_Vereshchagin::segment_info::u, KDL::Twist::vel, and KDL::ChainIdSolver_Vereshchagin::segment_info::Z.

Referenced by CartToJnt(), and ~ChainIdSolver_Vereshchagin().

§ final_upwards_sweep()

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

§ getError()

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

Return the latest error.

References KDL::SolverI::error.

§ initial_upwards_sweep()

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

§ strError()

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

§ updateInternalDataStructures()

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.

References chain, KDL::Chain::getNrOfSegments(), nc, ns, and results.

Referenced by ~ChainIdSolver_Vereshchagin().

Member Data Documentation

§ acc_root

Twist KDL::ChainIdSolver_Vereshchagin::acc_root
private

§ alfa_N

Jacobian KDL::ChainIdSolver_Vereshchagin::alfa_N
private

§ alfa_N2

Jacobian KDL::ChainIdSolver_Vereshchagin::alfa_N2
private

§ beta_N

JntArray KDL::ChainIdSolver_Vereshchagin::beta_N
private

§ chain

const Chain& KDL::ChainIdSolver_Vereshchagin::chain
private

§ error

int KDL::SolverI::error
protectedinherited

§ F_total

Frame KDL::ChainIdSolver_Vereshchagin::F_total
private

§ M_0_inverse

Eigen::MatrixXd KDL::ChainIdSolver_Vereshchagin::M_0_inverse
private

§ nc

unsigned int KDL::ChainIdSolver_Vereshchagin::nc
private

§ nj

unsigned int KDL::ChainIdSolver_Vereshchagin::nj
private

Referenced by CartToJnt(), and downwards_sweep().

§ ns

unsigned int KDL::ChainIdSolver_Vereshchagin::ns
private

§ nu

Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::nu
private

§ nu_sum

Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::nu_sum
private

§ qdotdot_sum

Wrench KDL::ChainIdSolver_Vereshchagin::qdotdot_sum
private

§ results

std::vector<segment_info, Eigen::aligned_allocator<segment_info> > KDL::ChainIdSolver_Vereshchagin::results
private

§ Sm

Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::Sm
private

§ tmpm

Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::tmpm
private

§ Um

Eigen::MatrixXd KDL::ChainIdSolver_Vereshchagin::Um
private

§ Vm

Eigen::MatrixXd KDL::ChainIdSolver_Vereshchagin::Vm
private

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