Rigid Body Dynamics Library
Dynamics

Functions

RBDL_DLLAPI void InverseDynamics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, Math::VectorNd &Tau, std::vector< Math::SpatialVector > *f_ext=NULL)
 Computes inverse dynamics with the Newton-Euler Algorithm. More...
 
RBDL_DLLAPI void NonlinearEffects (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau)
 Computes the coriolis forces. More...
 
RBDL_DLLAPI void CompositeRigidBodyAlgorithm (Model &model, const Math::VectorNd &Q, Math::MatrixNd &H, bool update_kinematics=true)
 Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm. More...
 
RBDL_DLLAPI void ForwardDynamics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext=NULL)
 Computes forward dynamics with the Articulated Body Algorithm. More...
 
RBDL_DLLAPI void ForwardDynamicsLagrangian (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::LinearSolver linear_solver=Math::LinearSolverColPivHouseholderQR, std::vector< Math::SpatialVector > *f_ext=NULL, Math::MatrixNd *H=NULL, Math::VectorNd *C=NULL)
 Computes forward dynamics by building and solving the full Lagrangian equation. More...
 
RBDL_DLLAPI void CalcMInvTimesTau (Model &model, const Math::VectorNd &Q, const Math::VectorNd &Tau, Math::VectorNd &QDDot, bool update_kinematics=true)
 Computes the effect of multiplying the inverse of the joint space inertia matrix with a vector in linear time. More...
 

Detailed Description

Function Documentation

RBDL_DLLAPI void CalcMInvTimesTau ( Model model,
const Math::VectorNd Q,
const Math::VectorNd Tau,
Math::VectorNd QDDot,
bool  update_kinematics = true 
)

Computes the effect of multiplying the inverse of the joint space inertia matrix with a vector in linear time.

Parameters
modelrigid body model
Qstate vector of the generalized positions
Tauthe vector that should be multiplied with the inverse of the joint space inertia matrix
QDDotvector where the result will be stored
update_kinematicswhether the kinematics should be updated (safer, but at a higher computational cost)

This function uses a reduced version of the Articulated Body Algorithm to compute

$ \ddot{q} = M(q)^{-1} ( -N(q, \dot{q}) + \tau)$

in $O(n_{\textit{dof}}$) time.

Note
When calling this function repeatedly for the same values of Q make sure to set the last parameter to false as this avoids expensive recomputations of transformations and articulated body inertias.

Definition at line 622 of file Dynamics.cc.

References Model::a, SpatialTransform::apply(), Model::c, Model::c_J, Model::d, Model::I, Model::IA, RigidBodyDynamics::jcalc_X_lambda_S(), RigidBodyDynamics::JointTypeCustom, Model::lambda, LOG, Model::mBodies, Model::mCustomJoints, Model::mJoints, Model::mJointUpdateOrder, Model::multdof3_Dinv, Model::multdof3_S, Model::multdof3_U, Model::multdof3_u, Model::pA, Model::S, Model::U, Model::u, Model::v, Model::v_J, and Model::X_lambda.

RBDL_DLLAPI void CompositeRigidBodyAlgorithm ( Model model,
const Math::VectorNd Q,
Math::MatrixNd H,
bool  update_kinematics = true 
)

Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm.

This function computes the joint space inertia matrix from a given model and the generalized state vector: $ M(q) $

Parameters
modelrigid body model
Qstate vector of the model
Ha matrix where the result will be stored in
update_kinematicswhether the kinematics should be updated (safer, but at a higher computational cost!)
Note
This function only evaluates the entries of H that are non-zero. One Before calling this function one has to ensure that all other values have been set to zero, e.g. by calling H.setZero().

Definition at line 163 of file Dynamics.cc.

References Model::dof_count, Model::I, Model::Ic, RigidBodyDynamics::jcalc_X_lambda_S(), RigidBodyDynamics::JointTypeCustom, Model::lambda, LOG, Model::mBodies, Model::mCustomJoints, Model::mJoints, Model::multdof3_S, Model::S, and Model::X_lambda.

RBDL_DLLAPI void ForwardDynamics ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDot,
const Math::VectorNd Tau,
Math::VectorNd QDDot,
std::vector< Math::SpatialVector > *  f_ext = NULL 
)

Computes forward dynamics with the Articulated Body Algorithm.

This function computes the generalized accelerations from given generalized states, velocities and forces: $ \ddot{q} = M(q)^{-1} ( -N(q, \dot{q}) + \tau)$ It does this by using the recursive Articulated Body Algorithm that runs in $O(n_{dof})$ with $n_{dof}$ being the number of joints.

Parameters
modelrigid body model
Qstate vector of the internal joints
QDotvelocity vector of the internal joints
Tauactuations of the internal joints
QDDotaccelerations of the internal joints (output)
f_extExternal forces acting on the body in base coordinates (optional, defaults to NULL)

