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. There are two main algorithms 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 a faster way to compute forward dynamics
when the robot is making contacts. It is for instance 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 external forces are zero 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.
In the alternative between ABA and CRBA + RNEA, ABA is a faster way to compute
forward dynamics when there are no contacts between the robot and its
environment. It is for instance the approach followed by the Bullet physics simulator.