Simulation and Visualization

Note

You can download this example as a Python script: simulation.py or Jupyter Notebook: simulation.ipynb.

import sympy as sm
import sympy.physics.mechanics as me
me.init_vprinting(use_latex='mathjax')

Learning Objectives

After completing this chapter readers will be able to:

  • evaluate equations of motion numerically

  • numerically integrate the ordinary differential equations of a multibody system

  • plot the system’s state trajectories versus time

  • compare integration methods to observe integration error

  • create a simple animation of the motion of the multibody system

Numerical Integration

As mentioned at the end of the prior chapter, we will need to numerically integrate the equations of motion. If they are in explicit form, this integral describes how we can arrive at trajectories in time for the state variables by integrating with respect to time from an initial time \(t_0\) to a final time \(t_f\). Recall that the time derivative of the state \(\bar{x}\) is:

(197)\[\dot{\bar{x}}(t) = \bar{f}_m(\bar{x}, t) = -\mathbf{M}_m^{-1}\bar{g}_m\]

We can then find \(\bar{x}\) by integration with respect to time:

(198)\[\bar{x}(t) = \int^{t_f}_{t_0} \bar{f}_m(\bar{x}, t) dt\]

It is possible to form \(-\mathbf{M}_m^{-1}\bar{g}_m\) symbolically and it may be suitable or preferable for a given problem, but there are some possible drawbacks. For example, if the degrees of freedom are quite large, the resulting symbolic equations become exponentially more complex. Thus, it is generally best to move from symbolics to numerics before formulating the explicit ordinary differential equations.

Numerical Evaluation

The NumPy library is the de facto base library for numeric computing with Python. NumPy allows us to do array programming with Python by providing floating point array data types and vectorized operators to enable repeat operations across arrays of values. In Sec. Evaluating Symbolic Expressions we introduced the SymPy function lambdify(). lambdify() will be our way to bridge the symbolic world of SymPy with the numeric world of NumPy.

We will import NumPy like so, by convention:

import numpy as np

Warning

Beware that mixing SymPy and NumPy data types will rarely, if at all, provide you with functioning code. Be careful because sometimes it may look like the two libraries mix. For example, you can do this:

a, b, c, d = sm.symbols('a, b, c, d')

mat = np.array([[a, b], [c, d]])
mat

array([[a, b],
       [c, d]], dtype=object)

which gives a NumPy array containing SymPy symbols. But this will almost certainly cause you problems as you move forward. The process you should always follow for the purposes of this text is:

sym_mat = sm.Matrix([[a, b], [c, d]])
eval_sym_mat = sm.lambdify((a, b, c, d), sym_mat)
num_mat = eval_sym_mat(1.0, 2.0, 3.0, 4.0)
num_mat

array([[1., 2.],
       [3., 4.]])

Also, be careful because NumPy and SymPy have many functions that are named the same and you likley don’t want to mix them up:

np.cos(5) + sm.cos(5)
\[\displaystyle 0.283662185463226 + \cos{\left(5 \right)}\]

We import NumPy as np and SymPy as sm to ensure functions with the same names can coexist.

Returning to the example of the two rods and the sliding mass from the previous chapter, we regenerate the symbolic equations of motion and stop when we have \(\bar{q}\), \(\bar{u}\), \(\mathbf{M}_k\), \(\bar{g}_k\), \(\mathbf{M}_d\), and \(\bar{g}_d\). The following drop down has the SymPy code to generate these symbolic vectors and matrices take from the prior chapter.

Symbolic Setup Code

m, g, kt, kl, l = sm.symbols('m, g, k_t, k_l, l')
q1, q2, q3 = me.dynamicsymbols('q1, q2, q3')
u1, u2, u3 = me.dynamicsymbols('u1, u2, u3')

N = me.ReferenceFrame('N')
A = me.ReferenceFrame('A')
B = me.ReferenceFrame('B')

A.orient_axis(N, q1, N.z)
B.orient_axis(A, q2, A.x)

A.set_ang_vel(N, u1*N.z)
B.set_ang_vel(A, u2*A.x)

O = me.Point('O')
Ao = me.Point('A_O')
Bo = me.Point('B_O')
Q = me.Point('Q')

Ao.set_pos(O, l/2*A.x)
Bo.set_pos(O, l*A.x)
Q.set_pos(Bo, q3*B.y)

O.set_vel(N, 0)
Ao.v2pt_theory(O, N, A)
Bo.v2pt_theory(O, N, A)
Q.set_vel(B, u3*B.y)
Q.v1pt_theory(Bo, N, B)

t = me.dynamicsymbols._t

qdot_repl = {q1.diff(t): u1,
             q2.diff(t): u2,
             q3.diff(t): u3}

Q.set_acc(N, Q.acc(N).xreplace(qdot_repl))

R_Ao = m*g*N.x
R_Bo = m*g*N.x + kl*q3*B.y
R_Q = m/4*g*N.x - kl*q3*B.y
T_A = -kt*q1*N.z + kt*q2*A.x
T_B = -kt*q2*A.x

I = m*l**2/12
I_A_Ao = I*me.outer(A.y, A.y) + I*me.outer(A.z, A.z)
I_B_Bo = I*me.outer(B.x, B.x) + I*me.outer(B.z, B.z)

points = [Ao, Bo, Q]
forces = [R_Ao, R_Bo, R_Q]
masses = [m, m, m/4]

frames = [A, B]
torques = [T_A, T_B]
inertias = [I_A_Ao, I_B_Bo]

Fr_bar = []
Frs_bar = []

