1. Lagrange vs. Newton–Euler — Same Dynamics Map

For an \( n \)-DOF rigid serial manipulator with generalized coordinates \( \mathbf{q} \in \mathbb{R}^n \), both the Lagrange–Euler and the Newton–Euler formulations produce the standard robot equations of motion

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}) \]

where \( \mathbf{M}(\mathbf{q}) \) is the joint-space inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) collects Coriolis and centrifugal effects, \( \mathbf{g}(\mathbf{q}) \) is the gravity vector, and \( \boldsymbol{\tau} \) is the joint torque vector.

Conceptually, the Lagrange formulation starts from kinetic and potential energies:

\[ L(\mathbf{q},\dot{\mathbf{q}}) = T(\mathbf{q},\dot{\mathbf{q}}) - V(\mathbf{q}), \quad \boldsymbol{\tau} = \frac{\mathrm{d}}{\mathrm{d}t} \left( \frac{\partial L}{\partial \dot{\mathbf{q}}} \right) - \frac{\partial L}{\partial \mathbf{q}}, \]

while the Newton–Euler algorithm recursively propagates spatial velocities and accelerations forward along the chain, and forces and torques backward, using link inertial parameters. Both routes describe the same physical system, so they must produce identical \( \boldsymbol{\tau} \) for any \( (\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}) \).

In software, however, it is alarmingly easy to introduce subtle mistakes (frame issues, sign errors, incorrect center-of-mass locations). A systematic validation of Newton–Euler against an independently derived Lagrange model is therefore essential.

flowchart TD
  A["Robot description (DH/PoE + inertial params)"]
    --> B["Lagrange route: \nT(q,qdot), V(q) -> M, C, g"]
  A --> C["Newton-Euler route: \nrecursive forward/backward passes"]
  B --> D["Joint torques tau_L(q,qdot,qddot)"]
  C --> E["Joint torques tau_NE(q,qdot,qddot)"]
  D --> F["Compare tau_L vs tau_NE on test trajectories"]
  E --> F
  F --> G["Check structural properties: M symmetric, energy identity"]
  G --> H["Validated dynamics model (for control, simulation)"]

2. Analytical Equivalence via Jacobians and Spatial Inertia

Let link \( i \) have spatial inertia \( \mathbf{I}_i \in \mathbb{R}^{6 \times 6} \), and let \( \mathbf{v}_i \in \mathbb{R}^6 \) be its spatial twist (angular and linear velocity) expressed in some base frame. From Chapter 10–11, the kinetic energy of the entire manipulator can be written as

\[ T(\mathbf{q},\dot{\mathbf{q}}) = \frac{1}{2}\sum_{i=1}^{n} \mathbf{v}_i^\top \mathbf{I}_i \mathbf{v}_i. \]

With the joint-space twist relation \( \mathbf{v}_i = \mathbf{J}_i(\mathbf{q})\dot{\mathbf{q}} \), where \( \mathbf{J}_i \) is the geometric Jacobian for link \( i \), we obtain

\[ T(\mathbf{q},\dot{\mathbf{q}}) = \frac{1}{2}\sum_{i=1}^{n} \dot{\mathbf{q}}^\top \mathbf{J}_i(\mathbf{q})^\top \mathbf{I}_i \mathbf{J}_i(\mathbf{q}) \dot{\mathbf{q}} = \frac{1}{2}\dot{\mathbf{q}}^\top \mathbf{M}(\mathbf{q}) \dot{\mathbf{q}}, \]

with

\[ \mathbf{M}(\mathbf{q}) = \sum_{i=1}^{n} \mathbf{J}_i(\mathbf{q})^\top \mathbf{I}_i \mathbf{J}_i(\mathbf{q}). \]

This is the Lagrangian expression for the inertia matrix in terms of link Jacobians and spatial inertias.

On the other hand, in the Newton–Euler recursion, the spatial wrench on link \( i \) is

\[ \mathbf{F}_i = \mathbf{I}_i \mathbf{a}_i + \boldsymbol{\nu}_i, \quad \boldsymbol{\nu}_i = \mathbf{v}_i \times^\ast (\mathbf{I}_i \mathbf{v}_i), \]

where \( \mathbf{a}_i \) is the spatial acceleration and \( \times^\ast \) denotes the spatial cross-product operator for wrenches. For a 1-DOF joint with motion subspace vector \( \mathbf{S}_j \in \mathbb{R}^6 \), the generalized torque is

\[ \tau_j = \mathbf{S}_j^\top \mathbf{F}_j. \]

Stacking the joint subspace columns in \( \mathbf{S} = [\mathbf{S}_1 \dots \mathbf{S}_n] \) and collecting the spatial accelerations into \( \mathbf{a} \), we can symbolically rearrange the recursive Newton–Euler algorithm into the joint-space form

\[ \boldsymbol{\tau} = \mathbf{M}_{\text{NE}}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}_{\text{NE}}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{g}_{\text{NE}}(\mathbf{q}), \]

where \( \mathbf{h}_{\text{NE}} \) are centrifugal and Coriolis terms plus any velocity-dependent components of external wrenches. One can show (proof in many rigid-body dynamics texts) that

\[ \mathbf{M}_{\text{NE}}(\mathbf{q}) = \sum_{i=1}^{n} \mathbf{J}_i(\mathbf{q})^\top \mathbf{I}_i \mathbf{J}_i(\mathbf{q}), \quad \mathbf{g}_{\text{NE}}(\mathbf{q}) = - \frac{\partial V(\mathbf{q})}{\partial \mathbf{q}}, \]

so \( \mathbf{M}_{\text{NE}} = \mathbf{M} \) and \( \mathbf{g}_{\text{NE}} = \mathbf{g} \). Equality of Coriolis/centrifugal terms follows from the identity of kinetic energy and the principle of virtual work (see problem set). Thus, the two formulations are analytically equivalent.

3. Energy-Based Properties Used for Validation

