ME 405 HW 0x04: Linearization and Simulation of the Balancing Platform Board

Authors: Ryan McLaughlin and Matthew Frost
Originally Created: 04/25/21
Last Modified: 05/03/21

Initial Setup and Variable Definitions

from sympy import *
init_printing()
import warnings 
warnings.filterwarnings("ignore", category=UserWarning)

# Create empty classes which are somewhat equivalent to MATLAB struct()
class functions:
    pass
class functions0:
    pass
class equations:
    pass
class equationNames:
    pass

Define fun, eqn, and eqnNames as the "structures." These will now allow for notation for fun.xB to save an equation xB within the functions class.

fun = functions()
fun0 = functions0()
eqn = equations()
eqnNames = equationNames()

Define symbolic variables (constants and functions of time)

r_C, t, r_B, r_G, m_B, m_P, I_bar_p_yy, M_y, g, T_x, L_p, r_M, I_B = symbols('r_C, t, r_B, r_G, m_B, m_P, \overline{I}_{pyy}, M_y, g, T_x, L_p, r_M I_B')     
x = Function('x')(t)
theta_y = Function('theta_y')(t)

Compute the moment of inertia of the ball, $\overline{I}_B$

I_B = I_B.subs([(Symbol('I_B'),.4*Symbol('m_B')*Symbol('r_B')**2)])

1) Ball Kinematics

X-Direction Kinematic Equations

fun.xB = (r_C + r_B)*sin(theta_y) + x*cos(theta_y) # x displacement of ball
# print('xB:')
# display(fun.xB)
fun.xBdot = diff(fun.xB,t)      # x velocity of ball
# print('xBdot:')
# display(fun.xBdot)
fun.xBddot = diff(fun.xBdot,t)  # x accceleration of ball
# print('xBddot:')
# display(fun.xBddot)

Z-Direction Kinematic Equations

fun.zB = (r_C + r_B)*cos(theta_y) - x*sin(theta_y) # z displacement of ball
# print('zB:')
# display(fun.zB)
fun.zBdot = diff(fun.zB,t)                        # z velocity of ball
# print('zBdot:')
# display(fun.zBdot)
fun.zBddot = diff(fun.zBdot,t)                    # z acceleration of ball
# print('zBddot:')
# display(fun.zBddot)

2) Motor Torque to Platform Moment

Define the effective moment on the platform, $ M_y $ in terms of the input motor torque, $ T_x $

## DEFINE My IN TERMS OF MOTOR TORQUE
M_y = -L_p*T_x/r_M
# display(M_y)

3) Equations of Motion

3.1) System 1: Ball and Platform Together

fun0.lhs1 = m_P*g*r_G*sin(theta_y) + m_B*g*( (r_C+r_B)*sin(theta_y)+x*cos(theta_y) ) + M_y;
fun0.rhs1 = ((I_bar_p_yy + m_P*r_G**2)*diff(theta_y,t,2) 
              - m_B*fun.zBddot*( (r_C+r_B)*sin(theta_y)+x*cos(theta_y) )
              + m_B*fun.xBddot*( (r_C+r_B)*cos(theta_y)-x*sin(theta_y)) 
              + (I_B)*((diff(x,t,2)/r_B) + diff(theta_y,t,2)));
fun0.EOM1 = Eq(fun0.lhs1,fun0.rhs1)
fun.EOM1  = fun0.EOM1.subs([(diff(x,t,2),Symbol('\ddot{x}')),(diff(x,t),Symbol('\dot{x}')),
                             (diff(theta_y,t,2),Symbol('\ddot{\\theta}_y')),(diff(theta_y,t),Symbol('\dot{\\theta}_y')),
                             (x,Symbol('x')),(theta_y,Symbol('\\theta_y'))])
# print('EOM1')
# display(fun.EOM1);

3.2) System 2: Ball Only

fun0.lhs2 = m_B*g*r_B*sin(theta_y);
fun0.rhs2 = (-m_B*fun.zBddot*r_B*sin(theta_y)
            + m_B*fun.xBddot*r_B*cos(theta_y) 
            + (I_B)*((diff(x,t,2)/r_B) + diff(theta_y,t,2)));