for ur in [u1, u2, u3]:

    Fr = 0
    Frs = 0

    for Pi, Ri, mi in zip(points, forces, masses):
        vr = Pi.vel(N).diff(ur, N)
        Fr += vr.dot(Ri)
        Rs = -mi*Pi.acc(N)
        Frs += vr.dot(Rs)

    for Bi, Ti, Ii in zip(frames, torques, inertias):
        wr = Bi.ang_vel_in(N).diff(ur, N)
        Fr += wr.dot(Ti)
        Ts = -(Bi.ang_acc_in(N).dot(Ii) +
               me.cross(Bi.ang_vel_in(N), Ii).dot(Bi.ang_vel_in(N)))
        Frs += wr.dot(Ts)

    Fr_bar.append(Fr)
    Frs_bar.append(Frs)

Fr = sm.Matrix(Fr_bar)
Frs = sm.Matrix(Frs_bar)

q = sm.Matrix([q1, q2, q3])
u = sm.Matrix([u1, u2, u3])

qd = q.diff(t)
ud = u.diff(t)

ud_zerod = {udr: 0 for udr in ud}

Mk = -sm.eye(3)
gk = u

Md = Frs.jacobian(ud)
gd = Frs.xreplace(ud_zerod) + Fr

q, u, qd, ud
\[\begin{split}\displaystyle \left( \left[\begin{matrix}q_{1}\\q_{2}\\q_{3}\end{matrix}\right], \ \left[\begin{matrix}u_{1}\\u_{2}\\u_{3}\end{matrix}\right], \ \left[\begin{matrix}\dot{q}_{1}\\\dot{q}_{2}\\\dot{q}_{3}\end{matrix}\right], \ \left[\begin{matrix}\dot{u}_{1}\\\dot{u}_{2}\\\dot{u}_{3}\end{matrix}\right]\right)\end{split}\]
Mk, gk
\[\begin{split}\displaystyle \left( \left[\begin{matrix}-1 & 0 & 0\\0 & -1 & 0\\0 & 0 & -1\end{matrix}\right], \ \left[\begin{matrix}u_{1}\\u_{2}\\u_{3}\end{matrix}\right]\right)\end{split}\]
Md, gd
\[\begin{split}\displaystyle \left( \left[\begin{matrix}- \frac{l^{2} m \cos^{2}{\left(q_{2} \right)}}{12} - \frac{19 l^{2} m}{12} - \frac{m q_{3}^{2} \cos^{2}{\left(q_{2} \right)}}{4} & \frac{l m q_{3} \sin{\left(q_{2} \right)}}{4} & - \frac{l m \cos{\left(q_{2} \right)}}{4}\\\frac{l m q_{3} \sin{\left(q_{2} \right)}}{4} & - \frac{l^{2} m}{12} - \frac{m q_{3}^{2}}{4} & 0\\- \frac{l m \cos{\left(q_{2} \right)}}{4} & 0 & - \frac{m}{4}\end{matrix}\right], \ \left[\begin{matrix}- \frac{7 g l m \sin{\left(q_{1} \right)}}{4} - \frac{g m q_{3} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}}{4} - k_{t} q_{1} + \frac{l^{2} m u_{1} u_{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)}}{6} - \frac{l m q_{3} u_{1}^{2} \cos{\left(q_{2} \right)}}{4} + l \left(- \frac{m \left(- q_{3} u_{1}^{2} \cos^{2}{\left(q_{2} \right)} - q_{3} u_{2}^{2}\right) \cos{\left(q_{2} \right)}}{4} + \frac{m \left(q_{3} u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)} + 2 u_{2} u_{3}\right) \sin{\left(q_{2} \right)}}{4}\right) + \frac{m \left(2 q_{3} u_{1} u_{2} \sin{\left(q_{2} \right)} - 2 u_{1} u_{3} \cos{\left(q_{2} \right)}\right) q_{3} \cos{\left(q_{2} \right)}}{4}\\\frac{g m q_{3} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)}}{4} - k_{t} q_{2} - \frac{l^{2} m u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)}}{12} - \frac{m \left(q_{3} u_{1}^{2} \sin{\left(q_{2} \right)} \cos{\left(q_{2} \right)} + 2 u_{2} u_{3}\right) q_{3}}{4}\\- \frac{g m \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)}}{4} - k_{l} q_{3} - \frac{m \left(- q_{3} u_{1}^{2} \cos^{2}{\left(q_{2} \right)} - q_{3} u_{2}^{2}\right)}{4}\end{matrix}\right]\right)\end{split}\]

Additionally, we will define a column vector \(\bar{p}\) that contains all of the constant parameters in the equations of motion. We should know these from our problem definition but they can also be found using free_symbols():

Mk.free_symbols | gk.free_symbols | Md.free_symbols | gd.free_symbols
\[\displaystyle \left\{g, k_{l}, k_{t}, l, m, t\right\}\]

The | operator applies the union of Python sets, which is the data type that free_symbols returns. \(t\) is not a constant parameter, but the rest are. We can then define the symbolic \(p\) as:

p = sm.Matrix([g, kl, kt, l, m])
p
\[\begin{split}\displaystyle \left[\begin{matrix}g\\k_{l}\\k_{t}\\l\\m\end{matrix}\right]\end{split}\]

Now we will create a function to evaluate \(\mathbf{M}_k\), \(\bar{g}_k\), \(\mathbf{M}_d\), and \(\bar{g}_d\). given \(\bar{q}\), \(\bar{u}\) and \(\bar{p}\).

eval_eom = sm.lambdify((q, u, p), [Mk, gk, Md, gd])

To test out the function eval_eom() we need some NumPy 1D arrays for \(\bar{q}\), \(\bar{u}\) and \(\bar{p}\).

