KDL
1.4.0
|
Recursive newton euler forward dynamics solver. More...
#include <src/chainfdsolver_recursive_newton_euler.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 | |
ChainFdSolver_RNE (const Chain &chain, Vector grav) | |
Constructor for the solver, it will allocate all the necessary memory. More... | |
~ChainFdSolver_RNE () | |
int | CartToJnt (const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches &f_ext, JntArray &q_dotdot) |
Function to calculate from Cartesian forces to joint torques. More... | |
virtual void | updateInternalDataStructures () |
Update the internal data structures. More... | |
void | RK4Integrator (unsigned int &nj, const double &t, double &dt, KDL::JntArray &q, KDL::JntArray &q_dot, KDL::JntArray &torques, KDL::Wrenches &f_ext, KDL::ChainFdSolver_RNE &fdsolver, KDL::JntArray &q_dotdot, KDL::JntArray &dq, KDL::JntArray &dq_dot, KDL::JntArray &q_temp, KDL::JntArray &q_dot_temp) |
Function to integrate the joint accelerations resulting from the forward dynamics solver. More... | |
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... | |
Protected Attributes | |
int | error |
Latest error, initialized to E_NOERROR in constructor. More... | |
Private Attributes | |
const Chain & | chain |
ChainDynParam | DynSolver |
ChainIdSolver_RNE | IdSolver |
unsigned int | nj |
unsigned int | ns |
JntSpaceInertiaMatrix | H |
JntArray | Tzeroacc |
Eigen::MatrixXd | H_eig |
Eigen::VectorXd | Tzeroacc_eig |
Eigen::MatrixXd | L_eig |
Eigen::VectorXd | D_eig |
Eigen::VectorXd | r_eig |
Eigen::VectorXd | acc_eig |
Recursive newton euler forward dynamics solver.
The algorithm implementation is based on the book "Rigid Body Dynamics Algorithms" of Roy Featherstone, 2008 (ISBN:978-0-387-74314-1) See Chapter 6 for basic algorithm.
It calculates the accelerations for the joints (qdotdot), given the position and velocity of the joints (q,qdot,qdotdot), external forces on the segments (expressed in the segments reference frame), and the dynamical parameters of the segments.
|
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 for the solver, it will allocate all the necessary memory.
chain | The kinematic chain to calculate the forward dynamics for, an internal copy will be made. |
grav | The gravity vector to use during the calculation. |
|
inline |
|
virtual |
Function to calculate from Cartesian forces to joint torques.
Input parameters;
q | The current joint positions |
q_dot | The current joint velocities |
torques | The current joint torques (applied by controller) |
f_ext | The external forces (no gravity) on the segments Output parameters: |
q_dotdot | The resulting joint accelerations |
Implements KDL::ChainFdSolver.
References acc_eig, KDL::ChainIdSolver_RNE::CartToJnt(), chain, D_eig, DynSolver, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), H, H_eig, IdSolver, KDL::ChainDynParam::JntToMass(), L_eig, nj, ns, r_eig, KDL::JntArray::rows(), Tzeroacc, and Tzeroacc_eig.
Referenced by RK4Integrator().
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
void KDL::ChainFdSolver_RNE::RK4Integrator | ( | unsigned int & | nj, |
const double & | t, | ||
double & | dt, | ||
KDL::JntArray & | q, | ||
KDL::JntArray & | q_dot, | ||
KDL::JntArray & | torques, | ||
KDL::Wrenches & | f_ext, | ||
KDL::ChainFdSolver_RNE & | fdsolver, | ||
KDL::JntArray & | q_dotdot, | ||
KDL::JntArray & | dq, | ||
KDL::JntArray & | dq_dot, | ||
KDL::JntArray & | q_temp, | ||
KDL::JntArray & | q_dot_temp | ||
) |
Function to integrate the joint accelerations resulting from the forward dynamics solver.
Input parameters;
nj | The number of joints |
t | The current time |
dt | The integration period |
q | The current joint positions |
q_dot | The current joint velocities |
torques | The current joint torques (applied by controller) |
f_ext | The external forces (no gravity) on the segments |
fdsolver | The forward dynamics solver Output parameters: |
t | The updated time |
q | The updated joint positions |
q_dot | The updated joint velocities |
q_dotdot | The current joint accelerations |
dq | The joint position increment |
dq_dot | The joint velocity increment Temporary parameters: |
qtemp | Intermediate joint positions |
qdtemp | Intermediate joint velocities |
References CartToJnt(), and nj.
|
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().
|
virtual |
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 chain, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), nj, and ns.
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
protectedinherited |
Latest error, initialized to E_NOERROR in constructor.
Referenced by KDL::ChainIdSolver_RNE::CartToJnt(), KDL::TreeIdSolver_RNE::CartToJnt(), 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().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt(), RK4Integrator(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().