Chapter 12: Newton–Euler Recursive Dynamics
Lesson 4: Implementation Lab (recursive algorithm)
In this lab-style lesson we turn the abstract Newton–Euler forward–backward recursion into concrete, efficient code in several languages (Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica). We assume you know the symbolic Newton–Euler equations from the previous lessons and focus here on data structures, loops, and numerical subtleties for general \( n \)-DOF serial manipulators.
1. Implementation Goals and Notation Recap
Our goal is to compute joint torques \( \boldsymbol{\tau} \in \mathbb{R}^n \) for a serial manipulator given:
- Joint positions \( \mathbf{q} \),
- Joint velocities \( \dot{\mathbf{q}} \),
- Joint accelerations \( \ddot{\mathbf{q}} \),
- Link inertial data \( m_i, \mathbf{r}_{c_i}, \mathbf{I}_i \),
- Gravity vector \( \mathbf{g}_0 \) expressed in the base frame.
The manipulator dynamics can be written as
\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}, \]
but instead of explicitly constructing \( \mathbf{M}, \mathbf{C}, \mathbf{g} \), the Newton–Euler algorithm directly outputs \( \boldsymbol{\tau} \) for any given \( (\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) \) in linear time in the number of joints.
We use the standard notation for link \( i \):
- \( \mathbf{R}_i^{i-1} \in \mathrm{SO}(3) \): rotation from frame \( i-1 \) to frame \( i \),
- \( \mathbf{p}_i^{i-1} \in \mathbb{R}^3 \): position of origin of frame \( i \) relative to frame \( i-1 \),
- \( \mathbf{z}_i \): joint axis expressed in frame \( i \) (usually \( [0,0,1]^\top \) for DH),
- \( \mathbf{r}_{c_i} \): vector from frame origin \( i \) to center of mass of link \( i \) expressed in frame \( i \).
Velocities and accelerations of link frames and centers of mass are propagated forward, and wrenches are propagated backward. The actual formulas were derived in Lessons 1–3 of this chapter; we recall them here in implementation-friendly form.
2. Forward Recursion: Velocities and Accelerations
Assume the base link is frame \( 0 \) with known angular and linear motion (often fixed: \( \boldsymbol{\omega}_0 = \mathbf{0} \), \( \dot{\boldsymbol{\omega}}_0 = \mathbf{0} \), \( \mathbf{a}_0 = -\mathbf{g}_0 \) to incorporate gravity). For a purely revolute joint \( i \), the forward recursion in frame \( i \) has:
\[ \boldsymbol{\omega}_i = \mathbf{R}_i^{i-1} \boldsymbol{\omega}_{i-1} + \mathbf{z}_i \dot{q}_i, \]
\[ \dot{\boldsymbol{\omega}}_i = \mathbf{R}_i^{i-1} \dot{\boldsymbol{\omega}}_{i-1} + \mathbf{z}_i \ddot{q}_i + \boldsymbol{\omega}_i \times (\mathbf{z}_i \dot{q}_i), \]
\[ \mathbf{a}_i = \mathbf{R}_i^{i-1} \left( \mathbf{a}_{i-1} + \dot{\boldsymbol{\omega}}_{i-1} \times \mathbf{p}_i^{i-1} + \boldsymbol{\omega}_{i-1} \times \big( \boldsymbol{\omega}_{i-1} \times \mathbf{p}_i^{i-1} \big) \right), \]
\[ \mathbf{a}_{c_i} = \mathbf{a}_i + \dot{\boldsymbol{\omega}}_i \times \mathbf{r}_{c_i} + \boldsymbol{\omega}_i \times \big( \boldsymbol{\omega}_i \times \mathbf{r}_{c_i} \big). \]
For prismatic joints, \( \dot{q}_i \) and \( \ddot{q}_i \) enter linearly in \( \mathbf{a}_i \) rather than through angular velocity, but the same code structure can handle both if we treat joint type as a flag.
3. Backward Recursion: Wrenches and Joint Torques
Starting from the end-effector link \( n \), we propagate forces and moments backward. Let \( \mathbf{f}_i \) and \( \mathbf{n}_i \) denote the net force and moment acting on link \( i \) expressed in frame \( i \). For internal joints without external contact forces:
\[ \mathbf{f}_i = m_i \mathbf{a}_{c_i} + \mathbf{R}_{i}^{i+1} \mathbf{f}_{i+1}, \]
\[ \mathbf{n}_i = \mathbf{I}_i \dot{\boldsymbol{\omega}}_i + \boldsymbol{\omega}_i \times (\mathbf{I}_i \boldsymbol{\omega}_i) + \mathbf{r}_{c_i} \times (m_i \mathbf{a}_{c_i}) + \mathbf{R}_i^{i+1} \mathbf{n}_{i+1} + \mathbf{p}_{i+1}^{i} \times \big( \mathbf{R}_i^{i+1} \mathbf{f}_{i+1} \big), \]
where \( \mathbf{p}_{i+1}^{i} \) is the vector from frame \( i \) to frame \( i+1 \), expressed in frame \( i \), and \( \mathbf{R}_i^{i+1} = (\mathbf{R}_{i+1}^{i})^\top \).
The joint torque (revolute) or force (prismatic) is finally:
\[ \tau_i = \begin{cases} \mathbf{n}_i^\top \mathbf{z}_i & \text{(revolute joint)} \\ \mathbf{f}_i^\top \mathbf{z}_i & \text{(prismatic joint)}. \end{cases} \]
From an implementation viewpoint, these updates are vector and matrix operations that can be efficiently expressed with linear algebra libraries.
4. Pseudocode and Algorithm Flow
A generic Newton–Euler routine for an \( n \)-DOF serial manipulator proceeds as:
- Precompute transforms \( \mathbf{R}_i^{i-1}, \mathbf{p}_i^{i-1} \) from FK.
- Forward loop \( i = 1,\dots,n \): propagate \( \boldsymbol{\omega}_i, \dot{\boldsymbol{\omega}}_i, \mathbf{a}_i, \mathbf{a}_{c_i} \).
- Initialize \( \mathbf{f}_{n+1} = \mathbf{0}, \mathbf{n}_{n+1} = \mathbf{0} \).
- Backward loop \( i = n,\dots,1 \): compute \( \mathbf{f}_i, \mathbf{n}_i, \tau_i \).
flowchart TD
IN["Joint states (q, qdot, qddot), link params, gravity"] --> FK["Compute link transforms R_i, p_i"]
FK --> FWD["Forward loop i = 1..n: omega_i, omegadot_i, a_i, a_ci"]
FWD --> INITB["Set f_{n+1} = 0, n_{n+1} = 0"]
INITB --> BWD["Backward loop i = n..1: f_i, n_i, tau_i"]
BWD --> OUT["Return tau vector"]
The following sections implement this structure language by language.
5. Python Implementation with NumPy
We implement a minimal Newton–Euler function for a serial
manipulator with revolute joints using numpy. We assume DH
or modified DH kinematics provide the arrays of R[i],
p[i], and r_c[i].
import numpy as np
def newton_euler(
R, p, r_c, m, I,
q, qd, qdd,
g0=np.array([0.0, 0.0, -9.81]),
joint_type=None
):
"""
R[i] : 3x3 rotation from frame i-1 to i
p[i] : 3-vector from frame i-1 to i (expressed in frame i-1)
r_c[i] : 3-vector CoM offset in frame i
m[i] : mass of link i
I[i] : 3x3 inertia matrix of link i about frame i origin
q, qd, qdd : joint states (length n)
g0 : gravity in base frame
joint_type[i] : 'R' or 'P' (default 'R')
"""
n = len(q)
if joint_type is None:
joint_type = ['R'] * n
z_axis = np.array([0.0, 0.0, 1.0])
# Forward recursion
omega = [np.zeros(3) for _ in range(n + 1)]
omegadot = [np.zeros(3) for _ in range(n + 1)]
a = [np.zeros(3) for _ in range(n + 1)]
a_c = [np.zeros(3) for _ in range(n)]
# Base: fixed base (can be generalized)
omega[0] = np.zeros(3)
omegadot[0] = np.zeros(3)
a[0] = -np.array(g0, dtype=float)
for i in range(1, n + 1):
Ri = R[i] # R_i^{i-1}
pi = p[i] # p_i^{i-1} expressed in frame i-1
jt = joint_type[i-1]
if jt == 'R':
# revolute joint
omega[i] = Ri @ omega[i-1] + z_axis * qd[i-1]
omegadot[i] = (
Ri @ omegadot[i-1]
+ z_axis * qdd[i-1]
+ np.cross(omega[i], z_axis * qd[i-1])
)
a[i] = (
Ri @ (
a[i-1]
+ np.cross(omegadot[i-1], pi)
+ np.cross(omega[i-1], np.cross(omega[i-1], pi))
)
)
else:
# prismatic joint
omega[i] = Ri @ omega[i-1]
omegadot[i] = Ri @ omegadot[i-1]
a[i] = (
Ri @ (
a[i-1]
+ np.cross(omegadot[i-1], pi)
+ np.cross(omega[i-1], np.cross(omega[i-1], pi))
)
+ 2.0 * np.cross(omega[i], z_axis * qd[i-1])
+ z_axis * qdd[i-1]
)
rc = r_c[i-1]
a_c[i-1] = (
a[i]
+ np.cross(omegadot[i], rc)
+ np.cross(omega[i], np.cross(omega[i], rc))
)
# Backward recursion
f = [np.zeros(3) for _ in range(n + 2)]
nvec = [np.zeros(3) for _ in range(n + 2)]
tau = np.zeros(n)
for i in range(n, 0, -1):
# link index i-1 for data arrays
mi = m[i-1]
Ii = I[i-1]
rc = r_c[i-1]
# transform child wrench
Ri_next = R[i+1].T if (i < n) else np.eye(3)
# For simplicity: p_{i+1}^i zero if i == n
pi_next = np.zeros(3) if i == n else p[i+1]
f[i] = mi * a_c[i-1] + Ri_next @ f[i+1]
nvec[i] = (
Ii @ omegadot[i]
+ np.cross(omega[i], Ii @ omega[i])
+ np.cross(rc, mi * a_c[i-1])
+ Ri_next @ nvec[i+1]
+ np.cross(pi_next + rc, Ri_next @ f[i+1])
)
if joint_type[i-1] == 'R':
tau[i-1] = nvec[i].dot(z_axis)
else:
tau[i-1] = f[i].dot(z_axis)
return tau
The structure mirrors the theoretical recursion: forward pass only uses parent states, backward pass only uses child wrenches. Performance can be improved by vectorization or pre-allocating arrays for multiple configurations.
6. C++ Implementation with Eigen
In C++, the Eigen library is a natural choice for 3D
vectors and matrices. Below is a skeleton for a Newton–Euler
routine for revolute joints:
#include <Eigen/Dense>
#include <vector>
using Eigen::Vector3d;
using Eigen::Matrix3d;
struct Link {
Matrix3d R; // R_i^{i-1}
Vector3d p; // p_i^{i-1} in frame i-1
Vector3d r_c; // CoM offset in frame i
double m;
Matrix3d I; // inertia about frame i origin
char joint_type; // 'R' or 'P'
};
std::vector<double> newtonEuler(
const std::vector<Link>& links,
const std::vector<double>& q,
const std::vector<double>& qd,
const std::vector<double>& qdd,
const Vector3d& g0
){
const int n = static_cast<int>(links.size());
const Vector3d z_axis(0.0, 0.0, 1.0);
std::vector<Vector3d> omega(n+1, Vector3d::Zero());
std::vector<Vector3d> omegadot(n+1, Vector3d::Zero());
std::vector<Vector3d> a(n+1, Vector3d::Zero());
std::vector<Vector3d> a_c(n, Vector3d::Zero());
// base quantities
omega[0].setZero();
omegadot[0].setZero();
a[0] = -g0;
// forward recursion
for (int i = 1; i <= n; ++i) {
const Link& L = links[i-1];
const Matrix3d& R = L.R;
const Vector3d& p = L.p;
char jt = L.joint_type;
if (jt == 'R') {
omega[i] = R * omega[i-1] + z_axis * qd[i-1];
omegadot[i] = R * omegadot[i-1]
+ z_axis * qdd[i-1]
+ omega[i].cross(z_axis * qd[i-1]);
a[i] = R * ( a[i-1]
+ omegadot[i-1].cross(p)
+ omega[i-1].cross(omega[i-1].cross(p)) );
} else { // prismatic
omega[i] = R * omega[i-1];
omegadot[i] = R * omegadot[i-1];
a[i] = R * ( a[i-1]
+ omegadot[i-1].cross(p)
+ omega[i-1].cross(omega[i-1].cross(p)) )
+ 2.0 * omega[i].cross(z_axis * qd[i-1])
+ z_axis * qdd[i-1];
}
a_c[i-1] = a[i]
+ omegadot[i].cross(L.r_c)
+ omega[i].cross(omega[i].cross(L.r_c));
}
// backward recursion
std::vector<Vector3d> f(n+2, Vector3d::Zero());
std::vector<Vector3d> nvec(n+2, Vector3d::Zero());
std::vector<double> tau(n, 0.0);
for (int i = n; i >= 1; --i) {
const Link& L = links[i-1];
Matrix3d R_next = Matrix3d::Identity();
Vector3d p_next = Vector3d::Zero();
if (i < n) {
R_next = links[i].R.transpose(); // R_i^{i+1}
p_next = links[i].p; // p_{i+1}^{i} approx
}
f[i] = L.m * a_c[i-1] + R_next * f[i+1];
nvec[i] = L.I * omegadot[i]
+ omega[i].cross(L.I * omega[i])
+ L.r_c.cross(L.m * a_c[i-1])
+ R_next * nvec[i+1]
+ (p_next + L.r_c).cross(R_next * f[i+1]);
if (L.joint_type == 'R') {
tau[i-1] = nvec[i].dot(z_axis);
} else {
tau[i-1] = f[i].dot(z_axis);
}
}
return tau;
}
This code assumes you have already filled links[i].R and
links[i].p from a forward kinematics routine (e.g., using a
DH parameter handler). In a ROS context, similar functionality is
available via KDL, but it is instructive to implement the recursion
yourself once.
7. Java Implementation with Basic Arrays
Java has fewer de facto standard robotics libraries, so we often use
small linear algebra helpers based on double[][] and
double[]. The code below sketches the core recursion:
public class NewtonEuler {
public static class Link {
public double[][] R; // 3x3
public double[] p; // length 3
public double[] r_c; // length 3
public double m;
public double[][] I; // 3x3
public char jointType; // 'R' or 'P'
}
private static final double[] Z_AXIS = new double[]{0.0, 0.0, 1.0};
// --- vector and matrix helpers omitted for brevity (add, cross, dot, matVec, etc.)
public static double[] newtonEuler(
Link[] links,
double[] q, double[] qd, double[] qdd,
double[] g0
) {
int n = links.length;
double[][] omega = new double[n+1][3];
double[][] omegadot = new double[n+1][3];
double[][] a = new double[n+1][3];
double[][] a_c = new double[n][3];
// base
a[0] = vecScale(g0, -1.0);
// forward recursion
for (int i = 1; i <= n; ++i) {
Link L = links[i-1];
if (L.jointType == 'R') {
omega[i] = vecAdd(
matVec(L.R, omega[i-1]),
vecScale(Z_AXIS, qd[i-1])
);
omegadot[i] = vecAdd(
matVec(L.R, omegadot[i-1]),
vecAdd(
vecScale(Z_AXIS, qdd[i-1]),
cross(omega[i], vecScale(Z_AXIS, qd[i-1]))
)
);
double[] term = vecAdd(
omegadot[i-1],
cross(omega[i-1], cross(omega[i-1], L.p))
);
a[i] = matVec(L.R, vecAdd(a[i-1], term));
} else {
omega[i] = matVec(L.R, omega[i-1]);
omegadot[i] = matVec(L.R, omegadot[i-1]);
double[] term = vecAdd(
omegadot[i-1],
cross(omega[i-1], cross(omega[i-1], L.p))
);
double[] baseA = matVec(L.R, vecAdd(a[i-1], term));
a[i] = vecAdd(
vecAdd(
baseA,
vecScale(cross(omega[i], vecScale(Z_AXIS, qd[i-1])), 2.0)
),
vecScale(Z_AXIS, qdd[i-1])
);
}
a_c[i-1] = vecAdd(
vecAdd(a[i], cross(omegadot[i], L.r_c)),
cross(omega[i], cross(omega[i], L.r_c))
);
}
// backward recursion
double[][] f = new double[n+2][3];
double[][] nvec = new double[n+2][3];
double[] tau = new double[n];
for (int i = n; i >= 1; --i) {
Link L = links[i-1];
double[][] R_next = identity3();
double[] p_next = new double[3];
if (i < n) {
R_next = transpose(links[i].R);
p_next = links[i].p;
}
f[i] = vecAdd(
vecScale(a_c[i-1], L.m),
matVec(R_next, f[i+1])
);
double[] Iomega = matVec(L.I, omega[i]);
nvec[i] = vecAdd(
vecAdd(
vecAdd(
matVec(L.I, omegadot[i]),
cross(omega[i], Iomega)
),
cross(L.r_c, vecScale(a_c[i-1], L.m))
),
vecAdd(
matVec(R_next, nvec[i+1]),
cross(vecAdd(p_next, L.r_c), matVec(R_next, f[i+1]))
)
);
if (L.jointType == 'R') {
tau[i-1] = dot(nvec[i], Z_AXIS);
} else {
tau[i-1] = dot(f[i], Z_AXIS);
}
}
return tau;
}
}
In a robotics software stack, such a class would be coupled with a
kinematics provider that sets R and p for each
configuration.
8. MATLAB/Simulink Implementation
MATLAB, especially with the Robotics System Toolbox, already offers
rigidBodyTree objects that internally use recursive
algorithms. Here, we implement a from-scratch Newton–Euler
function and then outline a Simulink realization using a MATLAB Function
block.
function tau = newtonEulerSerial(R, p, r_c, m, I, q, qd, qdd, g0, jointType)
% R, p, r_c, I : cell arrays of length n
% m : [n x 1] masses
% jointType : char vector 'R' or 'P'
% q, qd, qdd : [n x 1]
% g0 : [3 x 1] gravity in base frame
n = numel(m);
z = [0; 0; 1];
omega = zeros(3, n+1);
omegad = zeros(3, n+1);
a = zeros(3, n+1);
a_c = zeros(3, n);
% base
a(:,1) = -g0;
% forward recursion
for i = 1:n
Ri = R{i};
pi = p{i};
jt = jointType(i);
if jt == 'R'
omega(:,i+1) = Ri * omega(:,i) + z * qd(i);
omegad(:,i+1) = Ri * omegad(:,i) + z * qdd(i) ...
+ cross(omega(:,i+1), z * qd(i));
a(:,i+1) = Ri * ( a(:,i) ...
+ cross(omegad(:,i), pi) ...
+ cross(omega(:,i), cross(omega(:,i), pi)) );
else
omega(:,i+1) = Ri * omega(:,i);
omegad(:,i+1) = Ri * omegad(:,i);
a(:,i+1) = Ri * ( a(:,i) ...
+ cross(omegad(:,i), pi) ...
+ cross(omega(:,i), cross(omega(:,i), pi)) ) ...
+ 2 * cross(omega(:,i+1), z * qd(i)) + z * qdd(i);
end
rc = r_c{i};
a_c(:,i) = a(:,i+1) + cross(omegad(:,i+1), rc) ...
+ cross(omega(:,i+1), cross(omega(:,i+1), rc));
end
% backward recursion
f = zeros(3, n+2);
nvec = zeros(3, n+2);
tau = zeros(n,1);
for i = n:-1:1
Ri_next = eye(3);
pi_next = [0; 0; 0];
if i < n
Ri_next = R{i+1}';
pi_next = p{i+1};
end
mi = m(i);
Ii = I{i};
rc = r_c{i};
f(:,i+1) = mi * a_c(:,i) + Ri_next * f(:,i+2);
nvec(:,i+1) = Ii * omegad(:,i+1) + cross(omega(:,i+1), Ii * omega(:,i+1)) ...
+ cross(rc, mi * a_c(:,i)) ...
+ Ri_next * nvec(:,i+2) ...
+ cross(pi_next + rc, Ri_next * f(:,i+2));
if jointType(i) == 'R'
tau(i) = dot(nvec(:,i+1), z);
else
tau(i) = dot(f(:,i+1), z);
end
end
end
To build a Simulink version, wrap this function in a MATLAB Function
block. Inputs are q, qd, qdd, and
any variable parameters (e.g., changing payload mass). Constants like
inertia and kinematics can be stored as persistent
matrices or passed as tunable parameters. The output of the block is
tau, which can feed into a joint actuator model.
9. Wolfram Mathematica Implementation
Mathematica is well suited to expressing the recursion symbolically, which is especially useful when cross-checking against Lagrange–Euler derivations or generating optimized C code. The example below is kept compact and assumes all joints are revolute.
Clear[NewtonEulerSerial];
(* R, p, rc, I are lists of length n; q, qd, qdd are vectors *)
NewtonEulerSerial[R_List, p_List, rc_List, m_List, I_List,
q_List, qd_List, qdd_List, g0_List] :=
Module[{n = Length[m], z, omega, omegad, a, ac, f, nvec, i},
z = {0, 0, 1};
omega = ConstantArray[{0, 0, 0}, n + 1];
omegad = ConstantArray[{0, 0, 0}, n + 1];
a = ConstantArray[{0, 0, 0}, n + 1];
a[[1]] = -g0;
(* forward recursion *)
Do[
omega[[i + 1]] =
R[[i]].omega[[i]] + z qd[[i]];
omegad[[i + 1]] =
R[[i]].omegad[[i]] + z qdd[[i]] +
Cross[omega[[i + 1]], z qd[[i]]];
a[[i + 1]] =
R[[i]].(a[[i]] + Cross[omegad[[i]], p[[i]]] +
Cross[omega[[i]], Cross[omega[[i]], p[[i]]]]);
, {i, 1, n}];
ac = Table[
a[[i + 1]] + Cross[omegad[[i + 1]], rc[[i]]] +
Cross[omega[[i + 1]], Cross[omega[[i + 1]], rc[[i]]]],
{i, 1, n}
];
(* backward recursion *)
f = ConstantArray[{0, 0, 0}, n + 2];
nvec = ConstantArray[{0, 0, 0}, n + 2];
Do[
With[{RiNext = If[i < n, Transpose[R[[i + 1]]], IdentityMatrix[3]],
piNext = If[i < n, p[[i + 1]], {0, 0, 0}]},
f[[i + 1]] = m[[i]] ac[[i]] + RiNext.f[[i + 2]];
nvec[[i + 1]] =
I[[i]].omegad[[i + 1]] +
Cross[omega[[i + 1]], I[[i]].omega[[i + 1]]] +
Cross[rc[[i]], m[[i]] ac[[i]]] +
RiNext.nvec[[i + 2]] +
Cross[piNext + rc[[i]], RiNext.f[[i + 2]]];
];
, {i, n, 1, -1}];
Table[nvec[[i + 1]].z, {i, 1, n}]
]
Mathematica's CodeGenerate or
CCodeGenerate
can then be used to export the resulting routine to C for embedding in
real-time controllers.
10. Numerical Validation and Complexity
The Newton–Euler algorithm performs \( \mathcal{O}(n) \) operations per evaluation, compared with naive symbolic Lagrange approaches whose evaluation cost can grow superlinearly when implemented poorly. To validate a code implementation:
- For a simple 1-DOF or 2-DOF arm with known analytic dynamics \( \tau(q, \dot{q}, \ddot{q}) \) from Lagrange, check that the recursive implementation returns identical torque values (within numerical tolerance).
- Numerically differentiate \( \boldsymbol{\tau}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) \) with respect to \( \ddot{\mathbf{q}} \) to recover \( \mathbf{M}(\mathbf{q}) \) and verify symmetry and positive definiteness numerically.
- For \( \mathbf{q} = \mathbf{q}_0 \) and \( \dot{\mathbf{q}} = \mathbf{0}, \ddot{\mathbf{q}} = \mathbf{0} \), verify that \( \boldsymbol{\tau} = \mathbf{g}(\mathbf{q}_0) \) (pure gravity).
These tests are crucial before integrating the routine into a controller or trajectory optimization pipeline.
11. Problems and Solutions
Problem 1 (1-DOF Pendulum via Newton–Euler): Consider a single revolute joint with a rigid link of length \( \ell \), mass \( m \), center of mass at distance \( \ell/2 \) from the joint, and scalar inertia about the joint \( I \). Let the joint angle be \( q \), with gravity \( \mathbf{g}_0 = [0, 0, -g]^\top \). Derive the torque \( \tau \) using the Newton–Euler recursion and show that it reduces to the familiar equation:
\[ \tau = I \ddot{q} + \frac{m g \ell}{2} \sin(q). \]
Solution:
For a 1-DOF system, we have only link 1. The base acceleration is \( \mathbf{a}_0 = -\mathbf{g}_0 = [0, 0, g]^\top \). With planar motion about the \( z \)-axis, we can represent \( \boldsymbol{\omega}_1 = [0, 0, \dot{q}]^\top \), \( \dot{\boldsymbol{\omega}}_1 = [0, 0, \ddot{q}]^\top \), and take \( \mathbf{r}_{c_1} = [\ell/2, 0, 0]^\top \). The center-of-mass acceleration from the forward recursion is
\[ \mathbf{a}_{c_1} = \mathbf{a}_1 + \dot{\boldsymbol{\omega}}_1 \times \mathbf{r}_{c_1} + \boldsymbol{\omega}_1 \times \big( \boldsymbol{\omega}_1 \times \mathbf{r}_{c_1} \big). \]
In the link frame aligned with the link, the last term contributes the centripetal acceleration, while the term with \( \dot{\boldsymbol{\omega}}_1 \) contributes the tangential acceleration. Projecting onto the direction perpendicular to the link and using \( \mathbf{f}_1 = m \mathbf{a}_{c_1} \) gives the tangential force causing torque at the joint. The moment about the joint is
\[ \mathbf{n}_1 = I \dot{\boldsymbol{\omega}}_1 + \mathbf{r}_{c_1} \times (m \mathbf{a}_{c_1}), \]
and the joint torque is \( \tau = \mathbf{n}_1^\top \mathbf{z}_1 \). The inertial part yields \( I \ddot{q} \), while the gravity contribution (contained in \( \mathbf{a}_{c_1} \) via \( \mathbf{a}_0 \)) produces \( (m g \ell / 2) \sin(q) \). Thus the Newton–Euler recursion reproduces the standard pendulum equation.
Problem 2 (2R Planar Arm Consistency Check): For a planar 2-DOF arm in the horizontal plane (no gravity), with link masses \( m_1, m_2 \) and moments of inertia \( I_1, I_2 \) about their respective joint axes, apply the Newton–Euler algorithm symbolically to obtain \( \tau_1(q, \dot{q}, \ddot{q}) \) and \( \tau_2(q, \dot{q}, \ddot{q}) \). Show that the result can be written in the standard inertial form:
\[ \begin{bmatrix} \tau_1 \\ \tau_2 \end{bmatrix} = \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}}. \]
Solution (sketch):
- Construct frames so that joint 1 rotates about \( z \) at the base, joint 2 about \( z \) at the elbow.
- Apply the forward recursion to obtain \( \boldsymbol{\omega}_1, \boldsymbol{\omega}_2 \) and \( \dot{\boldsymbol{\omega}}_1, \dot{\boldsymbol{\omega}}_2 \), then the CoM accelerations \( \mathbf{a}_{c_1}, \mathbf{a}_{c_2} \).
- Apply the backward recursion: compute \( \mathbf{f}_2 = m_2 \mathbf{a}_{c_2} \), \( \mathbf{n}_2 = I_2 \dot{\boldsymbol{\omega}}_2 + \dots \), and \( \tau_2 = \mathbf{n}_2^\top \mathbf{z}_2 \). Then propagate to link 1: \( \mathbf{f}_1 = m_1 \mathbf{a}_{c_1} + \mathbf{R}_1^2 \mathbf{f}_2 \) and corresponding \( \mathbf{n}_1 \), giving \( \tau_1 \).
- Collect terms multiplying \( \ddot{q}_1, \ddot{q}_2 \) to form \( \mathbf{M}(\mathbf{q}) \) and terms involving quadratic forms in \( \dot{\mathbf{q}} \) to form \( \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} \). One recovers the classical expressions for the planar 2R arm known from Lagrange–Euler derivations, proving consistency.
Problem 3 (Algorithmic Complexity): Argue that, ignoring the cost of computing the forward kinematics transforms, the Newton–Euler recursion has cost \( \mathcal{O}(n) \) in the number of joints \( n \), while a naive evaluation of a fully symbolic Lagrange model can easily exhibit worse scaling.
Solution:
Each iteration of the forward loop computes a fixed number of cross-products and matrix–vector products, which we may regard as constant-cost operations. Thus the forward loop cost is proportional to \( n \). The backward loop likewise performs a fixed amount of work per link, again yielding a cost proportional to \( n \). Therefore the total cost is \( c_1 n + c_2 n = \mathcal{O}(n) \) for constants \( c_1, c_2 \). In contrast, a naive Lagrange approach that forms \( \mathbf{M}(\mathbf{q}) \) as partial derivatives of kinetic energy and then inverts it for simulation may involve matrix multiplications and inversions whose cost grows like \( \mathcal{O}(n^3) \) if implemented without exploiting structure. Hence the recursive method is asymptotically more efficient and numerically better conditioned.
Problem 4 (Gravity Vector Test Case): For a general serial manipulator, explain how to obtain the gravity vector \( \mathbf{g}(\mathbf{q}) \) using the same Newton–Euler implementation without computing any joint accelerations or velocities.
Solution:
Set \( \dot{\mathbf{q}} = \mathbf{0} \) and \( \ddot{\mathbf{q}} = \mathbf{0} \). Then only the gravity contribution in the base acceleration \( \mathbf{a}_0 = -\mathbf{g}_0 \) propagates through the forward recursion. The backward recursion in this configuration returns torques \( \boldsymbol{\tau} \) that exactly equal \( \mathbf{g}(\mathbf{q}) \). Thus, by calling the same function with zero joint velocities and accelerations, one obtains the gravity vector directly, which is a common validation test for implementations.
flowchart TD
CFG["Choose configuration q"] --> ZERO["Set qdot = 0, qddot = 0"]
ZERO --> CALL["Call Newton-Euler recursion"]
CALL --> TAU["Output tau(q, 0, 0)"]
TAU --> GRAV["Interpret tau as gravity vector g(q)"]
12. Summary
In this lesson we operationalized the Newton–Euler equations into efficient, reusable code across Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. We revisited the forward and backward recursions, mapped each algebraic step to code, and discussed validation strategies that compare against simpler analytic models or Lagrange–Euler dynamics. These implementations form the computational backbone for simulation and control of complex manipulators and will be used in later chapters for identification, trajectory generation, and advanced model structures.
13. References
- Luh, J.Y.S., Walker, M.W., & Paul, R.P.C. (1980). On-line computational scheme for mechanical manipulators. Journal of Dynamic Systems, Measurement, and Control, 102(2), 69–76.
- Hollerbach, J.M. (1980). A recursive Lagrangian formulation of manipulator dynamics and a comparative study of dynamics formulation complexity. IEEE Transactions on Systems, Man, and Cybernetics, 10(11), 730–736.
- Walker, M.W., & Orin, D.E. (1982). Efficient dynamic computer simulation of robotic mechanisms. ASME Journal of Dynamic Systems, Measurement, and Control, 104(3), 205–211.
- Orin, D.E., & Schrader, W.W. (1984). Efficient computation of the Jacobian for robot manipulators. International Journal of Robotics Research, 3(4), 66–75.
- Featherstone, R. (1983). The calculation of robot dynamics using articulated-body inertias. International Journal of Robotics Research, 2(1), 13–30.
- Wittenburg, J. (1977). Dynamics of Systems of Rigid Bodies. B.G. Teubner. (Foundational text on recursive rigid-body dynamics.)
- Angeles, J. (1988). The application of linear algebra to the kinematics and dynamics of mechanical systems. European Journal of Mechanics A/Solids, 7(2), 141–161.
- Spong, M.W., & Vidyasagar, M. (1987). Robust linear compensator design for nonlinear robotic control. IEEE Journal of Robotics and Automation, 3(4), 345–351.
- Craig, J.J. (1989). Introduction to Robotics: Mechanics and Control. Addison–Wesley. (Chapters on manipulator dynamics.)
- Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2010). Robotics: Modelling, Planning and Control. Springer. (Detailed treatment of Newton–Euler formulations.)