Mechatronics HW0x04: Simulation or Reality?

Step 0: Define System from HW0x02

Imports

from sympy.matrices import Matrix
from sympy import Symbol, Function
from sympy import sin, cos

Define Constants

r_m = 0.060   # [m]
l_r = 0.050   # [m]
r_B = 0.0105  # [m]
r_G = 0.042   # [m]
l_P = 0.110   # [m]
r_P = 0.0325  # [m]
r_C = 0.050   # [m]
m_B = 0.030   # [kg]
m_P = 0.400   # [kg]
I_P = 1.88e-3 # [kg*m^2]
b   = 10      # [m*Nm*s/rad]
g   = 9.81    # [m/s^2]

Define symbols and functions with respect to time

t = Symbol('t')
x = Function('x')(t)
theta_y = Function('theta_y')(t)
x_dot = x.diff(t)
theta_y_dot = theta_y.diff(t)
x_dd = x.diff((t, 2))
theta_y_dd = theta_y.diff((t, 2))

T_x = Symbol('T_x')

Define M matrix and f matrix

M = Matrix([[(-m_B*(7/5*r_B + r_C)), (-((I_P + m_P*r_G**2) + m_B*(2/5*r_B**2 + (r_B + r_C)**2 + x**2)))],
           [(-7/5*m_B*r_B), (-m_B*r_B*(7/5*r_B + r_C))]])
f = Matrix([[b*theta_y_dot - g*m_B*((r_B + r_C)*sin(theta_y) + x*cos(theta_y)) + T_x * l_P/r_m + 2*m_B*theta_y_dot*x*x_dot - g*m_P*r_G*sin(theta_y)],
           [-m_B*r_B*x*theta_y_dot**2 - g*m_B*r_B*sin(theta_y)]])

M

$\displaystyle \left[\begin{matrix}-0.001941 & - 0.03 x^{2}{\left(t \right)} - 0.0026967305\\-0.000441 & -2.03805 \cdot 10^{-5}\end{matrix}\right]$

f

$\displaystyle \left[\begin{matrix}1.83333333333333 T_{x} - 0.2943 x{\left(t \right)} \cos{\left(\theta_{y}{\left(t \right)} \right)} + 0.06 x{\left(t \right)} \frac{d}{d t} \theta_{y}{\left(t \right)} \frac{d}{d t} x{\left(t \right)} - 0.18261315 \sin{\left(\theta_{y}{\left(t \right)} \right)} + 10 \frac{d}{d t} \theta_{y}{\left(t \right)}\\- 0.000315 x{\left(t \right)} \left(\frac{d}{d t} \theta_{y}{\left(t \right)}\right)^{2} - 0.00309015 \sin{\left(\theta_{y}{\left(t \right)} \right)}\end{matrix}\right]$

Step 1: Linearize the system

Convert the M and f matrices to Standard state-space form, then perform Jacobian linearization to get A and B matrices.

g = M.inv() * f

g

$\displaystyle \left[\begin{matrix}\frac{\left(- 0.000315 x{\left(t \right)} \left(\frac{d}{d t} \theta_{y}{\left(t \right)}\right)^{2} - 0.00309015 \sin{\left(\theta_{y}{\left(t \right)} \right)}\right) \left(- 5.823 \cdot 10^{-5} x^{2}{\left(t \right)} - 5.2343539005 \cdot 10^{-6}\right)}{2.567943 \cdot 10^{-8} x^{2}{\left(t \right)} + 2.2315669236 \cdot 10^{-9}} + \frac{3.95585504999999 \cdot 10^{-8} \left(1.83333333333333 T_{x} - 0.2943 x{\left(t \right)} \cos{\left(\theta_{y}{\left(t \right)} \right)} + 0.06 x{\left(t \right)} \frac{d}{d t} \theta_{y}{\left(t \right)} \frac{d}{d t} x{\left(t \right)} - 0.18261315 \sin{\left(\theta_{y}{\left(t \right)} \right)} + 10 \frac{d}{d t} \theta_{y}{\left(t \right)}\right)}{2.567943 \cdot 10^{-8} x^{2}{\left(t \right)} + 2.2315669236 \cdot 10^{-9}}\\\frac{0.001941 \left(- 0.000315 x{\left(t \right)} \left(\frac{d}{d t} \theta_{y}{\left(t \right)}\right)^{2} - 0.00309015 \sin{\left(\theta_{y}{\left(t \right)} \right)}\right)}{1.323 \cdot 10^{-5} x^{2}{\left(t \right)} + 1.1496996 \cdot 10^{-6}} + \frac{0.000441 \left(1.83333333333333 T_{x} - 0.2943 x{\left(t \right)} \cos{\left(\theta_{y}{\left(t \right)} \right)} + 0.06 x{\left(t \right)} \frac{d}{d t} \theta_{y}{\left(t \right)} \frac{d}{d t} x{\left(t \right)} - 0.18261315 \sin{\left(\theta_{y}{\left(t \right)} \right)} + 10 \frac{d}{d t} \theta_{y}{\left(t \right)}\right)}{- 1.323 \cdot 10^{-5} x^{2}{\left(t \right)} - 1.1496996 \cdot 10^{-6}}\end{matrix}\right]$

