# 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$$.

## 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

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