explainer
Forward dynamics: predict an arm’s motion from joint torques
Solve a robot arm’s joint accelerations from torque, configuration, and velocity. Replay gravity release and compensation with RK4, then check energy balance and step-size error.
What you will learn
- Solve joint accelerations from the current state and applied torques.
- Explain why a single actuator can accelerate both joints of an arm.
- Integrate position and velocity together using classical RK4.
- Distinguish state-dependent gravity compensation from a frozen holding torque.
- Check a simulation using energy balance and a smaller integration step.
Before you start
Release a horizontal robot arm with its motors off, and gravity starts moving both joints. The first acceleration depends on the arm's mass distribution and current pose. Each later acceleration also depends on the velocity the arm has gained.
Forward dynamics calculates that acceleration from the current state and applied torques. This lesson follows a two-link arm from one linear solve to a complete simulated motion, with energy checks along the way.
Ask what acceleration the current torque produces
Let q contain the joint angles, q̇ their velocities, and τ the actuator torques. For this arm, with no external contact force, the equation of motion is:
M(q) q̈ = τ − c(q, q̇) − g(q) − Dq̇
The right side collects the torque left to accelerate the arm. Compute those terms at the current state, then solve the linear system for q̈. Lynch and Park's forward dynamics lesson describes this calculation and its use in robot simulation.
Zero actuator torque leaves gravity in the equation. Inverse dynamics starts with a desired acceleration and calculates the torque that would produce it. Both calculations use the same physical model.
Define the arm and its coordinates
The experiment uses two uniform slender rods. Each rod has length 1 m, mass 1 kg, a center of mass halfway along its length, and rotational inertia 1/12 kg m² about its center. The base joint stays fixed.
- World x points right, and world y points up.
- q₁ measures the first link's angle counterclockwise from world +x.
- q₂ measures the second link's angle relative to the first link. Its world angle is q₁ + q₂.
- Angles use radians, velocities use rad/s, and positive actuator torques act counterclockwise.
- Gravity points downward with magnitude γ, normally
9.81 m/s². Joint damping uses the same coefficient d at both joints:D = dI.
For these specific rods, the entries of M are M₁₁ = 5/3 + cos(q₂), M₁₂ = M₂₁ = 1/3 + cos(q₂)/2, and M₂₂ = 1/3, in kg m². The other terms are:
c₁ = −½ sin(q₂)(2q̇₁q̇₂ + q̇₂²)
c₂ = ½ sin(q₂)q̇₁²
g₁ = γ[1.5 cos(q₁) + 0.5 cos(q₁ + q₂)]
g₂ = 0.5γ cos(q₁ + q₂)
The robot dynamics lesson explains these terms. Here c already denotes the full velocity-dependent torque vector. The model omits joint limits, contact, payloads, motor gearing, and actuator saturation; the links can pass through each other in the drawing.
Actuator dynamics adds the motor's winding current and rotor inertia to this picture. Its single-joint example starts with voltage, follows the torque as current changes, and shows how gearing changes both speed and the inertia the joint must accelerate.
Calculate the first gravity-driven acceleration
Start at q = (0, 0) and q̇ = (0, 0). Both links extend horizontally to the right. Set actuator torque and damping to zero, with γ = 9.81.
At this instant, c = 0, g = (19.62, 4.905) N m, and the mass matrix gives:
(8/3)q̈₁ + (5/6)q̈₂ = −19.62
(5/6)q̈₁ + (1/3)q̈₂ = −4.905
Multiply the second row by three: 2.5q̈₁ + q̈₂ = −14.715. Substitute q̈₂ = −14.715 − 2.5q̈₁ into the first row. This leaves (7/12)q̈₁ = −7.3575.
The solution is q̈₁ = −12.612857 rad/s² and q̈₂ = 16.817143 rad/s². The second value describes relative elbow acceleration. The second link's absolute angular acceleration is their sum, 4.204286 rad/s².
The tip initially accelerates downward: at rest, its vertical acceleration is 2q̈₁ + q̈₂ = −8.408571 m/s². A positive relative elbow acceleration therefore fits a falling tip. The simulation solves M through Cholesky factors; this worked example uses elimination so you can check each step.
Let the mass matrix couple the joints
Keep the same horizontal pose at rest, turn gravity off, and apply τ = (1, 0) N m. Solving Mq̈ = (1, 0) gives q̈ = (12/7, −30/7) rad/s², or (1.714286, −4.285714).
The elbow accelerates even though its motor applies zero torque. The nonzero off-diagonal entries of M connect the joint accelerations. Treating each joint as a separate scalar torque/inertia calculation would lose that coupling.
At full extension, the tip Jacobian loses rank. The joint-space mass matrix still has positive leading entry 8/3 and determinant 7/36. It remains symmetric positive definite, so this acceleration solve remains well defined.
Russ Tedrake's multibody notes explain the manipulator equation and the mass matrix's energy structure. Their simple double pendulum uses point masses and angles measured from downward; the coefficients here describe uniform rods with q₁ measured from +x.
Advance four state variables together
One acceleration does not specify a whole path. Define the four-dimensional state s = (q₁, q₂, q̇₁, q̇₂) and its derivative f(s) = (q̇₁, q̇₂, q̈₁, q̈₂). Each call to f evaluates the torque rule and solves the dynamics at that state.
Classical fourth-order Runge–Kutta, or RK4, samples four slopes per interval of length h:
k₁ = f(s)
k₂ = f(s + hk₁/2)
k₃ = f(s + hk₂/2)
k₄ = f(s + hk₃)
s next = s + h(k₁ + 2k₂ + 2k₃ + k₄)/6
These torque rules have no explicit time dependence. Every stage recomputes M, c, g, and damping from its own trial state. The angles stay unwrapped as integration proceeds.
Driscoll and Braun's numerical computation text gives the RK4 construction. For a smooth problem over a fixed interval, its global error has fourth-order scaling in the small-step regime. The numerical integration lesson works through the stages on a simpler oscillator.
Recompute gravity compensation as the arm moves
Gravity compensation applies τ = g(q) using the current configuration. With exact model parameters, no damping, and q̇ = 0, the remaining acceleration is zero. The arm can hold its starting pose.
Existing velocity changes that result. Gravity compensation cancels g, while c and any damping remain in the solve. It supplies no command to stop the arm or return it to a target angle.
For a checkable moving case, choose q(0) = (0, 0) and q̇(0) = (0.5, 0) rad/s, with d = 0. Along q(t) = (0.5t, 0), sin(q₂) stays zero, so c = 0 and the velocity stays (0.5, 0). At 0.5 s, the torque has changed to (19.010062, 4.752515) N m.
Frozen initial gravity torque holds τ = g(q(0)) = (19.62, 4.905) N m throughout that motion. As q changes, this value exceeds the required gravity compensation. At 0.5 s with h = 0.01, the frozen rule gives q = (0.258510, −0.011739) and q̇ = (0.570046, −0.099129).
At exact rest, both gravity rules can hold the initial pose in this ideal model. Use the moving preset to see their difference. The changing rule assumes continuous access to the modeled state; a real controller also has sensing error, sampling delay, and torque limits.
Replay the arm under different torque rules
The default view shows an unpowered release at 0.5 s, integrated with h = 0.01 s. The arm starts horizontal at rest. Its current angles are (-1.122654, 0.593820) rad, and its velocities are (-2.414309, -3.534794) rad/s.
- Choose Shoulder torque, gravity off and move replay time to zero. Compare the two initial accelerations with the coupling calculation.
- Choose Gravity compensation at rest, then scrub to 2 s. The ideal arm keeps its pose.
- Choose Moving with gravity compensation. Switch the actuator rule to Initial gravity torque (frozen) and compare the same replay time.
- Choose Damped gravity release. Watch energy decrease while dissipated energy increases.
- Choose Downward equilibrium. Both rods hang straight down at rest, with total energy
−19.62 Junder this potential reference.
Selecting a preset replaces its initial state and physical settings. It keeps the chosen replay time and integration step. The time slider selects a saved state; the experiment has no running clock.
Account for energy, actuator work, and damping
The mechanical energy is E = T + V, where T = q̇ᵀMq̇/2. For these rods, V = γ[1.5 sin(q₁) + 0.5 sin(q₁ + q₂)]. The horizontal pose sets V = 0, so a lower pose can have negative potential energy.
With no external force, the ideal energy balance is:
dE/dt = τᵀq̇ − d(q̇₁² + q̇₂²)
E(t) − E(0) = actuator work − dissipated energy
An unpowered arm with zero damping conserves E while gravity exchanges potential and kinetic energy. Modern Robotics uses this conservation check to assess a falling-arm simulation. RK4 only approximates that conservation.
Positive d removes energy whenever the joints move. Constant shoulder torque 1 N m with zero elbow torque does work W = q₁(t) − q₁(0) joules. The gravity-compensated moving arm receives actuator work as it gains height, even though its kinetic energy stays constant.
The readouts integrate actuator power and damping loss at the same RK4 stages. The energy balance residual is E(t) − E(0) − W + loss. Compare this residual with the energies involved; a display rounded to zero does not establish exact conservation.
Repeat the simulation with a smaller step
Repeat the same release with h = 0.005 s. At 0.5 s, the shoulder velocity changes from −2.414309 to −2.414308 rad/s, and the elbow velocity changes from −3.534794 to −3.534796 rad/s. The six-decimal angles happen to stay the same.
Over the full two seconds, the largest sampled absolute energy drift is about 0.000145388 J with h = 0.01 and 0.000011084 J with h = 0.005. This decrease supports the smaller-step result. A further h = 0.0005 comparison also reduces the differences in the joint trajectory.
Energy agreement alone cannot establish the correct path or timing. Compare q and q̇ at the same physical time, check the energy balance, and repeat with a finer step. Contact, stiff forces, and long simulations can require other integration methods; this smooth two-second model does not test those cases.
Reproduce the motion in Python
This standard-library example implements the same rod model and torque policies. It uses two-row elimination for the mass solve and RK4 for the four mechanical states. The drift measurements below compare sampled energy with the initial energy of the unpowered release.
from math import cos, sin
def terms(s, gravity=9.81):
a, b, u, v = s
m11 = 5 / 3 + cos(b)
m12 = 1 / 3 + 0.5 * cos(b)
m22 = 1 / 3
c = (-0.5 * sin(b) * (2 * u * v + v * v),
0.5 * sin(b) * u * u)
g = (gravity * (1.5 * cos(a) + 0.5 * cos(a + b)),
gravity * 0.5 * cos(a + b))
energy = 0.5 * (m11 * u * u + 2 * m12 * u * v + m22 * v * v)
energy += gravity * (1.5 * sin(a) + 0.5 * sin(a + b))
return m11, m12, m22, c, g, energy
def simulate(h, duration, initial=(0, 0, 0, 0), policy="zero",
torque=(1, 0), gravity=9.81, damping=0):
s = list(initial)
frozen = terms(s, gravity)[4]
initial_energy = terms(s, gravity)[5]
max_drift = 0.0
def slope(at):
m11, m12, m22, c, g, _ = terms(at, gravity)
applied = {"zero": (0, 0), "constant": torque,
"gravity": g, "frozen": frozen}[policy]
r1 = applied[0] - c[0] - g[0] - damping * at[2]
r2 = applied[1] - c[1] - g[1] - damping * at[3]
elbow = (r2 - m12 * r1 / m11) / (m22 - m12 * m12 / m11)
shoulder = (r1 - m12 * elbow) / m11
return [at[2], at[3], shoulder, elbow]
def offset(at, k, scale):
return [x + scale * dx for x, dx in zip(at, k)]
steps = round(duration / h)
assert abs(steps * h - duration) < 1e-12
for _ in range(steps):
k1 = slope(s)
k2 = slope(offset(s, k1, h / 2))
k3 = slope(offset(s, k2, h / 2))
k4 = slope(offset(s, k3, h))
s = [s[i] + h * (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) / 6
for i in range(4)]
max_drift = max(max_drift, abs(terms(s, gravity)[5] - initial_energy))
return s, terms(s, gravity)[5] - initial_energy, max_drift
def pair(values):
return "(" + ", ".join(f"{x:.6f}" for x in values) + ")"
for h in (0.01, 0.005):
state, change, _ = simulate(h, 0.5)
print(f"release h={h:.3f}: q={pair(state[:2])}; "
f"qd={pair(state[2:])}; dE={change:.6f}")
_, _, drift = simulate(h, 2)
print(f"2 s maximum energy drift h={h:.3f}: {drift:.9f}")
state, change, _ = simulate(0.01, 0.5, policy="constant", gravity=0)
print(f"shoulder torque: q1={state[0]:.6f}; dE={change:.6f}")
for policy in ("gravity", "frozen"):
state, _, _ = simulate(0.01, 0.5, initial=(0, 0, 0.5, 0), policy=policy)
print(f"moving {policy}: q={pair(state[:2])}; qd={pair(state[2:])}")
Expected output:
release h=0.010: q=(-1.122654, 0.593820); qd=(-2.414309, -3.534794); dE=0.000002
2 s maximum energy drift h=0.010: 0.000145388
release h=0.005: q=(-1.122654, 0.593820); qd=(-2.414308, -3.534796); dE=0.000000
2 s maximum energy drift h=0.005: 0.000011084
shoulder torque: q1=0.202009; dE=0.202009
moving gravity: q=(0.250000, 0.000000); qd=(0.500000, 0.000000)
moving frozen: q=(0.258510, -0.011739); qd=(0.570046, -0.099129)
Try it yourself
Exercise 1. At the horizontal pose with both joints at rest, set gravity and damping to zero. Apply τ = (2, 0) N m. Find both accelerations and check them by multiplying by M.
Check the torque solve
The mass solve is linear in torque at this fixed state. Doubling the worked shoulder input gives q̈ = (24/7, −60/7), or (3.428571, −8.571429) rad/s².
The first row gives (8/3)(24/7) + (5/6)(−60/7) = 64/7 − 50/7 = 2. The second gives (5/6)(24/7) + (1/3)(−60/7) = 20/7 − 20/7 = 0. You can reproduce these initial accelerations with the constant-torque controls at replay time zero.
Exercise 2. Start at q = (0, 0) with q̇ = (0.5, 0) rad/s. Apply exact gravity compensation and no damping. Find q, kinetic energy, and actuator work at t = 1 s.
Check motion and energy
Because q₂ = 0 throughout this motion, c = 0. The angles reach (0.5, 0) rad and the velocities remain (0.5, 0) rad/s. Kinetic energy stays ½ × (8/3) × 0.5² = 1/3 J.
Potential energy rises from zero to 19.62 sin(0.5) = 9.406329 J. The actuator work equals this increase in potential energy. Gravity compensation keeps the modeled gravity from changing the motion while its actuators supply that energy.
Sources and further study
- Kevin Lynch and Frank Park, Modern Robotics: Forward Dynamics of Open Chains, for the acceleration solve, simulation loop, and falling-arm energy check.
- Russ Tedrake, Underactuated Robotics: Multi-Body Dynamics, for the manipulator equation and mass-matrix structure. Its example uses different mass and angle conventions from the uniform rods here.
- Tobin Driscoll and Richard Braun, Fundamentals of Numerical Computation: Runge–Kutta Methods, for the classical four-stage integrator and step-size accuracy.
Continue with numerical integration to compare energy drift and trajectory timing on a system with a known exact solution.