fun0.EOM2 = Eq(fun0.lhs2,fun0.rhs2)
fun.EOM2 = fun0.EOM2.subs([(diff(x,t,2),Symbol('\ddot{x}')),(diff(x,t),Symbol('\dot{x}')),
                             (diff(theta_y,t,2),Symbol('\ddot{\\theta}_y')),(diff(theta_y,t),Symbol('\dot{\\theta}_y')),
                             (x,Symbol('x')),(theta_y,Symbol('\\theta_y'))])
# print('EOM2')
# display(fun.EOM2)

4) Convert EOM's to Matrix Form

Next, the linear_eq_to_matrix function (similar to equationsToMatrix in MATLAB®) is used.

# eqnsForMatrix = Matrix([[fun.EOM1],[fun.EOM2]])
# varsForMatrix = Matrix([[diff(theta_y,t,2)],[diff(x,t,2)]])
M, f = linear_eq_to_matrix([fun.EOM1, fun.EOM2], [Symbol('\ddot{x}'), Symbol('\ddot{\\theta}_y')])
print('The M matrix is:')
display(simplify(M))
print('The f matrix is:')
display(simplify(f))

The M matrix is:

$\displaystyle \left[\begin{matrix}- m_{B} \left(1.4 r_{B} + 1.0 r_{C}\right) & - 1.0 \overline{I}_{pyy} - 1.4 m_{B} r_{B}^{2} - 2.0 m_{B} r_{B} r_{C} - 1.0 m_{B} r_{C}^{2} - 1.0 m_{B} x^{2} - 1.0 m_{P} r_{G}^{2}\\- 1.4 m_{B} r_{B} & - m_{B} r_{B} \left(1.4 r_{B} + 1.0 r_{C}\right)\end{matrix}\right]$

The f matrix is:

$\displaystyle \left[\begin{matrix}\frac{L_{p} T_{x}}{r_{M}} + 2 \dot{\theta}_y \dot{x} m_{B} x - g m_{B} r_{B} \sin{\left(\theta_{y} \right)} - g m_{B} r_{C} \sin{\left(\theta_{y} \right)} - g m_{B} x \cos{\left(\theta_{y} \right)} - g m_{P} r_{G} \sin{\left(\theta_{y} \right)}\\- m_{B} r_{B} \left(\dot{\theta}_y^{2} x + g \sin{\left(\theta_{y} \right)}\right)\end{matrix}\right]$

Convert matrices to form of $x=M^{-1}f$

G = simplify(M.inv()*f)
# print('xDotMatrix')
# display(xDotMatrix)
h = Matrix([[Symbol('\dot{x}')], [Symbol('\dot{\\theta}_y')],[G[0]],[G[1]]])
# display(G)

Create $h$ matrix for state space representation

# CREATE h MATRIX FOR STATE SPACE
h = Matrix([[Symbol('\dot{x}')], [Symbol('\dot{\\theta}_y')],[G[0]],[G[1]]])
# print('h')
# display(h)

5) Jacobian Linearization

Define the state variable matrix

x_vec_variables = Matrix([[Symbol('x')],[Symbol('\\theta_y')],[Symbol('\dot{x}')],[Symbol('\dot{\\theta}_y')]])

Compute the Jacobian with respect to the state vector, and define operating point by subsituting zeros for all state variables present

Jx = h.jacobian(x_vec_variables)
Jx_subbed = Jx.subs([(Symbol('x'),0),(Symbol('\\theta_y'),0),(Symbol('\dot{x}'),0),(Symbol('\dot{\\theta}_y'),0)])
# display(Jx_subbed)

Substitue values for all other constants to determine a numerical form of the $A$ matrix

A = Jx_subbed.subs([(r_M,0.06),(r_B,0.0105),(r_G, 0.042),(L_p, 0.110),(r_C, 0.050),(m_B, 0.030),(m_P, 0.400),(I_bar_p_yy, 1.88e-3),(g,9.81),(Symbol('x'),0),(Symbol('\\theta_y'),0),(Symbol('\dot{x}'),0),(Symbol('\dot{\\theta}_y'),0),(Symbol('T_x'),0)])
print('A Matrix:')
display(A)

