Lagrangian dynamics of a serial arm

Forward kinematics gives geometry. The Jacobian gives velocity. To control the arm with torques, you need dynamics — how the joints accelerate when you apply torques and what the arm's mass distribution does to that motion. Lagrangian mechanics is the cleanest derivation. The output is the equation every arm controller in the world is built on.

The recipe

Lagrangian dynamics is mechanical: define energies, take derivatives, get equations of motion. The Lagrangian is

$$L=T-V$$

where $T$ is total kinetic energy and $V$ is total potential energy. The Euler-Lagrange equations are:

$$\frac{d}{dt}\left(\frac{\partial L}{\partial {\dot{q}}_i}\right)-\frac{\partial L}{\partial q_i}={\tau }_i$$

One equation per joint. Crank through them and the result is the standard manipulator equation:

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

Three terms:

• $M(\mathbf{q})$ — the mass matrix (n×n, symmetric, positive-definite). How much force is needed to accelerate each joint, given the current configuration.
• $C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}$ — Coriolis and centrifugal forces. Quadratic in joint velocity; arises from cross-coupling between joints.
• $\mathbf{g}(\mathbf{q})$ — gravity torques. The torque each joint must exert just to hold the arm against gravity at configuration q.

That's the canonical form. Plug in a controller, simulate, control. Every arm software stack is built on it.

What each term does intuitively

The mass matrix

$M$ changes with configuration. A 2-link arm folded has tiny effective inertia at the shoulder; fully extended it has 4× more (the second link's mass acts on a long lever). This is why a fixed-gain PID retuned after extending an arm doesn't behave the same.

$M$ is always positive-definite — for any non-zero $\dot{\mathbf{q}}$, kinetic energy $$T=\frac{1}{2}{\dot{\mathbf{q}}}^{T}M\dot{\mathbf{q}}$$ is positive.

Coriolis and centrifugal

If joint 1 spins, joint 2 feels a centrifugal force pulling it outward. If both spin together, they each feel Coriolis forces from the other. $C$ captures all that. Quadratic in $\dot{\mathbf{q}}$ — proportional to the speeds, not the positions.

For slow arms, $C$ is small enough to often ignore. For fast arms or close to high-speed maneuvers, it matters significantly.

Gravity

$\mathbf{g}(\mathbf{q})$ is the constant tax. Without joint torques compensating gravity, the arm falls. Gravity compensation is the simplest controller: $\mathbf{\tau }=\mathbf{g}(\mathbf{q})$ alone makes the arm float in place under gravity.

Worked example: 2-link planar arm

Two revolute joints; link masses $m_1,m_2$ concentrated at link tips; link lengths $L_1,L_2$.

Mass matrix:

$$M=\left[\begin{array}{cc}{m}_1{L}_1^2+m_2(L_1^2+L_2^2+2L_1{L}_2\cos {\theta }_2)& m_2(L_2^2+L_1{L}_2\cos {\theta }_2)\\ m_2(L_2^2+L_1{L}_2\cos {\theta }_2)& m_2{L}_2^2\end{array}\right]$$

Coriolis matrix:

$$C\dot{\mathbf{q}}=\left[\begin{array}{c}-m_2{L}_1{L}_2\sin {\theta }_2(2{\dot{\theta }}_1{\dot{\theta }}_2+{\dot{\theta }}_2^2)\\ m_2{L}_1{L}_2\sin {\theta }_2{\dot{\theta }}_1^2\end{array}\right]$$

Gravity:

$$\mathbf{g}=\left[\begin{array}{c}(m_1+m_2)g{L}_1\cos {\theta }_1+m_{2}g{L}_2\cos ({\theta }_1+{\theta }_2)\\ m_{2}g{L}_2\cos ({\theta }_1+{\theta }_2)\end{array}\right]$$

Three matrices. Plug in to $$M\ddot{\mathbf{q}}+C\dot{\mathbf{q}}+\mathbf{g}=\mathbf{\tau }$$ and you have a complete model. Forward simulation: solve for $\ddot{\mathbf{q}}$, integrate. Inverse dynamics: given desired $\ddot{\mathbf{q}}$, compute $\mathbf{\tau }$.

What you do with it

Inverse dynamics control

Given a desired motion ${\ddot{\mathbf{q}}}_{\text{des}}$, command the torques:

$$\mathbf{\tau }=M(\mathbf{q}){\ddot{\mathbf{q}}}_{\text{des}}+C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}+\mathbf{g}(\mathbf{q})$$