Warning

Make sure to use consistent units when you introduce numbers! I recommend always using \(\textrm{force}=\textrm{mass}\times\textrm{acceleration}\rightarrow N=kg \ m \cdot s^{-2}\) and \(\textrm{torque}=\textrm{inertia} \times \textrm{angular acceleration}\rightarrow N \ m = kg \ m^2 \cdot rad \ s^{-2}\).

The deg2rad() and rad2deg() are helpful for angle conversions. All SymPy and NumPy trigonometric functions operate on radians, so you’ll have to convert if you prefer thinking in degrees. My recommendation is to only use degrees when displaying the outputs, so keep any calls to these two functions at the input and output of your whole computation pipeline.

Here I introduce q_vals, u_vals, and p_vals, each a 1D NumPy array. Make sure to use a different variable name than your symbols so you can distinguish the symbolic and numeric matrices and arrays.

q_vals = np.array([
    np.deg2rad(25.0),  # q1, rad
    np.deg2rad(5.0),  # q2, rad
    0.1,  # q3, m
])
q_vals, type(q_vals), q_vals.shape

(array([0.43633231, 0.08726646, 0.1       ]), numpy.ndarray, (3,))

u_vals = np.array([
    0.1,  # u1, rad/s
    2.2,  # u2, rad/s
    0.3,  # u3, m/s
])
u_vals, type(u_vals), u_vals.shape

(array([0.1, 2.2, 0.3]), numpy.ndarray, (3,))

p_vals = np.array([
    9.81,  # g, m/s**2
    2.0,  # kl, N/m
    0.01,  # kt, Nm/rad
    0.6,  # l, m
    1.0,  # m, kg
])
p_vals, type(p_vals), p_vals.shape

(array([9.81, 2.  , 0.01, 0.6 , 1.  ]), numpy.ndarray, (5,))

Now we can call eval_eom with the numeric inputs to get the numerical values of all of the equation of motion matrices and vectors:

Mk_vals, gk_vals, Md_vals, gd_vals = eval_eom(q_vals, u_vals, p_vals)
Mk_vals, gk_vals, Md_vals, gd_vals

(array([[-1,  0,  0],
        [ 0, -1,  0],
        [ 0,  0, -1]]),
 array([[0.1],
        [2.2],
        [0.3]]),
 array([[-0.60225313,  0.00130734, -0.1494292 ],
        [ 0.00130734, -0.0325    ,  0.        ],
        [-0.1494292 ,  0.        , -0.25      ]]),
 array([[-4.48963535],
        [-0.02486744],
        [-1.1112791 ]]))

Now we can solve for the state derivatives, \(\dot{\bar{q}}\) and \(\dot{\bar{u}}\), numerically using NumPy’s solve() function (not the same as SymPy’s solve()!) for linear systems of equations (\(\mathbf{A}\bar{x}=\bar{b}\) type systems).

We first numerically solve the kinematical differential equations for \(\dot{\bar{q}}\):

qd_vals = np.linalg.solve(-Mk_vals, np.squeeze(gk_vals))
qd_vals

array([0.1, 2.2, 0.3])

In this case, \(\dot{\bar{q}}=\bar{u}\) but for nontrivial generalized speed definitions that will not be so. This next linear system solve gives the accelerations \(\dot{\bar{u}}\):

ud_vals = np.linalg.solve(-Md_vals, np.squeeze(gd_vals))
ud_vals

array([-7.46056427, -1.06525862,  0.01418834])

Note

Note the use of squeeze(). This forces gk_vals and gd_vals to be a 1D array with shape(3,) instead of a 2D array of shape(3, 1). This then causes qd_vals and ud_vals to be 1D arrays instead of 2D.

np.linalg.solve(-Mk_vals, gk_vals)

array([[0.1],
       [2.2],
       [0.3]])

Simulation

To simulate the system forward in time, we solve the initial value problem of the ordinary differential equations by numerically integrating \(\bar{f}_m(t, \bar{x}, \bar{p})\). A simple way to do so, is to use Euler’s Method:

(199)\[\bar{x}_{i + 1} = \bar{x}_i + \Delta t \bar{f}_m(t_i, \bar{x}_i, \bar{p})\]

Starting with \(t_i=t_0\) and some initial values of the states \(\bar{x}_i=\bar{x}_0\), the state at \(\Delta t\) in the future is computed. We repeat this until \(t_i=t_f\) to find the trajectories of \(\bar{x}\) with respect to time.

The following function implements Euler’s Method:

def euler_integrate(rhs_func, tspan, x0_vals, p_vals, delt=0.03):
    """Returns state trajectory and corresponding values of time resulting
    from integrating the ordinary differential equations with Euler's
    Method.

    Parameters
    ==========
    rhs_func : function
       Python function that evaluates the derivative of the state and takes
       this form ``dxdt = f(t, x, p)``.
    tspan : 2-tuple of floats
       The initial time and final time values: (t0, tf).
    x0_vals : array_like, shape(2*n,)
       Values of the state x at t0.
    p_vals : array_like, shape(o,)
       Values of constant parameters.
    delt : float
       Integration time step in seconds/step.

    Returns
    =======
    ts : ndarray(m, )
       Monotonically equally spaced increasing values of time spaced apart
       by ``delt``.
    xs : ndarray(m, 2*n)
       State values at each time in ts.

    """
    # generate monotonically increasing values of time.
    duration = tspan[1] - tspan[0]
    num_samples = round(duration/delt) + 1
    ts = np.arange(tspan[0], tspan[0] + delt*num_samples, delt)

    # create an empty array to hold the state values.
    x = np.empty((len(ts), len(x0_vals)))

    # set the initial conditions to the first element.
    x[0, :] = x0_vals

    # use a for loop to sequentially calculate each new x.
    for i, ti in enumerate(ts[:-1]):
        x[i + 1, :] = x[i, :] + delt*rhs_func(ti, x[i, :], p_vals)

    return ts, x

