The Seesaw/Pendulum Process

Modeling, Controller Design, Code Generation,
and Simulation

Mats Jirstrand, Johan Gunnarsson, and Andreas Karström, MathCore AB

Introduction

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

The physical system is a seesaw in which two carts C1 and C2 can be moved along two tracks by controlling the forces acting on them. Cart C1 carries a weight, and cart C2 has an inverted pendulum attached to it by a friction-free joint.

The model of the open loop system has the forces (Subscript[F, 1]) and (Subscript[F, 2]) acting on each cart as inputs. We first model the system by Lagrangian methodology and then linearize the obtained nonlinear model. The linearized model is used for control system design for which  the Control System Professional (CSP) application package is used. The closed loop system is then simulated both within Mathematica and by external code generated using MathCode C++.

Variables and Constants

Variables:
	(Subscript[x, 1])	translation of cart 1 from center of track
	(Subscript[x, 2])	translation of cart 2 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 at 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

\(In[1] := 
      \(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}
          \)\)

Mathematical Modeling

The Lagrangian methodology for deriving dynamic equations of a mechanical system consists of introducing so-called generalized coordinates that describe (angular) positions and (angular) velocities of subsystems of the true system. Using these coordinates, expressions for the kinetic and potential energy of each subsystem can be formed. Once this is done, the dynamic equations describing the whole system can be derived automatically.

Computation of the Lagrangian

Coordinates of the center of mass of the seesaw

In[2] :=  (Subscript[x, s]) := c Sin[θ[t]]
In[3] :=  (Subscript[y, s]) := c Cos[θ[t]]

The potential and kinetic energies of the seesaw

In[4] :=  (Subscript[V, s]) := (Subscript[m, s]) g (Subscript[y, s])
In[5] :=  
      (Subscript[T, s]) := Simplify[(1/2) J ((((∂_t) θ[t]))^2)]

Coordinates of the center of track

In[6] :=  (Subscript[x, c]) := h Sin[θ[t]]
In[7] :=  (Subscript[y, c]) := h Cos[θ[t]]

Coordinates of cart 1

In[8] :=  (Subscript[x, m1]) := (Subscript[x, c]) + (Subscript[x, 1])[t] Cos[θ[t]]
In[9] :=  (Subscript[y, m1]) := (Subscript[y, c]) - (Subscript[x, 1])[t] Sin[θ[t]]

The potential and kinetic energies of cart 1

In[10] := (Subscript[V, m1]) := (Subscript[m, 1]) g (Subscript[y, m1])
In[11] := 
      (Subscript[T, m1]) := Simplify[
          (1/2) (Subscript[m, 1]) (((((∂_t) (Subscript[x, m1])))^2) + 
                ((((∂_t) (Subscript[y, m1])))^2))]

Coordinates of cart 2

In[12] := (Subscript[x, m2]) := (Subscript[x, c]) + (Subscript[x, 2])[t] Cos[θ[t]]
In[13] := (Subscript[y, m2]) := (Subscript[y, c]) - (Subscript[x, 2])[t] Sin[θ[t]]

The potential and kinetic energies of cart 2

In[14] := (Subscript[V, m2]) := (Subscript[m, 2]) g (Subscript[y, m2])
In[15] := 
      (Subscript[T, m2]) := Simplify[
          (1/2) (Subscript[m, 2]) (((((∂_t) (Subscript[x, m2])))^2) + 
                ((((∂_t) (Subscript[y, m2])))^2))]

Coordinates of the center of mass of the pendulum

In[16] := (Subscript[x, p]) := (Subscript[x, m2]) + (Subscript[l, p]) Sin[α[t] + θ[t]]
In[17] := (Subscript[y, p]) := (Subscript[y, m2]) + (Subscript[l, p]) Cos[α[t] + θ[t]]

The potential and kinetic energies of the pendulum

In[18] := (Subscript[V, p]) := (Subscript[m, p]) g (Subscript[y, p])
In[19] := 
      (Subscript[T, p]) := Simplify[
          (1/2) (Subscript[m, p]) (((((∂_t) (Subscript[x, p])))^2) + 
                ((((∂_t) (Subscript[y, p])))^2))]

The total potential energy

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

The total kinetic energy

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

The Lagrangian of the system

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

Computation of the Equations of Motion

The equations of motion are given by
	 (d/(d t))\(∂L\/∂q\& . \_i\)-∂(L/∂)(Subscript[q, i])==(Subscript[Q, i])
(Note: This computation may take a while...)

In[22] := 
        (Subscript[eqn, 1]) = 
          Simplify[
            (∂_t)((∂_((∂_t) (Subscript[x, 1])[t]))L) - 
                (∂_((Subscript[x, 1])[t]))L == (Subscript[F, 1])];
(Subscript[eqn, 2]) = 
        Simplify[
          (∂_t)((∂_((∂_t) (Subscript[x, 2])[t]))L) - 
              (∂_((Subscript[x, 2])[t]))L == (Subscript[F, 2])];
(Subscript[eqn, 3]) = 
        Simplify[
          (∂_t)((∂_((∂_t) θ[t]))L) - 
              (∂_(θ[t]))L == 0];
(Subscript[eqn, 4]) = 
        Simplify[
          (∂_t)((∂_((∂_t) α[t]))L) - 
              (∂_(α[t]))L == 0];

These equations are coupled ordinary differential equations (ODEs) of second order in the generalized coordinates (Subscript[x, 1]),  θ,  (Subscript[x, 2]),  α. To rewrite these in standard state space form (a system of first order ODEs), we introduce the following states:
	x=(({{(Subscript[x, 1]), θ, (Subscript[x, 2]), α, \(x\& . \_1\), \(θ\& . \), \(x\& . \_2\), \(α\& . \)}}))
The inputs to the system are
	u=(({{(Subscript[F, 1]), (Subscript[F, 2])}}))
We observe that the second-order time derivatives of the positions and angles appear linearly which makes it easy to solve for these in terms of the states.

Solve for second order derivatives:

\(\(In[23] := 
        \(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]\ }\ ]\);\)\)

A look at the solutions. (These will become the right hand sides of the differential equations used for simulating the system).

In[24]:=sol

Out[24]={{(Subscript[x, 1]^″)[t]->-((-(Subscript[F, 1])+(Subscript[m, 1]) (-g Sin[θ[t]]-(Subscript[x, 1])[t] ((θ'[t])^2)))/((Subscript[m, 1])))+h ((Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-(Subscript[F, 2])+(Subscript[m, 2]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2))+(Subscript[m, p]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2)+(Subscript[l, p]) (-Sin[α[t]] ((α'[t])^2)-2 Sin[α[t]] α'[t] θ'[t]-Sin[α[t]] ((θ'[t])^2))))-(Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p])) (-g Sin[α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t]))/(Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (h (Subscript[m, 2]) + 
                                  (h + Cos[α[t]] (Subscript[l, p])) (Subscript[m, p])) - 
                            (Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + 
                                  (Subscript[m, p])) (h Cos[α[t]] + (Subscript[l, p]) + 
                                  Sin[α[t]] (Subscript[x, 2])[t]))-((Cos[α[t]]^2 (Subscript[l, p]^2) (Subscript[m, p]^2) - 
                                (Subscript[l, p]^2) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p]))) (\(-\((\(-l\_p\)\ m\_1\ m\_p\ \((h\ m\_2 + 
                                        h\ m\_p + 
                                        Cos[α[
                                        t]]\ l\_p\ m\_p)\)\ \((h\ Cos[
                                        α[t]] + l\_p + 
                                        Sin[α[t]]\ x\_2[t])\) + 
                                        Cos[α[
                                        t]]\ l\_p\ m\_p\ \((\(-h\^2\)\ m\_1\%2 + m\_1\ \((J + h\^2\ m\_p + 2\ h\ Cos[α[t]]\ l\_p\ m\_p + 
                                        l\_p\%2\ m\_p + 
                                        m\_1\ \((h\^2 + x\_1[t]\^2)\) + 
                                        2\ Sin[α[t]]\ l\_p\ m\_p\ x\_2[
                                        t] + m\_p\ x\_2[t]\^2 + 
                                        m\_2\ \((h\^2 + 
                                        x\_2[t]\^2)\))\))\))\)\) (Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-(Subscript[F, 2])+(Subscript[m, 2]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2))+(Subscript[m, p]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2)+(Subscript[l, p]) (-Sin[α[t]] ((α'[t])^2)-2 Sin[α[t]] α'[t] θ'[t]-Sin[α[t]] ((θ'[t])^2))))-(Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p])) (-g Sin[
                                        α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t]))+(Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (h (Subscript[m, 2]) + 
                                        (h + Cos[α[
                                        t]] (Subscript[l, p])) (Subscript[m, p])) - 
                                      (Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + 
                                        (Subscript[m, p])) (h Cos[α[t]] + 
                                        (Subscript[l, p]) + 
                                        Sin[α[t]] (Subscript[x, 2])[t])) (-(Subscript[l, p]) (Subscript[m, 1]) (Subscript[m, p]) (h (Subscript[m, 2]) + h (Subscript[m, p]) + 
                                        Cos[α[t]] (Subscript[l, p]) (Subscript[m, p])) (-g Sin[
                                        α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t])+Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-h (Subscript[m, 1]) (-(Subscript[F, 1])+(Subscript[m, 1]) (-g Sin[θ[t]]-(Subscript[x, 1])[t] ((θ'[t])^2)))+(Subscript[m, 1]) (-g h Sin[
                                        θ[t]] (Subscript[m, p])-g Sin[α[t] + 
                                        θ[t]] (Subscript[l, p]) (Subscript[m, p])-c g Sin[θ[t]] (Subscript[m, s])-g Cos[θ[
                                        t]] (Subscript[m, p]) (Subscript[x, 2])[t]-h Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) ((α'[t])^2)+Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[t] ((α'[t])^2)-2 h Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) α'[t] θ'[t]+2 Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[t] α'[t] θ'[t]+(Subscript[m, 1]) (-g h Sin[θ[t]]+(Subscript[x, 1])[t] (-g Cos[θ[t]]+2 θ'[t] (Subscript[x, 1]^′)[t]))+2 Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) θ'[t] (Subscript[x, 2]^′)[t]+2 (Subscript[m, p]) (Subscript[x, 2])[t] θ'[t] (Subscript[x, 2]^′)[t]+(Subscript[m, 2]) (-g h Sin[θ[t]]-g Cos[θ[t]] (Subscript[x, 2])[t]+2 (Subscript[x, 2])[t] θ'[t] (Subscript[x, 2]^′)[t]))))))/\((\((J\ Cos[
                                    α[
                                      t]]\ l\_p\%3\ m\_1\ m\_2\ m\_p\%2 + 
                                J\ Cos[α[t]]\ l\_p\%3\ m\_1\ m\_p\%3 - 
                                J\ Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_p\%3 + Cos[α[t]]\ l\_p\%3\ m\_1\%2\ m\_2\ m\_p\%2\ x\_1[t]\^2 + 
                                Cos[α[
                                      t]]\ l\_p\%3\ m\_1\%2\ m\_p\%3\ x\_1[t]\^2 - Cos[α[t]]\^3\ l\_p\%3\ m\_1\%2\ m\_p\%3\ x\_1[t]\^2 + 
                                Cos[α[
                                      t]]\ l\_p\%3\ m\_1\ m\_2\%2\ m\_p\%2\ x\_2[t]\^2 + 2\ Cos[α[t]]\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 - 
                                Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 - 
                                Cos[α[
                                      t]]\ Sin[α[t]]\^2\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 + Cos[α[
                                      t]]\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2 
                                - Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2 - 
                                Cos[α[
                                      t]]\ Sin[α[t]]\^2\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2)\)\ \((Cos[α[t]]\ l\_p\ m\_p\ \((h\ m\_2 + 
                                      \((h + 
                                        Cos[α[t]]\ l\_p)\)\ m\_p)\) - 
                                l\_p\ m\_p\ \((m\_2 + 
                                      m\_p)\)\ \((h\ Cos[α[t]] + l\_p 
                                      + Sin[α[t]]\ x\_2[t])\))\))\)),(Subscript[x, 2]^″)[t]->-Sec[α[t]] (-g Sin[α[t] + θ[t]]+(h Sin[α[t]] - Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t])+(Sec[α[t]] (Subscript[l, p]) (\(-\((\(-l\_p\)\ m\_1\ m\_p\ \((h\ m\_2 + h\ m\_p + 
                                        Cos[α[
                                        t]]\ l\_p\ m\_p)\)\ \((h\ Cos[
                                        α[t]] + l\_p + 
                                        Sin[α[t]]\ x\_2[t])\) + 
                                  Cos[α[
                                        t]]\ l\_p\ m\_p\ \((\(-h\^2\)\ m\_1\%2 + m\_1\ \((J + h\^2\ m\_p + 2\ h\ Cos[α[t]]\ l\_p\ m\_p + 
                                        l\_p\%2\ m\_p + 
                                        m\_1\ \((h\^2 + x\_1[t]\^2)\) + 
                                        2\ Sin[α[t]]\ l\_p\ m\_p\ x\_2[
                                        t] + m\_p\ x\_2[t]\^2 + 
                                        m\_2\ \((h\^2 + 
                                        x\_2[t]\^2)\))\))\))\)\) (Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-(Subscript[F, 2])+(Subscript[m, 2]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2))+(Subscript[m, p]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2)+(Subscript[l, p]) (-Sin[α[t]] ((α'[t])^2)-2 Sin[α[t]] α'[t] θ'[t]-Sin[α[t]] ((θ'[t])^2))))-(Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p])) (-g Sin[
                                        α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t]))+(Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (h (Subscript[m, 2]) + 
                                      (h + 
                                        Cos[α[t]] (Subscript[l, p])) (Subscript[m, p])) - 
                                (Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + 
                                      (Subscript[m, p])) (h Cos[α[t]] + (Subscript[l, p]) 
                                      + Sin[α[t]] (Subscript[x, 2])[t])) (-(Subscript[l, p]) (Subscript[m, 1]) (Subscript[m, p]) (h (Subscript[m, 2]) + h (Subscript[m, p]) + 
                                      Cos[α[t]] (Subscript[l, p]) (Subscript[m, p])) (-g Sin[
                                        α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t])+Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-h (Subscript[m, 1]) (-(Subscript[F, 1])+(Subscript[m, 1]) (-g Sin[θ[t]]-(Subscript[x, 1])[t] ((θ'[t])^2)))+(Subscript[m, 1]) (-g h Sin[
                                        θ[t]] (Subscript[m, p])-g Sin[
                                        α[t] + 
                                        θ[t]] (Subscript[l, p]) (Subscript[m, p])-c g Sin[θ[t]] (Subscript[m, s])-g Cos[θ[t]] (Subscript[m, p]) (Subscript[x, 2])[
                                        t]-h Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) ((α'[t])^2)+Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[t] ((α'[t])^2)-2 h Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) α'[t] θ'[t]+2 Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[t] α'[t] θ'[t]+(Subscript[m, 1]) (-g h Sin[θ[t]]+(Subscript[x, 1])[t] (-g Cos[θ[t]]+2 θ'[t] (Subscript[x, 1]^′)[t]))+2 Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) θ'[t] (Subscript[x, 2]^′)[t]+2 (Subscript[m, p]) (Subscript[x, 2])[t] θ'[t] (Subscript[x, 2]^′)[t]+(Subscript[m, 2]) (-g h Sin[θ[t]]-g Cos[θ[t]] (Subscript[x, 2])[t]+2 (Subscript[x, 2])[t] θ'[t] (Subscript[x, 2]^′)[t]))))))/\((J\ Cos[α[t]]\ l\_p\%3\ m\_1\ m\_2\ m\_p\%2 + 
                      J\ Cos[α[t]]\ l\_p\%3\ m\_1\ m\_p\%3 - 
                      J\ Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_p\%3 + 
                      Cos[α[
                            t]]\ l\_p\%3\ m\_1\%2\ m\_2\ m\_p\%2\ x\_1[t]\^2 
                      + Cos[α[
                            t]]\ l\_p\%3\ m\_1\%2\ m\_p\%3\ x\_1[t]\^2 - 
                      Cos[α[t]]\^3\ l\_p\%3\ m\_1\%2\ m\_p\%3\ x\_1[t]\^2 + Cos[α[t]]\ l\_p\%3\ m\_1\ m\_2\%2\ m\_p\%2\ x\_2[t]\^2 + 
                      2\ Cos[α[
                            t]]\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 - 
                      Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 - 
                      Cos[α[
                            t]]\ Sin[α[t]]\^2\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 + Cos[α[t]]\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2 - 
                      Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2 
                      - Cos[α[
                            t]]\ Sin[α[t]]\^2\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2)\)+Sec[α[t]] (h Cos[α[t]] + (Subscript[l, p]) + 
                      Sin[α[t]] (Subscript[x, 2])[t]) ((Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-(Subscript[F, 2])+(Subscript[m, 2]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2))+(Subscript[m, p]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2)+(Subscript[l, p]) (-Sin[α[t]] ((α'[t])^2)-2 Sin[α[t]] α'[t] θ'[t]-Sin[α[t]] ((θ'[t])^2))))-(Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p])) (-g Sin[α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t]))/(Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (h (Subscript[m, 2]) + 
                                  (h + Cos[α[t]] (Subscript[l, p])) (Subscript[m, p])) - 
                            (Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + 
                                  (Subscript[m, p])) (h Cos[α[t]] + (Subscript[l, p]) + 
                                  Sin[α[t]] (Subscript[x, 2])[t]))-((Cos[α[t]]^2 (Subscript[l, p]^2) (Subscript[m, p]^2) - 
                                (Subscript[l, p]^2) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p]))) (\(-\((\(-l\_p\)\ m\_1\ m\_p\ \((h\ m\_2 + 
                                        h\ m\_p + 
                                        Cos[α[
                                        t]]\ l\_p\ m\_p)\)\ \((h\ Cos[
                                        α[t]] + l\_p + 
                                        Sin[α[t]]\ x\_2[t])\) + 
                                        Cos[α[
                                        t]]\ l\_p\ m\_p\ \((\(-h\^2\)\ m\_1\%2 + m\_1\ \((J + h\^2\ m\_p + 2\ h\ Cos[α[t]]\ l\_p\ m\_p + 
                                        l\_p\%2\ m\_p + 
                                        m\_1\ \((h\^2 + x\_1[t]\^2)\) + 
                                        2\ Sin[α[t]]\ l\_p\ m\_p\ x\_2[
                                        t] + m\_p\ x\_2[t]\^2 + 
                                        m\_2\ \((h\^2 + 
                                        x\_2[t]\^2)\))\))\))\)\) (Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-(Subscript[F, 2])+(Subscript[m, 2]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2))+(Subscript[m, p]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2)+(Subscript[l, p]) (-Sin[α[t]] ((α'[t])^2)-2 Sin[α[t]] α'[t] θ'[t]-Sin[α[t]] ((θ'[t])^2))))-(Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p])) (-g Sin[
                                        α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t]))+(Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (h (Subscript[m, 2]) + 
                                        (h + Cos[α[
                                        t]] (Subscript[l, p])) (Subscript[m, p])) - 
                                      (Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + 
                                        (Subscript[m, p])) (h Cos[α[t]] + 
                                        (Subscript[l, p]) + 
                                        Sin[α[t]] (Subscript[x, 2])[t])) (-(Subscript[l, p]) (Subscript[m, 1]) (Subscript[m, p]) (h (Subscript[m, 2]) + h (Subscript[m, p]) + 
                                        Cos[α[t]] (Subscript[l, p]) (Subscript[m, p])) (-g Sin[
                                        α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t])+Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-h (Subscript[m, 1]) (-(Subscript[F, 1])+(Subscript[m, 1]) (-g Sin[θ[t]]-(Subscript[x, 1])[t] ((θ'[t])^2)))+(Subscript[m, 1]) (-g h Sin[
                                        θ[t]] (Subscript[m, p])-g Sin[α[t] + 
                                        θ[t]] (Subscript[l, p]) (Subscript[m, p])-c g Sin[θ[t]] (Subscript[m, s])-g Cos[θ[
                                        t]] (Subscript[m, p]) (Subscript[x, 2])[t]-h Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) ((α'[t])^2)+Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[t] ((α'[t])^2)-2 h Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) α'[t] θ'[t]+2 Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[t] α'[t] θ'[t]+(Subscript[m, 1]) (-g h Sin[θ[t]]+(Subscript[x, 1])[t] (-g Cos[θ[t]]+2 θ'[t] (Subscript[x, 1]^′)[t]))+2 Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) θ'[t] (Subscript[x, 2]^′)[t]+2 (Subscript[m, p]) (Subscript[x, 2])[t] θ'[t] (Subscript[x, 2]^′)[t]+(Subscript[m, 2]) (-g h Sin[θ[t]]-g Cos[θ[t]] (Subscript[x, 2])[t]+2 (Subscript[x, 2])[t] θ'[t] (Subscript[x, 2]^′)[t]))))))/\((\((J\ Cos[
                                    α[
                                      t]]\ l\_p\%3\ m\_1\ m\_2\ m\_p\%2 + 
                                J\ Cos[α[t]]\ l\_p\%3\ m\_1\ m\_p\%3 - 
                                J\ Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_p\%3 + Cos[α[t]]\ l\_p\%3\ m\_1\%2\ m\_2\ m\_p\%2\ x\_1[t]\^2 + 
                                Cos[α[
                                      t]]\ l\_p\%3\ m\_1\%2\ m\_p\%3\ x\_1[t]\^2 - Cos[α[t]]\^3\ l\_p\%3\ m\_1\%2\ m\_p\%3\ x\_1[t]\^2 + 
                                Cos[α[
                                      t]]\ l\_p\%3\ m\_1\ m\_2\%2\ m\_p\%2\ x\_2[t]\^2 + 2\ Cos[α[t]]\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 - 
                                Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 - 
                                Cos[α[
                                      t]]\ Sin[α[t]]\^2\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 + Cos[α[
                                      t]]\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2 
                                - Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2 - 
                                Cos[α[
                                      t]]\ Sin[α[t]]\^2\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2)\)\ \((Cos[α[t]]\ l\_p\ m\_p\ \((h\ m\_2 + 
                                      \((h + 
                                        Cos[α[t]]\ l\_p)\)\ m\_p)\) - 
                                l\_p\ m\_p\ \((m\_2 + 
                                      m\_p)\)\ \((h\ Cos[α[t]] + l\_p 
                                      + Sin[α[t]]\ x\_2[t])\))\))\)),θ''[t]->-(Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-(Subscript[F, 2])+(Subscript[m, 2]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2))+(Subscript[m, p]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2)+(Subscript[l, p]) (-Sin[α[t]] ((α'[t])^2)-2 Sin[α[t]] α'[t] θ'[t]-Sin[α[t]] ((θ'[t])^2))))-(Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p])) (-g Sin[α[t] + θ[t]]+(h Sin[α[t]] - 
                                    Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t]))/(Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (h (Subscript[m, 2]) + 
                              (h + Cos[α[t]] (Subscript[l, p])) (Subscript[m, p])) - 
                        (Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p])) (h Cos[α[t]] 
                              + (Subscript[l, p]) + Sin[α[t]] (Subscript[x, 2])[t]))+((Cos[α[t]]^2 (Subscript[l, p]^2) (Subscript[m, p]^2) - 
                          (Subscript[l, p]^2) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p]))) (\(-\((\(-l\_p\)\ m\_1\ m\_p\ \((h\ m\_2 + h\ m\_p + 
                                        Cos[α[
                                        t]]\ l\_p\ m\_p)\)\ \((h\ Cos[
                                        α[t]] + l\_p + 
                                        Sin[α[t]]\ x\_2[t])\) + 
                                  Cos[α[
                                        t]]\ l\_p\ m\_p\ \((\(-h\^2\)\ m\_1\%2 + m\_1\ \((J + h\^2\ m\_p + 2\ h\ Cos[α[t]]\ l\_p\ m\_p + 
                                        l\_p\%2\ m\_p + 
                                        m\_1\ \((h\^2 + x\_1[t]\^2)\) + 
                                        2\ Sin[α[t]]\ l\_p\ m\_p\ x\_2[
                                        t] + m\_p\ x\_2[t]\^2 + 
                                        m\_2\ \((h\^2 + 
                                        x\_2[t]\^2)\))\))\))\)\) (Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-(Subscript[F, 2])+(Subscript[m, 2]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2))+(Subscript[m, p]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2)+(Subscript[l, p]) (-Sin[α[t]] ((α'[t])^2)-2 Sin[α[t]] α'[t] θ'[t]-Sin[α[t]] ((θ'[t])^2))))-(Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p])) (-g Sin[
                                        α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t]))+(Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (h (Subscript[m, 2]) + 
                                      (h + 
                                        Cos[α[t]] (Subscript[l, p])) (Subscript[m, p])) - 
                                (Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + 
                                      (Subscript[m, p])) (h Cos[α[t]] + (Subscript[l, p]) 
                                      + Sin[α[t]] (Subscript[x, 2])[t])) (-(Subscript[l, p]) (Subscript[m, 1]) (Subscript[m, p]) (h (Subscript[m, 2]) + h (Subscript[m, p]) + 
                                      Cos[α[t]] (Subscript[l, p]) (Subscript[m, p])) (-g Sin[
                                        α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t])+Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-h (Subscript[m, 1]) (-(Subscript[F, 1])+(Subscript[m, 1]) (-g Sin[θ[t]]-(Subscript[x, 1])[t] ((θ'[t])^2)))+(Subscript[m, 1]) (-g h Sin[
                                        θ[t]] (Subscript[m, p])-g Sin[
                                        α[t] + 
                                        θ[t]] (Subscript[l, p]) (Subscript[m, p])-c g Sin[θ[t]] (Subscript[m, s])-g Cos[θ[t]] (Subscript[m, p]) (Subscript[x, 2])[
                                        t]-h Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) ((α'[t])^2)+Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[t] ((α'[t])^2)-2 h Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) α'[t] θ'[t]+2 Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[t] α'[t] θ'[t]+(Subscript[m, 1]) (-g h Sin[θ[t]]+(Subscript[x, 1])[t] (-g Cos[θ[t]]+2 θ'[t] (Subscript[x, 1]^′)[t]))+2 Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) θ'[t] (Subscript[x, 2]^′)[t]+2 (Subscript[m, p]) (Subscript[x, 2])[t] θ'[t] (Subscript[x, 2]^′)[t]+(Subscript[m, 2]) (-g h Sin[θ[t]]-g Cos[θ[t]] (Subscript[x, 2])[t]+2 (Subscript[x, 2])[t] θ'[t] (Subscript[x, 2]^′)[t]))))))/\((\((J\ Cos[α[t]]\ l\_p\%3\ m\_1\ m\_2\ m\_p\%2 + 
                          J\ Cos[α[t]]\ l\_p\%3\ m\_1\ m\_p\%3 - 
                          J\ Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_p\%3 + 
                          Cos[α[
                                t]]\ l\_p\%3\ m\_1\%2\ m\_2\ m\_p\%2\ x\_1[t]\^2 + Cos[α[t]]\ l\_p\%3\ m\_1\%2\ m\_p\%3\ x\_1[t]\^2 - 
                          Cos[α[t]]\^3\ l\_p\%3\ m\_1\%2\ m\_p\%3\ x\_1[t]\^2 + Cos[α[t]]\ l\_p\%3\ m\_1\ m\_2\%2\ m\_p\%2\ x\_2[t]\^2 + 
                          2\ Cos[
                              α[
                                t]]\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 
                          - Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 - 
                          Cos[α[
                                t]]\ Sin[α[t]]\^2\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 + Cos[α[t]]\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2 
                          - Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2 - 
                          Cos[α[
                                t]]\ Sin[α[t]]\^2\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2)\)\ \((Cos[α[t]]\ l\_p\ m\_p\ \((h\ m\_2 + 
                                \((h + Cos[α[t]]\ l\_p)\)\ m\_p)\) - 
                          l\_p\ m\_p\ \((m\_2 + 
                                m\_p)\)\ \((h\ Cos[α[t]] + l\_p + 
                                Sin[α[t]]\ x\_2[t])\))\))\),α''[t]->-(-(-(Subscript[l, p]) (Subscript[m, 1]) (Subscript[m, p]) (h (Subscript[m, 2]) + h (Subscript[m, p]) + 
                                    Cos[α[
                                        t]] (Subscript[l, p]) (Subscript[m, p])) (h Cos[
                                        α[t]] + (Subscript[l, p]) + 
                                    Sin[α[t]] (Subscript[x, 2])[t]) + 
                              Cos[α[
                                    t]] (Subscript[l, p]) (Subscript[m, p]) (-(h^2) (Subscript[m, 1]^2) + 
                                    (Subscript[m, 1]) (J + (h^2) (Subscript[m, p]) + 
                                        2 h Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) + 
                                        (Subscript[l, p]^2) (Subscript[m, p]) + 
                                        (Subscript[m, 1]) ((h^2) + (Subscript[x, 1])[t]^2) + 
                                        2 Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[
                                        t] + (Subscript[m, p]) (Subscript[x, 2])[t]^2 + 
                                        (Subscript[m, 2]) ((h^2) + 
                                        (Subscript[x, 2])[t]^2)))) (Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-(Subscript[F, 2])+(Subscript[m, 2]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2))+(Subscript[m, p]) (-g Sin[θ[t]]-(Subscript[x, 2])[t] ((θ'[t])^2)+(Subscript[l, p]) (-Sin[α[t]] ((α'[t])^2)-2 Sin[α[t]] α'[t] θ'[t]-Sin[α[t]] ((θ'[t])^2))))-(Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + (Subscript[m, p])) (-g Sin[α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t]))+(Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (h (Subscript[m, 2]) + 
                                  (h + Cos[α[t]] (Subscript[l, p])) (Subscript[m, p])) - 
                            (Subscript[l, p]) (Subscript[m, p]) ((Subscript[m, 2]) + 
                                  (Subscript[m, p])) (h Cos[α[t]] + (Subscript[l, p]) + 
                                  Sin[α[t]] (Subscript[x, 2])[t])) (-(Subscript[l, p]) (Subscript[m, 1]) (Subscript[m, p]) (h (Subscript[m, 2]) + h (Subscript[m, p]) + 
                                  Cos[α[t]] (Subscript[l, p]) (Subscript[m, p])) (-g Sin[α[t] + θ[t]]+(h Sin[α[t]] - 
                                        Cos[α[t]] (Subscript[x, 2])[t]) ((θ'[t])^2)+2 Sin[α[t]] θ'[t] (Subscript[x, 2]^′)[t])+Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (-h (Subscript[m, 1]) (-(Subscript[F, 1])+(Subscript[m, 1]) (-g Sin[θ[t]]-(Subscript[x, 1])[t] ((θ'[t])^2)))+(Subscript[m, 1]) (-g h Sin[θ[t]] (Subscript[m, p])-g Sin[
                                        α[t] + 
                                        θ[t]] (Subscript[l, p]) (Subscript[m, p])-c g Sin[θ[t]] (Subscript[m, s])-g Cos[θ[t]] (Subscript[m, p]) (Subscript[x, 2])[
                                        t]-h Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) ((α'[t])^2)+Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[t] ((α'[t])^2)-2 h Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) α'[t] θ'[t]+2 Cos[α[t]] (Subscript[l, p]) (Subscript[m, p]) (Subscript[x, 2])[t] α'[t] θ'[t]+(Subscript[m, 1]) (-g h Sin[θ[t]]+(Subscript[x, 1])[t] (-g Cos[θ[t]]+2 θ'[t] (Subscript[x, 1]^′)[t]))+2 Sin[α[t]] (Subscript[l, p]) (Subscript[m, p]) θ'[t] (Subscript[x, 2]^′)[t]+2 (Subscript[m, p]) (Subscript[x, 2])[t] θ'[t] (Subscript[x, 2]^′)[t]+(Subscript[m, 2]) (-g h Sin[θ[t]]-g Cos[θ[t]] (Subscript[x, 2])[t]+2 (Subscript[x, 2])[t] θ'[t] (Subscript[x, 2]^′)[t])))))/\((J\ Cos[α[t]]\ l\_p\%3\ m\_1\ m\_2\ m\_p\%2 + 
                      J\ Cos[α[t]]\ l\_p\%3\ m\_1\ m\_p\%3 - 
                      J\ Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_p\%3 + 
                      Cos[α[
                            t]]\ l\_p\%3\ m\_1\%2\ m\_2\ m\_p\%2\ x\_1[t]\^2 
                      + Cos[α[
                            t]]\ l\_p\%3\ m\_1\%2\ m\_p\%3\ x\_1[t]\^2 - 
                      Cos[α[t]]\^3\ l\_p\%3\ m\_1\%2\ m\_p\%3\ x\_1[t]\^2 + Cos[α[t]]\ l\_p\%3\ m\_1\ m\_2\%2\ m\_p\%2\ x\_2[t]\^2 + 
                      2\ Cos[α[
                            t]]\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 - 
                      Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 - 
                      Cos[α[
                            t]]\ Sin[α[t]]\^2\ l\_p\%3\ m\_1\ m\_2\ m\_p\%3\ x\_2[t]\^2 + Cos[α[t]]\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2 - 
                      Cos[α[t]]\^3\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2 
                      - Cos[α[
                            t]]\ Sin[α[t]]\^2\ l\_p\%3\ m\_1\ m\_p\%4\ x\_2[t]\^2)\)}}

Turn off the spell checking.

In[25]:=Off[General::"spell1"]

Conversion rules to get rid of explicit time dependence:

\(\(In[26] := 
        \(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α\ }\);\)\)