From \( T(\mathbf{q},\dot{\mathbf{q}}) = \tfrac{1}{2}\dot{\mathbf{q}}^\top \mathbf{M}(\mathbf{q}) \dot{\mathbf{q}} \), we obtain

\[ \dot{T} = \dot{\mathbf{q}}^\top \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \frac{1}{2}\dot{\mathbf{q}}^\top \dot{\mathbf{M}}(\mathbf{q}) \dot{\mathbf{q}}. \]

Neglecting dissipation and non-conservative forces, the mechanical power balance reads

\[ \dot{T} + \dot{V} = \dot{\mathbf{q}}^\top \boldsymbol{\tau}, \quad \dot{V} = \frac{\partial V}{\partial \mathbf{q}}^\top \dot{\mathbf{q}} = -\dot{\mathbf{q}}^\top \mathbf{g}(\mathbf{q}). \]

Substituting the dynamics \( \boldsymbol{\tau} = \mathbf{M}\ddot{\mathbf{q}} + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g} \) gives

\[ \dot{\mathbf{q}}^\top \mathbf{M} \ddot{\mathbf{q}} + \frac{1}{2}\dot{\mathbf{q}}^\top \dot{\mathbf{M}} \dot{\mathbf{q}} = \dot{\mathbf{q}}^\top \mathbf{M}\ddot{\mathbf{q}} + \dot{\mathbf{q}}^\top \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}, \]

hence the celebrated skew-symmetry property

\[ \dot{\mathbf{q}}^\top \left(\dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\right) \dot{\mathbf{q}} = 0 \quad \forall\, \mathbf{q},\dot{\mathbf{q}}. \]

In practice, this identity is used as a powerful structural validation test: given \( \mathbf{M}, \mathbf{C} \) constructed from a Newton–Euler implementation, one checks

Symmetry and positive-definiteness of \( \mathbf{M}(\mathbf{q}) \): numerically, \( \mathbf{M} - \mathbf{M}^\top \) must be near zero, and eigenvalues must be positive.
Skew-symmetry test: sample \( (\mathbf{q},\dot{\mathbf{q}}) \), approximate \( \dot{\mathbf{M}} \) numerically, and verify that \( \dot{\mathbf{q}}^\top (\dot{\mathbf{M}} - 2\mathbf{C}) \dot{\mathbf{q}} \) is numerically negligible.
Power consistency: compare \( \dot{\mathbf{q}}^\top \boldsymbol{\tau} \) with \( \dot{T} + \dot{V} \) along trajectories.

If all these tests pass and pointwise torque comparisons with a Lagrange-based implementation are within numerical tolerance, the Newton–Euler code is very likely correct.

4. 2-DOF Planar Manipulator — Lagrange Closed Form

Consider a 2R planar arm in the horizontal plane, with revolute joints about the out-of-plane axis. Let \( q_1, q_2 \) be joint angles, \( l_1 \) the length of link 1, \( c_1, c_2 \) the distances from the preceding joint to each link center-of-mass, \( m_1, m_2 \) the masses, and \( I_1, I_2 \) the planar inertias around the joint axes.

The kinetic energy (using standard planar kinematics) is

\[ T = \frac{1}{2}\dot{\mathbf{q}}^\top \mathbf{M}(\mathbf{q})\dot{\mathbf{q}}, \quad \mathbf{q} = \begin{bmatrix} q_1 \\ q_2 \end{bmatrix}, \quad \dot{\mathbf{q}} = \begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \end{bmatrix}. \]

Defining the constants

\[ a = I_1 + I_2 + m_1 c_1^2 + m_2 (l_1^2 + c_2^2), \quad b = m_2 l_1 c_2, \quad d = I_2 + m_2 c_2^2, \]

the inertia matrix is

\[ \mathbf{M}(\mathbf{q}) = \begin{bmatrix} a + 2b \cos q_2 & d + b \cos q_2 \\ d + b \cos q_2 & d \end{bmatrix}. \]

The gravity potential (with gravity magnitude \( g \) acting opposite the \( y \)-axis) yields

\[ \mathbf{g}(\mathbf{q}) = \begin{bmatrix} g\big(c_1 m_1 \cos q_1 + m_2(l_1 \cos q_1 + c_2 \cos(q_1+q_2))\big) \\ g\, m_2 c_2 \cos(q_1 + q_2) \end{bmatrix}. \]

The Coriolis/centrifugal vector \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \) (often written as \( \mathbf{C}\dot{\mathbf{q}} \)) can be computed either via Christoffel symbols or by direct Lagrange derivatives. A convenient expression is

\[ \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) = \begin{bmatrix} -b \sin q_2 (2\dot{q}_1\dot{q}_2 + \dot{q}_2^2) \\ \;\;b \sin q_2 \dot{q}_1^2 \end{bmatrix}. \]

The full Lagrange-based dynamics are then

\[ \boldsymbol{\tau} = \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{g}(\mathbf{q}). \]

In this lesson, we will treat these analytic expressions as the reference Lagrange model against which a Newton–Euler implementation is validated.

5. Numerical Validation Strategy and Matrix Reconstruction

Let \( \boldsymbol{\tau}_{\text{L}} \) be computed from a Lagrange model and \( \boldsymbol{\tau}_{\text{NE}} \) from a Newton–Euler implementation. A basic numerical strategy is:

Sample joint trajectories \( (\mathbf{q}(t_k),\dot{\mathbf{q}}(t_k),\ddot{\mathbf{q}}(t_k)) \) for \( k = 1,\dots,N \).
For each sample, compute \( \boldsymbol{\tau}_{\text{L}}(t_k) \) and \( \boldsymbol{\tau}_{\text{NE}}(t_k) \).
Accumulate the error statistics: \( \|\boldsymbol{\tau}_{\text{L}} - \boldsymbol{\tau}_{\text{NE}}\| \) (e.g., infinity norm) across samples.
Additionally reconstruct \( \mathbf{M}, \mathbf{g} \) from the Newton–Euler algorithm by exploiting the linearity of \( \boldsymbol{\tau} \) in \( \ddot{\mathbf{q}} \) and \( \mathbf{g}_0 \) (base gravity vector).

