Forward dynamics

Forward dynamics refers to the computation of motions from forces: given the configuration \(\bfq\), generalized velocity \(\qd\), joint torques \(\bftau\) and contact forces \(\bff\) acting on our robot, forward dynamics is the calculation of joint accelerations \(\qdd\) satisfying the constrained equations of motion:

\begin{align} \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff \\ \bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd & = \boldsymbol{0} \end{align}

Mathematically, we can write it as a function:

\begin{equation*} \qdd = \mathrm{FD}(\bfq, \qd, \bftau, \bff) \end{equation*}

Note that this function implicitly depends on the underlying robot model, as for instance different inertias yield different inertia matrices \(\bfM(\bfq)\), thus different equations of motion that have different solutions. There are two main algorithms to compute forward dynamics:

  • The articulared body algorithm: \(\qdd = \mathrm{ABA}(\bfq, \qd, \bftau, \bff^\mathit{ext})\)
  • The composite rigid body algorithm: \(\qdd = \bfM(\bfq)^{-1} (\bftau - \bftau_0)\)

Let us dive into these two formulas.

Articulated body algorithm

The articulated body algorithm (ABA) computes the unconstrained forward dynamics:

\begin{align} \qdd = & \mathrm{ABA}(\bfq, \qd, \bftau, \bff) \\ & \mathrm{s.t.} \ \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ^\top \bff \end{align}

Its output \(\qdd\) satisfies the unconstrained equation of motion, but it doesn't take into account the holonomic contact constraint \(\bfJ(\bfq) \qdd + \qd^\top \bfH(\bfq) \qd = \bfzero\).

This article is a stub. Post a comment below if I don't come back to it!

Composite rigid body algorithm

The composite rigid body algorithm (CRBA) computes the joint inertia matrix \(\bfM(\bfq)\) from the joint configuration \(\bfq\).

\begin{equation*} \bfM(\bfq) = \mathrm{CRBA}(\bfq) \end{equation*}

Once the inertia matrix is known, we can solve forward dynamics as:

\begin{align} \qdd & = \bfM(\bfq)^{-1} (\bftau - \bftau_0) \\ \bftau_0 & = \mathrm{RNEA}(\bfq, \qd, \bfzero, \bff^\mathit{ext}) \end{align}

where the zero-acceleration torque \(\bftau_0\) is computed using the recursive Newton-Euler algorithm.

To go further

Both the articulated body algorithm (ABA) and composite rigid body algorithm (CRBA) are described in Featherstone's Rigid body dynamics algorithms, a seminal book that has been implemented in rigid body dynamics libraries, such as Pinocchio or RBDL, as well as dynamic simulators, such as Bullet or Dart. ABA is the main algorithm in both Bullet and Dart.

Discussion

There are no comments yet. Feel free to leave a reply using the form below.

Post a comment

You can use Markdown with $\LaTeX$ formulas in your comment.

You agree to the publication of your comment on this page under the CC BY 4.0 license.

Your email address will not be published.

© Stéphane Caron — All content on this website is under the CC BY 4.0 license.
τ