A Matrix:

$\displaystyle \left[\begin{matrix}0 & 0 & 1 & 0\\0 & 0 & 0 & 1\\-5.21699855336125 & 4.01109511649826 & 0 & 0\\112.887140258203 & 64.8294719768538 & 0 & 0\end{matrix}\right]$

Compute the Jacobian with respect to the input vector, define operating point, and substitute numerical values to determine $B$ matrix

Ju = h.jacobian([Symbol('T_x')])
Ju_subbed = Ju.subs([(Symbol('x'),0),(Symbol('\\theta_y'),0),(Symbol('\dot{x}'),0),(Symbol('\dot{\\theta}_y'),0),(Symbol('T_x'),0)])
# display(Ju_subbed)

B = Ju_subbed.subs([(r_M,0.06),(r_B,0.0105),(r_G, 0.042),(L_p, 0.110),(r_C, 0.050),(m_B, 0.030),(m_P, 0.400),(I_bar_p_yy, 1.88e-3),(g,9.81),(Symbol('x'),0),(Symbol('\\theta_y'),0),(Symbol('\dot{x}'),0),(Symbol('\dot{\\theta}_y'),0),(Symbol('T_x'),0)])
print('B Matrix')
display(B)

B Matrix

$\displaystyle \left[\begin{matrix}0\\0\\32.4991415148792\\-703.227173428607\end{matrix}\right]$

6) Simulation of State Space Model

After linearizing the system, we are now ready to use the scipy Python library to simulate the time response of the balancing platform system in both open- and closed-loop configurations

from scipy.signal import lti, lsim
from numpy import array, eye, linspace, ones, round, zeros_like
import matplotlib.pyplot as plt
plt.rcParams['font.size'] = '9'
plt.rcParams['font.family'] = 'serif'
plt.rcParams['font.sans-serif'] = 'times'

Convert earlier defined $A$ and $B$ matrices to numpy arrays

AA = array([[A[0,0], A[0,1], A[0,2], A[0,3]], [A[1,0], A[1,1], A[1,2], A[1,3]], [A[2,0], A[2,1], A[2,2], A[2,3]], [A[3,0], A[3,1], A[3,2], A[3,3]]])
BB = array([[B[0,0]], [B[1,0]], [B[2,0]], [B[3,0]]])
CC = array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
DD = array([[0], [0], [0], [0]])
# display(AA)
# display(BB)
# display(CC)
# display(DD)

Define a linear, time-invariant state space system based on the $A$, $B$, $C$, and $D$ matrices defined above using the scipy.lti

ltiSystem = lti(AA, BB, CC, DD)

6.1) Open Loop Simulation 1

Initial conditions: Ball is at rest on a level platform, located in the center of the platfrom, and no input is applied

t1 = linspace(0, 1, num=50)
u1 = zeros_like(t)
x01 = array([[0, 0, 0, 0]])

tout1, y1, x1 = lsim(ltiSystem, U = u1, T = t1, X0 = x01)

fig1,axs1 = plt.subplots(2,2,figsize = (9,5), dpi = 100, constrained_layout=True)
plt.subplots_adjust(hspace = 0.7,wspace = 0.4)

axs1[0,0].plot(t1, y1[:,0],'k',linewidth = 0.75)
axs1[0,0].set(title='Ball Displacement',xlabel = r'Time, $t$ [ s ]',ylabel = r'$x(t)$ [ m ]')
axs1[0,1].plot(t1, y1[:,1],'k',linewidth = 0.75)
axs1[0,1].set(title='Platform Angle',xlabel = r'Time, $t$ [ s ]',ylabel = r'${\theta}_y(t)$ [ rad ]')
axs1[1,0].plot(t1, y1[:,2],'k',linewidth = 0.75)
axs1[1,0].set(title='Ball Velocity',xlabel = r'Time, $t$ [ s ]',ylabel = r'$\dot{x}(t)$ [ m/s ]')
axs1[1,1].plot(t1, y1[:,3],'k',linewidth = 0.75)
axs1[1,1].set(title='Platform Angular Velocity',xlabel = r'Time, $t$ [ s ]',ylabel = r'$\dot{\theta}_y(t)$ [ rad/s ]')