I used arange() to generate equally spaced values between \(t_0\) and \(t_f\). Now we need a Python function that represents \(\bar{f}_m(t_i, \bar{x}_i, \bar{p})\). This function evaluates the right hand side of the explicitly ordinary differential equations which calculates the time derivatives of the state.

def eval_rhs(t, x, p):
    """Return the right hand side of the explicit ordinary differential
    equations which evaluates the time derivative of the state ``x`` at time
    ``t``.

    Parameters
    ==========
    t : float
       Time in seconds.
    x : array_like, shape(6,)
       State at time t: [q1, q2, q3, u1, u2, u3]
    p : array_like, shape(5,)
       Constant parameters: [g, kl, kt, l, m]

    Returns
    =======
    xd : ndarray, shape(6,)
        Derivative of the state with respect to time at time ``t``.

    """

    # unpack the q and u vectors from x
    q = x[:3]
    u = x[3:]

    # evaluate the equations of motion matrices with the values of q, u, p
    Mk, gk, Md, gd = eval_eom(q, u, p)

    # solve for q' and u'
    qd = np.linalg.solve(-Mk, np.squeeze(gk))
    ud = np.linalg.solve(-Md, np.squeeze(gd))

    # pack dq/dt and du/dt into a new state time derivative vector dx/dt
    xd = np.empty_like(x)
    xd[:3] = qd
    xd[3:] = ud

    return xd

With the function evaluated and numerical values already defined above we can check to see if it works. First combine \(\bar{q}\) and \(\bar{u}\) into a single column vector of the initial conditions x0 and pick an arbitrary value for time.

x0 = np.empty(6)
x0[:3] = q_vals
x0[3:] = u_vals

t0 = 0.1

Now execute the function:

eval_rhs(t0, x0, p_vals)

array([ 0.1       ,  2.2       ,  0.3       , -7.46056427, -1.06525862,
        0.01418834])

It seems to work, giving a result for the time derivative of the state vector, matching the results we had above. Now we can try out the euler_integrate() function to integration from t0 to tf:

tf = 2.0

ts, xs = euler_integrate(eval_rhs, (t0, tf), x0, p_vals)

Our euler_integrate() function returns the state trajectory and the corresponding time. They look like:

ts

array([0.1 , 0.13, 0.16, 0.19, 0.22, 0.25, 0.28, 0.31, 0.34, 0.37, 0.4 ,
       0.43, 0.46, 0.49, 0.52, 0.55, 0.58, 0.61, 0.64, 0.67, 0.7 , 0.73,
       0.76, 0.79, 0.82, 0.85, 0.88, 0.91, 0.94, 0.97, 1.  , 1.03, 1.06,
       1.09, 1.12, 1.15, 1.18, 1.21, 1.24, 1.27, 1.3 , 1.33, 1.36, 1.39,
       1.42, 1.45, 1.48, 1.51, 1.54, 1.57, 1.6 , 1.63, 1.66, 1.69, 1.72,
       1.75, 1.78, 1.81, 1.84, 1.87, 1.9 , 1.93, 1.96, 1.99])

type(ts), ts.shape

(numpy.ndarray, (64,))

xs

array([[ 4.36332313e-01,  8.72664626e-02,  1.00000000e-01,
         1.00000000e-01,  2.20000000e+00,  3.00000000e-01],
       [ 4.39332313e-01,  1.53266463e-01,  1.09000000e-01,
        -1.23816928e-01,  2.16804224e+00,  3.00425650e-01],
       [ 4.35617805e-01,  2.18307730e-01,  1.18012770e-01,
        -3.48867554e-01,  2.13303503e+00,  2.99423633e-01],
       [ 4.25151779e-01,  2.82298781e-01,  1.26995479e-01,
        -5.72475739e-01,  2.09463526e+00,  2.97361542e-01],
       [ 4.07977506e-01,  3.45137839e-01,  1.35916325e-01,
        -7.91929289e-01,  2.05197871e+00,  2.94628416e-01],
       [ 3.84219628e-01,  4.06697200e-01,  1.44755177e-01,
        -1.00444789e+00,  2.00382451e+00,  2.91554565e-01],
       [ 3.54086191e-01,  4.66811935e-01,  1.53501814e-01,
        -1.20715867e+00,  1.94877129e+00,  2.88335792e-01],
       [ 3.17871431e-01,  5.25275074e-01,  1.62151888e-01,
        -1.39708735e+00,  1.88551903e+00,  2.84974403e-01],
       [ 2.75958810e-01,  5.81840645e-01,  1.70701120e-01,
        -1.57117194e+00,  1.81314156e+00,  2.81247424e-01],
       [ 2.28823652e-01,  6.36234892e-01,  1.79138543e-01,
        -1.72630399e+00,  1.73132870e+00,  2.76708292e-01],
       [ 1.77034532e-01,  6.88174753e-01,  1.87439792e-01,
        -1.85939863e+00,  1.64055711e+00,  2.70722509e-01],
       [ 1.21252574e-01,  7.37391466e-01,  1.95561467e-01,
        -1.96749077e+00,  1.54215738e+00,  2.62531540e-01],
       [ 6.22278505e-02,  7.83656188e-01,  2.03437413e-01,
        -2.04785035e+00,  1.43826241e+00,  2.51334050e-01],
       [ 7.92340069e-04,  8.26804060e-01,  2.10977435e-01,
        -2.09810628e+00,  1.33164459e+00,  2.36370890e-01],
       [-6.21508484e-02,  8.66753398e-01,  2.18068561e-01,
        -2.11636717e+00,  1.22547077e+00,  2.17000797e-01],
       [-1.25641864e-01,  9.03517521e-01,  2.24578585e-01,
        -2.10132625e+00,  1.12301845e+00,  1.92757262e-01],
       [-1.88681651e-01,  9.37208074e-01,  2.30361303e-01,
        -2.05234005e+00,  1.02739894e+00,  1.63382255e-01],
       [-2.50251853e-01,  9.68030042e-01,  2.35262771e-01,
        -1.96947297e+00,  9.41324864e-01,  1.28837671e-01],
       [-3.09336042e-01,  9.96269788e-01,  2.39127901e-01,
        -1.85350427e+00,  8.66944026e-01,  8.92990821e-02],
       [-3.64941170e-01,  1.02227811e+00,  2.41806873e-01,
        -1.70589822e+00,  8.05745012e-01,  4.51378231e-02],
       [-4.16118116e-01,  1.04645046e+00,  2.43161008e-01,
        -1.52874283e+00,  7.58527111e-01, -3.10326579e-03],
       [-4.61980401e-01,  1.06920627e+00,  2.43067910e-01,
        -1.32466575e+00,  7.25419993e-01, -5.47370444e-02],
       [-5.01720374e-01,  1.09096887e+00,  2.41425799e-01,
        -1.09673819e+00,  7.05937072e-01, -1.08961098e-01],
       [-5.34622520e-01,  1.11214698e+00,  2.38156966e-01,
        -8.48377776e-01,  6.99048783e-01, -1.64889059e-01],
       [-5.60073853e-01,  1.13311845e+00,  2.33210294e-01,
        -5.83259685e-01,  7.03266085e-01, -2.21585204e-01],
       [-5.77571643e-01,  1.15421643e+00,  2.26562738e-01,
        -3.05242994e-01,  7.16728855e-01, -2.78102687e-01],
       [-5.86728933e-01,  1.17571830e+00,  2.18219657e-01,
        -1.83153063e-02,  7.37297421e-01, -3.33524242e-01],
       [-5.87278392e-01,  1.19783722e+00,  2.08213930e-01,
         2.73444426e-01,  7.62647870e-01, -3.87002450e-01],
       [-5.79075060e-01,  1.22071666e+00,  1.96603856e-01,
         5.65888142e-01,  7.90372680e-01, -4.37795193e-01],
       [-5.62098415e-01,  1.24442784e+00,  1.83470001e-01,
         8.54811324e-01,  8.18087698e-01, -4.85291412e-01],
       [-5.36454076e-01,  1.26897047e+00,  1.68911258e-01,
         1.13596271e+00,  8.43544507e-01, -5.29023016e-01],
       [-5.02375194e-01,  1.29427680e+00,  1.53040568e-01,
         1.40505807e+00,  8.64744480e-01, -5.68660838e-01],
       [-4.60223452e-01,  1.32021914e+00,  1.35980743e-01,
         1.65780471e+00,  8.80047808e-01, -6.03995480e-01],
       [-4.10489311e-01,  1.34662057e+00,  1.17860878e-01,
         1.88994245e+00,  8.88268730e-01, -6.34907053e-01],
       [-3.53791037e-01,  1.37326863e+00,  9.88136667e-02,
         2.09730514e+00,  8.88747420e-01, -6.61330213e-01],
       [-2.90871883e-01,  1.39993105e+00,  7.89737603e-02,
         2.27590461e+00,  8.81389870e-01, -6.83221797e-01],
       [-2.22594745e-01,  1.42637275e+00,  5.84771064e-02,
         2.42203585e+00,  8.66668771e-01, -7.00537408e-01],
       [-1.49933669e-01,  1.45237281e+00,  3.74609842e-02,
         2.53239896e+00,  8.45580400e-01, -7.13220652e-01],
       [-7.39617005e-02,  1.47774023e+00,  1.60643646e-02,
         2.60422940e+00,  8.19554682e-01, -7.21205288e-01],
       [ 4.16518154e-03,  1.50232687e+00, -5.57179403e-03,
         2.63542555e+00,  7.90319326e-01, -7.24427319e-01],
       [ 8.32279481e-02,  1.52603645e+00, -2.73046136e-02,
         2.62466048e+00,  7.59725678e-01, -7.22841890e-01],
       [ 1.61967762e-01,  1.54882822e+00, -4.89898703e-02,
         2.57146487e+00,  7.29553925e-01, -7.16439450e-01],
       [ 2.39111708e-01,  1.57071483e+00, -7.04830538e-02,
         2.47627011e+00,  7.01325433e-01, -7.05256667e-01],
       [ 3.13399812e-01,  1.59175460e+00, -9.16407538e-02,
         2.34040397e+00,  6.76154998e-01, -6.89379799e-01],
       [ 3.83611931e-01,  1.61203925e+00, -1.12322148e-01,
         2.16603688e+00,  6.54671168e-01, -6.68940608e-01],
       [ 4.48593037e-01,  1.63167938e+00, -1.32390366e-01,
         1.95608296e+00,  6.37018712e-01, -6.44106831e-01],
       [ 5.07275526e-01,  1.65078994e+00, -1.51713571e-01,
         1.71406620e+00,  6.22938813e-01, -6.15070314e-01],
       [ 5.58697512e-01,  1.66947811e+00, -1.70165680e-01,
         1.44396681e+00,  6.11906749e-01, -5.82036029e-01],
       [ 6.02016516e-01,  1.68783531e+00, -1.87626761e-01,
         1.15006549e+00,  6.03298605e-01, -5.45214730e-01],
       [ 6.36518481e-01,  1.70593427e+00, -2.03983203e-01,
         8.36802201e-01,  5.96558484e-01, -5.04821104e-01],
       [ 6.61622547e-01,  1.72383102e+00, -2.19127836e-01,
         5.08663225e-01,  5.91343485e-01, -4.61078217e-01],
       [ 6.76882444e-01,  1.74157133e+00, -2.32960183e-01,
         1.70104973e-01,  5.87631757e-01, -4.14227908e-01],
       [ 6.81985593e-01,  1.75920028e+00, -2.45387020e-01,
        -1.74482332e-01,  5.85786780e-01, -3.64545604e-01],
       [ 6.76751123e-01,  1.77677388e+00, -2.56323388e-01,
        -5.20773373e-01,  5.86577261e-01, -3.12357072e-01],
       [ 6.61127922e-01,  1.79437120e+00, -2.65694100e-01,
        -8.64475606e-01,  5.91156371e-01, -2.58054046e-01],
       [ 6.35193654e-01,  1.81210589e+00, -2.73435722e-01,
        -1.20127673e+00,  6.01006553e-01, -2.02105739e-01],
       [ 5.99155352e-01,  1.83013609e+00, -2.79498894e-01,
        -1.52678495e+00,  6.17857094e-01, -1.45063860e-01],
       [ 5.53351803e-01,  1.84867180e+00, -2.83850810e-01,
        -1.83647709e+00,  6.43581213e-01, -8.75598764e-02],
       [ 4.98257491e-01,  1.86797924e+00, -2.86477606e-01,
        -2.12567032e+00,  6.80078039e-01, -3.02944186e-02],
       [ 4.34487381e-01,  1.88838158e+00, -2.87386438e-01,
        -2.38953235e+00,  7.29142906e-01,  2.59803906e-02],
       [ 3.62801410e-01,  1.91025587e+00, -2.86607027e-01,
        -2.62314224e+00,  7.92327527e-01,  8.04846656e-02],
       [ 2.84107143e-01,  1.93402569e+00, -2.84192487e-01,
        -2.82160885e+00,  8.70790639e-01,  1.32439931e-01],
       [ 1.99458878e-01,  1.96014941e+00, -2.80219289e-01,
        -2.98024641e+00,  9.65140541e-01,  1.81104004e-01],
       [ 1.10051485e-01,  1.98910363e+00, -2.74786169e-01,
        -3.09479777e+00,  1.07527459e+00,  2.25812638e-01]])

type(xs), xs.shape

(numpy.ndarray, (64, 6))

Plotting Simulation Trajectories

Matplotlib is the most widely used Python library for making plots. Browse their example gallery to get an idea of the library’s capabilities. We will use matplotlib to visualize the state trajectories and animate our system. The convention for importing the main functionality of matplotlib is:

import matplotlib.pyplot as plt

The plot() function offers the simplest way to plot a chart of \(x\) values versus \(y\) values. I designed the output of euler_integrate() to work well with this plotting function. To make a basic plot use:

plt.plot(ts, xs);
_images/simulation_31_0.png

Note

The closing semicolon at the end of the statement suppresses the display of the returned objects in Jupyter. See the difference here:

plt.plot(ts, xs)

[<matplotlib.lines.Line2D at 0x7f3bbadeec90>,
 <matplotlib.lines.Line2D at 0x7f3bba819390>,
 <matplotlib.lines.Line2D at 0x7f3bba9296d0>,
 <matplotlib.lines.Line2D at 0x7f3bba819990>,
 <matplotlib.lines.Line2D at 0x7f3bba9e9e50>,
 <matplotlib.lines.Line2D at 0x7f3bba81a190>]
_images/simulation_32_1.png

This plot shows that the state trajectory changes with respect to time, but without some more information it is hard to interpret. The following function uses subplots() to make a figure with panels for the different state variables. I use vlatex() to include the symbolic symbol names in the legends. The other matplotlib functions and methods I use are: set_size_inches(), plot(), legend(), set_ylabel(), set_xlabel(), and tight_layout().

I also make use of array slicing notation to select which rows and columns I want from each array. See the NumPy documentation Indexing on ndarrays for information on how this works.

def plot_results(ts, xs):
    """Returns the array of axes of a 4 panel plot of the state trajectory
    versus time.

    Parameters
    ==========
    ts : array_like, shape(m,)
       Values of time.
    xs : array_like, shape(m, 6)
       Values of the state trajectories corresponding to ``ts`` in order
       [q1, q2, q3, u1, u2, u3].

    Returns
    =======
    axes : ndarray, shape(4,)
       Matplotlib axes for each panel.

    """

    fig, axes = plt.subplots(4, 1, sharex=True)

    fig.set_size_inches((10.0, 6.0))

    axes[0].plot(ts, np.rad2deg(xs[:, :2]))
    axes[1].plot(ts, xs[:, 2])
    axes[2].plot(ts, np.rad2deg(xs[:, 3:5]))
    axes[3].plot(ts, xs[:, 5])

    axes[0].legend([me.vlatex(q[0], mode='inline'),
                    me.vlatex(q[1], mode='inline')])
    axes[1].legend([me.vlatex(q[2], mode='inline')])
    axes[2].legend([me.vlatex(u[0], mode='inline'),
                    me.vlatex(u[1], mode='inline')])
    axes[3].legend([me.vlatex(u[2], mode='inline')])

    axes[0].set_ylabel('Angle [deg]')
    axes[1].set_ylabel('Distance [m]')
    axes[2].set_ylabel('Angular Rate [deg/s]')
    axes[3].set_ylabel('Speed [m/s]')

    axes[3].set_xlabel('Time [s]')

    fig.tight_layout()

    return axes

Our function now gives an interpretable view of the results:

plot_results(ts, xs);
_images/simulation_34_0.png

We now see that \(q_1\) oscillates between \(\pm 40 \textrm{deg}\) with a single period. \(q_2\) grows to around \(100 \textrm{deg}\), and \(q_3\) has half an oscillation period ranging between -0.2 and 0.2 meters. For the initial conditions and constants we choose, this seems physically feasible.

Integration with SciPy

Our euler_integrate() function seems to do the trick, but all numerical integrators suffer from two types of errors: truncation error and floating point arithmetic error. Truncation error is the dominant error and is due to having to integrate over finite integration steps with approximations to the area under the integral’s curve. Paying careful attention to truncation error is needed to keep the error in the resulting trajectories within some acceptable tolerance, usually close in magnitude to the floating point arithmetic error. Euler’s Method has poor truncation error unless very small time steps are chosen. But more time steps results in longer computation time. There are a large number of other numerical integration methods that provide better results with fewer time steps, but at the cost of more complexity in the integration algorithm.

SciPy is built on top of NumPy and provides a large assortment of battle tested numerical methods for NumPy arrays, including numerical methods for integration. We are solving the initial value problem of ordinary differential equations and SciPy includes the function solve_ivp() for this purpose. solve_ivp() provides access to a several different integration methods that are suitable for different problems. The default method used is a Runge-Kutta method that works well for non-stiff problems.

We will only be using solve_ivp() from SciPy so we can import it directly with:

from scipy.integrate import solve_ivp

We can use solve_ivp() in much the same way as our euler_integrate() function (in fact I designed euler_integrate() to mimic solve_ivp()). The difference is that solve_ivp() takes a function that evaluates the right hand side of the ordinary differential equations that is of the form f(t, x) (no p!). Our parameter vector p must be passed to the args= optional keyword argument in solve_ivp() to get things to work. If we only have one extra argument, as we do f(t, x, p), then we must make a 1-tuple (p_vals,). Other than that, the inputs are the same as euler_integrate(). solve_ivp() returns a solution object that contains quite a bit of information (other than the trajectories). See the documentation for solve_ivp() for all the details and more examples.

Here is how we use the integrator with our previously defined system:

result = solve_ivp(eval_rhs, (t0, tf), x0, args=(p_vals,))

The time values are in the result.t attribute:

result.t

array([0.1       , 0.13120248, 0.29762659, 0.54422251, 0.80410512,
       1.06256808, 1.31993114, 1.57487353, 1.83715709, 2.        ])

and the state trajectory is in the result.y attribute:

result.y

array([[ 0.43633231,  0.4358189 ,  0.31525018, -0.11015295, -0.46375645,
        -0.34485627,  0.12132567,  0.43601266,  0.27143017, -0.01911091],
       [ 0.08726646,  0.15537688,  0.49605864,  0.88149593,  1.13596246,
         1.39359322,  1.67416762,  1.92225131,  2.13926064,  2.29230085],
       [ 0.1       ,  0.10935888,  0.15799993,  0.21797738,  0.21721945,
         0.11520445, -0.03955252, -0.17887644, -0.25034975, -0.24594355],
       [ 0.1       , -0.13284291, -1.26834506, -1.88255608, -0.55453818,
         1.39013663,  1.86357587,  0.37219434, -1.5003001 , -1.93004498],
       [ 2.2       ,  2.16517843,  1.90078084,  1.19931529,  0.90164316,
         1.09025742,  1.04941978,  0.88566504,  0.83634928,  1.07386933],
       [ 0.3       ,  0.299615  ,  0.28218305,  0.16978927, -0.2076191 ,
        -0.54349971, -0.61398805, -0.44313122, -0.08440758,  0.13024152]])

Note the shape of the trajectory array:

np.shape(result.y)
\[\displaystyle \left( 6, \ 10\right)\]

It is the transpose of our xs computed above. Knowing that we can use our plot_results() function to view the results. I use transpose() to transpose the array before passing it into the plot function.

plot_results(result.t, np.transpose(result.y));
_images/simulation_40_0.png

The default result is very coarse in time (only 10 time steps!). This is because the underlying integration algorithm adaptively selects the necessary time steps to stay within the desired maximum truncation error. The Runge-Kutta method gives good accuracy with fewer integration steps in this case.

If you want to specify which time values you’d like the result presented at you can do so by interpolating the results by providing the time values with the keyword argument t_eval=.

result = solve_ivp(eval_rhs, (t0, tf), x0, args=(p_vals,), t_eval=ts)

plot_results(result.t, np.transpose(result.y));
_images/simulation_42_0.png

Lastly, let’s compare the results from euler_inegrate() with solve_ivp(), the later of which uses a Runge-Kutta method that has lower truncation error. We’ll plot only \(q_1\) for this comparison.

fig, ax = plt.subplots()
fig.set_size_inches((10.0, 6.0))

ax.plot(ts, np.rad2deg(xs[:, 0]), 'k',
        result.t, np.rad2deg(np.transpose(result.y)[:, 0]), 'b');
ax.legend(['euler_integrate', 'solve_ivp'])
ax.set_xlabel('Time [s]')
ax.set_ylabel('Angle [deg]');
_images/simulation_43_0.png

You can clearly see that the Euler Method deviates from the more accurate Runge-Kutta method. You’ll need to learn more about truncation error and the various integration methods to ensure you are getting the results you desire. For now, be aware that truncation error and floating point arithmetic error can give you inaccurate results.

