Forward dynamics refers to the computation of motions from forces. Given the
configuration q∈C, tangent velocity v,
joint torques τ and contact forces f acting on our
robot, forward dynamics is the calculation of joint accelerations v˙
consistent with the constrained equations of motion:
This function depends on the underlying robot model, as for instance different
masses yield different inertia matrices M(q) in the equations of
motion. In the following note, we review the two main ways to compute forward
dynamics: the articulated body algorithm (ABA), and a combination of the
composite rigid body algorithm and recursive Newton-Euler algorithm (CRBA +
RNEA). Let's check out these two approaches and their interfaces in Pinocchio.
Composite rigid body with recursive Newton-Euler
The composite rigid body algorithm (CRBA) computes the joint inertia matrix
M(q) from the joint configuration q:
M(q)=CRBA(q)Once this inertia matrix is available, we can solve forward dynamics as:
v˙τ0=M(q)−1(τ−τ0)=C(q,v)v−τg(q)−J(q)⊤fThe bias torques τ0 are the solution to the equations of motion
when the acceleration is zero. We can therefore compute them efficiently as
τ0=RNEA(q,v,0,f) using the
recursive Newton-Euler algorithm. In the alternative between
ABA and CRBA + RNEA, the latter is the approach followed by the MuJoCo (open source) and RaiSim
(closed source) physics simulators.
Example in Python
Here is for instance how we can compute forward dynamics for a manipulator in
Pinocchio:
import numpy as np
import pinocchio as pin
from robot_descriptions.loaders.pinocchio import load_robot_description
manipulator = load_robot_description("edo_description")
model = manipulator.model
data = manipulator.data
# Inertia matrix with CRBA
q = pin.randomConfiguration(model)
M = pin.crba(model, data, q)
# Bias torques with RNEA
v = np.zeros(model.nv)
zero_accel = np.zeros(model.nv)
tau_0 = pin.rnea(model, data, q, v, zero_accel)
# Forward dynamics by solving the linear system
tau = np.random.uniform(-42.0, +42.0, size=model.nv)
a = np.linalg.solve(M, tau - tau_0)
Note that we set f=0 (no external force) in this example.
Articulated body algorithm
The articulated body algorithm (ABA) computes the unconstrained forward dynamics:
v˙=s.t. ABA(q,v,τ,f)M(q)v˙+C(q,v)v=S⊤τ+τg(q)+J⊤fIts output v˙ may not satisfy the holonomic contact constraint
J(q)v˙+J˙(q,v)v=0.
ABA provides a straightforward way to compute forward dynamics when there are
no contacts between the robot and its environment. The number of operations it
performs is linear, larger than with RNEA but less than with CRBA whose
complexity is quadratic:
Algorithm |
Multiplications |
Additions |
ABA |
≤300n−200 |
≤300n−100 |
RNEA |
≤200n−40 |
≤150n−40 |
CRBA |
≤12n2+100n |
7n2+100n |
More details are reported in Table 10.1 of Roy Featherstone's Rigid body
dynamics algorithms.
When there are contact forces between the robot and its environment, we don't
use ABA as a standalone forward dynamics algorithm, but it can be used as a
sub-routine of a forward dynamics function. This is the approach followed in
the Bullet 3 physics simulator, where ABA is called
as a sub-routine of stepSimulation
(code pointers: in the
solveExternalForces
and solveInternalConstraints
functions of the btMultiBodyDynamicsWorld
class). Bullet then computes
contact forces f via a linear complementarity problem (LCP) solved
with the projected Gauss-Seidel algorithm (context: Bullet Constraint Solver
Algorithm). For
more details on this approach, you can check out for instance the survey in Le
Lidec et al. (2023).