Added MPC examples

_A_arm/python/MPC_armSim.py

@@ -0,0 +1,37 @@
+import matplotlib.pyplot as plt
+import numpy as np
+import armParam as P
+from signalGenerator import signalGenerator
+from armAnimation import armAnimation
+from dataPlotter import dataPlotter
+from armDynamics import armDynamics
+from ctrlMPC import MPC
+
+
+arm = armDynamics(alpha=0)
+controller = MPC()
+reference = signalGenerator(amplitude=np.radians(30), frequency=0.05)
+
+dataPlot = dataPlotter()
+animation = armAnimation()
+
+t = P.t_start
+y = arm.h()
+while t < P.t_end:
+    t_next_plot = t + P.t_plot
+    while t < t_next_plot: # updates control and dynamics at faster simulation rate
+        theta_r = reference.square(t)
+        x_r = np.array([theta_r, 0.0])
+        x = arm.state # could use an estimator instead of true state
+        u = controller.update(x_r, x)
+        y = arm.update(u)
+        t += P.Ts
+
+    animation.update(arm.state)
+    dataPlot.update(t, arm.state, u, theta_r)
+    plt.pause(0.0001) # the pause causes the figure to display during simulation
+
+# Keeps the program from closing until the user presses a button
+print('Press key to close')
+plt.waitforbuttonpress()
+plt.close()

_A_arm/python/ctrlMPC.py

@@ -0,0 +1,44 @@
+import numpy as np
+import affine_mpc_py as ampc
+import armParam as P
+
+
+class MPC:
+    def __init__(self):
+        #  tuning parameters
+        Q_diag = np.array([1.0, 0.1])
+        T = 100
+        p = 5
+
+        # MPC setup
+        n,m = 2,1
+        self.mpc = ampc.ImplicitMPC(n, m, T, p)
+        A,B,w = self._get_model(np.zeros(n))
+        self.mpc.setModelContinuous2Discrete(A, B, w, P.Ts)
+        self.mpc.setStateWeights(Q_diag)
+        u_lim = np.array([P.tau_max])
+        self.mpc.setInputLimits(-u_lim, u_lim)
+        self.mpc.initializeSolver()
+
+    def update(self, x_r, x):
+        self.mpc.setReferenceState(x_r)
+        A,B,w = self._get_model(x)
+        self.mpc.setModelContinuous2Discrete(A, B, w, P.Ts)
+        self.mpc.solve(x)
+        u = self.mpc.getNextInput()
+        return u[0]
+
+    def _get_model(self, x_eq):
+        den = P.m * P.ell**2
+        A = np.array([[0, 1],
+                      [0, -3*P.b / den]])
+        B = np.array([[0],
+                      [3 / den]])
+        x_eq = np.array([x_eq.item(0), 0])
+        u_eq = self._get_equilibrium_input(x_eq)
+        w = - (A @ x_eq + B @ u_eq)
+        return A, B, w
+
+    def _get_equilibrium_input(self, x):
+        tau_eq = 0.5 * P.m * P.g * P.ell * np.cos(x[0])
+        return np.array([tau_eq])

_B_pendulum/python/MPC_pendulumSim.py

@@ -0,0 +1,37 @@
+import matplotlib.pyplot as plt
+import numpy as np
+import pendulumParam as P
+from signalGenerator import signalGenerator
+from pendulumAnimation import pendulumAnimation
+from dataPlotter import dataPlotter
+from pendulumDynamics import pendulumDynamics
+from ctrlMPC import MPC
+
+
+pendulum = pendulumDynamics()
+controller = MPC()
+reference = signalGenerator(amplitude=0.5, frequency=0.04)
+
+dataPlot = dataPlotter()
+animation = pendulumAnimation()
+
+t = P.t_start
+y = pendulum.h()
+while t < P.t_end:
+    t_next_plot = t + P.t_plot
+    while t < t_next_plot: # updates control and dynamics at faster simulation rate
+        z_r = reference.square(t)
+        x_r = np.array([z_r, 0.0, 0.0, 0.0])
+        x = pendulum.state # could use an estimator instead of true state
+        u = controller.update(x_r, x)
+        y = pendulum.update(u)
+        t += P.Ts
+
+    animation.update(pendulum.state)
+    dataPlot.update(t, pendulum.state, u, z_r)
+    plt.pause(0.0001) # the pause causes the figure to be displayed during the simulation
+
+# Keeps the program from closing until user presses a button
+print('Press key to close')
+plt.waitforbuttonpress()
+plt.close()

_B_pendulum/python/ctrlMPC.py

