KDL 1.5.1
Loading...
Searching...
No Matches
Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
KDL::ChainIdSolver_Vereshchagin Class Reference

#include <src/chainidsolver_vereshchagin.hpp>

Inheritance diagram for KDL::ChainIdSolver_Vereshchagin:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainIdSolver_Vereshchagin:
Collaboration graph
[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< 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.
 
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 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
 
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 
)

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
qThe current joint positions
q_dotThe current joint velocities
alphaThe active constraint directions (unit constraint forces expressed w.r.t. robot's base frame)
betaThe acceleration energy setpoints (expressed w.r.t. above-defined unit constraint forces)
f_extThe external forces (no gravity, it is given in root acceleration) on the segments
ff_torquesThe feed-forward joint space torques

Output parameters:

Parameters
q_dotdotThe resulting joint accelerations
constraint_torquesThe resulting joint constraint torques (what each joint feels due to the constraint forces acting on the end-effector)
Returns
error/success code

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

◆ constraint_calculation()

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

◆ 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

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

Referenced by KDL::ChainHdSolver_Vereshchagin::CartToJnt().

◆ final_upwards_sweep()

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

◆ getContraintForceMagnitude()

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

◆ getError()

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

Return the latest error.

References KDL::SolverI::error.

◆ getTotalTorque()

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

◆ getTransformedLinkAcceleration()

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

◆ 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

◆ strError()

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

◆ 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 KDL::ChainHdSolver_Vereshchagin::chain, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), KDL::ChainHdSolver_Vereshchagin::nc, KDL::ChainHdSolver_Vereshchagin::nj, KDL::ChainHdSolver_Vereshchagin::ns, KDL::ChainHdSolver_Vereshchagin::results, and KDL::ChainHdSolver_Vereshchagin::total_torques.

Member Data Documentation

◆ acc_root

Twist KDL::ChainHdSolver_Vereshchagin::acc_root
privateinherited

◆ alfa_N

Jacobian KDL::ChainHdSolver_Vereshchagin::alfa_N
privateinherited

◆ alfa_N2

Jacobian KDL::ChainHdSolver_Vereshchagin::alfa_N2
privateinherited

◆ beta_N

JntArray KDL::ChainHdSolver_Vereshchagin::beta_N
privateinherited

◆ chain

const Chain& KDL::ChainHdSolver_Vereshchagin::chain
privateinherited

◆ error

int KDL::SolverI::error
protectedinherited

Latest error, initialized to E_NOERROR in constructor.

Referenced by KDL::ChainIdSolver_RNE::CartToJnt(), KDL::TreeIdSolver_RNE::CartToJnt(), KDL::ChainFdSolver_RNE::CartToJnt(), KDL::ChainHdSolver_Vereshchagin::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_wdls::getSigma(), KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainFkSolverVel_recursive::JntToCart(), KDL::ChainExternalWrenchEstimator::JntToExtWrench(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainDynParam::JntToMass(), KDL::ChainExternalWrenchEstimator::setInitialMomentum(), KDL::ChainIkSolverPos_NR_JL::setJointLimits(), KDL::ChainJntToJacDotSolver::setLockedJoints(), KDL::ChainJntToJacSolver::setLockedJoints(), KDL::ChainIkSolverVel_pinv_nso::setOptPos(), KDL::ChainIkSolverVel_wdls::setWeightJS(), KDL::ChainIkSolverVel_pinv_nso::setWeights(), KDL::ChainIkSolverVel_wdls::setWeightTS(), KDL::ChainExternalWrenchEstimator::strError(), KDL::ChainIkSolverPos_LMA::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverVel_wdls::strError(), KDL::ChainJntToJacDotSolver::strError(), and KDL::SolverI::strError().

◆ F_total

Frame KDL::ChainHdSolver_Vereshchagin::F_total
privateinherited

◆ M_0_inverse

Eigen::MatrixXd KDL::ChainHdSolver_Vereshchagin::M_0_inverse
privateinherited

◆ nc

unsigned int KDL::ChainHdSolver_Vereshchagin::nc
privateinherited

◆ nj

unsigned int KDL::ChainHdSolver_Vereshchagin::nj
privateinherited

◆ ns

unsigned int KDL::ChainHdSolver_Vereshchagin::ns
privateinherited

◆ nu

Eigen::VectorXd KDL::ChainHdSolver_Vereshchagin::nu
privateinherited

◆ nu_sum

Eigen::VectorXd KDL::ChainHdSolver_Vereshchagin::nu_sum
privateinherited

◆ qdotdot_sum

Wrench KDL::ChainHdSolver_Vereshchagin::qdotdot_sum
privateinherited

◆ results

std::vector<segment_info, Eigen::aligned_allocator<segment_info> > KDL::ChainHdSolver_Vereshchagin::results
privateinherited

◆ Sm

Eigen::VectorXd KDL::ChainHdSolver_Vereshchagin::Sm
privateinherited

◆ tmpm

Eigen::VectorXd KDL::ChainHdSolver_Vereshchagin::tmpm
privateinherited

◆ total_torques

Eigen::VectorXd KDL::ChainHdSolver_Vereshchagin::total_torques
privateinherited

◆ Um

Eigen::MatrixXd KDL::ChainHdSolver_Vereshchagin::Um
privateinherited

◆ Vm

Eigen::MatrixXd KDL::ChainHdSolver_Vereshchagin::Vm
privateinherited

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