Recursive newton euler inverse dynamics solver. More...
#include <chainidsolver_recursive_newton_euler.hpp>
Public Member Functions | |
int | CartToJnt (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques) |
ChainIdSolver_RNE (const Chain &chain, Vector grav) | |
virtual void | updateInternalDataStructures () |
~ChainIdSolver_RNE () | |
Private Attributes | |
std::vector< Twist > | a |
Twist | ag |
const Chain & | chain |
std::vector< Wrench > | f |
unsigned int | nj |
unsigned int | ns |
std::vector< Twist > | S |
std::vector< Twist > | v |
std::vector< Frame > | X |
Recursive newton euler inverse 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 page 96 for the pseudo-code.
It calculates the torques for the joints, given the motion of the joints (q,qdot,qdotdot), external forces on the segments (expressed in the segments reference frame) and the dynamical parameters of the segments.
Definition at line 40 of file chainidsolver_recursive_newton_euler.hpp.
KDL::ChainIdSolver_RNE::ChainIdSolver_RNE | ( | const Chain & | chain, |
Vector | grav | ||
) |
Constructor for the solver, it will allocate all the necessary memory
chain | The kinematic chain to calculate the inverse dynamics for, an internal copy will be made. |
grav | The gravity vector to use during the calculation. |
Definition at line 27 of file chainidsolver_recursive_newton_euler.cpp.
KDL::ChainIdSolver_RNE::~ChainIdSolver_RNE | ( | ) | [inline] |
Definition at line 48 of file chainidsolver_recursive_newton_euler.hpp.
int KDL::ChainIdSolver_RNE::CartToJnt | ( | const JntArray & | q, |
const JntArray & | q_dot, | ||
const JntArray & | q_dotdot, | ||
const Wrenches & | f_ext, | ||
JntArray & | torques | ||
) | [virtual] |
Function to calculate from Cartesian forces to joint torques. Input parameters;
q | The current joint positions |
q_dot | The current joint velocities |
q_dotdot | The current joint accelerations |
f_ext | The external forces (no gravity) on the segments Output parameters: |
torques | the resulting torques for the joints |
Implements KDL::ChainIdSolver.
Definition at line 44 of file chainidsolver_recursive_newton_euler.cpp.
void KDL::ChainIdSolver_RNE::updateInternalDataStructures | ( | ) | [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.
Definition at line 34 of file chainidsolver_recursive_newton_euler.cpp.
std::vector<Twist> KDL::ChainIdSolver_RNE::a [private] |
Definition at line 72 of file chainidsolver_recursive_newton_euler.hpp.
Twist KDL::ChainIdSolver_RNE::ag [private] |
Definition at line 74 of file chainidsolver_recursive_newton_euler.hpp.
const Chain& KDL::ChainIdSolver_RNE::chain [private] |
Definition at line 66 of file chainidsolver_recursive_newton_euler.hpp.
std::vector<Wrench> KDL::ChainIdSolver_RNE::f [private] |
Definition at line 73 of file chainidsolver_recursive_newton_euler.hpp.
unsigned int KDL::ChainIdSolver_RNE::nj [private] |
Definition at line 67 of file chainidsolver_recursive_newton_euler.hpp.
unsigned int KDL::ChainIdSolver_RNE::ns [private] |
Definition at line 68 of file chainidsolver_recursive_newton_euler.hpp.
std::vector<Twist> KDL::ChainIdSolver_RNE::S [private] |
Definition at line 70 of file chainidsolver_recursive_newton_euler.hpp.
std::vector<Twist> KDL::ChainIdSolver_RNE::v [private] |
Definition at line 71 of file chainidsolver_recursive_newton_euler.hpp.
std::vector<Frame> KDL::ChainIdSolver_RNE::X [private] |
Definition at line 69 of file chainidsolver_recursive_newton_euler.hpp.