Create the h vector

h = Matrix([[x_dot],
            [theta_y_dot]]).col_join(g)
h

$\displaystyle \left[\begin{matrix}\frac{d}{d t} x{\left(t \right)}\\\frac{d}{d t} \theta_{y}{\left(t \right)}\\\frac{\left(- 0.000315 x{\left(t \right)} \left(\frac{d}{d t} \theta_{y}{\left(t \right)}\right)^{2} - 0.00309015 \sin{\left(\theta_{y}{\left(t \right)} \right)}\right) \left(- 5.823 \cdot 10^{-5} x^{2}{\left(t \right)} - 5.2343539005 \cdot 10^{-6}\right)}{2.567943 \cdot 10^{-8} x^{2}{\left(t \right)} + 2.2315669236 \cdot 10^{-9}} + \frac{3.95585504999999 \cdot 10^{-8} \left(1.83333333333333 T_{x} - 0.2943 x{\left(t \right)} \cos{\left(\theta_{y}{\left(t \right)} \right)} + 0.06 x{\left(t \right)} \frac{d}{d t} \theta_{y}{\left(t \right)} \frac{d}{d t} x{\left(t \right)} - 0.18261315 \sin{\left(\theta_{y}{\left(t \right)} \right)} + 10 \frac{d}{d t} \theta_{y}{\left(t \right)}\right)}{2.567943 \cdot 10^{-8} x^{2}{\left(t \right)} + 2.2315669236 \cdot 10^{-9}}\\\frac{0.001941 \left(- 0.000315 x{\left(t \right)} \left(\frac{d}{d t} \theta_{y}{\left(t \right)}\right)^{2} - 0.00309015 \sin{\left(\theta_{y}{\left(t \right)} \right)}\right)}{1.323 \cdot 10^{-5} x^{2}{\left(t \right)} + 1.1496996 \cdot 10^{-6}} + \frac{0.000441 \left(1.83333333333333 T_{x} - 0.2943 x{\left(t \right)} \cos{\left(\theta_{y}{\left(t \right)} \right)} + 0.06 x{\left(t \right)} \frac{d}{d t} \theta_{y}{\left(t \right)} \frac{d}{d t} x{\left(t \right)} - 0.18261315 \sin{\left(\theta_{y}{\left(t \right)} \right)} + 10 \frac{d}{d t} \theta_{y}{\left(t \right)}\right)}{- 1.323 \cdot 10^{-5} x^{2}{\left(t \right)} - 1.1496996 \cdot 10^{-6}}\end{matrix}\right]$

Now linearize h by calculating the Jacobian with respect to x at 0 to calculate A matrix

x_vector = Matrix([[x],
                   [theta_y],
                   [x_dot],
                   [theta_y_dot]])
x_vector

$\displaystyle \left[\begin{matrix}x{\left(t \right)}\\\theta_{y}{\left(t \right)}\\\frac{d}{d t} x{\left(t \right)}\\\frac{d}{d t} \theta_{y}{\left(t \right)}\end{matrix}\right]$

x_i = [(i, 0) for i in x_vector]

A = h.jacobian(x_vector).subs(x_i).doit()
A

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

And do the same to find the B matrix (but with respect to u)

u_vector = Matrix([T_x])

B = h.jacobian(u_vector).subs(x_i).doit()
B

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

Step 2: Open Loop model

Define a Model class, on which to perform all of the responses:

import os

from sympy import shape
from matplotlib import pyplot
from datetime import datetime