To reconstruct the inertia matrix \( \mathbf{M}_{\text{NE}}(\mathbf{q}) \) at a fixed configuration \( \mathbf{q} \), set velocity and gravity to zero and apply unit accelerations:

\[ \ddot{\mathbf{q}} = \mathbf{e}_j,\quad \dot{\mathbf{q}}=\mathbf{0},\quad \mathbf{g}_0=\mathbf{0} \quad\Rightarrow\quad \boldsymbol{\tau}_{\text{NE}} = \mathbf{M}_{\text{NE}}(\mathbf{q})\mathbf{e}_j, \]

so the \( j \)-th column of \( \mathbf{M}_{\text{NE}}(\mathbf{q}) \) is \( \boldsymbol{\tau}_{\text{NE}} \). Repeating for \( j=1,\dots,n \) yields the full matrix.

Likewise, the gravity vector can be obtained by

\[ \ddot{\mathbf{q}}=\mathbf{0},\quad \dot{\mathbf{q}}=\mathbf{0},\quad \mathbf{g}_0 \neq \mathbf{0} \quad\Rightarrow\quad \boldsymbol{\tau}_{\text{NE}} = \mathbf{g}_{\text{NE}}(\mathbf{q}). \]

The velocity-dependent terms can then be isolated as \( \mathbf{h}_{\text{NE}}(\mathbf{q},\dot{\mathbf{q}}) = \boldsymbol{\tau}_{\text{NE}} - \mathbf{M}_{\text{NE}}\ddot{\mathbf{q}} - \mathbf{g}_{\text{NE}} \).

flowchart TD
  S["Fix configuration q"] --> M1["For each joint j: set qddot = e_j, qdot = 0, g0 = 0"]
  M1 --> M2["Run Newton-Euler -> tau_NE"]
  M2 --> M3["Set column j of M_NE(q) = tau_NE"]
  M3 --> G1["Set qddot = 0, qdot = 0, g0 != 0"]
  G1 --> G2["Run Newton-Euler -> tau_NE = g_NE(q)"]
  G2 --> H1["For general (q, qdot, qddot): h_NE = tau_NE - M_NE qddot - g_NE"]
  H1 --> V["Compare {M_NE, h_NE, g_NE} against Lagrange results and check symmetry/energy"]

6. Python Implementation — 2R Arm Validation

Below we implement the analytic Lagrange model for the 2R planar arm and a generic interface for a Newton–Euler implementation (which you should already have from Lesson 4). We then reconstruct \( \mathbf{M}_{\text{NE}} \), \( \mathbf{g}_{\text{NE}} \), and compare torques.


import numpy as np

class TwoLinkParams:
    def __init__(self, m1, m2, l1, c1, c2, I1, I2, g=9.81):
        self.m1 = m1
        self.m2 = m2
        self.l1 = l1
        self.c1 = c1
        self.c2 = c2
        self.I1 = I1
        self.I2 = I2
        self.g  = g

def M_lagrange_2R(q, p):
    q1, q2 = q
    a = p.I1 + p.I2 + p.m1 * p.c1**2 + p.m2 * (p.l1**2 + p.c2**2)
    b = p.m2 * p.l1 * p.c2
    d = p.I2 + p.m2 * p.c2**2
    c2 = np.cos(q2)
    M11 = a + 2.0 * b * c2
    M12 = d + b * c2
    M22 = d
    return np.array([[M11, M12],
                     [M12, M22]], dtype=float)

def h_lagrange_2R(q, dq, p):
    q1, q2 = q
    dq1, dq2 = dq
    b = p.m2 * p.l1 * p.c2
    s2 = np.sin(q2)
    h1 = -b * s2 * (2.0 * dq1 * dq2 + dq2**2)
    h2 =  b * s2 * dq1**2
    return np.array([h1, h2], dtype=float)

def g_lagrange_2R(q, p):
    q1, q2 = q
    g1 = p.g * (p.c1 * p.m1 * np.cos(q1)
                + p.m2 * (p.l1 * np.cos(q1) + p.c2 * np.cos(q1 + q2)))
    g2 = p.g * (p.m2 * p.c2 * np.cos(q1 + q2))
    return np.array([g1, g2], dtype=float)

def tau_lagrange_2R(q, dq, ddq, p):
    M = M_lagrange_2R(q, p)
    h = h_lagrange_2R(q, dq, p)
    g = g_lagrange_2R(q, p)
    return M.dot(ddq) + h + g

# ----------------------------------------------------------------------
# Newton-Euler interface: you should plug in your own implementation
# from Chapter 12, Lesson 4. Here we declare the expected signature.
# ----------------------------------------------------------------------
def tau_newton_euler_2R(q, dq, ddq, p, g_base=None):
    """
    Placeholder: call your validated Newton-Euler implementation here.
    Arguments:
        q, dq, ddq: arrays of shape (2,)
        p: TwoLinkParams
        g_base: 3D gravity vector in base frame (e.g. [0, -p.g, 0])
    Returns:
        tau_NE: array of shape (2,)
    """
    raise NotImplementedError("Connect to your Newton-Euler implementation.")

# ----------------------------------------------------------------------
# Reconstruction of M_NE(q) and g_NE(q) using the strategy of Section 5
# ----------------------------------------------------------------------
def reconstruct_M_NE(q, p):
    n = 2
    M = np.zeros((n, n), dtype=float)
    dq = np.zeros(n)
    g_base = np.zeros(3)  # zero gravity
    for j in range(n):
        ddq = np.zeros(n)
        ddq[j] = 1.0
        tau = tau_newton_euler_2R(q, dq, ddq, p, g_base=g_base)
        M[:, j] = tau
    return M

def reconstruct_g_NE(q, p):
    n = 2
    ddq = np.zeros(n)
    dq = np.zeros(n)
    # gravity pointing along negative y-axis
    g_base = np.array([0.0, -p.g, 0.0])
    tau = tau_newton_euler_2R(q, dq, ddq, p, g_base=g_base)
    return tau

