Recursive newton euler inverse dynamics solver for kinematic trees. More...
#include <treeidsolver_recursive_newton_euler.hpp>
Public Member Functions | |
int | CartToJnt (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques) |
TreeIdSolver_RNE (const Tree &tree, Vector grav) | |
virtual void | updateInternalDataStructures () |
Public Member Functions inherited from KDL::SolverI | |
virtual int | getError () const |
Return the latest error. More... | |
SolverI () | |
Initialize latest error to E_NOERROR. More... | |
virtual const char * | strError (const int error) const |
virtual | ~SolverI () |
Private Member Functions | |
void | initAuxVariables () |
Helper function to initialize private members X, S, v, a, f. More... | |
void | rne_step (SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques) |
One recursion step. More... | |
Private Attributes | |
std::map< std::string, Twist > | a |
Twist | ag |
std::map< std::string, Wrench > | f |
unsigned int | nj |
unsigned int | ns |
std::map< std::string, Twist > | S |
const Tree & | tree |
std::map< std::string, Twist > | v |
std::map< std::string, Frame > | X |
Additional Inherited Members | |
Public Types inherited from KDL::SolverI | |
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 } |
Protected Attributes inherited from KDL::SolverI | |
int | error |
Latest error, initialized to E_NOERROR in constructor. More... | |
Recursive newton euler inverse dynamics solver for kinematic trees.
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.
This is an extension of the inverse dynamic solver for kinematic chains,
Definition at line 41 of file treeidsolver_recursive_newton_euler.hpp.
Constructor for the solver, it will allocate all the necessary memory
tree | The kinematic tree to calculate the inverse dynamics for, an internal reference will be stored. |
grav | The gravity vector to use during the calculation. |
Definition at line 28 of file treeidsolver_recursive_newton_euler.cpp.
|
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::TreeIdSolver.
Definition at line 52 of file treeidsolver_recursive_newton_euler.cpp.
|
private |
Helper function to initialize private members X, S, v, a, f.
Definition at line 41 of file treeidsolver_recursive_newton_euler.cpp.
|
private |
One recursion step.
Definition at line 78 of file treeidsolver_recursive_newton_euler.cpp.
|
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 35 of file treeidsolver_recursive_newton_euler.cpp.
|
private |
Definition at line 78 of file treeidsolver_recursive_newton_euler.hpp.
|
private |
Definition at line 80 of file treeidsolver_recursive_newton_euler.hpp.
|
private |
Definition at line 79 of file treeidsolver_recursive_newton_euler.hpp.
|
private |
Definition at line 73 of file treeidsolver_recursive_newton_euler.hpp.
|
private |
Definition at line 74 of file treeidsolver_recursive_newton_euler.hpp.
|
private |
Definition at line 76 of file treeidsolver_recursive_newton_euler.hpp.
|
private |
Definition at line 72 of file treeidsolver_recursive_newton_euler.hpp.
|
private |
Definition at line 77 of file treeidsolver_recursive_newton_euler.hpp.
|
private |
Definition at line 75 of file treeidsolver_recursive_newton_euler.hpp.