Week 05 | Day 02

Lagrangian Dynamics: Energy-Based Equations of Motion

Published: April 21, 2026 | Author: Smartotics Learning Journey | Reading Time: 14 min

TL;DR: Instead of balancing forces on every body, the Lagrangian method computes the equations of motion from kinetic and potential energy. It’s cleaner, less error-prone, and scales to robots with dozens of joints. L = T - V, then apply Euler-Lagrange.

---

The Problem with Force Balance

Yesterday, we wrote Newton-Euler equations for individual links. For a 6-DOF industrial robot, that means:

• 6 free-body diagrams
• 36 constraint force components to track
• Cross-coupling between every pair of links
• Easy to make sign errors

The Lagrangian approach eliminates constraint forces entirely. It works at the system level, not the component level.

---

Core Idea: Energy Never Lies

If you can write the total kinetic energy T and total potential energy V of the robot as functions of joint angles q and velocities q̇, you can derive the equations of motion directly.

The Lagrangian

L(q, q̇) = T(q, q̇) - V(q)

L is not energy — it’s a mathematical object whose derivatives produce the equations of motion.

Euler-Lagrange Equation

For each joint i:

d/dt(∂L/∂q̇ᵢ) - ∂L/∂qᵢ = τᵢ

Where τᵢ is the generalized force (torque for revolute joints, force for prismatic joints).

---

Worked Example: Single Pendulum

Before tackling multi-link robots, let’s verify the method on a simple pendulum.

Setup

• Mass m at distance L from pivot
• Angle θ from vertical
• No friction

Step 1: Kinetic Energy

The mass moves in a circle. Its velocity is v = L·θ̇.

T = ½·m·v² = ½·m·L²·θ̇²

Step 2: Potential Energy

Height above lowest point: h = L(1 - cos θ)

V = m·g·h = m·g·L(1 - cos θ)

Step 3: Lagrangian

L = T - V = ½·m·L²·θ̇² - m·g·L(1 - cos θ)

Step 4: Apply Euler-Lagrange

import sympy as sp

def derive_pendulum_dynamics():
    """
    Derive pendulum equations of motion using symbolic Lagrangian mechanics
    """
    # Symbols
    t = sp.Symbol('t')
    theta = sp.Function('theta')(t)
    m, L, g = sp.symbols('m L g', positive=True)
    
    # Derivatives
    theta_dot = sp.diff(theta, t)
    theta_ddot = sp.diff(theta, t, 2)
    
    # Kinetic energy
    T = sp.Rational(1,2) * m * L**2 * theta_dot**2
    
    # Potential energy
    V = m * g * L * (1 - sp.cos(theta))
    
    # Lagrangian
    Lagrangian = T - V
    
    print("Lagrangian:")
    print(Lagrangian)
    
    # Euler-Lagrange: d/dt(dL/dθ̇) - dL/dθ = τ
    dL_dtheta_dot = sp.diff(Lagrangian, theta_dot)
    d_dt_dL = sp.diff(dL_dtheta_dot, t)
    dL_dtheta = sp.diff(Lagrangian, theta)
    
    # Equation of motion
    eom = sp.Eq(d_dt_dL - dL_dtheta, 0)
    
    print("\nEquation of motion:")
    print(eom)
    
    # Simplify
    eom_simplified = sp.simplify(eom.lhs)
    print(f"\nSimplified: {eom_simplified} = τ")
    
    return eom

eom = derive_pendulum_dynamics()
# Output: m·L²·θ̈ + m·g·L·sin(θ) = τ
# Which gives: θ̈ = (τ - m·g·L·sin(θ)) / (m·L²)

This matches what we get from Newton’s second law: τ = I·α + m·g·L·sin(θ), where I = m·L².

---

2-DOF Planar Arm: Full Derivation

Now let’s apply Lagrangian mechanics to our standard 2-DOF arm.

Link Positions and Velocities

import numpy as np
import sympy as sp

