Added LQR examples

_A_arm/python/LQR_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 ctrlLQR import LQR
+
+
+arm = armDynamics(alpha=0)
+controller = LQR()
+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]]).T
+        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/ctrlLQR.py

@@ -0,0 +1,37 @@
+import numpy as np
+import control as cnt
+import armParam as P
+
+
+class LQR:
+    def __init__(self):
+        #  tuning parameters
+        Q = np.diag([1.0, 0.1])
+        R = np.diag([1.0])
+
+        # system model
+        den = P.m * P.ell**2
+        A = np.array([[0, 1],
+                      [0, -3*P.b / den]])
+        B = np.array([[0],
+                      [3 / den]])
+
+        # feedback gain matrix
+        self.K,_,_ = cnt.lqr(A, B, Q, R)
+
+    def update(self, x_r, x):
+        x_tilde = x - x_r
+        u_tilde = -self.K @ x_tilde
+        u = u_tilde + self._get_equilibrium_input(x)
+        u = saturate(u, P.tau_max)
+        return u[0,0]
+
+    def _get_equilibrium_input(self, x):
+        tau_eq = 0.5 * P.m * P.g * P.ell * np.cos(x[0,0])
+        return tau_eq
+
+
+def saturate(u, limit):
+    if np.abs(u) > limit:
+        u = limit * np.sign(u)
+    return u

_B_pendulum/python/LQR_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 ctrlLQR import LQR
+
+
+pendulum = pendulumDynamics()
+controller = LQR()
+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]]).T
+        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/ctrlLQR.py

@@ -0,0 +1,43 @@
+import numpy as np
+import control as cnt
+import pendulumParam as P
+
+
+class LQR:
+    def __init__(self):
+        #  tuning parameters
+        Q = np.diag([1.0, 1.0, 1.0, 1.0])
+        R = np.diag([1.0])
+
+        # system model
+        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]])
+
+        # feedback gain matrix
+        self.K,_,_ = cnt.lqr(A, B, Q, R)
+
+    def update(self, x_r, x):
+        x_tilde = x - x_r
+        u_tilde = -self.K @ x_tilde
+        u = u_tilde + self._get_equilibrium_input(x)
+        u = saturate(u, P.F_max)
+        return u[0,0]
+
+    def _get_equilibrium_input(self, x):
+        force_eq = 0.0
+        return force_eq
+
+
+def saturate(u, limit):
+    if np.abs(u) > limit:
+        u = limit * np.sign(u)
+    return u

_C_satellite/python/LQR_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 ctrlLQR import LQR
+
+
+satellite = satelliteDynamics()
+controller = LQR()
+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]]).T
+        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/ctrlLQR.py

@@ -0,0 +1,37 @@
+import numpy as np
+import control as cnt
+import satelliteParam as P
+
+
+class LQR:
+    def __init__(self):
+        #  tuning parameters
+        Q = np.diag([0.1, 1.0, 0.9, 4.5])
+        R = np.diag([0.0001])
+
+        # system model
+        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]])
+
+        # feedback gain matrix
+        self.K,_,_ = cnt.lqr(A, B, Q, R)
+
+    def update(self, x_r, x):
+        x_error = x - x_r
+        u = -self.K @ x_error
+        u = saturate(u, P.tau_max)
+        return u[0,0]
+
+
+def saturate(u, limit):
+    if np.abs(u) > limit:
+        u = limit * np.sign(u)
+    return u