multi-link pendulum - python simulation and animation of a multi-link pendulum

This jupyter notebook uses Lagrange formalism to derive the dynamics of a multi-link pendulum, simulates it and shows an animation of resulting solution. Unfortunately, it seems to be not very time-performant, so the best I could do in a reasonable amount of time was 6 links. That is why the dynamic equations exported earlier are simulated in rust here and the results are re-imported to python to be shown and animated.

[1] 1994 - Murray, Li, Sastry: A Mathematical Introduction to Robotic Manipulation (https://www.ce.cit.tum.de/fileadmin/w00cgn/rm/pdf/murray-li-sastry-94-complete.pdf)

Notes:

• indexing starts with zero
• angular positions and velocities are defined absolutely (in general joint based measurements of a link orientation is relative to the previous link)

(Benjamin Jahn, TU Ilmenau, 2025)

Initilization

initialization: clear workspace, define latex use for display

# clear workspace
%reset -f

# latex math display
from IPython.display import Math, Latex

def latexDisplay(latex_obj): display(Math(latex_obj))
# def latexDisplay(latex_obj): display(Latex(r'$$' + (latex_obj) + r'$$'))

Run Rust for Solution

! cargo run -q 2>/dev/null

########## Robotic Equation Solver ##########
Time elapsed for numerical integration is: 10.324547584s

Import Solution

class Solution(object):
    def __init__(self,**kwargs):
        self.__dict__.update(kwargs)

sol = Solution(x=None, y = None)

# read csv
import pandas
import numpy as np
df = pandas.read_csv('multi_link_pendulum_solution.csv', sep=',', header=None)
N = df.shape[1] - 1

sol.t = df.iloc[:, 0].values
tmp = df.iloc[:, 1:N+1].values
sol.y = np.asarray(np.transpose(np.array(tmp)))

# implicte knowledge about the link lengths
l_num = np.linspace(0.5, 0.35, N) # length of links [m]

Plots

# plot imports
import plotly.graph_objs as go
import plotly.io as pio
pio.templates.default = "none"
pio.renderers.default = "notebook_connected"

# define generic plot function for angular position of triple pendulum
from colour import Color

def plot_pendulum(sol):
    start_color = '#0000ff'; end_color = '#00ff80'
    colorscale = [x.hex for x in list(Color(start_color).range_to(Color(end_color), N))]

    fig = go.Figure()
    for idx in range(0, N):
        fig.add_trace(go.Scatter(x=sol.t, y=sol.y[idx, :] / np.pi, name=r"$q_{" + str(idx) + "}$", 
            line=dict(width=2, color=colorscale[idx])))

    fig.update_layout(height=600, width=900, margin=go.layout.Margin(t=20), 
        xaxis=dict(title=r"$\text{time in seconds}$", mirror=True, ticks='outside', showline=True),
        yaxis=dict(title=r"$\text{angular position in } \pi \text{ rad}$", 
            tickmode="linear", tick0=0.0, dtick=1, mirror=True, ticks='outside', showline=True))

    fig.show()

# plot angular positions
plot_pendulum(sol)

Animation

## import matplotlib for animations
import matplotlib
import matplotlib.pyplot as plt
from matplotlib import animation, rc, colormaps
matplotlib.rcParams['animation.embed_limit'] = 2**128
from math import *
rc('animation', html='jshtml')

# define pendulum animation function
def animate_pendulum(sol, fps=30, colormap='winter'):

    # colors = cm.get_cmap(colormap, N)
    colors = colormaps[colormap].resampled(N)

    # frame number and index distance for given fps (tried to avoid interpolation to not slow down animation even further)
    T2anim = sol.t[-1]
    frameNumber = fps*T2anim
    didx = floor(len(sol.t)/frameNumber)

    # create and setup figure
    px = 1/plt.rcParams['figure.dpi']  # pixel in inches
    fig, ax = plt.subplots(figsize=(900*px,540*px))
    ax.set_aspect('equal')
    # plt.axis('equal'); 
    plt.close()
    l_sum = sum(l_num)
    ax.set_xlim((-l_sum*1.65, l_sum*1.65)); ax.set_ylim((-l_sum*1.35, l_sum*1.35))

    # create line objects
    line_base, = ax.plot([], [], lw=3, c=(0, 0, 0))
    lines = [None]*N
    for idx in range(0,N):
        # lines[idx], = ax.plot([], [], lw=4, c=colors(idx/N))
        lines[idx], = ax.plot([], [], lw=4, c=colors(idx/N), solid_capstyle='round')

    # initialization function: plot the background of each frame
    def init():
        # draw rail
        line_base.set_data(np.linspace(-l_sum*1.65, l_sum*1.65, 10), 0*np.linspace(-2, 2, 10)) 
        
        return (line_base,*lines, ) # return line objects

    # animation function: this is called sequentially
    def animate(i):

        # current configuration of pendulum
        p = np.zeros((2,N+1))
        for idx in range(1,N+1):
            p[:,idx] = p[:,idx-1] + np.array([-l_num[idx-1]*sin(sol.y[idx-1,i*didx]), l_num[idx-1]*cos(sol.y[idx-1,i*didx])])

        for idx in range(0,N):
            lines[idx].set_data([p[0,idx], p[0, idx+1]],[p[1,idx], p[1, idx+1]])

        return (*lines,) # return line objects

    # return animation object
    return animation.FuncAnimation(fig, animate, init_func=init, frames=floor(frameNumber), interval=1/fps*1000, blit=True)

anim = animate_pendulum(sol)
anim 

Once Loop Reflect