def derive_2dof_lagrangian():
    """
    Full symbolic derivation of 2-DOF planar arm dynamics
    """
    # Define symbols
    q1, q2 = sp.symbols('q1 q2')           # Joint angles
    dq1, dq2 = sp.symbols('dq1 dq2')       # Joint velocities
    m1, m2 = sp.symbols('m1 m2', positive=True)
    L1, L2 = sp.symbols('L1 L2', positive=True)
    g = sp.Symbol('g', positive=True)
    
    # Link 1 COM position
    x1 = (L1/2) * sp.cos(q1)
    y1 = (L1/2) * sp.sin(q1)
    
    # Link 2 COM position
    x2 = L1 * sp.cos(q1) + (L2/2) * sp.cos(q1 + q2)
    y2 = L1 * sp.sin(q1) + (L2/2) * sp.sin(q1 + q2)
    
    # Velocities (time derivatives)
    dx1 = sp.diff(x1, q1) * dq1
    dy1 = sp.diff(y1, q1) * dq1
    dx2 = sp.diff(x2, q1) * dq1 + sp.diff(x2, q2) * dq2
    dy2 = sp.diff(y2, q1) * dq1 + sp.diff(y2, q2) * dq2
    
    # Kinetic energy (translational + rotational)
    T1 = sp.Rational(1,2) * m1 * (dx1**2 + dy1**2) + sp.Rational(1,2) * (m1*L1**2/12) * dq1**2
    T2 = sp.Rational(1,2) * m2 * (dx2**2 + dy2**2) + sp.Rational(1,2) * (m2*L2**2/12) * (dq1 + dq2)**2
    T = sp.simplify(T1 + T2)
    
    # Potential energy
    V1 = m1 * g * y1
    V2 = m2 * g * y2
    V = sp.simplify(V1 + V2)
    
    # Lagrangian
    L = sp.simplify(T - V)
    
    print("Kinetic Energy T:")
    print(T)
    print("\nPotential Energy V:")
    print(V)
    
    return L, T, V

L, T, V = derive_2dof_lagrangian()

Extracting the Mass Matrix

The kinetic energy of a robot always has the quadratic form:

T = ½ · q̇ᵀ · M(q) · q̇

This means we can read the mass matrix directly from the kinetic energy:

def extract_mass_matrix(T, dq_vars):
    """
    Extract mass matrix M(q) from kinetic energy T = ½·q̇ᵀ·M·q̇
    
    Args:
        T: Kinetic energy expression
        dq_vars: List of velocity symbols [dq1, dq2, ...]
    
    Returns:
        Mass matrix as SymPy Matrix
    """
    n = len(dq_vars)
    M = sp.zeros(n, n)
    
    for i in range(n):
        for j in range(n):
            # M[i,j] = coefficient of dq_i * dq_j
            # For diagonal: coefficient of dq_i² / 2
            if i == j:
                coeff = sp.expand(T).coeff(dq_vars[i], 2)
                M[i, j] = 2 * coeff if coeff != 0 else 0
            else:
                coeff = sp.expand(T).coeff(dq_vars[i] * dq_vars[j], 1)
                M[i, j] = coeff if coeff != 0 else 0
    
    return sp.simplify(M)

# Extract for our 2-DOF arm
dq1, dq2 = sp.symbols('dq1 dq2')
M = extract_mass_matrix(T, [dq1, dq2])
print("Mass Matrix M(q):")
print(M)

Computing Coriolis and Gravity Terms

def compute_coriolis_matrix(M, q_vars, dq_vars):
    """
    Compute Coriolis/centrifugal matrix C(q, q̇) from mass matrix
    
    Uses Christoffel symbols:
    C[i,j] = Σₖ ½ (∂M[i,j]/∂qₖ + ∂M[i,k]/∂qⱼ - ∂M[j,k]/∂qᵢ) · q̇ₖ
    """
    n = len(q_vars)
    C = sp.zeros(n, n)
    
    for i in range(n):
        for j in range(n):
            c_ij = 0
            for k in range(n):
                term = (sp.diff(M[i,j], q_vars[k]) + 
                       sp.diff(M[i,k], q_vars[j]) - 
                       sp.diff(M[j,k], q_vars[i])) * dq_vars[k] / 2
                c_ij += term
            C[i, j] = sp.simplify(c_ij)
    
    return C

def compute_gravity_vector(V, q_vars):
    """
    Compute gravity vector G(q) = ∂V/∂q
    """
    G = sp.Matrix([sp.diff(V, q) for q in q_vars])
    return sp.simplify(G)

q1, q2 = sp.symbols('q1 q2')
C = compute_coriolis_matrix(M, [q1, q2], [dq1, dq2])
G = compute_gravity_vector(V, [q1, q2])

print("Coriolis Matrix C(q, q̇):")
print(C)
print("\nGravity Vector G(q):")
print(G)

---

The Complete Equations of Motion

τ = M(q)·q̈ + C(q, q̇)·q̇ + G(q)

Let’s verify this is equivalent to the Lagrangian result:

def verify_equations_of_motion(M, C, G, q_vars, dq_vars):
    """
    Verify that M·q̈ + C·q̇ + G matches Euler-Lagrange result
    """
    n = len(q_vars)
    ddq_vars = sp.symbols([f'ddq{i+1}' for i in range(n)])
    
    # Compute left-hand side: M·q̈ + C·q̇ + G
    ddq_vec = sp.Matrix(ddq_vars)
    dq_vec = sp.Matrix(dq_vars)
    
    lhs = M * ddq_vec + C * dq_vec + G
    lhs = sp.simplify(lhs)
    
    print("Equations of motion (τ = M·q̈ + C·q̇ + G):")
    for i in range(n):
        print(f"τ{i+1} = {lhs[i]}")
    
    return lhs

tau = verify_equations_of_motion(M, C, G, [q1, q2], [dq1, dq2])