def monte_carlo_validation(num_samples=100):
    p = TwoLinkParams(m1=2.0, m2=1.0,
                      l1=1.0, c1=0.5, c2=0.5,
                      I1=0.2, I2=0.1)
    max_err = 0.0
    for k in range(num_samples):
        q  = np.array([np.random.uniform(-np.pi, np.pi),
                       np.random.uniform(-np.pi, np.pi)])
        dq = np.array([np.random.uniform(-1.0, 1.0),
                       np.random.uniform(-1.0, 1.0)])
        ddq = np.array([np.random.uniform(-2.0, 2.0),
                        np.random.uniform(-2.0, 2.0)])

        tau_L  = tau_lagrange_2R(q, dq, ddq, p)
        tau_NE = tau_newton_euler_2R(q, dq, ddq, p,
                                     g_base=np.array([0.0, -p.g, 0.0]))
        err = np.linalg.norm(tau_L - tau_NE, ord=np.inf)
        max_err = max(max_err, err)

    print("Max absolute torque error over", num_samples, "samples:", max_err)

    # Single configuration structural test
    q0 = np.array([0.3, -0.7])
    M_L = M_lagrange_2R(q0, p)
    g_L = g_lagrange_2R(q0, p)
    M_NE = reconstruct_M_NE(q0, p)
    g_NE = reconstruct_g_NE(q0, p)

    print("M_L(q0) =\n", M_L)
    print("M_NE(q0) =\n", M_NE)
    print("M_L - M_NE =\n", M_L - M_NE)
    print("g_L(q0) =", g_L)
    print("g_NE(q0) =", g_NE)
    print("g_L - g_NE =", g_L - g_NE)

if __name__ == "__main__":
    # Once tau_newton_euler_2R is connected, this will
    # provide a strong regression test of your implementation.
    monte_carlo_validation()

In a larger Python robotics codebase (e.g., based on numpy/sympy and optionally a robotics toolbox), the same validation logic extends directly to \( n \)-DOF manipulators: reconstruct \( \mathbf{M}_{\text{NE}} \) and \( \mathbf{g}_{\text{NE}} \) via unit accelerations and gravity tests, and compare with closed-form or symbolic Lagrange results.

7. C++ Implementation — Eigen-Based Validation Skeleton

In C++, it is natural to use Eigen for linear algebra and a dedicated dynamics library (e.g., RBDL, KDL, or your own implementation) for Newton–Euler. The snippet below mirrors the Python code and is suitable as a unit test module.


#include <iostream>
#include <cmath>
#include <Eigen/Dense>

struct TwoLinkParams {
    double m1, m2;
    double l1, c1, c2;
    double I1, I2;
    double g;
};

Eigen::Matrix2d M_lagrange_2R(const Eigen::Vector2d& q,
                              const TwoLinkParams& p) {
    double q2 = q(1);
    double a = p.I1 + p.I2 + p.m1 * p.c1 * p.c1
               + p.m2 * (p.l1 * p.l1 + p.c2 * p.c2);
    double b = p.m2 * p.l1 * p.c2;
    double d = p.I2 + p.m2 * p.c2 * p.c2;
    double c2 = std::cos(q2);

    Eigen::Matrix2d M;
    M(0,0) = a + 2.0 * b * c2;
    M(0,1) = d + b * c2;
    M(1,0) = M(0,1);
    M(1,1) = d;
    return M;
}

Eigen::Vector2d h_lagrange_2R(const Eigen::Vector2d& q,
                              const Eigen::Vector2d& dq,
                              const TwoLinkParams& p) {
    double q2  = q(1);
    double dq1 = dq(0);
    double dq2 = dq(1);
    double b   = p.m2 * p.l1 * p.c2;
    double s2  = std::sin(q2);

    Eigen::Vector2d h;
    h(0) = -b * s2 * (2.0 * dq1 * dq2 + dq2 * dq2);
    h(1) =  b * s2 * dq1 * dq1;
    return h;
}

Eigen::Vector2d g_lagrange_2R(const Eigen::Vector2d& q,
                              const TwoLinkParams& p) {
    double q1 = q(0);
    double q2 = q(1);

    double g1 = p.g * (p.c1 * p.m1 * std::cos(q1)
                       + p.m2 * (p.l1 * std::cos(q1)
                                 + p.c2 * std::cos(q1 + q2)));
    double g2 = p.g * (p.m2 * p.c2 * std::cos(q1 + q2));

    Eigen::Vector2d g;
    g(0) = g1;
    g(1) = g2;
    return g;
}

Eigen::Vector2d tau_lagrange_2R(const Eigen::Vector2d& q,
                                const Eigen::Vector2d& dq,
                                const Eigen::Vector2d& ddq,
                                const TwoLinkParams& p) {
    Eigen::Matrix2d M = M_lagrange_2R(q, p);
    Eigen::Vector2d h = h_lagrange_2R(q, dq, p);
    Eigen::Vector2d g = g_lagrange_2R(q, p);
    return M * ddq + h + g;
}

// ------------------------------------------------------------------
// Newton-Euler function to be supplied by you (Lesson 4).
// g_base is the 3D gravity vector in the base frame.
// ------------------------------------------------------------------
Eigen::Vector2d tau_newton_euler_2R(const Eigen::Vector2d& q,
                                    const Eigen::Vector2d& dq,
                                    const Eigen::Vector2d& ddq,
                                    const TwoLinkParams& p,
                                    const Eigen::Vector3d& g_base) {
    // TODO: call your library / implementation here.
    throw std::runtime_error("Connect to your Newton-Euler implementation.");
}

Eigen::Matrix2d reconstruct_M_NE(const Eigen::Vector2d& q,
                                 const TwoLinkParams& p) {
    Eigen::Matrix2d M;
    Eigen::Vector2d dq = Eigen::Vector2d::Zero();
    Eigen::Vector3d g_base(0.0, 0.0, 0.0); // zero gravity

    for (int j = 0; j < 2; ++j) {
        Eigen::Vector2d ddq = Eigen::Vector2d::Zero();
        ddq(j) = 1.0;
        Eigen::Vector2d tau =
            tau_newton_euler_2R(q, dq, ddq, p, g_base);
        M.col(j) = tau;
    }
    return M;
}