plt.show()
fig1.savefig('405_HW0x04_OpenLoop1.png')

6.2) Open Loop Simulation 2

Initial conditions: Ball is at rest on a level platform, offset 5 cm horizontally from the center of the platform, and no input is applied

t2 = linspace(0, 0.4, num=50)
u2 = zeros_like(t)
x02 = array([[0.05, 0, 0, 0]])

tout2, y2, x2 = lsim(ltiSystem, U = u2, T = t2, X0 = x02)

fig2,axs2 = plt.subplots(2,2,figsize = (9,5), dpi = 100, constrained_layout=True)
plt.subplots_adjust(hspace = 0.7,wspace = 0.4)

axs2[0,0].plot(t2, y2[:,0],'k',linewidth = 0.75)
axs2[0,0].set(title='Ball Displacement',xlabel = r'Time, $t$ [ s ]',ylabel = r'$x(t)$ [ m ]')
axs2[0,1].plot(t2, y2[:,1],'k',linewidth = 0.75)
axs2[0,1].set(title='Platform Angle',xlabel = r'Time, $t$ [ s ]',ylabel = r'${\theta}_y(t)$ [ rad ]')
axs2[1,0].plot(t2, y2[:,2],'k',linewidth = 0.75)
axs2[1,0].set(title='Ball Velocity',xlabel = r'Time, $t$ [ s ]',ylabel = r'$\dot{x}(t)$ [ m/s ]')
axs2[1,1].plot(t2, y2[:,3],'k',linewidth = 0.75)
axs2[1,1].set(title='Platform Angular Velocity',xlabel = r'Time, $t$ [ s ]',ylabel = r'$\dot{\theta}_y(t)$ [ rad/s ]')

plt.show()
fig2.savefig('405_HW0x04_OpenLoop2.png')

6.3) Closed Loop Simulation

Initial conditions and controller: Ball is at rest on a level platform, offset 5 cm horizontally from the center of the platform, and closed-loop feedback is utilized

Define the controller gain matrix, and find the new $A$, $B$, $C$, and $D$ matrices

K = array([[-0.3, -0.2, -0.05, -0.02]])
A_CL = AA-BB*K
B_CL = array([[0], [0], [0], [0]])
C_CL = CC-DD*K
D_CL = array([[0], [0], [0], [0]]) 

New state space system definition

ltiSystemClosed = lti(A_CL, B_CL, C_CL, D_CL)

Plot the system response

t3 = linspace(0, 10, num=500)

tout3, y3, x3 = lsim(ltiSystemClosed, U = u2, T = t3, X0 = x02)

fig3,axs3 = plt.subplots(2,2,figsize = (9,5), dpi = 100, constrained_layout=True)
plt.subplots_adjust(hspace = 0.7,wspace = 0.4)

axs3[0,0].plot(t3, y3[:,0],'k',linewidth = 0.75)
axs3[0,0].set(title='Ball Displacement',xlabel = r'Time, $t$ [ s ]',ylabel = r'$x(t)$ [ m ]')
axs3[0,1].plot(t3, y3[:,1],'k',linewidth = 0.75)
axs3[0,1].set(title='Platform Angle',xlabel = r'Time, $t$ [ s ]',ylabel = r'${\theta}_y(t)$ [ rad ]')
axs3[1,0].plot(t3, y3[:,2],'k',linewidth = 0.75)
axs3[1,0].set(title='Ball Velocity',xlabel = r'Time, $t$ [ s ]',ylabel = r'$\dot{x}(t)$ [ m/s ]')
axs3[1,1].plot(t3, y3[:,3],'k',linewidth = 0.75)
axs3[1,1].set(title='Platform Angular Velocity',xlabel = r'Time, $t$ [ s ]',ylabel = r'$\dot{\theta}_y(t)$ [ rad/s ]')

plt.show()
fig3.savefig('405_HW0x04_ClosedLoop.png')