From efd402a1cecda923976c39a36049521a2df129c2 Mon Sep 17 00:00:00 2001 From: Mat Haskell Date: Wed, 8 Jan 2025 14:52:33 -0700 Subject: [PATCH 1/2] Added MPC examples --- _A_arm/python/MPC_armSim.py | 37 ++++++++++++++++++++ _A_arm/python/ctrlMPC.py | 44 ++++++++++++++++++++++++ _B_pendulum/python/MPC_pendulumSim.py | 37 ++++++++++++++++++++ _B_pendulum/python/ctrlMPC.py | 43 +++++++++++++++++++++++ _C_satellite/python/MPC_satelliteSim.py | 37 ++++++++++++++++++++ _C_satellite/python/ctrlMPC.py | 45 +++++++++++++++++++++++++ 6 files changed, 243 insertions(+) create mode 100644 _A_arm/python/MPC_armSim.py create mode 100644 _A_arm/python/ctrlMPC.py create mode 100644 _B_pendulum/python/MPC_pendulumSim.py create mode 100644 _B_pendulum/python/ctrlMPC.py create mode 100644 _C_satellite/python/MPC_satelliteSim.py create mode 100644 _C_satellite/python/ctrlMPC.py diff --git a/_A_arm/python/MPC_armSim.py b/_A_arm/python/MPC_armSim.py new file mode 100644 index 00000000..7f89b67b --- /dev/null +++ b/_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() diff --git a/_A_arm/python/ctrlMPC.py b/_A_arm/python/ctrlMPC.py new file mode 100644 index 00000000..5869cbcd --- /dev/null +++ b/_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._linearize_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 _linearize_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]) + + def update(self, x_r, x): + self.mpc.setReferenceState(x_r) + A,B,w = self._linearize_model(x) + self.mpc.setModelContinuous2Discrete(A, B, w, P.Ts) + self.mpc.solve(x) + u = self.mpc.getNextInput() + return u[0] diff --git a/_B_pendulum/python/MPC_pendulumSim.py b/_B_pendulum/python/MPC_pendulumSim.py new file mode 100644 index 00000000..028d0d4a --- /dev/null +++ b/_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() diff --git a/_B_pendulum/python/ctrlMPC.py b/_B_pendulum/python/ctrlMPC.py new file mode 100644 index 00000000..0fc06915 --- /dev/null +++ b/_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._linearize_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 _linearize_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 + + def update(self, x_r, x): + self.mpc.setReferenceState(x_r) + self.mpc.solve(x) + u = self.mpc.getNextInput() + return u[0] diff --git a/_C_satellite/python/MPC_satelliteSim.py b/_C_satellite/python/MPC_satelliteSim.py new file mode 100644 index 00000000..04991320 --- /dev/null +++ b/_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() diff --git a/_C_satellite/python/ctrlMPC.py b/_C_satellite/python/ctrlMPC.py new file mode 100644 index 00000000..5ec0ecd0 --- /dev/null +++ b/_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 _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 + + def update(self, x_r, x): + self.mpc.setReferenceState(x_r) + self.mpc.solve(x) + u = self.mpc.getNextInput() + return u[0] From c3b573506534742a2e01eafe95fb72ffb3f646cc Mon Sep 17 00:00:00 2001 From: Mat Haskell Date: Mon, 20 Jan 2025 13:59:43 -0700 Subject: [PATCH 2/2] Move 'private' functions to bottom of class and renamed function because it's not actually linearization --- _A_arm/python/ctrlMPC.py | 20 ++++++++++---------- _B_pendulum/python/ctrlMPC.py | 16 ++++++++-------- _C_satellite/python/ctrlMPC.py | 12 ++++++------ 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/_A_arm/python/ctrlMPC.py b/_A_arm/python/ctrlMPC.py index 5869cbcd..a210ace1 100644 --- a/_A_arm/python/ctrlMPC.py +++ b/_A_arm/python/ctrlMPC.py @@ -13,14 +13,22 @@ def __init__(self): # MPC setup n,m = 2,1 self.mpc = ampc.ImplicitMPC(n, m, T, p) - A,B,w = self._linearize_model(np.zeros(n)) + 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 _linearize_model(self, x_eq): + 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]]) @@ -34,11 +42,3 @@ def _linearize_model(self, x_eq): 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]) - - def update(self, x_r, x): - self.mpc.setReferenceState(x_r) - A,B,w = self._linearize_model(x) - self.mpc.setModelContinuous2Discrete(A, B, w, P.Ts) - self.mpc.solve(x) - u = self.mpc.getNextInput() - return u[0] diff --git a/_B_pendulum/python/ctrlMPC.py b/_B_pendulum/python/ctrlMPC.py index 0fc06915..0ce85c5e 100644 --- a/_B_pendulum/python/ctrlMPC.py +++ b/_B_pendulum/python/ctrlMPC.py @@ -13,14 +13,20 @@ def __init__(self): # MPC setup n,m = 4,1 self.mpc = ampc.ImplicitMPC(n, m, T, p) - A,B,w = self._linearize_model() + 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 _linearize_model(self): + 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], @@ -35,9 +41,3 @@ def _linearize_model(self): u_eq = np.zeros(1) w = - (A @ x_eq + B @ u_eq) return A, B, w - - def update(self, x_r, x): - self.mpc.setReferenceState(x_r) - self.mpc.solve(x) - u = self.mpc.getNextInput() - return u[0] diff --git a/_C_satellite/python/ctrlMPC.py b/_C_satellite/python/ctrlMPC.py index 5ec0ecd0..3aabc8b4 100644 --- a/_C_satellite/python/ctrlMPC.py +++ b/_C_satellite/python/ctrlMPC.py @@ -22,6 +22,12 @@ def __init__(self): 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 @@ -37,9 +43,3 @@ def _get_model(self): u_eq = np.zeros(1) w = - (A @ x_eq + B @ u_eq) return A, B, w - - def update(self, x_r, x): - self.mpc.setReferenceState(x_r) - self.mpc.solve(x) - u = self.mpc.getNextInput() - return u[0]