class Model():
    
    def __init__(self, x_0, A, B, time_step=None):
        self.x_0 = x_0
        self.state = x_0   # Initial Conditions
        self.A = A
        self.B = B
        self.t = 0
        self.time_step = time_step
        
    def step(self, u, time_step=None):
        """
        @brief Step the model by one unit of time.
        @param u The state input vector (must be a 1x1 matrix)
        @param time_step The amount of time to step forward
        @return The new state vector (at time t+time_step)
        """
        if time_step is None:
            if self.time_step:
                time_step = self.time_step
            else:
                raise RuntimeError('Time step is undefined')
        if shape(u) != (1, 1):
            raise RuntimeError('Input vector must be a 1x1 Matrix')

            
        self.state += (self.A * self.state + self.B * u) * time_step
        self.t += time_step
        return self.state
  

    def response(self, time, controller, time_step=None):
        """
        @brief Generate a model response for the specified amount of time.
        @param initial_u The state input vector (must be a 1x1 matrix)
        @param time The total length of the response.
        @param time_step The amount of time to step forward at each iteration
        @param controller A function to apply to the current state to calculate the next input vector.
        @return A dictionary of the complete response, made of equal-length lists.
        """  
        if time_step is None:
            if self.time_step:
                time_step = self.time_step
            else:
                raise RuntimeError('Time step is undefined')
        
        self.reset()
        resp = {       "time" : [],
                          "x" : [],
                    "theta_y" : [],
                      "x_dot" : [],
                "theta_y_dot" : []}
       
        while self.t <= time:
            resp["time"].append(self.t)
            resp["x"].append(self.state[0])
            resp["theta_y"].append(self.state[1])
            resp["x_dot"].append(self.state[2])
            resp["theta_y_dot"].append(self.state[3])
            self.step(controller(self.state), time_step)
            
        return resp
    
    def reset(self):
        """
        @brief Reset the state to initial conditions
        """
        self.t = 0
        self.state = self.x_0
    
    @staticmethod
    def plot_response(resp, loc="plots"):
        """
        @brief Create a plot for each state variable vs time for the whole response.
        @param resp The response to create, as generated by Model.respone()
        @param loc The folder in which to save the plots.  If unspecified,
                      they will be saved to the directory this was called in.
        """
        
        # Create directory for plots
        file_time = datetime.now().strftime("%Y_%m_%d_%I-%M-%S_%p")
        location = "{}/{}".format(loc, file_time)
        os.mkdir(location)
        
        # Plot horizontal distance response
        pyplot.figure(1)
        pyplot.plot(resp["time"], [x*1000 for x in resp["x"]])
        pyplot.title("Horizontal distance vs. Time")
        pyplot.xlabel(r'Time, $t$ (s)')
        pyplot.ylabel(r'Horizontal distance, $x$ (mm)')
        pyplot.savefig("{}/{}.png".format(location, "x-vs-time"))
        
        # Plot Platform angle response
        pyplot.figure(2)
        pyplot.plot(resp["time"], resp["theta_y"])
        pyplot.title(r'Platform Angle, $\theta_{y}$ vs. Time')
        pyplot.xlabel(r'Time, $t$ (s)')
        pyplot.ylabel(r'Platform Angle, $\theta_{y}$ (Radians)')
        pyplot.savefig("{}/{}.png".format(location, "theta_y-vs-time"))
                
        # Plot horizontal velocity response
        pyplot.figure(3)
        pyplot.plot(resp["time"], resp["x_dot"])
        pyplot.title("Horizontal velocity vs. Time")
        pyplot.xlabel(r'Time, $t$ (s)')
        pyplot.ylabel(r'Horizontal Velocity, $\dot{x}$ (m/s)')
        pyplot.savefig("{}/{}.png".format(location, "x_dot-vs-time"))
        
        # Plot angular velocity response
        pyplot.figure(4)
        pyplot.plot(resp["time"], resp["theta_y_dot"])
        pyplot.title(r'Platform angular velocity, $\dot{\theta}_{y}$ vs. Time')
        pyplot.xlabel(r'Time, $t$ (s)')
        pyplot.ylabel(r'Platform Anglular Velocity, $\dot{\theta}_{y}$ (rad/s)')
        pyplot.savefig("{}/{}.png".format(location, "theta_y_dot-vs-time"))
        
        

Define a controller function for open-loop control, where the torque provided is zero.

def no_controller(state):
    """
    @brief Ignore the state.
    @param state The current state of the model
    @return 0
    """
    
    return Matrix([0])

Step 3: Run the simulation for two open-loop cases

Step 3a)

The ball is initially at rest on a level platform directly above the center of gravity of the platform and there is no torque input from the motor. Run this simulation for 1 [s].

from sympy import zeros
x_0 = zeros(rows=4, cols=1)

# Set time step to 0.01 seconds, with initial conditions
model = Model(x_0, A, B, time_step=0.01)

# plot the model response for 1s
Model.plot_response(model.response(1, no_controller))

Step 3b)

The ball is initially at rest on a level platform offset horizontally from the center of gravity of the platform by 5 [cm] and there is no torque input from the motor. Run this simulation for 0.4 [s].

x_0 = zeros(rows=4, cols=1)
x_0[0,0] = 0.05 # [m]

# Set time step to 0.0001 seconds, with initial conditions
model = Model(x_0, A, B, time_step=0.0001)

# plot the model response for 1s
Model.plot_response(model.response(0.4, no_controller))

Step 4: Run the simulation, implementing a regulator

Define a controller function for closed-loop control that acs as a regulator using full state feedback.

def regulator_controller(state):
    """
    @brief Implement a regulator using full state feedback.
    @param state The current state of the model
    @return Input vector, u = -kx, where k is specified in the lab spec
    """
    k = Matrix([[0.3, 0.2, 0.05, 0.02]])
    
    # These gains implement a nearly fuel-optimal controller; 
    # that is, these gains stabilize the system while minimizing the
    # amount of actuation effort required for stability. These gains 
    # will therefore not offer high performance and would be expected
    # to oscillate significantly.
    
    return k * state

Test the ball with conditions at 3b) for 20s

The ball is initially at rest on a level platform offset horizontally from the center of gravity of the platform by 5 [cm] and there is no torque input from the motor. Run this simulation for 20 [s].

x_0 = zeros(rows=4, cols=1)
x_0[0,0] = 0.05 # [m]

# Set time step to 0.01 seconds, with initial conditions
model = Model(x_0, A, B, time_step=0.0001)

# plot the model response for 20s
Model.plot_response(model.response(20, regulator_controller))