---

Numerical Simulation

Now let’s simulate the 2-DOF arm using our derived dynamics:

def simulate_2dof_arm(q0, dq0, tau_func, m1, m2, L1, L2, duration=5.0, dt=0.001):
    """
    Simulate 2-DOF arm dynamics using derived equations
    
    Args:
        q0: Initial joint angles [q1, q2]
        dq0: Initial joint velocities [dq1, dq2]
        tau_func: Function(t, q, dq) -> [tau1, tau2]
        duration: Simulation duration (seconds)
        dt: Time step
    
    Returns:
        times, q_trajectory, dq_trajectory
    """
    g = 9.81
    num_steps = int(duration / dt)
    
    # Storage
    times = np.zeros(num_steps)
    q = np.zeros((num_steps, 2))
    dq = np.zeros((num_steps, 2))
    
    # Initial conditions
    q[0] = q0
    dq[0] = dq0
    
    for i in range(num_steps - 1):
        t = i * dt
        times[i] = t
        
        q1_i, q2_i = q[i]
        dq1_i, dq2_i = dq[i]
        
        # Compute mass matrix
        c2 = np.cos(q2_i)
        s2 = np.sin(q2_i)
        
        M11 = m1*L1**2/3 + m2*(L1**2 + L1*L2*c2 + L2**2/3)
        M12 = m2*(L1*L2*c2/2 + L2**2/3)
        M22 = m2*L2**2/3
        
        M_num = np.array([[M11, M12], [M12, M22]])
        
        # Compute Coriolis terms
        h = -m2 * L1 * L2 * s2 / 2
        C11 = h * dq2_i
        C12 = h * (dq1_i + dq2_i)
        C21 = -h * dq1_i
        C22 = 0
        
        C_num = np.array([[C11, C12], [C21, C22]])
        
        # Compute gravity
        c1 = np.cos(q1_i)
        c12 = np.cos(q1_i + q2_i)
        
        G1 = m1*g*(L1/2)*c1 + m2*g*(L1*c1 + (L2/2)*c12)
        G2 = m2*g*(L2/2)*c12
        
        G_num = np.array([G1, G2])
        
        # Get control torques
        tau = tau_func(t, q[i], dq[i])
        
        # Solve for accelerations: q̈ = M⁻¹(τ - C·q̇ - G)
        rhs = tau - C_num @ dq[i] - G_num
        ddq = np.linalg.solve(M_num, rhs)
        
        # Integrate (semi-implicit Euler)
        dq[i+1] = dq[i] + ddq * dt
        q[i+1] = q[i] + dq[i+1] * dt
    
    times[-1] = duration
    return times, q, dq

# Example: free fall with gravity compensation
compensation = lambda t, q, dq: np.array([
    1.0*9.81*(0.6*np.cos(q[0]) + 0.4*np.cos(q[0]+q[1])),
    0.8*9.81*0.25*np.cos(q[0]+q[1])
])

times, q_traj, dq_traj = simulate_2dof_arm(
    q0=[np.pi/4, 0],
    dq0=[0, 0],
    tau_func=compensation,
    m1=1.0, m2=0.8, L1=0.6, L2=0.5,
    duration=3.0, dt=0.001
)

print(f"Simulation complete: {len(times)} steps")
print(f"Final position: q1={q_traj[-1,0]:.3f}, q2={q_traj[-1,1]:.3f}")

---

When to Use Lagrangian vs. Newton-Euler

Scenario	Recommended Method	Why
Single rigid body	Newton-Euler	Direct, intuitive
Few links, force analysis needed	Newton-Euler	Constraint forces are visible
Many links (6+ DOF)	Lagrangian	Scales better, no constraint bookkeeping
Symbolic derivation	Lagrangian	Energy expressions are easier to differentiate
Real-time control (1 kHz)	Recursive Newton-Euler	O(n) complexity vs. O(n²) for Lagrangian
Optimal control / learning	Lagrangian	Clean parameterization of dynamics

---

Key Takeaways

• Lagrangian mechanics derives equations of motion from energy, eliminating constraint forces
• The mass matrix M(q) encodes configuration-dependent inertia and can be extracted from kinetic energy
• Coriolis terms C(q, q̇) capture velocity-dependent forces that don’t do work
• Gravity terms G(q) come directly from potential energy derivatives
• The form τ = M(q)·q̈ + C(q, q̇)·q̇ + G(q) is universal for all mechanical systems

---

Next Steps

Tomorrow, we’ll explore the Newton-Euler recursive algorithm — an O(n) method that computes dynamics efficiently for real-time control, used in virtually every industrial robot controller.

Practice Exercise: Derive the Lagrangian dynamics for a cart-pole system (inverted pendulum on a moving cart). Simulate it under PD control and observe how the cart must actively move to stabilize the pole.

---

Disclaimer

For educational purposes only. This article is part of a structured learning curriculum and does not constitute professional engineering advice.