Oliver Natt: Physik mit Python.

Lösung zu Aufgabe 8.2

Zwangsbedingungen/Loesungen/dreifachpendel-Anfangsbedingungen.py
"""Simultane Simulation von zwei Dreifachpendeln mit leicht
unterschiedlichen Anfangsbedingungen. """

import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
import matplotlib.animation
import scipy.integrate

# Raumdimension dim, Anzahl der Teilchen N und
# Anzahl der Zwangsbedingungen R.
dim = 2
N = 3
R = 3

# Massen der Pendelkörpers [kg].
m1 = 1.0
m2 = 1.0
m3 = 1.0

# Länge der Pendelstangen [m].
l1 = 0.6
l2 = 0.3
l3 = 0.15

# Simulationsdauer T und Zeitschrittweite dt [s].
T = 10
dt = 0.002

# Anfangsauslenkungen für die beiden Simulationsläufe [rad].
phi1 = np.radians(np.array([130.0, 130.001]))
phi2 = np.radians(np.array([0.0, 0.0]))
phi3 = np.radians(np.array([0.0, 0.0]))

# Array der Anfangspositionen [m].
r01 = l1 * np.array([np.sin(phi1), -np.cos(phi1)])
r02 = r01 + l2 * np.array([np.sin(phi2), -np.cos(phi2)])
r03 = r02 + l3 * np.array([np.sin(phi3), -np.cos(phi3)])

# Array mit den Komponenten der Anfangspositionen [m].
r0 = np.concatenate((r01, r02, r03))

# Array mit den Komponenten der Anfangsgeschwindigkeit [m/s].
v0 = np.zeros((N * dim, 2))

# Array der Masse für jede Komponente [kg].
m = np.array([m1, m1, m2, m2, m3, m3])

# Betrag der Erdbeschleunigung [m/s²].
g = 9.81

# Parameter für die Baumgarte-Stabilisierung [1/s].
alpha = 10.0
beta = alpha


def h(r):
    """Zwangsbedingungen h_a(r). """
    r = r.reshape(N, dim)
    d1 = r[0]
    d2 = r[1] - r[0]
    d3 = r[2] - r[1]
    return np.array([d1 @ d1 - l1 ** 2,
                     d2 @ d2 - l2 ** 2,
                     d3 @ d3 - l3 ** 2])


def grad_h(r):
    """Gradient g der Zwangsbedingungen.
        g[a, i] =  dh_a / dx_i """
    r = r.reshape(N, dim)
    g = np.zeros((R, N, dim))

    # Erste Zwangsbedingung.
    g[0, 0] = 2 * r[0]

    # Zweite Zwangsbedingung.
    g[1, 0] = 2 * (r[0] - r[1])
    g[1, 1] = 2 * (r[1] - r[0])

    # Dritte Zwangsbedingung.
    g[2, 1] = 2 * (r[1] - r[2])
    g[2, 2] = 2 * (r[2] - r[1])

    return g.reshape(R, N * dim)


def hesse_h(r):
    """Hesse-Matrix H der Zwangsbedingungen.
        H[a, i, j] =  d²h_a / (dx_i dx_j) """
    h = np.zeros((R, N, dim, N, dim))

    # Erstelle eine dim x dim - Einheitsmatrix.
    E = np.eye(dim)

    # Erste Zwangsbedingung.
    h[0, 0, :, 0, :] = 2 * E

    # Zweite Zwangsbedingung.
    h[1, 0, :, 0, :] = 2 * E
    h[1, 0, :, 1, :] = -2 * E
    h[1, 1, :, 0, :] = -2 * E
    h[1, 1, :, 1, :] = 2 * E

    # Dritte Zwangsbedingung.
    h[2, 1, :, 1, :] = 2 * E
    h[2, 1, :, 2, :] = -2 * E
    h[2, 2, :, 1, :] = -2 * E
    h[2, 2, :, 2, :] = 2 * E

    return h.reshape(R, N * dim, N * dim)


def dgl(t, u):
    r, v = np.split(u, 2)

    # Lege die externe Kraft fest.
    F_ext = m * np.array([0, -g, 0, -g, 0, -g])

    # Stelle die Gleichungen für die lambdas auf.
    grad = grad_h(r)
    hesse = hesse_h(r)
    F = - v @ hesse @ v - grad @ (F_ext / m)
    F += - 2 * alpha * grad @ v - beta ** 2 * h(r)
    G = (grad / m) @ grad.T

    # Berechne die lambdas.
    lam = np.linalg.solve(G, F)

    # Berechne die Beschleunigung mithilfe der newtonschen
    # Bewegungsgleichung inkl. Zwangskräften.
    a = (F_ext + lam @ grad) / m

    return np.concatenate([v, a])


# Lege die Zustandsvektoren zum Zeitpunkt t=0 fest.
u0array = np.concatenate((r0, v0))

# Erzeuge Arrays für die Ergebnisse für beide Simulationsläufe
t = np.arange(0, T, dt)
r = np.zeros((u0array.shape[1], N, dim, t.size))
v = np.zeros((u0array.shape[1], N, dim, t.size))

# Führe für jeden Satz von Anfangsbedingungen eine Simulation
# durch.
for i, u0 in enumerate(u0array.T):
    result = scipy.integrate.solve_ivp(dgl, [0, T], u0,
                                       rtol=1e-6,
                                       t_eval=t)
    t = result.t
    r[i], v[i] = np.reshape(result.y, (2, N, dim, -1))


# Erzeuge eine Figure und eine Axes.
fig = plt.figure(figsize=(7, 7))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_xlim([-1.1, 1.1])
ax.set_ylim([-1.1, 0.4])
ax.set_aspect('equal')
ax.grid()

# Erzeuge je einen Punktplot, für die Position des Massen.
p1, = ax.plot([0], [0], 'bo', markersize=8, zorder=5)
p2, = ax.plot([0], [0], 'ro', markersize=8, zorder=5)
p3, = ax.plot([0], [0], 'go', markersize=8, zorder=5)

# Erzeuge je einen Linienplot für die Darstellung der
# Pendelstangen.
line1, = ax.plot([0, 0], [0, 0], 'k-', zorder=4)
line2, = ax.plot([0, 0], [0, 0], 'k-', zorder=4)

# Erzeuge ein Textfeld für die Angabe der Zeit.
text = ax.text(-1.0, 0.3, '')


def update(n):
    # Aktualisiere die Position der Pendelkörper.
    p1.set_data(r[:, 0, :, n].T)
    p2.set_data(r[:, 1, :, n].T)
    p3.set_data(r[:, 2, :, n].T)

    # Aktualisiere die Position der Pendelstangen der ersten
    # Simulation.
    points = np.zeros((2, 4))
    points[:, 1:] = r[0, :, :, n].T
    line1.set_data(points)

    # Aktualisiere die Position der Pendelstangen der zweiten
    # Simulation.
    points = np.zeros((2, 4))
    points[:, 1:] = r[1, :, :, n].T
    line2.set_data(points)

    # Aktualisiere den Text des Textfeldes.
    text.set_text(f't = {t[n]:5.2f}')

    return p1, p2, p3, line1, line2, text


# Erzeuge das Animationsobjekt und starte die Animation.
ani = mpl.animation.FuncAnimation(fig, update, frames=t.size,
                                  interval=30, blit=True)
plt.show()
Aufgaben

Lösung zu Aufgabe 8.2
