From 6e2acdb5535c6e4bb80cc811ce261d36facda565 Mon Sep 17 00:00:00 2001 From: Mat Haskell Date: Wed, 8 Jan 2025 14:53:10 -0700 Subject: [PATCH 1/2] Added LQR examples --- _A_arm/python/LQR_armSim.py | 37 +++++++++++++++++++++ _A_arm/python/ctrlLQR.py | 37 +++++++++++++++++++++ _B_pendulum/python/LQR_pendulumSim.py | 37 +++++++++++++++++++++ _B_pendulum/python/ctrlLQR.py | 43 +++++++++++++++++++++++++ _C_satellite/python/LQR_satelliteSim.py | 37 +++++++++++++++++++++ _C_satellite/python/ctrlLQR.py | 37 +++++++++++++++++++++ 6 files changed, 228 insertions(+) create mode 100644 _A_arm/python/LQR_armSim.py create mode 100644 _A_arm/python/ctrlLQR.py create mode 100644 _B_pendulum/python/LQR_pendulumSim.py create mode 100644 _B_pendulum/python/ctrlLQR.py create mode 100644 _C_satellite/python/LQR_satelliteSim.py create mode 100644 _C_satellite/python/ctrlLQR.py diff --git a/_A_arm/python/LQR_armSim.py b/_A_arm/python/LQR_armSim.py new file mode 100644 index 00000000..26aadc5d --- /dev/null +++ b/_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() diff --git a/_A_arm/python/ctrlLQR.py b/_A_arm/python/ctrlLQR.py new file mode 100644 index 00000000..47595d6c --- /dev/null +++ b/_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 _get_equilibrium_input(self, x): + tau_eq = 0.5 * P.m * P.g * P.ell * np.cos(x[0,0]) + return tau_eq + + 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 saturate(u, limit): + if np.abs(u) > limit: + u = limit * np.sign(u) + return u diff --git a/_B_pendulum/python/LQR_pendulumSim.py b/_B_pendulum/python/LQR_pendulumSim.py new file mode 100644 index 00000000..1227ccc9 --- /dev/null +++ b/_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() diff --git a/_B_pendulum/python/ctrlLQR.py b/_B_pendulum/python/ctrlLQR.py new file mode 100644 index 00000000..1c7095ab --- /dev/null +++ b/_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 _get_equilibrium_input(self, x): + force_eq = 0.0 + return force_eq + + 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 saturate(u, limit): + if np.abs(u) > limit: + u = limit * np.sign(u) + return u diff --git a/_C_satellite/python/LQR_satelliteSim.py b/_C_satellite/python/LQR_satelliteSim.py new file mode 100644 index 00000000..e1d62221 --- /dev/null +++ b/_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() diff --git a/_C_satellite/python/ctrlLQR.py b/_C_satellite/python/ctrlLQR.py new file mode 100644 index 00000000..0a87fb1a --- /dev/null +++ b/_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 From bdad5ce73941b5f29381c40edb282fcf7c8a1767 Mon Sep 17 00:00:00 2001 From: Mat Haskell Date: Mon, 20 Jan 2025 14:01:14 -0700 Subject: [PATCH 2/2] Move 'private' functions to bottom of class --- _A_arm/python/ctrlLQR.py | 8 ++++---- _B_pendulum/python/ctrlLQR.py | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/_A_arm/python/ctrlLQR.py b/_A_arm/python/ctrlLQR.py index 47595d6c..2b288bb1 100644 --- a/_A_arm/python/ctrlLQR.py +++ b/_A_arm/python/ctrlLQR.py @@ -19,10 +19,6 @@ def __init__(self): # feedback gain matrix self.K,_,_ = cnt.lqr(A, B, Q, R) - 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 update(self, x_r, x): x_tilde = x - x_r u_tilde = -self.K @ x_tilde @@ -30,6 +26,10 @@ def update(self, x_r, 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: diff --git a/_B_pendulum/python/ctrlLQR.py b/_B_pendulum/python/ctrlLQR.py index 1c7095ab..3701b98f 100644 --- a/_B_pendulum/python/ctrlLQR.py +++ b/_B_pendulum/python/ctrlLQR.py @@ -25,10 +25,6 @@ def __init__(self): # feedback gain matrix self.K,_,_ = cnt.lqr(A, B, Q, R) - def _get_equilibrium_input(self, x): - force_eq = 0.0 - return force_eq - def update(self, x_r, x): x_tilde = x - x_r u_tilde = -self.K @ x_tilde @@ -36,6 +32,10 @@ def update(self, x_r, 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: