Modelling of the Seesaw/Pendulum
with Mathematica

created by Mats Jirstrand and Johan Gunnarsson Linköping University

Introduction

This notebook illustrates how to build a dynamic model of a physical system and generate efficient code for simulating the model.

The physical system is a seesaw where two wagons C1 and C2 are freely moving due to the forces acting on them. On wagon C2 there is an inverted pendulum attached by a friction free joint.

The model will show how the system will behave if it is in rest and balanced at time zero and then is disturbed by some small changes in the pendulum angle.

Variables and Constants

Variables:
	(Subscript[x, 1])	translation of cart 1 from center of track
	(Subscript[x, 2])	translation of cart 1 from center of track
	θ	angle of seesaw with vertical
	α	angle of pendulum with normal to track
	(Subscript[F, 1])	force applied to cart 1
	(Subscript[F, 2])	force applied to cart 2

Constants:
	J	inertia of seesaw with track att height h
	(Subscript[m, s])	mass of seesaw with track
	c	center of gravity
	h	height of track from pivot point
	(Subscript[m, 1])	mass of cart 1 (weight cart, on the back track)
	(Subscript[m, 2])	mass of cart 2	 (pendulum cart, on the front track)
	(Subscript[m, p])	mass of pendulum
	(Subscript[l, p])	center of mass of pendulum rod (half of full length)
	g	gravitational acceleration

\(physicalvalues := {J -> 1.6, m\_s -> 6.6, c -> 0.06, 
        h -> 0.115, 

        \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ m\_1 -> 0.48 + 0.38, m\_2 -> 0.48, m\_p -> 0.2, 
        
\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ l\_p -> 0.61\/2 + 0.03, g -> 9.81}\)

Off[General::"spell1"];

Mathematical Modeling

Computation of the Lagrangian

Coordinates of the center of mass of the seesaw

\(x\_s := c\ Sin[θ[t]]; 
y\_s := c\ Cos[θ[t]]; \)

The potential and kinetic energies of the seesaw

\(V\_s := m\_s\ g\ y\_s; 

    T\_s := Simplify[1\/2\ J\ \((∂\_t θ[t])\)\^2]; \)

Coordinates of the center of track

\(x\_c := h\ Sin[θ[t]]; 
y\_c := h\ Cos[θ[t]]; \)

Coordinates of cart 1

\(x\_m1 := x\_c + x\_1[t]\ Cos[θ[t]]; 

    y\_m1 := y\_c - x\_1[t]\ Sin[θ[t]]; \)

The potential and kinetic energies of cart 1

\(V\_m1 := m\_1\ g\ y\_m1; 

    T\_m1 := Simplify[
        1\/2\ m\_1\ 
          \((\((∂\_t x\_m1)\)\^2 + \((∂\_t y\_m1)\)\^2)
            \)]; \)

Coordinates of cart 2

\(x\_m2 := x\_c + x\_2[t]\ Cos[θ[t]]; 

    y\_m2 := y\_c - x\_2[t]\ Sin[θ[t]]; \)

The potential and kinetic energies of cart 2

\(V\_m2 := m\_2\ g\ y\_m2; 

    T\_m2 := Simplify[
        1\/2\ m\_2\ 
          \((\((∂\_t x\_m2)\)\^2 + \((∂\_t y\_m2)\)\^2)
            \)]; \)

Coordinates of the center of mass of the pendulum

\(x\_p := x\_m2 + l\_p\ Sin[α[t] + θ[t]]; 

    y\_p := y\_m2 + l\_p\ Cos[α[t] + θ[t]]; \)

The potential and kinetic energies of the pendulum

\(V\_p := m\_p\ g\ y\_p; 

    T\_p := Simplify[
        1\/2\ m\_p\ 
          \((\((∂\_t x\_p)\)\^2 + \((∂\_t y\_p)\)\^2)\)]; 
    \)

The total potential energy

(Subscript[V, tot]) := (Subscript[V, s]) + (Subscript[V, m1]) + (Subscript[V, m2]) + (Subscript[V, p]);

The total kinetic energy

(Subscript[T, tot]) := (Subscript[T, s]) + (Subscript[T, m1]) + (Subscript[T, m2]) + (Subscript[T, p]);

The Lagrangian

L := (Subscript[T, tot]) - (Subscript[V, tot]);

Computation of the Equations of Motion

Equations of motion:  D[D[L,D[qi,t]],t]-D[L,qi[t]]  ==  Qi
(This computation takes a minute or two ...)

\(eqn\_1 = 
      Simplify[∂\_t\((∂\_\(∂\_t x\_1[t]\)L)\) - 
            ∂\_\(x\_1[t]\)L == F\_1]; 

    eqn\_2 = Simplify[
        ∂\_t\((∂\_\(∂\_t x\_2[t]\)L)\) - 
            ∂\_\(x\_2[t]\)L == F\_2]; 

    eqn\_3 = Simplify[
        ∂\_t\((∂\_\(∂\_t θ[t]\)L)\) - 
            ∂\_\(θ[t]\)L == 0]; 

    eqn\_4 = Simplify[
        ∂\_t\((∂\_\(∂\_t α[t]\)L)\) - 
            ∂\_\(α[t]\)L == 0]; \)