Eigen::Vector2d reconstruct_g_NE(const Eigen::Vector2d& q,
                                 const TwoLinkParams& p) {
    Eigen::Vector2d dq  = Eigen::Vector2d::Zero();
    Eigen::Vector2d ddq = Eigen::Vector2d::Zero();
    Eigen::Vector3d g_base(0.0, -p.g, 0.0);
    return tau_newton_euler_2R(q, dq, ddq, p, g_base);
}

int main() {
    TwoLinkParams p;
    p.m1 = 2.0; p.m2 = 1.0;
    p.l1 = 1.0; p.c1 = 0.5; p.c2 = 0.5;
    p.I1 = 0.2; p.I2 = 0.1;
    p.g  = 9.81;

    Eigen::Vector2d q(0.3, -0.7);
    Eigen::Matrix2d M_L  = M_lagrange_2R(q, p);
    Eigen::Matrix2d M_NE = reconstruct_M_NE(q, p);

    std::cout << "M_L(q) =\n" << M_L << std::endl;
    std::cout << "M_NE(q) =\n" << M_NE << std::endl;
    std::cout << "M_L - M_NE =\n" << (M_L - M_NE) << std::endl;

    Eigen::Vector2d g_L  = g_lagrange_2R(q, p);
    Eigen::Vector2d g_NE = reconstruct_g_NE(q, p);
    std::cout << "g_L(q)  = " << g_L.transpose()  << std::endl;
    std::cout << "g_NE(q) = " << g_NE.transpose() << std::endl;

    return 0;
}

In a full C++ stack, tau_newton_euler_2R would typically call a library such as RBDL or an in-house recursive Newton–Euler implementation, while M_lagrange_2R and companions would be generated symbolically or via automatic differentiation.

8. Java Implementation — Array-Based Validation

Java does not have a de facto standard robotics dynamics library, but simple validation code can be written with primitive arrays or with a linear algebra package (e.g., EJML). The example below uses plain double arrays.


public class TwoLinkValidation {

    static class Params {
        double m1, m2;
        double l1, c1, c2;
        double I1, I2;
        double g;
    }

    static double[][] M_lagrange_2R(double[] q, Params p) {
        double q2 = q[1];
        double a = p.I1 + p.I2 + p.m1 * p.c1 * p.c1
                   + p.m2 * (p.l1 * p.l1 + p.c2 * p.c2);
        double b = p.m2 * p.l1 * p.c2;
        double d = p.I2 + p.m2 * p.c2 * p.c2;
        double c2 = Math.cos(q2);

        double[][] M = new double[2][2];
        M[0][0] = a + 2.0 * b * c2;
        M[0][1] = d + b * c2;
        M[1][0] = M[0][1];
        M[1][1] = d;
        return M;
    }

    static double[] h_lagrange_2R(double[] q, double[] dq, Params p) {
        double q2  = q[1];
        double dq1 = dq[0];
        double dq2 = dq[1];
        double b   = p.m2 * p.l1 * p.c2;
        double s2  = Math.sin(q2);

        return new double[] {
            -b * s2 * (2.0 * dq1 * dq2 + dq2 * dq2),
             b * s2 * dq1 * dq1
        };
    }

    static double[] g_lagrange_2R(double[] q, Params p) {
        double q1 = q[0];
        double q2 = q[1];

        double g1 = p.g * (p.c1 * p.m1 * Math.cos(q1)
                + p.m2 * (p.l1 * Math.cos(q1)
                + p.c2 * Math.cos(q1 + q2)));
        double g2 = p.g * (p.m2 * p.c2 * Math.cos(q1 + q2));
        return new double[]{g1, g2};
    }

    static double[] tau_lagrange_2R(double[] q, double[] dq,
                                    double[] ddq, Params p) {
        double[][] M = M_lagrange_2R(q, p);
        double[] h   = h_lagrange_2R(q, dq, p);
        double[] g   = g_lagrange_2R(q, p);

        double[] tau = new double[2];
        for (int i = 0; i < 2; ++i) {
            tau[i] = h[i] + g[i];
            for (int j = 0; j < 2; ++j) {
                tau[i] += M[i][j] * ddq[j];
            }
        }
        return tau;
    }

    // --------------------------------------------------------------
    // Newton-Euler interface (to be provided by your Java robotics code).
    // gBase is a 3D vector [gx, gy, gz] in the base frame.
    // --------------------------------------------------------------
    static double[] tauNewtonEuler2R(double[] q, double[] dq,
                                     double[] ddq, Params p,
                                     double[] gBase) {
        throw new UnsupportedOperationException(
            "Connect to your Newton-Euler implementation."
        );
    }

    static double[][] reconstructM_NE(double[] q, Params p) {
        double[][] M = new double[2][2];
        double[] dq  = new double[]{0.0, 0.0};
        double[] gBase = new double[]{0.0, 0.0, 0.0};
        for (int j = 0; j < 2; ++j) {
            double[] ddq = new double[]{0.0, 0.0};
            ddq[j] = 1.0;
            double[] tau =
                tauNewtonEuler2R(q, dq, ddq, p, gBase);
            M[0][j] = tau[0];
            M[1][j] = tau[1];
        }
        return M;
    }

    static double[] reconstructG_NE(double[] q, Params p) {
        double[] dq   = new double[]{0.0, 0.0};
        double[] ddq  = new double[]{0.0, 0.0};
        double[] gBase = new double[]{0.0, -p.g, 0.0};
        return tauNewtonEuler2R(q, dq, ddq, p, gBase);
    }

