KDL
1.4.0
|
This abstract class encapsulates the inverse dynamics solver for a KDL::Chain. More...
#include <src/chainfdsolver.hpp>
Public Types | |
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 } |
Public Member Functions | |
virtual int | CartToJnt (const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches &f_ext, JntArray &q_dotdot)=0 |
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... | |
virtual void | updateInternalDataStructures ()=0 |
Update the internal data structures. More... | |
Protected Attributes | |
int | error |
Latest error, initialized to E_NOERROR in constructor. More... | |
This abstract class encapsulates the inverse dynamics solver for a KDL::Chain.
|
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. |
|
pure virtual |
Calculate forward dynamics from joint positions, joint velocities, joint torques/forces, and externally applied forces/torques to joint accelerations. @param q input joint positions @param q_dot input joint velocities
torque | input joint torques |
f_ext | external forces |
q_dotdot | output joint accelerations @return if < 0 something went wrong |
Implemented in KDL::ChainFdSolver_RNE.
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
|
inlinevirtualinherited |
Return a description of the latest error.
Reimplemented in KDL::ChainJntToJacDotSolver, KDL::ChainIkSolverVel_wdls, KDL::ChainIkSolverVel_pinv, KDL::ChainIkSolverPos_NR_JL, KDL::ChainIkSolverPos_NR, and KDL::ChainIkSolverPos_LMA.
References KDL::SolverI::E_DEGRADED, KDL::SolverI::E_MAX_ITERATIONS_EXCEEDED, KDL::SolverI::E_NO_CONVERGE, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_IMPLEMENTED, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_OUT_OF_RANGE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::E_SVD_FAILED, KDL::SolverI::E_UNDEFINED, and KDL::SolverI::error.
Referenced by KDL::ChainIkSolverPos_LMA::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverVel_wdls::strError(), and KDL::ChainJntToJacDotSolver::strError().
|
pure 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.
Implemented in KDL::ChainIkSolverAcc, KDL::ChainIkSolverVel, KDL::ChainIkSolverPos, KDL::ChainFkSolverAcc, KDL::ChainFkSolverVel, KDL::ChainFkSolverPos, KDL::TreeIdSolver_RNE, KDL::ChainJntToJacSolver, KDL::ChainJntToJacDotSolver, KDL::ChainIkSolverVel_wdls, KDL::ChainIkSolverVel_pinv_nso, KDL::ChainIkSolverVel_pinv_givens, KDL::ChainIkSolverVel_pinv, KDL::ChainIkSolverPos_NR_JL, KDL::ChainIkSolverPos_NR, KDL::ChainIkSolverPos_LMA, KDL::ChainIdSolver_Vereshchagin, KDL::ChainIdSolver_RNE, KDL::ChainFkSolverVel_recursive, KDL::ChainFkSolverPos_recursive, KDL::ChainFdSolver_RNE, and KDL::ChainDynParam.
|
protectedinherited |
Latest error, initialized to E_NOERROR in constructor.
Referenced by KDL::ChainIdSolver_RNE::CartToJnt(), KDL::TreeIdSolver_RNE::CartToJnt(), KDL::ChainFdSolver_RNE::CartToJnt(), KDL::ChainIdSolver_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::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainDynParam::JntToMass(), 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::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().