Definition at line 311 of file Dynamics.cc.

References Model::a, SpatialTransform::apply(), Model::c, Model::c_J, RigidBodyDynamics::Math::crossf(), RigidBodyDynamics::Math::crossm(), Model::d, Model::gravity, Model::I, Model::IA, RigidBodyDynamics::jcalc(), RigidBodyDynamics::JointTypeCustom, Model::lambda, LOG, Model::mBodies, Model::mCustomJoints, Model::mJoints, Model::multdof3_Dinv, Model::multdof3_S, Model::multdof3_U, Model::multdof3_u, Model::pA, Model::S, RigidBodyDynamics::Math::SpatialVectorZero, Model::U, Model::u, Model::v, Model::v_J, Model::X_base, and Model::X_lambda.

RBDL_DLLAPI void ForwardDynamicsLagrangian ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDot,
const Math::VectorNd Tau,
Math::VectorNd QDDot,
Math::LinearSolver  linear_solver = Math::LinearSolverColPivHouseholderQR,
std::vector< Math::SpatialVector > *  f_ext = NULL,
Math::MatrixNd H = NULL,
Math::VectorNd C = NULL 
)

Computes forward dynamics by building and solving the full Lagrangian equation.

This method builds and solves the linear system

\[ H \ddot{q} = -C + \tau \]

for $\ddot{q}$ where $H$ is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), $C$ the bias force (sometimes called "non-linear effects").

Parameters
modelrigid body model
Qstate vector of the internal joints
QDotvelocity vector of the internal joints
Tauactuations of the internal joints
QDDotaccelerations of the internal joints (output)
linear_solverspecification which method should be used for solving the linear system
f_extExternal forces acting on the body in base coordinates (optional, defaults to NULL)
Hpreallocated workspace area for the joint space inertia matrix of size dof_count x dof_count (optional, defaults to NULL and allocates temporary matrix)
Cpreallocated workspace area for the right hand side vector of size dof_count x 1 (optional, defaults to NULL and allocates temporary vector)

Definition at line 552 of file Dynamics.cc.

References RigidBodyDynamics::CompositeRigidBodyAlgorithm(), Model::dof_count, RigidBodyDynamics::InverseDynamics(), LinearSolverColPivHouseholderQR, LinearSolverHouseholderQR, LinearSolverLLT, LinearSolverPartialPivLU, RigidBodyDynamics::Math::LinSolveGaussElimPivot(), and LOG.

RBDL_DLLAPI void InverseDynamics ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDot,
const Math::VectorNd QDDot,
Math::VectorNd Tau,
std::vector< Math::SpatialVector > *  f_ext = NULL 
)

Computes inverse dynamics with the Newton-Euler Algorithm.

This function computes the generalized forces from given generalized states, velocities, and accelerations: $ \tau = M(q) \ddot{q} + N(q, \dot{q}) $

Parameters
modelrigid body model
Qstate vector of the internal joints
QDotvelocity vector of the internal joints
QDDotaccelerations of the internals joints
Tauactuations of the internal joints (output)
f_extExternal forces acting on the body in base coordinates (optional, defaults to NULL)

Definition at line 26 of file Dynamics.cc.

References Model::a, Model::c, Model::c_J, RigidBodyDynamics::Math::crossf(), RigidBodyDynamics::Math::crossm(), Model::f, Model::gravity, Model::I, RigidBodyDynamics::jcalc(), RigidBodyDynamics::JointTypeCustom, Model::lambda, LOG, Model::mBodies, Model::mCustomJoints, Model::mJoints, Model::multdof3_S, Model::S, Model::v, Model::v_J, Model::X_base, and Model::X_lambda.

RBDL_DLLAPI void NonlinearEffects ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDot,
Math::VectorNd Tau 
)

Computes the coriolis forces.

This function computes the generalized forces from given generalized states, velocities, and accelerations: $ \tau = M(q) \ddot{q} + N(q, \dot{q}) $

Parameters
modelrigid body model
Qstate vector of the internal joints
QDotvelocity vector of the internal joints
Tauactuations of the internal joints (output)

Definition at line 107 of file Dynamics.cc.

References Model::a, Model::c, Model::c_J, RigidBodyDynamics::Math::crossf(), RigidBodyDynamics::Math::crossm(), Model::f, Model::gravity, Model::I, RigidBodyDynamics::jcalc(), RigidBodyDynamics::JointTypeCustom, Model::lambda, LOG, Model::mBodies, Model::mCustomJoints, Model::mJoints, Model::mJointUpdateOrder, Model::multdof3_S, Model::S, Model::v, Model::v_J, and Model::X_lambda.