#include <src/chainidsolver_vereshchagin.hpp>
◆ ChainIdSolver_Vereshchagin()
KDL::ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin |
( |
const Chain & |
chain, |
|
|
const Twist & |
root_acc, |
|
|
const unsigned int |
nc |
|
) |
| |
◆ CartToJnt()
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
-
q | The current joint positions |
q_dot | The current joint velocities |
alpha | The active constraint directions (unit constraint forces expressed w.r.t. robot's base frame) |
beta | The acceleration energy setpoints (expressed w.r.t. above-defined unit constraint forces) |
f_ext | The external forces (no gravity, it is given in root acceleration) on the segments |
ff_torques | The feed-forward joint space torques |
Output parameters:
- Parameters
-
q_dotdot | The resulting joint accelerations |
constraint_torques | The 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 |
The documentation for this class was generated from the following files: