Constrained Double Pendulum

This notebook models a double pendulum as two indipendent rods joined by a hinge treated as constraint. We use the same logic as in the getting started notebook.

# Project and symbolic tools
import heyoka as hy
import quivira as qv

# Numerics and visualization
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from IPython.display import HTML

Matplotlib is building the font cache; this may take a moment.

Generalized Coordinates

For each rod we use its center position and orientation:

\[q = (x_0, y_0, \theta_0, x_1, y_1, \theta_1).\]

# Generalized coordinates: center positions and orientations of the two rods.
x0, y0, theta0, x1, y1, theta1 = hy.make_vars("x0", "y0", "theta0", "x1", "y1", "theta1")
dx0, dy0, dtheta0, dx1, dy1, dtheta1 = hy.make_vars("dx0", "dy0", "dtheta0", "dx1", "dy1", "dtheta1")

q = [x0, y0, theta0, x1, y1, theta1]
qd = [dx0, dy0, dtheta0, dx1, dy1, dtheta1]

Kinetic Energy

We model each rod as uniform, so its moment of inertia about its center is \(mL^2/12\). The kinetic energy includes center translation and rod rotation:

\[T = \frac{1}{2} m_0 (\dot x_0^2 + \dot y_0^2) + \frac{1}{24} m_0 L_0^2 \dot \theta_0^2 + \frac{1}{2} m_1 (\dot x_1^2 + \dot y_1^2) + \frac{1}{24} m_1 L_1^2 \dot \theta_1^2.\]

L0 = 1.0
L1 = 1.0
m0 = 1.0
m1 = 1.0
g = 9.81

# Kinetic and potential energies for the two rigid rods. Each rod contributes
# center-of-mass translational energy and rotational energy about its center.
T = 0.5 * m0 * (qd[0] ** 2 + qd[1] ** 2)
T += 0.5 * m0 * (L0 ** 2) * (qd[2] ** 2) / 12
T += 0.5 * m1 * (qd[3] ** 2 + qd[4] ** 2)
T += 0.5 * m1 * (L1 ** 2) * (qd[5] ** 2) / 12

Potential Energy

With gravity acting in the negative \(y\) direction, the potential is

\[V = m_0 g y_0 + m_1 g y_1\]

V = m0 * g * q[1] + m1 * g * q[4]

Lagrangian

L = T - V

Holonomic constraints.

The first rod is pinned at the origin, so its upper endpoint stays fixed. The second rod starts at the lower endpoint of the first. These connections give four holonomic constraints:

\[\begin{split}\begin{aligned} F_1 &= x_0 - \frac{L_0}{2}\cos\theta_0 = 0, \\ F_2 &= y_0 - \frac{L_0}{2}\sin\theta_0 = 0, \\ F_3 &= x_1 - x_0 - \frac{L_0}{2}\cos\theta_0 - \frac{L_1}{2}\cos\theta_1 = 0, \\ F_4 &= y_1 - y_0 - \frac{L_0}{2}\sin\theta_0 - \frac{L_1}{2}\sin\theta_1 = 0. \end{aligned}\end{split}\]

# Top attachment: the first rod starts at the origin and is anchored there.
# The second rod connects to the end of the first rod.
F = np.asarray([
    q[0] - 0.5 * L0 * hy.cos(q[2]),
    q[1] - 0.5 * L0 * hy.sin(q[2]),
    q[3] - q[0] - 0.5 * L0 * hy.cos(q[2]) - 0.5 * L1 * hy.cos(q[5]),
    q[4] - q[1] - 0.5 * L0 * hy.sin(q[2]) - 0.5 * L1 * hy.sin(q[5]),
], dtype=object)

EOM

We assemble the equations of motion for the bead using Quivira amazing capabilities.

lagrange_eom returns the first-order ODE system and, when requested, the Lagrange multipliers. The multipliers follow the constraint order and represent the joint reactions.

ode_system, lambda_expr = qv.lagrange_eom(L, F, q, qd, return_multipliers=True)

Taylor Integrator

We can now use the assembled equations of motion to instantiate heyoka Taylor adaptive integrator

ta = hy.taylor_adaptive(ode_system)

Initial conditions

We release the rods from rest at these angles:

\[\theta_0 = 1.1, \qquad \theta_1 = -1.5, \qquad \dot q = 0.\]

The rod centers are calculated from the constraints, so the initial state starts on the allowed geometry.

# Start from a sideways double-pendulum configuration with zero initial speeds.
theta0 = 1.1
theta1 = -1.5

x0 = 0.5 * L0 * np.cos(theta0)
y0 = 0.5 * L0 * np.sin(theta0)
x1 = x0 + 0.5 * L0 * np.cos(theta0) + 0.5 * L1 * np.cos(theta1)
y1 = y0 + 0.5 * L0 * np.sin(theta0) + 0.5 * L1 * np.sin(theta1)

state0 = [x0, y0, theta0, x1, y1, theta1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Simulation

We set the integrator to the constraint-consistent initial state and sample the motion at 401 times from 0 to 8. The final item returned by propagate_grid is the array of sampled states.

ta.time = 0.0
ta.state[:] = state0

t_end = 8.
tgrid = np.linspace(0.0, t_end, 401)
solution = ta.propagate_grid(tgrid)[-1]

Animate the motion.

# Draw each rod as a segment centered at its coordinates.
fig, ax = plt.subplots(figsize=(6, 6))
ax.set_aspect('equal')
ax.grid(True)
ax.set_xlabel('x')
ax.set_ylabel('y')
frame_extent = L0 + L1 + 0.25
ax.set_xlim(-frame_extent, frame_extent)
ax.set_ylim(-frame_extent, frame_extent)

rod0_line, = ax.plot([], [], 'o-', lw=3, color='tab:blue')
rod1_line, = ax.plot([], [], 'o-', lw=3, color='tab:orange')
trail, = ax.plot([], [], '-', lw=1, alpha=0.6, color='black')

# Find rod endpoints from the center, angle, and length.
def rod_points(state, rod_length):
    x, y, theta = state[0], state[1], state[2]
    u = np.array([np.cos(theta), np.sin(theta)])
    start = np.array([x, y]) - 0.5 * rod_length * u
    end = np.array([x, y]) + 0.5 * rod_length * u
    return start, end

# Reset the plotted objects before the animation starts.
def init_anim():
    rod0_line.set_data([], [])
    rod1_line.set_data([], [])
    trail.set_data([], [])
    return rod0_line, rod1_line, trail

# Move the rods and extend the trail at each frame.
def update_anim(frame):
    state = solution[frame]
    x0, y0, theta0, x1, y1, theta1 = state[:6]

    start0, end0 = rod_points(np.array([x0, y0, theta0]), L0)
    start1, end1 = rod_points(np.array([x1, y1, theta1]), L1)

    rod0_line.set_data([start0[0], end0[0]], [start0[1], end0[1]])
    rod1_line.set_data([start1[0], end1[0]], [start1[1], end1[1]])

    tip_x = solution[: frame + 1, 3] + 0.5 * L1 * np.cos(solution[: frame + 1, 5])
    tip_y = solution[: frame + 1, 4] + 0.5 * L1 * np.sin(solution[: frame + 1, 5])
    trail.set_data(tip_x, tip_y)

    return rod0_line, rod1_line, trail

anim = FuncAnimation(
    fig,
    update_anim,
    frames=len(solution),
    interval=20,
    blit=False,
    init_func=init_anim,
)

display(HTML(anim.to_jshtml()))
plt.close(fig)

Once Loop Reflect

Analysis

Check the geometric constraints.

At every saved time, evaluate the four constraint functions. An exact constrained trajectory satisfies \(\bm F(\bm q(t))=\bm 0\). The plot shows the absolute residuals on a logarithmic scale; each residual measures a positional error, so it has units of length.

# Evaluate the position-constraint residuals at every sampled state.
constraint_func = hy.cfunc(list(F), vars=q)
constraint_inputs = np.ascontiguousarray(solution[:, :6].T)
constraint_values = constraint_func(constraint_inputs).T

fig, ax = plt.subplots(figsize=(8, 4))
for index in range(constraint_values.shape[1]):
    ax.semilogy(
        tgrid,
        np.maximum(np.abs(constraint_values[:, index]), np.finfo(float).tiny),
        label=f"$|F_{index + 1}|$",
    )

ax.set_xlabel("Time")
ax.set_ylabel("Absolute constraint residual")
ax.set_title("Double-pendulum constraint drift")
ax.grid(True, which="both")
ax.legend()
plt.ylim(1e-20, 1e-6)
plt.show()

Analyze the joint reactions.

The multipliers give the reaction components for each constraint. The top pin applies \((\lambda_1, \lambda_2)\) to rod 0. At the hinge, \((\lambda_3, \lambda_4)\) acts on rod 1 and the opposite force acts on rod 0. The plots show reaction magnitudes, not stresses inside the rods; this rigid-rod model does not calculate internal stress.

# Evaluate the four multipliers at every saved state.
lambda_func = hy.cfunc(list(lambda_expr), vars=q + qd)
lambda_inputs = np.ascontiguousarray(solution.T)
lambda_values = lambda_func(lambda_inputs).T

# Constraint order: top pin (x, y), then the hinge (x, y).
# The hinge reactions on the two rods are equal and opposite.
bar0_top_reaction = lambda_values[:, :2]
bar0_hinge_reaction = -lambda_values[:, 2:4]
bar1_hinge_reaction = lambda_values[:, 2:4]

bar0_top_magnitude = np.linalg.norm(bar0_top_reaction, axis=1)
bar0_hinge_magnitude = np.linalg.norm(bar0_hinge_reaction, axis=1)
bar1_hinge_magnitude = np.linalg.norm(bar1_hinge_reaction, axis=1)

fig, axes = plt.subplots(1, 2, figsize=(11, 4), sharey=True)

axes[0].plot(tgrid, bar0_top_magnitude, label="Top pin")
axes[0].plot(tgrid, bar0_hinge_magnitude, label="Hinge")
axes[0].set_title("Rod 0 joint reactions")

axes[1].plot(tgrid, bar1_hinge_magnitude, label="Hinge")
axes[1].set_title("Rod 1 joint reactions")

for ax in axes:
    ax.set_xlabel("Time")
    ax.set_ylabel("Reaction-force magnitude")
    ax.grid(True)
    ax.legend()

plt.tight_layout()
plt.show()