    public static void main(String[] args) {
        Params p = new Params();
        p.m1 = 2.0; p.m2 = 1.0;
        p.l1 = 1.0; p.c1 = 0.5; p.c2 = 0.5;
        p.I1 = 0.2; p.I2 = 0.1;
        p.g  = 9.81;

        double[] q  = new double[]{0.3, -0.7};
        double[] dq = new double[]{0.1, -0.2};
        double[] ddq = new double[]{0.5, 0.3};

        double[] tauL =
            tau_lagrange_2R(q, dq, ddq, p);
        double[] tauNE =
            tauNewtonEuler2R(q, dq, ddq, p,
                             new double[]{0.0, -p.g, 0.0});

        System.out.printf("tau_L:  [%.6f, %.6f]%n",
                          tauL[0], tauL[1]);
        System.out.printf("tau_NE: [%.6f, %.6f]%n",
                          tauNE[0], tauNE[1]);
    }
}

In a Java-based robotics framework, one can wrap the Newton–Euler routine and these Lagrange-based checks into unit tests that run automatically whenever kinematic or inertial parameters are changed.

9. MATLAB/Simulink and Mathematica Validation

9.1 MATLAB script and function

In MATLAB, the Symbolic Math Toolbox and Robotics System Toolbox allow you to derive Lagrange equations and compute Newton–Euler dynamics (via RNE-like functions). Below is a minimal numeric implementation for the 2R arm.


function lesson12_5_validation_matlab()
    p.m1 = 2.0; p.m2 = 1.0;
    p.l1 = 1.0; p.c1 = 0.5; p.c2 = 0.5;
    p.I1 = 0.2; p.I2 = 0.1;
    p.g  = 9.81;

    q   = [0.3; -0.7];
    dq  = [0.1; -0.2];
    ddq = [0.5; 0.3];

    tauL  = tau_lagrange_2R(q, dq, ddq, p);
    % tauNE should be computed by your Newton-Euler function
    % tauNE = tau_newton_euler_2R(q, dq, ddq, p, [0; -p.g; 0]);

    disp('tau_L (Lagrange) =');
    disp(tauL);

    % Example of reconstructing M_NE and g_NE for testing:
    MNE = reconstruct_M_NE(q, p);
    gNE = reconstruct_g_NE(q, p);
    disp('M_NE(q) ='); disp(MNE);
    disp('g_NE(q) ='); disp(gNE);
end

function M = M_lagrange_2R(q, p)
    q2 = q(2);
    a  = p.I1 + p.I2 + p.m1 * p.c1^2 + p.m2 * (p.l1^2 + p.c2^2);
    b  = p.m2 * p.l1 * p.c2;
    d  = p.I2 + p.m2 * p.c2^2;
    c2 = cos(q2);

    M = [a + 2*b*c2, d + b*c2;
         d + b*c2,   d];
end

function h = h_lagrange_2R(q, dq, p)
    q2  = q(2);
    dq1 = dq(1);
    dq2 = dq(2);
    b   = p.m2 * p.l1 * p.c2;
    s2  = sin(q2);

    h = [-b * s2 * (2*dq1*dq2 + dq2^2);
          b * s2 * dq1^2];
end

function g = g_lagrange_2R(q, p)
    q1 = q(1); q2 = q(2);
    g1 = p.g * (p.c1 * p.m1 * cos(q1) ...
             + p.m2 * (p.l1 * cos(q1) + p.c2 * cos(q1 + q2)));
    g2 = p.g * (p.m2 * p.c2 * cos(q1 + q2));
    g  = [g1; g2];
end

function tau = tau_lagrange_2R(q, dq, ddq, p)
    M = M_lagrange_2R(q, p);
    h = h_lagrange_2R(q, dq, p);
    g = g_lagrange_2R(q, p);
    tau = M * ddq + h + g;
end

function MNE = reconstruct_M_NE(q, p)
    dq   = [0; 0];
    gBase = [0; 0; 0];
    MNE  = zeros(2,2);
    for j = 1:2
        ddq = zeros(2,1);
        ddq(j) = 1;
        tau = tau_newton_euler_2R(q, dq, ddq, p, gBase);
        MNE(:, j) = tau;
    end
end

function gNE = reconstruct_g_NE(q, p)
    dq   = [0; 0];
    ddq  = [0; 0];
    gBase = [0; -p.g; 0];
    gNE  = tau_newton_euler_2R(q, dq, ddq, p, gBase);
end

In Simulink, one typically encapsulates the Newton–Euler algorithm in a subsystem (or S-function) and the Lagrange model in another subsystem. Driving both with the same joint trajectories and comparing the torque outputs via a scope or assertion blocks provides a graphical validation.

9.2 Wolfram Mathematica symbolic/numeric validation

Mathematica is particularly convenient for symbolic derivation of the Lagrange dynamics and automatic generation of numerically efficient functions. The following sketch shows how to obtain \( \mathbf{M}(\mathbf{q}) \), \( \mathbf{h} \), and \( \mathbf{g} \) symbolically and then validate a Newton–Euler implementation numerically.


(* Define symbolic variables *)
Clear[q1, q2, dq1, dq2, ddq1, ddq2, l1, c1, c2, m1, m2, I1, I2, g];
q  = {q1, q2};
dq = {dq1, dq2};
ddq = {ddq1, ddq2};

(* Positions of centers of mass *)
x1 = c1 Cos[q1];
y1 = c1 Sin[q1];

x2 = l1 Cos[q1] + c2 Cos[q1 + q2];
y2 = l1 Sin[q1] + c2 Sin[q1 + q2];

(* Velocities via Jacobian *)
dx1 = D[x1, q1] dq1 + D[x1, q2] dq2;
dy1 = D[y1, q1] dq1 + D[y1, q2] dq2;

dx2 = D[x2, q1] dq1 + D[x2, q2] dq2;
dy2 = D[y2, q1] dq1 + D[y2, q2] dq2;

(* Kinetic and potential energies *)
T = 1/2 m1 (dx1^2 + dy1^2) + 1/2 I1 dq1^2
  + 1/2 m2 (dx2^2 + dy2^2) + 1/2 I2 (dq1 + dq2)^2;

