KDL 1.5.1
Loading...
Searching...
No Matches
KDL::ChainIdSolver_Vereshchagin Class Reference

#include <src/chainidsolver_vereshchagin.hpp>

Inheritance diagram for KDL::ChainIdSolver_Vereshchagin:
Collaboration diagram for KDL::ChainIdSolver_Vereshchagin:

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

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

◆ Matrix6d

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

◆ Matrix6Xd

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

◆ Twists

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

◆ Vector6d

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

◆ 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, KDL::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()

◆ 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()

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

◆ 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

◆ error

int KDL::SolverI::error
protectedinherited

Latest error, initialized to E_NOERROR in constructor.

Referenced by KDL::ChainFdSolver_RNE::CartToJnt(), KDL::ChainHdSolver_Vereshchagin::CartToJnt(), KDL::ChainIdSolver_RNE::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::TreeIdSolver_RNE::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::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

◆ nj

◆ ns

◆ nu

◆ nu_sum

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

◆ qdotdot_sum

Wrench KDL::ChainHdSolver_Vereshchagin::qdotdot_sum
privateinherited

◆ results

◆ Sm

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

◆ tmpm

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

◆ total_torques

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