KDL  1.5.1
Public Member Functions | 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. More...
 
virtual void updateInternalDataStructures ()
 Update the internal data structures. More...
 
void getTransformedLinkAcceleration (Twists &x_dotdot)
 
void getTotalTorque (JntArray &total_tau)
 
void getContraintForceMagnitude (Eigen::VectorXd &nu_)
 

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

◆ getContraintForceMagnitude()

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

◆ getTotalTorque()

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

◆ getTransformedLinkAcceleration()

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

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


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