V = g (m1 y1 + m2 y2); (* sign convention can be adapted *)

L = T - V;

(* Lagrange equations *)
tau = Table[
   D[D[L, dq[[i]]], t] /. {
      t -> 0,
      q1' -> dq1, q2' -> dq2,
      q1'' -> ddq1, q2'' -> ddq2
   } - D[L, q[[i]]]
   , {i, 2}];

(* Extract M(q), h(q,dq), g(q) by comparing with M ddq + h + g *)
(* Use Coefficient and SeriesCoefficient utilities *)

(* Numeric validation at a sample configuration *)
params = {m1 -> 2.0, m2 -> 1.0, l1 -> 1.0,
          c1 -> 0.5, c2 -> 0.5, I1 -> 0.2, I2 -> 0.1, g -> 9.81};

qNum    = {q1 -> 0.3, q2 -> -0.7};
dqNum   = {dq1 -> 0.1, dq2 -> -0.2};
ddqNum  = {ddq1 -> 0.5, ddq2 -> 0.3};

tauLNum = tau /. params /. qNum /. dqNum /. ddqNum // N

(* tauNENum should be imported from a Newton-Euler implementation, e.g.
   from an external C++ or Python library called via LibraryLink.
   Then compare tauLNum and tauNENum to validate. *)

After obtaining symbolic expressions, Mathematica can generate optimized C code, which you may integrate into a real-time dynamics library and still validate numerically against a Newton–Euler implementation using the same strategies as above.

10. Problems and Solutions

Problem 1 (Deriving the joint-space inertia from Newton–Euler): Consider an \( n \)-DOF serial manipulator. Assume you can compute \( \boldsymbol{\tau}_{\text{NE}}(\mathbf{q}, \dot{\mathbf{q}},\ddot{\mathbf{q}}) \) using a recursive Newton–Euler algorithm. Show that the inertia matrix \( \mathbf{M}_{\text{NE}}(\mathbf{q}) \) can be computed by running the algorithm \( n \) times with \( \ddot{\mathbf{q}} = \mathbf{e}_j \), \( \dot{\mathbf{q}}=\mathbf{0} \), \( \mathbf{g}_0 = \mathbf{0} \), and explain why this yields exactly the same matrix as the Jacobian-based expression from Lagrange.

Solution: With \( \dot{\mathbf{q}}=\mathbf{0} \) and \( \mathbf{g}_0=\mathbf{0} \), all velocity-dependent and gravity terms vanish, so the dynamics reduce to \( \boldsymbol{\tau}_{\text{NE}} = \mathbf{M}_{\text{NE}}(\mathbf{q})\ddot{\mathbf{q}} \). Setting \( \ddot{\mathbf{q}} = \mathbf{e}_j \) yields \( \boldsymbol{\tau}_{\text{NE}} = \mathbf{M}_{\text{NE}}(\mathbf{q})\mathbf{e}_j \), which is the \( j \)-th column of \( \mathbf{M}_{\text{NE}}(\mathbf{q}) \). Repeating for all coordinate directions reconstructs the full matrix. By the analytical result in Section 2, this \( \mathbf{M}_{\text{NE}}(\mathbf{q}) \) coincides with the Lagrange matrix \( \sum_i \mathbf{J}_i^\top \mathbf{I}_i \mathbf{J}_i \) constructed from link Jacobians and spatial inertias.

Problem 2 (Energy-based skew-symmetry property): Starting from the dynamics \( \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} \) and the kinetic energy \( T = \tfrac{1}{2}\dot{\mathbf{q}}^\top \mathbf{M}(\mathbf{q})\dot{\mathbf{q}} \), derive the identity \( \dot{\mathbf{q}}^\top (\dot{\mathbf{M}} - 2\mathbf{C}) \dot{\mathbf{q}} = 0 \).

Solution: Differentiate \( T = \tfrac{1}{2}\dot{\mathbf{q}}^\top \mathbf{M}\dot{\mathbf{q}} \):

\[ \dot{T} = \dot{\mathbf{q}}^\top \mathbf{M} \ddot{\mathbf{q}} + \frac{1}{2}\dot{\mathbf{q}}^\top \dot{\mathbf{M}} \dot{\mathbf{q}}. \]

Multiply the dynamics by \( \dot{\mathbf{q}}^\top \):

\[ \dot{\mathbf{q}}^\top \boldsymbol{\tau} = \dot{\mathbf{q}}^\top \mathbf{M}\ddot{\mathbf{q}} + \dot{\mathbf{q}}^\top \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \dot{\mathbf{q}}^\top \mathbf{g}(\mathbf{q}). \]

From conservation of mechanical energy (no dissipation), \( \dot{T} + \dot{V} = \dot{\mathbf{q}}^\top \boldsymbol{\tau} \) with \( \dot{V} = \tfrac{\partial V}{\partial \mathbf{q}}^\top \dot{\mathbf{q}} = -\dot{\mathbf{q}}^\top \mathbf{g} \). Equate the two expressions for \( \dot{T} \) and cancel \( \dot{\mathbf{q}}^\top \mathbf{M}\ddot{\mathbf{q}} \):

\[ \frac{1}{2}\dot{\mathbf{q}}^\top \dot{\mathbf{M}} \dot{\mathbf{q}} = \dot{\mathbf{q}}^\top \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \dot{\mathbf{q}}, \]

which implies \( \dot{\mathbf{q}}^\top (\dot{\mathbf{M}} - 2\mathbf{C}) \dot{\mathbf{q}} = 0 \) for all \( \dot{\mathbf{q}} \), hence \( \dot{\mathbf{M}} - 2\mathbf{C} \) is skew-symmetric. This property should hold irrespective of whether \( \mathbf{M},\mathbf{C} \) are obtained from a Lagrange or Newton–Euler implementation.