We compute the right-hand side of the nonlinear state space equations:	\(x\& . [t]\)=f [x[t], u[t]]

In[27]:= f=(({{(∂_t) (Subscript[x, 1])[t]}, {(∂_t) θ[t]}, {(∂_t) (Subscript[x, 2])[t]}, {(∂_t) α[t]}, {\(∂\_{t, 2}x\_1[t]\)}, {\(∂\_{t, 2}θ[t]\)}, {\(∂\_{t, 2}x\_2[t]\)}, {\(∂\_{t, 2}α[t]\)}}))/.sol[[1]]/.ssvariables//Flatten;

The Linearized Model

Load the Control System Professional (CSP) package.

Needs["ControlSystems`Master`"];

Linearize the equations around the origin
	x=(({{0, 0, 0, 0, 0, 0, 0, 0}})), u=(({{0, 0}})) => \(x\& . [t] == A\ x[t] + \ B\ u[t]\).
using the Linearize function in CSP

\(ss = 
      Linearize[f, {x\_1, θ, x\_2, α}, 

          		{{x\_1, 0}, {θ, 0}, {x\_2, 0}, {α, 0}, {dx\_1, 
              0}, {dθ, 0}, {dx\_2, 0}, {dα, 0}}, 

          		{{F\_1, 0}, {F\_2, 0}}] // Simplify; 
Map[MatrixForm, ss]\)