Solve for second order derivatives

\(\(sol = 
      Solve[\ {\ eqn\_1, eqn\_2, eqn\_3, eqn\_4\ }, 

        												{\ ∂\_{t, 2}x\_1[t], 
          ∂\_{t, 2}x\_2[t], 
													\ 
          ∂\_{t, 2}θ[t], ∂\_{t, 2}α[t]\ }
          \ ]; \)\)

Conversion rules to get rid of explicit time dependence

\(\(ssvariables := {\ \ x\_1[t] -> x\_1, 
        θ[t] -> θ, 
																
        x\_2[t] -> x\_2, α[t] -> α, 

        																∂\_t x\_1[t] -> dx\_1, 
        ∂\_t θ[t] -> dθ, 

        																∂\_t x\_2[t] -> dx\_2, 
        ∂\_t α[t] -> dα\ }; \)\)

The right hand side of the nonlinear state space equations,  \(TraditionalForm\`x\& . \)[t] = f[x[t],u[t]]

\(\(f = 
      \({∂\_{t, 2}x\_1[t], ∂\_{t, 2}θ[t], 
            ∂\_{t, 2}x\_2[t], ∂\_{t, 2}α[t]}

          			 /. sol[[1]]\) /. 
        ssvariables; \)\)

The Linearized Model

Linearize the equations around {0,0,0,0,0,0,0,0}  => \(TraditionalForm\`x\& . \)[t] = A x[t] + B u[t]

\(\(stationarypoint = {\ \ x\_1 -> 0, θ -> 0, 
        x\_2 -> 0, α -> 0, 

        																			dx\_1 -> 0, 
        dθ -> 0, \ dx\_2 -> 0, dα -> 0, 

        																			F\_1 -> 0, 
        F\_2 -> 0\ }; \)\)

The jacobian function computes the Jacobian matrix of a vector-valued function.

jacobian[h_, vars_] := Outer[D, h, vars]

\(\(A\  = \ 
      Simplify[
					
        jacobian[\ f, 
            \ {\ x\_1, θ, x\_2, α, 

              																		\ \ dx\_1, dθ, 
              dx\_2, dα}\ ] /. stationarypoint\ ]; \)\)

B = Simplify[ jacobian[ f, {(Subscript[F, 1]), (Subscript[F, 2])} ] /. stationarypoint ];

Augment the system matrices

<< LinearAlgebra`MatrixManipulation`\(A\_a = 
      AppendColumns[
						
        AppendRows[Table[0, {4}, {4}], IdentityMatrix[4]], A\ ]; 

    B\_a = AppendColumns[Table[0, {4}, {2}], B]; \)

Insert numerical values

MatrixForm[(Subscript[A, a]) /. physicalvalues]

MatrixForm[(Subscript[B, a]) /. physicalvalues]

Code Generation

Load MathCode and set Current Directory

Needs["MathCode`"];

SetDirectory[$MCRoot<>"/Demos/Pendulum"];

ExistFileQ["Pendulum.nb"];

C++ code using MathCode

Create the right hand side of the nonlinear state space function

\(\(Cvariables\  = 

      		{\ Subscript[x, \ 1]\  -> \ x1, θ\  -> \ theta, 

        			Subscript[x, \ 2]\  -> \ x2, α\  -> \ alpha, 

        			Subscript[dx, \ 1]\  -> \ dx1, 
        dθ\  -> \ dtheta, 
			
        Subscript[dx, \ 2]\  -> \ dx2, \ dα -> \ dalpha, 

        			Subscript[F, \ 1]\  -> \ F1, 
        Subscript[F, \ 2]\  -> \ F2\ }; \)\)

rhs = f /. physicalvalues  /.  Cvariables;

Store the huge expression rhs on file for later use.

If you have memory problems, e.g., the CompilePackage fails and stops and nothing happens, then the previous computations in this notebook have made the memory used by Mathematica too large for the external compiler to work. This risk is greatest on a Windows 95 system. In such a case restart and load the rhs from a file instead.

(* rhs = << PendulumExpr.m; *)

To free some memory.

Share[];

Create the Global package

This defines the function PendFunc1 which contains the huge rhs expression from above as its body. Note that the expression can read from the file PendulumExpr.m which is created above. This file makes it quicker to try out the compilation in different sessions without computing rhs each time.

The PendFunc1 function maps 10 real parameters to a list with 4 elements of reals. Since all function arguments have the same type we map Real on the arguments for convenience.

Evaluate[(Real /@
PendFunc1[x1_, theta_, x2_, alpha_, dx1_, dtheta_, dx2_, dalpha_, F1_, F2_])-> Real[4]] = rhs;

This function is only used for measuring the performance.

\(\(calcfunc[Integer@\ n_] -> Real[4]\  := \ 
	
      Module[{
				Integer@\ \ count, 
				Real[4]@\ \ res

          			}, 
		
        For\ [count = 1, count <= n, count = count + 1, 
				
          res = PendFunc1[1. , 2. , 3. , 4. , 5. , 6. , 7. , 8. , 9. , 10. ]

          		]; 
		res
	]; \)\)

Since NDSolve must have scalar equations, we must create a function returning each component of the result from PendFunc1.

Evaluate[ScalarPendFunc1[Sequence@@ Real/@
{x1_, theta_, x2_, alpha_, dx1_, dtheta_, dx2_, dalpha_, F1_, F2_}, Integer@ k_]-> Real] :=
	PendFunc1[x1, theta, x2, alpha, dx1, dtheta, dx2, dalpha, F1, F2][[k]];

Compilation

We are now ready to compile the package.

CompilePackage["Global", EvaluateFunctions -> {PendFunc1}]

The size of the of the symbolic expression is obvious by just take a look at it.

?? PendFunc1

The corresponding generated code can be seen as the first function in the file pend.cc

!! "Global.cc"

The types of the functions in the package do not influence the evaluation of these functions.

Timing[PendFunc1[1. , 2. , 3. , 4. , 5. , 6. , 7. , 8. , 9. , 10. ]]

ScalarPendFunc1[1.,2.,3.,4.,5.,6.,7.,8.,9.,10.,1]

ScalarPendFunc1[1.,2.,3.,4.,5.,6.,7.,8.,9.,10.,4]

calcfunc[10]

All necessary files for building binaries are now generated. The MakeBinary command will build both a standalone application using the code specified by the function GenerateMainFileAndFunction used in the compilation script in the package definition, and a MathLink version which is ready to install into Mathematica.

MakeBinary[];

Simulation of the Nonlinear Model

We define f1[t] to be the PendFunc1 with the inputs F1 and F2 set to zero.

\(f1[t_] := 
      Array[
		
        \(PendFunc1[x1[t], theta[t], x2[t], alpha[t], dx1[t], dtheta[t], 
              dx2[t], dalpha[t], 0. , 0. ]\)[[#1
            ]]&, {4}]\)

The state vector.

x[t_] := {x1[t], theta[t], x2[t], alpha[t], dx1[t], dtheta[t], dx2[t], 
        dalpha[t]}

The dynamic equations. The f1[t] function is the right hand side of the last four state variables in x[t]. The first 4 equations are simple integrations.

deq:=Thread[x'[t]==Join[Take[x[t], -4], f1[t]]]

Initial conditions. Cart 1 is positioned at -1, the seesaw is horizontal, cart 2 is at +1 and the pendulum has a small deviation from the vertical axis. No initial movements.

initeq := Thread[x[0] == {-1, 0, 1, 0.1, 0, 0, 0, 0}]

Simulate the system.

Timing[dsol = NDSolve[deq ⋃ initeq, Head/@x[t], {t, 0. , 1}]]

Plot the result for the first 4 state variables.

Map[Plot[#1, {t, 0, 1}]&, 
      Partition[Take[Flatten[x[t] /. dsol], 4], 2], {2}]

To use the generated code for the simulation, install the compiled package. This will replace the definition of the PendFunc1 and calcfunc functions.

InstallCode["Global"];

This is a fix to be able to measure time for external processes.

\(SetAttributes[AbsTime, HoldFirst]; 
    AbsTime[x_] := 
      Module[{start, res}, 
		start = AbsoluteTime[]; 
        res = x; {\((AbsoluteTime[] - start)\)\ Second, res}]; \)

We will redefine f1 to use ScalarPendFunc1 instead

\(f1[t_] := 
      Array[
		
        ScalarPendFunc1[x1[t], theta[t], x2[t], alpha[t], dx1[t], dtheta[t], 
            dx2[t], dalpha[t], 0. , 0. , #1]&, {4}]\)

This will do the same simulation but using MathLink and the external package.

AbsTime[dsol = NDSolve[deq ⋃ initeq, Head/@x[t], {t, 0. , 1}]]

Hopefully the result will be the same.

Map[Plot[#1, {t, 0, 1}]&, 
      Partition[Take[Flatten[x[t] /. dsol], 4], 2], {2}]

The time measurements are better for the simulation using the internal expression compared to the external process. The reason for this is that the MathLink communication takes too much time so we do not gain any perfomance by using the external process. A much more effective solution would be to have an exernal simulator aswell.

The time of one iteration of PendFunc1 in the external process is

AbsTime[calcfunc[1000000]]exttime  =  First[%]/1000000

DeactivateCode[calcfunc];

Timing[calcfunc[50]]
inttime = First[%] / 50

The performance increase of the external code is

inttime / exttime

which is rather good.

Compare this with the Mathematica Compile.

PendFunc1=Compile[{x1, theta, x2, alpha, dx1, dtheta, dx2, dalpha, F1, F2}, Evaluate[rhs]];

Timing[calcfunc[50]]
comptime = First[%] / 50

The performance improvement of the external code compared to the internally compiled code is

comptime / exttime

PendFunc1=.

The time of one iteration of PendFunc1 + the MathLink's calls is

(1/500) 
      AbsTime[
          Do[PendFunc1[1. , 2. , 3. , 4. , 5. , 6. , 7. , 8. , 9. , 10. ], {
              500}]][[1]]

Cleaning Up

UninstallCode["Global"];

CleanMathCodeFiles[];