@@ -0,0 +1,43 @@
+import numpy as np
+import affine_mpc_py as ampc
+import pendulumParam as P
+
+
+class MPC:
+    def __init__(self):
+        #  tuning parameters
+        Q_diag = np.array([1.0, 1.0, 0.1, 0.1])
+        T = 100
+        p = 5
+
+        # MPC setup
+        n,m = 4,1
+        self.mpc = ampc.ImplicitMPC(n, m, T, p)
+        A,B,w = self._get_model()
+        self.mpc.setModelContinuous2Discrete(A, B, w, P.Ts)
+        self.mpc.setStateWeights(Q_diag)
+        u_lim = np.array([P.F_max])
+        self.mpc.setInputLimits(-u_lim, u_lim)
+        self.mpc.initializeSolver()
+
+    def update(self, x_r, x):
+        self.mpc.setReferenceState(x_r)
+        self.mpc.solve(x)
+        u = self.mpc.getNextInput()
+        return u[0]
+
+    def _get_model(self):
+        zd_den = .25*P.m1 + P.m2
+        thd_den = 2 * zd_den * P.ell
+        A = np.array([[0, 0, 1, 0],
+                      [0, 0, 0, 1],
+                      [0, -0.75*P.m1*P.g / zd_den, -P.b / zd_den, 0],
+                      [0, 3*(P.m1 + P.m2)*P.g / thd_den, 3*P.b / thd_den, 0]])
+        B = np.array([[0.0],
+                      [0.0],
+                      [1 / zd_den],
+                      [-3 / thd_den]])
+        x_eq = np.zeros(4)
+        u_eq = np.zeros(1)
+        w = - (A @ x_eq + B @ u_eq)
+        return A, B, w

_C_satellite/python/MPC_satelliteSim.py

@@ -0,0 +1,37 @@
+import matplotlib.pyplot as plt
+import numpy as np
+import satelliteParam as P
+from signalGenerator import signalGenerator
+from satelliteAnimation import satelliteAnimation
+from dataPlotter import dataPlotter
+from satelliteDynamics import satelliteDynamics
+from ctrlMPC import MPC
+
+
+satellite = satelliteDynamics()
+controller = MPC()
+reference = signalGenerator(amplitude=np.radians(15), frequency=0.04)
+
+dataPlot = dataPlotter()
+animation = satelliteAnimation()
+
+t = P.t_start
+y = satellite.h()
+while t < P.t_end:
+    t_next_plot = t + P.t_plot
+    while t < t_next_plot: # updates control and dynamics at faster simulation rate
+        phi_r = reference.square(t)
+        x_r = np.array([phi_r, phi_r, 0.0, 0.0])
+        x = satellite.state # could use an estimator instead of true state
+        u = controller.update(x_r, x)
+        y = satellite.update(u)
+        t += P.Ts
+
+    animation.update(satellite.state)
+    dataPlot.update(t, satellite.state, u, phi_r)
+    plt.pause(0.0001) # the pause causes the figure to be displayed during the simulation
+
+# Keeps the program from closing until the user presses a button
+print('Press key to close')
+plt.waitforbuttonpress()
+plt.close()

_C_satellite/python/ctrlMPC.py

@@ -0,0 +1,45 @@
+import numpy as np
+import affine_mpc_py as ampc
+import satelliteParam as P
+
+
+class MPC:
+    def __init__(self):
+        #  tuning parameters
+        Q_diag = np.array([0.1, 1.2, 0.9, 4.5]) # smoother and less effort
+        Q_diag = np.array([0.1, 6.0, 1.0, 13.0]) # in between
+        Q_diag = np.array([0.0, 1.2, 0.0, 1.0]) # fast (if you don't care about theta)
+        T = 200
+        p = 5
+
+        # MPC setup
+        n,m = 4,1
+        self.mpc = ampc.ImplicitMPC(n, m, T, p)
+        A,B,w = self._get_model()
+        self.mpc.setModelContinuous2Discrete(A, B, w, P.Ts)
+        self.mpc.setStateWeights(Q_diag)
+        u_lim = np.array([P.tau_max])
+        self.mpc.setInputLimits(-u_lim, u_lim)
+        self.mpc.initializeSolver()
+
+    def update(self, x_r, x):
+        self.mpc.setReferenceState(x_r)
+        self.mpc.solve(x)
+        u = self.mpc.getNextInput()
+        return u[0]
+
+    def _get_model(self):
+        k_Js, k_Jp = P.k / P.Js, P.k / P.Jp
+        b_Js, b_Jp = P.b / P.Js, P.b / P.Jp
+        A = np.array([[0, 0, 1, 0],
+                      [0, 0, 0, 1],
+                      [-k_Js, k_Js, -b_Js, b_Js],
+                      [k_Jp, -k_Jp, b_Jp, -b_Jp]])
+        B = np.array([[0],
+                      [0],
+                      [1 / P.Js],
+                      [0]])
+        x_eq = np.zeros(4)
+        u_eq = np.zeros(1)
+        w = - (A @ x_eq + B @ u_eq)
+        return A, B, w