The arm follows the desired acceleration exactly. Pair with a PD on tracking error:

$$\mathbf{\tau }=M({\ddot{\mathbf{q}}}_d+K_p({\mathbf{q}}_d-\mathbf{q})+K_d({\dot{\mathbf{q}}}_d-\dot{\mathbf{q}}))+C\dot{\mathbf{q}}+\mathbf{g}$$

This is feedback linearization in disguise. The "linearizing" inner loop cancels M, C, g; the outer PD operates on what's now a linear system. Covered in the Feedback Linearization lesson.

Gravity-only control

The simplest non-trivial controller:

$$\mathbf{\tau }=K_p({\mathbf{q}}_d-\mathbf{q})+K_d({\dot{\mathbf{q}}}_d-\dot{\mathbf{q}})+\mathbf{g}(\mathbf{q})$$

PD plus gravity comp. The arm holds position against gravity; PD handles tracking. Robust, easy to tune, used on most cobots.

Forward dynamics for simulation

Solve the equation for $\ddot{\mathbf{q}}$:

$$\ddot{\mathbf{q}}=M(\mathbf{q}{)}^{-1}(\mathbf{\tau }-C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}-\mathbf{g}(\mathbf{q}))$$

Integrate $\dot{\mathbf{q}}$ forward in time, then $\mathbf{q}$ forward. Welcome to physics simulation. MuJoCo, PyBullet, Drake all do this internally — you just supply the URDF + masses + torques.

The Newton-Euler alternative

Lagrangian gives clean derivation. Numerical robotics code rarely uses the analytical Lagrangian; instead they use recursive Newton-Euler — a forward+backward pass through the chain that computes torques in O(n) instead of O(n³). Pinocchio, KDL, RBDL all use Newton-Euler under the hood.

Pseudocode:

# Forward pass: propagate kinematic quantities outward
for i in 1..n:
    velocity[i] = velocity[i-1] + joint_velocity[i] * axis[i]
    acceleration[i] = ...

# Backward pass: propagate forces inward
for i in n..1:
    force[i] = mass[i] * acceleration[i] + gravity_term + force[i+1]
    torque[i] = ... project force onto axis[i]

Same physics, different organization. Faster.

The matrices in 2026 toolkits

• Pinocchio: state-of-the-art; symbolic + numerical. Used by Stack-of-Tasks, Crocoddyl. Recommended.
• KDL / Orocos: long-standing C++ library. ROS-default for inverse kinematics.
• Drake: dynamics + planning + optimization in one library. MIT-developed.
• RBDL: Rigid Body Dynamics Library. Lightweight, well-documented.
• MuJoCo: dynamics + sim. Use this when you want both.

What the math doesn't capture

• Friction: not in the Lagrangian. Add it as $F(\dot{\mathbf{q}})$ term.
• Joint flexibility: real harmonic-drive arms aren't perfectly rigid. Need extended models for high-bandwidth control.
• Cable / belt drives: introduce hysteresis and backlash. Modeled empirically.
• Contact: the dynamics above describe free motion. Contact adds constraint forces — handled by Lagrangian multipliers (covered separately).

Exercise

For the 2-link arm above, implement forward simulation in Python: integrate the Lagrangian equations with a torque input. First with $\mathbf{\tau }=0$ — watch the arm fall under gravity. Then with $\mathbf{\tau }=\mathbf{g}(\mathbf{q})$ (gravity comp) — arm holds anywhere it starts. Then add a PD outer loop and command it to a target. Three controllers, ten lines each, complete dynamics intuition.

Next

Mobile robot kinematics — the same machinery applied to wheels: differential, Ackermann, omnidirectional, all in one lesson.