We extract the matrices from the linearization and substitute with the physical values of the parameters

{𝒜, ℬ, 𝒞, 
        𝒟} = 
      ss /. StateSpace -> List /. physicalvalues;

𝒜 // MatrixForm

ℬ // MatrixForm

𝒞 // MatrixForm

𝒟 // MatrixForm

Check controllability.

Controllable[ss /. physicalvalues]

Compute a full state feedback controller using the LQG-method (LQRegulatorGains is a CSP command).

\(ℒ = 
      LQRegulatorGains[
						ss /. physicalvalues, 
						
        DiagonalMatrix[{1, 5. , 1, 5. , 0, 0, 0, 0}], 
						
        0.1\ IdentityMatrix[2]\ ]; 
ℒ // MatrixForm\)

The control law to be used is
	u[t] = -ℒ . x[t] 
where x[t] are measurements of the states. This gives the following closed loop system to simulate
	\(TraditionalForm
      \`x\& . [t] = f\ [x[t], \(-ℒ . x[t]\)]\)

Simulation

Load MathCode C++ and set Current Directory

Load the MathCode C++ package.

Needs["MathCode`"];

Since a number of temporary files will be generated by MathCode C++, we create a temporary directory and set this to the current working directory.

\(CreateDirectory["PendulumTmp"]; 

    SetDirectory[ToFileName[{".", "PendulumTmp"}]]; \)

Preliminaries

We store the states as a vector

(Subscript[x, ss]) := {(Subscript[x, 1]), θ, (Subscript[x, 2]), α, (Subscript[dx, 1]), dθ, (Subscript[dx, 2]), 
        dα}

and close the feedback loop which gives the following right-hand side of the state space equations.

f1 = 
      f /. Thread[{(Subscript[F, 1]), (Subscript[F, 2])} -> -ℒ . (Subscript[x, ss])];

Turn off spell checking.

Off[General::"spell1"]

Change names of variables to only ASCII characters (needed for code generation).

\(\(Cvariables\  = 

      		{x\_1 -> \ x1, θ\  -> \ theta, 
			
        x\_2\  -> \ x2, α\  -> \ alpha, 
			
        dx\_1\  -> \ dx1, dθ\  -> \ dtheta, 
			
        dx\_2\  -> \ dx2, \ dα -> \ dalpha}; \)\)

The right hand side of the closed loop state space equations (intended for code generation):

rhs = f1 /. physicalvalues  /.  Cvariables;

The Right-Hand Side of the State SpacEquations

We define the function ClosedLoopPendulumEquationsRHS which has the huge rhs expression from above as its body. Note that the expression also can read from the file PendulumExpr.m which is created above (in case of memory problems). This file makes it quicker to try out the compilation in different sessions without recomputing rhs each time.

\(\(ClosedLoopPendulumEquationsRHS[
	Real@\ x1_, Real@\ theta_, 
          Real@\ x2_, Real@\ alpha_, 
	Real@\ dx1_, Real@\ dtheta_, 
          Real@\ dx2_, Real@\ dalpha_] -> Real[8] = rhs; \)\)

Note that we have added type declarations to be able to generate code for this function later on.

Simulation of the Nonlinear Model within Mathematica

We define (Subscript[f, cl])[t] to be a short notation for the ClosedLoopPendulumEquationsRHS.

(Subscript[f, cl])[t_] := 
      ClosedLoopPendulumEquationsRHS[x1[t], theta[t], x2[t], alpha[t], 
        dx1[t], dtheta[t], dx2[t], dalpha[t]]

The state vector is

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

The differential equations for the closed loop system are

deq := Thread[x'[t] == (Subscript[f, cl])[t]]

Here are the initial conditions for the simulation. Cart 1 is positioned at -1, the seesaw is horizontal, cart 2 is positioned at +1 and the pendulum has a small deviation of 5.7° from the vertical axis. The system is at rest at these positions (all time derivatives are zero).

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

We simulate the system using NDSolve.

Standard usage

dsolStandard = 
        NDSolve[ 
          deq  ⋃  initeq, {x1, theta, x2, alpha, dx1, dtheta, dx2, 
            dalpha}, {t, 0, 10} ];  // TimingNDSolveStandardTime = First[%];

The linear system

dlineq := 
      Thread[x'[t] == 
            (𝒜 - ℬ . ℒ) . 
              x[t]] // Chop

We simulate the linear system using NDSolve.

dsollin = 
      NDSolve[ dlineq  ⋃  initeq, {x1, theta, x2, alpha, dx1, 
          dtheta, dx2, dalpha}, {t, 0, 8} ];

Plot the result for the first 4 state variables (the Mathematica standard usage of NDSolve) together with the result for the linear system. The other simulation methods above can be shown to give the same result as the standard usage of NDSolve.

\(\(Map[
	
      Plot[#, {t, 0, 8}, PlotRange -> All, 
          DisplayFunction -> Identity, 
          PlotStyle -> {{RGBColor[0, 0, 0]}, {RGBColor[1, 0, 0]}}]&, {
          Part[Flatten[x[t] /. dsolStandard], Range[4]], 
		
          Part[Flatten[x[t] /. dsollin], Range[4]]} // Transpose\ ]; \)\)

\(\(Show[GraphicsArray[%], 
      Display
		Function -> $DisplayFunction]; \)\)

We notice that the linear response (red) differs significantly from the nonlinear one.

External Simulation of the Nonlinear Model

Code for External Simulation of the Nonlinear Model

We first make an implementation of a state equation solver using the Runge-Kutta method.

\(RK[Integer@\ n_, Real@\ h_, Real@\ t0_, Real[_]@\ startv_, 
          Integer@\ dimen_, Integer@\ dimPlusOne_] -> 
        Real[n, dimPlusOne] := 
			Module[{
					Real[dimen]@\ {x, 
              k1, k2, k3, k4}, 
					Integer@\ i, 
					Real@\ 
            t, 
					Real[n, dimPlusOne]@\ 
            res\ 
				}, 
				t = t0; 
				x = 
          startv; 
				res[[1, 
            1]] = 
          t; 
				res[[1, 
            2 | dimPlusOne]] = x; 
				For[i = 1, \ 
          i ≤ n - 1, \ \(i++\), 
					k1 = 
            h\ ClosedLoopPendulumEquationsRHS[
                x[[1]], 
                x[[2]], 
                x[[3]], 
                x[[4]], 
                x[[5]], 
                x[[6]], 
                x[[7]], 
                x[[8]]]; 
					k2 = h\ ClosedLoopPendulumEquationsRHS[
                x[[1]] + 
                  k1[[1]]/2, 
                x[[2]] + 
                  k1[[2]]/2, 
                x[[3]] + 
                  k1[[3]]/2, 
                x[[4]] + 
                  k1[[4]]/2, 
                x[[5]] + 
                  k1[[5]]/2, 
                x[[6]] + 
                  k1[[6]]/2, 
                x[[7]] + 
                  k1[[7]]/2, 
                x[[8]] + 
                  k1[[8]]/
                    2]; 
					k3 = 
            h\ ClosedLoopPendulumEquationsRHS[
                x[[1]] + 
                  k2[[1]]/2, 
                x[[2]] + 
                  k2[[2]]/2, 
                x[[3]] + 
                  k2[[3]]/2, 
                x[[4]] + 
                  k2[[4]]/2, 
                x[[5]] + 
                  k2[[5]]/2, 
                x[[6]] + 
                  k2[[6]]/2, 
                x[[7]] + 
                  k2[[7]]/2, 
                x[[8]] + 
                  k2[[8]]/
                    2]; 
					k4 = 
            h\ ClosedLoopPendulumEquationsRHS[
						x[[1]] + 
                  k3[[1]], 
                x[[2]] + 
                  k3[[2]], 
                x[[3]] + 
                  k3[[3]], 
                x[[4]] + 
                  k3[[4]], 
                x[[5]] + 
                  k3[[5]], 
                x[[6]] + 
                  k3[[6]], 
                x[[7]] + 
                  k3[[7]], 
                x[[8]] + 
                  k3[[8]]]; 
					x = x + 1/6\ \((k1 + 2\ k2 + 2  k3 + k4)\); 
					t = 
            t + h; 
					res[[i + 1, 
              1]] = 
            t; 
					res[[i + 1, 
              2 | dimPlusOne]] = 
            x;
				]; 
				res
			]\)

We use RK in the following ODE solver.

\(ndSolve[initvalue_, maxstepsize_, {x_, xmin_, xmax_}] := 
	
      Module[{n, dimen}, 
		
        n = \ IntegerPart[\((xmax - xmin)\)/maxstepsize] + 1; 
		
        dimen = Length[initvalue]; 
		
        res = RK[n, maxstepsize, xmin, initvalue, dimen, dimen + 1]; 
		
        Array[\((
              Interpolation[
                AppendRows[
                  Transpose[{
                      res[[_, 1]]}], 

                  									
                  Transpose[{
                      res[[_, # + 1
                          ]]}]]])\)&, \ {8}]
	]\)

To get a correct estimate of the time spent by the external code for simulating the system, we define the following function which repeats the external simulation n times.

\(RKRepeated[Integer@\ n_, Real@\ h_, Real@\ t0_, Real[_]@\ startv_, 
          Integer@\ dimen_, Integer@\ dimPlusOne_, Integer@loops_] -> 
        Real[n, dimPlusOne] := 
	\ 
      Module[{Real[n, dimPlusOne]@\ res}, \ 
		
        Do[	res\  = \ RK[n, h, t0, startv, dimen, dimPlusOne], {loops}]; 

        		res
	]\)

\(ndSolveRepeated[
	initvalue_, maxstepsize_, {x_, xmin_, xmax_}, 
        loops_] := 
	
      Module[{n, dimen}, 
		
        n = \ IntegerPart[\((xmax - xmin)\)/maxstepsize] + 1; 
		
        dimen = Length[initvalue]; 
		
        res = RKRepeated[n, maxstepsize, xmin, initvalue, dimen, dimen + 1, \ 
            loops]; 
		
        Array[\((
              Interpolation[
                AppendRows[
                  Transpose[{
                      res[[_, 1]]}], 

                  									
                  Transpose[{
                      res[[_, # + 1
                          ]]}]]])\)&, \ {8}]
	]\)

Compilation

We are now ready to compile the package. The rhs of the state equations are optimized using common subexpression elimination.

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

Here is the corresponding code.

! ! Global . cc

All necessary files for building binaries are now generated. The MakeBinary command can build either a stand-alone application using the code specified by the option GenerateMainFileAndFunction or a MathLink version which can be easily installed into Mathematica. We build the MathLink version.

MakeBinary[];

To use the generated code for the simulation, we only have to install the code using the Install command.

External Simulation

This functionwill allow us to measure the time for external processes.

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

Uncompiled code (computations within Mathematica)

dsolUncompiled = 
        ndSolve[{-1, 0, 1, 0.1, 0, 0, 0, 0}, 0.01, {t, 0. , 10. }];  // 
      TimingndSolveUncompiledTime = First[%];

We plot the result to verify the Runge-Kutta solver.

Map[
      Plot[#[t], {t, 0, 8}, PlotRange -> All, 
          DisplayFunction -> Identity]&, 
      Part[dsolUncompiled, Range[4]]];

Show[GraphicsArray[%], DisplayFunction -> $DisplayFunction];

Compiled code (external computations)

We install the executable code.

InstallCode[];

The external simulation is performed 200 times to get an accurate measure of its timing.

\(\(dsolCompiled = 
        ndSolveRepeated[{\(-1\), 0, 1, 0.1, 0, 0, 0, 0}, 
          0.01, {t, 0. , 10. }, 200]; \)
		 // AbsTime\)ndSolveCompiledTime = First[%]/200;

We plot the result to verify the compiled Runge-Kutta solver.

Map[
      Plot[#[t], {t, 0, 8}, PlotRange -> All, 
          DisplayFunction -> Identity]&, Part[dsolCompiled, Range[4]]];

Show[GraphicsArray[%], DisplayFunction -> $DisplayFunction];

Looks good!

Comparing Performance

We present the comparision of performance in the following table.