Now set xs equal to the solve_ivp() result for use in the next section:

xs = np.transpose(result.y)

Animation with Matplotlib

Matplotlib also provides tools to make animations by iterating over data and updating the plot. I’ll create a very simple set of plots that give 4 views of interesting points in our system.

Matplotlib’s plot axes default to displaying the abscissa (\(x\)) horizontal and positive towards the right and the ordinate (\(y\)) vertical and positive upwards. The coordinate system in Fig. 47 has \(\hat{n}_x\) positive downwards and \(\hat{n}_y\) positive to the right. We can create a viewing reference frame \(M\) that matches matplotlib’s axes like so:

M = me.ReferenceFrame('M')
M.orient_axis(N, sm.pi/2, N.z)

Now \(\hat{m}_x\) is positive to the right, \(\hat{m}_y\) is positive upwards, and \(\hat{m}_z\) points out of the screen.

I’ll also introduce a couple of points on each end of the rod \(B\), just for visualization purposes:

Bl = me.Point('B_l')
Br = me.Point('B_r')
Bl.set_pos(Bo, -l/2*B.y)
Br.set_pos(Bo, l/2*B.y)

Now, we can project the four points \(B_o,Q,B_l,B_r\) onto the unit vectors of \(M\) using lambdify() to get the Cartesian coordinates of each point relative to point \(O\). I use row_join() to stack the matrices together to build a single matrix with all points’ coordinates.

coordinates = O.pos_from(O).to_matrix(M)
for point in [Bo, Q, Bl, Br]:
    coordinates = coordinates.row_join(point.pos_from(O).to_matrix(M))

eval_point_coords = sm.lambdify((q, p), coordinates)
eval_point_coords(q_vals, p_vals)

array([[ 0.        ,  0.25357096,  0.34385686, -0.01728675,  0.52442866],
       [ 0.        , -0.54378467, -0.50168367, -0.67008769, -0.41748165],
       [ 0.        ,  0.        ,  0.00871557, -0.02614672,  0.02614672]])

The first row are the \(x\) coordinates of each point, the second row has the \(y\) coordinates, and the last the \(z\) coordinates.

Now create the desired 4 panel figure with three 2D views of the system and one with a 3D view using the initial conditions and constant parameters shown. I make use of add_subplot() to control if the panel is 2D or 3D. set_aspect() ensures that the abscissa and ordinate dimensions display in a 1:1 ratio.

# initial configuration of the points
x, y, z = eval_point_coords(q_vals, p_vals)

# create a figure
fig = plt.figure()
fig.set_size_inches((10.0, 10.0))

# setup the subplots
ax_top = fig.add_subplot(2, 2, 1)
ax_3d = fig.add_subplot(2, 2, 2, projection='3d')
ax_front = fig.add_subplot(2, 2, 3)
ax_right = fig.add_subplot(2, 2, 4)

# common line and marker properties for each panel
line_prop = {
    'color': 'black',
    'marker': 'o',
    'markerfacecolor': 'blue',
    'markersize': 10,
}

# top view
lines_top, = ax_top.plot(x, z, **line_prop)
ax_top.set_xlim((-0.5, 0.5))
ax_top.set_ylim((0.5, -0.5))
ax_top.set_title('Top View')
ax_top.set_xlabel('x')
ax_top.set_ylabel('z')
ax_top.set_aspect('equal')

# 3d view
lines_3d, = ax_3d.plot(x, z, y, **line_prop)
ax_3d.set_xlim((-0.5, 0.5))
ax_3d.set_ylim((0.5, -0.5))
ax_3d.set_zlim((-0.8, 0.2))
ax_3d.set_xlabel('x')
ax_3d.set_ylabel('z')
ax_3d.set_zlabel('y')

# front view
lines_front, = ax_front.plot(x, y, **line_prop)
ax_front.set_xlim((-0.5, 0.5))
ax_front.set_ylim((-0.8, 0.2))
ax_front.set_title('Front View')
ax_front.set_xlabel('x')
ax_front.set_ylabel('y')
ax_front.set_aspect('equal')

# right view
lines_right, = ax_right.plot(z, y, **line_prop)
ax_right.set_xlim((0.5, -0.5))
ax_right.set_ylim((-0.8, 0.2))
ax_right.set_title('Right View')
ax_right.set_xlabel('z')
ax_right.set_ylabel('y')
ax_right.set_aspect('equal')

fig.tight_layout()
_images/simulation_48_0.png

Now we will use FuncAnimation to generate an animation. See the animation examples for more information on creating animations with matplotib.

First import FuncAnimation():

from matplotlib.animation import FuncAnimation

Now create a function that takes an frame index i, calculates the configuration of the points for the ith state in xs, and updates the data for the lines we have already plotted with set_data() and set_data_3d().

def animate(i):
    x, y, z = eval_point_coords(xs[i, :3], p_vals)
    lines_top.set_data(x, z)
    lines_3d.set_data_3d(x, z, y)
    lines_front.set_data(x, y)
    lines_right.set_data(z, y)

Now provide the figure, the animation update function, and the number of frames to FuncAnimation:

ani = FuncAnimation(fig, animate, len(ts))

FuncAnimation can create an interactive animation, movie files, and other types of outputs. Here I take advantage of IPython’s HTML display function and the to_jshtml() method to create a web browser friendly visualization of the animation.

from IPython.display import HTML

HTML(ani.to_jshtml(fps=30))

If we’ve setup our animation correctly and our equations of motion are correct, we should see physically believable motion of our system. In this case, it looks like we’ve successfully simulated and visualized our first multibody system!