Problem 3 (2R arm numerical check): For the 2R arm of Section 4 with parameters \( m_1 = 2 \), \( m_2 = 1 \), \( l_1 = 1 \), \( c_1 = c_2 = 0.5 \), \( I_1 = 0.2 \), \( I_2 = 0.1 \), \( g = 9.81 \), and configuration \( q_1 = 0 \), \( q_2 = 0 \), compute \( \mathbf{M}(\mathbf{q}) \) and \( \mathbf{g}(\mathbf{q}) \) from the Lagrange formulas.

Solution: With \( q_2 = 0 \), \( \cos q_2 = 1 \). First compute

\[ a = I_1 + I_2 + m_1 c_1^2 + m_2(l_1^2 + c_2^2) = 0.2 + 0.1 + 2\cdot 0.5^2 + 1\cdot(1^2 + 0.5^2) = 2.2, \]

\[ b = m_2 l_1 c_2 = 1\cdot 1\cdot 0.5 = 0.5, \quad d = I_2 + m_2 c_2^2 = 0.1 + 1\cdot 0.25 = 0.35. \]

Thus

\[ \mathbf{M}(\mathbf{q}) = \begin{bmatrix} a + 2b & d + b \\ d + b & d \end{bmatrix} = \begin{bmatrix} 2.2 + 1.0 & 0.35 + 0.5 \\ 0.35 + 0.5 & 0.35 \end{bmatrix} = \begin{bmatrix} 3.2 & 0.85 \\ 0.85 & 0.35 \end{bmatrix}. \]

For the gravity vector, \( q_1 = 0 \) and \( q_1+q_2 = 0 \), so cosines are 1:

\[ g_1 = g(c_1 m_1 + m_2(l_1 + c_2)) = 9.81(0.5\cdot 2 + 1\cdot(1 + 0.5)) = 9.81(1 + 1.5) = 9.81\cdot 2.5, \]

\[ g_2 = g m_2 c_2 = 9.81\cdot 1\cdot 0.5. \]

Numerically, \( \mathbf{g}(\mathbf{q}) \approx \begin{bmatrix} 24.525 \\ 4.905 \end{bmatrix} \). These values should match the gravity torque returned by a Newton–Euler implementation when \( \dot{\mathbf{q}} = \ddot{\mathbf{q}} = \mathbf{0} \) and base gravity is nonzero.

Problem 4 (Uniqueness of dynamics under parameter variation): Suppose two dynamics models (Model A and Model B) share the same kinematic structure and produce identical joint torques for all joint trajectories and gravity vectors. Show that, under mild regularity assumptions, their mass matrices, Coriolis/centrifugal terms, and gravity vectors must be identical, i.e., \( \mathbf{M}_A = \mathbf{M}_B \), \( \mathbf{C}_A = \mathbf{C}_B \), \( \mathbf{g}_A = \mathbf{g}_B \).

Solution: Fix \( \mathbf{q} \) and consider first \( \dot{\mathbf{q}} = \mathbf{0} \), \( \mathbf{g}_0 = \mathbf{0} \). The equality of torques for all accelerations implies \( \mathbf{M}_A(\mathbf{q})\ddot{\mathbf{q}} = \mathbf{M}_B(\mathbf{q})\ddot{\mathbf{q}} \) for all \( \ddot{\mathbf{q}} \), hence \( \mathbf{M}_A = \mathbf{M}_B \) by linear independence of the standard basis. Next, with \( \ddot{\mathbf{q}} = \mathbf{0} \), \( \dot{\mathbf{q}} = \mathbf{0} \), equality of torques for any base gravity vector yields \( \mathbf{g}_A(\mathbf{q}) = \mathbf{g}_B(\mathbf{q}) \). Finally, with \( \ddot{\mathbf{q}} = \mathbf{0} \), \( \mathbf{g}_0 = \mathbf{0} \), equality of torques for arbitrary velocities gives \( \mathbf{C}_A(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} = \mathbf{C}_B(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) for all \( \dot{\mathbf{q}} \), which implies \( \mathbf{C}_A = \mathbf{C}_B \). Thus, agreement of torques for all trajectories guarantees equality of the decomposed dynamic terms, which justifies using Lagrange-based expressions as a reference “ground truth” for validating Newton–Euler implementations.

11. Summary

In this lesson we established the analytical equivalence between the Lagrange and Newton–Euler formulations of robot dynamics through Jacobian and spatial inertia arguments. We exploited this equivalence to design robust numerical validation procedures: reconstruction of \( \mathbf{M}(\mathbf{q}) \) and \( \mathbf{g}(\mathbf{q}) \) using unit accelerations and gravity tests, comparison of torque outputs along trajectories, and verification of structural properties such as symmetry, positive-definiteness, and the skew-symmetry identity \( \dot{\mathbf{q}}^\top (\dot{\mathbf{M}} - 2\mathbf{C}) \dot{\mathbf{q}} = 0 \). Implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica showed how to embed these checks into practical robotics software and simulation environments.

12. 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.
Wittenburg, J. (1977). Dynamics of Systems of Rigid Bodies. Teubner, Stuttgart.
Featherstone, R. (1983). The calculation of robot dynamics using articulated-body inertias. International Journal of Robotics Research, 2(1), 13–30.
Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer.
Angeles, J. (1988). The formulation of dynamical equations of motion of manipulators in terms of generalized coordinates. International Journal of Robotics Research, 7(5), 45–59.
Khatib, O. (1987). A unified approach for motion and force control of robot manipulators: The operational space formulation. IEEE Journal of Robotics and Automation, 3(1), 43–53.
Walker, M.W., & Orin, D.E. (1982). Efficient dynamic computer simulation of robotic mechanisms. Journal of Dynamic Systems, Measurement, and Control, 104(3), 205–211.
Park, J., & Chung, W.K. (2005). Geometric integration on Euclidean group with application to articulated multibody systems. IEEE Transactions on Robotics, 21(5), 850–863.
Selig, J.M. (1996). Geometrical methods in robotics. Acta Applicandae Mathematicae, 47(1), 127–151.

Chapter 12: Newton–Euler Recursive Dynamics

Lesson 5: Validation Against Lagrange Results