raiot
/
PythonLinearNonlinearControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

write compare method of mpc

This commit is contained in:
Shunichi09 2018-12-28 15:53:40 +09:00
parent 54320b3ab4
commit dca46b0889
8 changed files with 494 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
mpc/__pycache__/mpc_func.cpython-36.pyc
View File

Binary file not shown.

BIN
mpc/__pycache__/mpc_func_with_cvxopt.cpython-36.pyc Normal file
View File

Binary file not shown.

BIN
mpc/__pycache__/mpc_func_with_scipy.cpython-36.pyc Normal file
View File

Binary file not shown.

22
mpc/main_example.py
View File

@ -3,7 +3,8 @@ import matplotlib.pyplot as plt
import math
import copy
from mpc_func import MpcController
from mpc_func_with_scipy import MpcController as MpcController_scipy
from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
from control import matlab
class FirstOrderSystem():
@ -86,7 +87,7 @@ class FirstOrderSystem():
def main():
dt = 0.05
simulation_time = 50 # in seconds
simulation_time = 100 # in seconds
iteration_num = int(simulation_time / dt)
# you must be care about this matrix
@ -117,20 +118,21 @@ def main():
Bd = sysd.B
# evaluation function weight
Q = np.diag([1., 1., 1., 1.])
R = np.diag([0.1, 0.1])
Q = np.diag([1., 1., 10., 10.])
R = np.diag([0.01, 0.01])
pre_step = 5
# make controller with discreted matrix
controller = MpcController(Ad, Bd, Q, R, pre_step,
dt_input_upper=np.array([0.5 * dt, 0.5 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]),
input_upper=np.array([2. ,5.]), input_lower=np.array([-2., -5.]))
# please check the solver, if you want to use the scipy, set the MpcController_scipy
controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_step,
dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]),
input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.]))
controller.initialize_controller()
for i in range(iteration_num):
print("simulation time = {0}".format(i))
reference = np.array([[0., 0., -10., -5.] for _ in range(pre_step)]).flatten()
reference = np.array([[0., 0., -5., 7.5] for _ in range(pre_step)]).flatten()
states = plant.xs
opt_u = controller.calc_input(states, reference)
plant.update_state(opt_u)
@ -144,18 +146,22 @@ def main():
v_y_fig = time_history_fig.add_subplot(414)
v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 0])
v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
v_x_fig.set_xlabel("time [s]")
v_x_fig.set_ylabel("v_x")
v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 1])
v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
v_y_fig.set_xlabel("time [s]")
v_y_fig.set_ylabel("v_y")
x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 2])
x_fig.plot(np.arange(0, simulation_time+0.01, dt), [-5. for _ in range(iteration_num+1)], linestyle="dashed")
x_fig.set_xlabel("time [s]")
x_fig.set_ylabel("x")
y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 3])
y_fig.plot(np.arange(0, simulation_time+0.01, dt), [7.5 for _ in range(iteration_num+1)], linestyle="dashed")
y_fig.set_xlabel("time [s]")
y_fig.set_ylabel("y")
time_history_fig.tight_layout()

257
mpc/mpc_func_with_cvxopt.py Normal file
View File

@ -0,0 +1,257 @@
import numpy as np
import matplotlib.pyplot as plt
import math
import copy
from cvxopt import matrix, solvers
class MpcController():
"""
Attributes
------------
A : numpy.ndarray
system matrix
B : numpy.ndarray
input matrix
Q : numpy.ndarray
evaluation function weight
R : numpy.ndarray
evaluation function weight
pre_step : int
prediction step
dt_input_upper : numpy.ndarray
constraints of input dt
dt_input_lower : numpy.ndarray
constraints of input dt
input_upper : numpy.ndarray
constraints of input
input_lower : numpy.ndarray
constraints of input
history_
"""
def __init__(self, A, B, Q, R, pre_step, initial_input=None, dt_input_upper=None, dt_input_lower=None, input_upper=None, input_lower=None):
"""
A : numpy.ndarray
system matrix
B : numpy.ndarray
input matrix
Q : numpy.ndarray
evaluation function weight
R : numpy.ndarray
evaluation function weight
pre_step : int
prediction step
dt_input_upper : numpy.ndarray
constraints of input dt
dt_input_lower : numpy.ndarray
constraints of input dt
input_upper : numpy.ndarray
constraints of input
input_lower : numpy.ndarray
constraints of input
"""
self.A = np.array(A)
self.B = np.array(B)
self.Q = np.array(Q)
self.R = np.array(R)
self.pre_step = pre_step
self.Qs = None
self.Rs = None
self.state_size = self.A.shape[0]
self.input_size = self.B.shape[1]
self.history_us = [np.zeros(self.input_size)]
# initial state
if initial_input is not None:
self.history_us = [initial_input]
# constraints
self.dt_input_lower = dt_input_lower
self.dt_input_upper = dt_input_upper
self.input_upper = input_upper
self.input_lower = input_lower
self.W = None
self.omega = None
self.F = None
self.f = None
def initialize_controller(self):
"""
make matrix to calculate optimal control input
"""
A_factorials = [self.A]
self.phi_mat = copy.deepcopy(self.A)
for _ in range(self.pre_step - 1):
temp_mat = np.dot(A_factorials[-1], self.A)
self.phi_mat = np.vstack((self.phi_mat, temp_mat))
A_factorials.append(temp_mat) # after we use this factorials
print("phi_mat = \n{0}".format(self.phi_mat))
self.gamma_mat = copy.deepcopy(self.B)
gammma_mat_temp = copy.deepcopy(self.B)
for i in range(self.pre_step - 1):
temp_1_mat = np.dot(A_factorials[i], self.B)
gammma_mat_temp = temp_1_mat + gammma_mat_temp
self.gamma_mat = np.vstack((self.gamma_mat, gammma_mat_temp))
print("gamma_mat = \n{0}".format(self.gamma_mat))
self.theta_mat = copy.deepcopy(self.gamma_mat)
for i in range(self.pre_step - 1):
temp_mat = np.zeros_like(self.gamma_mat)
temp_mat[int((i + 1)*self.state_size): , :] = self.gamma_mat[:-int((i + 1)*self.state_size) , :]
self.theta_mat = np.hstack((self.theta_mat, temp_mat))
print("theta_mat = \n{0}".format(self.theta_mat))
# evaluation function weight
diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)])
diag_Rs = np.array([np.diag(self.R) for _ in range(self.pre_step)])
self.Qs = np.diag(diag_Qs.flatten())
self.Rs = np.diag(diag_Rs.flatten())
print("Qs = \n{0}".format(self.Qs))
print("Rs = \n{0}".format(self.Rs))
# constraints
# about dt U
if self.input_lower is not None:
# initialize
self.F = np.zeros((self.input_size * 2, self.pre_step * self.input_size))
for i in range(self.input_size):
self.F[i * 2: (i + 1) * 2, i] = np.array([1., -1.])
temp_F = copy.deepcopy(self.F)
print("F = \n{0}".format(self.F))
for i in range(self.pre_step - 1):
temp_F = copy.deepcopy(temp_F)
for j in range(self.input_size):
temp_F[j * 2: (j + 1) * 2, ((i+1) * self.input_size) + j] = np.array([1., -1.])
self.F = np.vstack((self.F, temp_F))
self.F1 = self.F[:, :self.input_size]
temp_f = []
for i in range(self.input_size):
temp_f.append(-1 * self.input_upper[i])
temp_f.append(self.input_lower[i])
self.f = np.array([temp_f for _ in range(self.pre_step)]).flatten()
print("F = \n{0}".format(self.F))
print("F1 = \n{0}".format(self.F1))
print("f = \n{0}".format(self.f))
# about dt_u
if self.dt_input_lower is not None:
self.W = np.zeros((2, self.pre_step * self.input_size))
self.W[:, 0] = np.array([1., -1.])
for i in range(self.pre_step * self.input_size - 1):
temp_W = np.zeros((2, self.pre_step * self.input_size))
temp_W[:, i+1] = np.array([1., -1.])
self.W = np.vstack((self.W, temp_W))
temp_omega = []
for i in range(self.input_size):
temp_omega.append(self.dt_input_upper[i])
temp_omega.append(-1. * self.dt_input_lower[i])
self.omega = np.array([temp_omega for _ in range(self.pre_step)]).flatten()
print("W = \n{0}".format(self.W))
print("omega = \n{0}".format(self.omega))
# about state
print("check the matrix!! if you think rite, plese push enter")
input()
def calc_input(self, states, references):
"""calculate optimal input
Parameters
-----------
states : numpy.array
the size should have (state length * 1)
references :
the size should have (state length * pre_step)
References
------------
opt_input : numpy.ndarray
optimal input, size is (1, input_length)
"""
temp_1 = np.dot(self.phi_mat, states.reshape(-1, 1))
temp_2 = np.dot(self.gamma_mat, self.history_us[-1].reshape(-1, 1))
error = references.reshape(-1, 1) - temp_1 - temp_2
G = 2. * np.dot(self.theta_mat.T, np.dot(self.Qs, error) )
H = np.dot(self.theta_mat.T, np.dot(self.Qs, self.theta_mat)) + self.Rs
# constraints
A = []
b = []
if self.W is not None:
A.append(self.W)
b.append(self.omega.reshape(-1, 1))
if self.F is not None:
b_F = - np.dot(self.F1, self.history_us[-1].reshape(-1, 1)) - self.f.reshape(-1, 1)
A.append(self.F)
b.append(b_F)
A = np.array(A).reshape(-1, self.input_size * self.pre_step)
# b = np.array(b).reshape(-1, 1)
ub = np.array(b).flatten()
# print(np.dot(self.F1, self.history_us[-1].reshape(-1, 1)))
# make cvxpy problem formulation
P = 2*matrix(H)
q = matrix(-1 * G)
A = matrix(A)
b = matrix(ub)
# constraint
if self.W is not None or self.F is not None :
# print("consider constraint!")
opt_result = solvers.qp(P, q, G=A, h=b)
# print(list(opt_result['x']))
opt_dt_us = list(opt_result['x'])
# print("current_u = {0}".format(self.history_us[-1]))
# print("opt_dt_u = {0}".format(np.round(opt_dt_us, 5)))
opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
# print("opt_u = {0}".format(np.round(opt_u, 5)))
# save
self.history_us.append(opt_u)
# a = input()
return opt_u
"""
constraint = []
for i in range(self.pre_step * self.input_size):
sums = -1. * (np.dot(A[i], init_dt_us) - b[i])[0]
constraint.append(sums)
"""

32
mpc/mpc_func.py → mpc/mpc_func_with_scipy.py
View File

@ -4,6 +4,7 @@ import math
import copy
from scipy.optimize import minimize
from scipy.optimize import LinearConstraint
class MpcController():
"""
@ -220,7 +221,9 @@ class MpcController():
b.append(b_F)
A = np.array(A).reshape(-1, self.input_size * self.pre_step)
b = np.array(b).reshape(-1, 1)
# b = np.array(b).reshape(-1, 1)
ub = np.array(b).flatten()
# print(np.dot(self.F1, self.history_us[-1].reshape(-1, 1)))
def optimized_func(dt_us):
"""
@ -228,37 +231,26 @@ class MpcController():
temp_dt_us = np.array([dt_us[i] for i in range(self.input_size * self.pre_step)])
return (np.dot(temp_dt_us, np.dot(H, temp_dt_us.reshape(-1, 1))) - np.dot(G.T, temp_dt_us.reshape(-1, 1)))[0]
def constraint_func(dt_us):
""" we consider the constraints in Ax <= b
however the scipy constraints is -(Ax - b) >= 0
so we should recalculate the A and B
"""
temp_dt_us = np.array([dt_us[i] for i in range(self.input_size * self.pre_step)])
constraint = []
for i in range(self.pre_step * self.input_size):
sums = -1. * (np.dot(A[i], temp_dt_us) - b[i])[0]
constraint.append(sums)
return np.array(sums)
# constraint
lb = np.array([-np.inf for _ in range(len(ub))])
linear_cons = LinearConstraint(A, lb, ub)
init_dt_us = np.zeros(self.input_size * self.pre_step)
# constraint
if self.W is not None or self.F is not None :
print("consider constraint!")
cons = ({'type' : 'ineq', 'fun' : constraint_func})
opt_result = minimize(optimized_func, init_dt_us, constraints=cons)
# print("consider constraint!")
opt_result = minimize(optimized_func, init_dt_us, constraints=[linear_cons])
opt_dt_us = opt_result.x
print("opt_u = {0}".format(np.round(opt_dt_us, 5)))
# print("current_u = {0}".format(self.history_us[-1]))
# print("opt_dt_u = {0}".format(np.round(opt_dt_us, 5)))
opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
# print("opt_u = {0}".format(np.round(opt_u, 5)))
# save
self.history_us.append(opt_u)
return opt_u

70
mpc/simulator_func.py
View File

@ -1,70 +0,0 @@
import numpy as np
import matplotlib.pyplot as plt
import math
class FirstOrderSystem():
"""FirstOrderSystem
Attributes
-----------
state : float
system state, this system should have one input - one output
a : float
parameter of the system
b : float
parameter of the system
history_state : list
time history of state
"""
def __init__(self, a, b, init_state=0.0):
"""
Parameters
-----------
a : float
parameter of the system
b : float
parameter of the system
init_state : float, optional
initial state of system default is 0.0
"""
self.state = init_state
self.a = a
self.b = b
self.history_state = [init_state]
def update_state(self, u, dt=0.01):
"""calculating input
Parameters
------------
u : float
input of system in some cases this means the reference
dt : float in seconds, optional
sampling time of simulation, default is 0.01 [s]
"""
# solve Runge-Kutta
k0 = dt * self._func(self.state, u)
k1 = dt * self._func(self.state + k0/2.0, u)
k2 = dt * self._func(self.state + k1/2.0, u)
k3 = dt * self._func(self.state + k2, u)
self.state += (k0 + 2 * k1 + 2 * k2 + k3) / 6.0
# for oylar
# self.state += k0
# save
self.history_state.append(self.state)
def _func(self, y, u):
"""
Parameters
------------
y : float
state of system
u : float
input of system in some cases this means the reference
"""
y_dot = -self.a * y + self.b * u
return y_dot

211
mpc/test_compare_methods.py Normal file
View File

@ -0,0 +1,211 @@
import numpy as np
import matplotlib.pyplot as plt
import math
import copy
from mpc_func_with_scipy import MpcController as MpcController_scipy
from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
from control import matlab
class FirstOrderSystem():
"""FirstOrderSystemWithStates
Attributes
-----------
states : float
system states
A : numpy.ndarray
system matrix
B : numpy.ndarray
control matrix
C : numpy.ndarray
observation matrix
history_state : list
time history of state
"""
def __init__(self, A, B, C, D=None, init_states=None):
"""
Parameters
-----------
A : numpy.ndarray
system matrix
B : numpy.ndarray
control matrix
C : numpy.ndarray
observation matrix
C : numpy.ndarray
directly matrix
init_state : float, optional
initial state of system default is None
history_xs : list
time history of system states
"""
self.A = A
self.B = B
self.C = C
if D is not None:
self.D = D
self.xs = np.zeros(self.A.shape[0])
if init_states is not None:
self.xs = copy.deepcopy(init_states)
self.history_xs = [init_states]
def update_state(self, u, dt=0.01):
"""calculating input
Parameters
------------
u : float
input of system in some cases this means the reference
dt : float in seconds, optional
sampling time of simulation, default is 0.01 [s]
"""
temp_x = self.xs.reshape(-1, 1)
temp_u = u.reshape(-1, 1)
# solve Runge-Kutta
k0 = dt * (np.dot(self.A, temp_x) + np.dot(self.B, temp_u))
k1 = dt * (np.dot(self.A, temp_x + k0/2.) + np.dot(self.B, temp_u))
k2 = dt * (np.dot(self.A, temp_x + k1/2.) + np.dot(self.B, temp_u))
k3 = dt * (np.dot(self.A, temp_x + k2) + np.dot(self.B, temp_u))
self.xs += ((k0 + 2 * k1 + 2 * k2 + k3) / 6.).flatten()
# for oylar
# self.xs += k0.flatten()
# print("xs = {0}".format(self.xs))
# a = input()
# save
save_states = copy.deepcopy(self.xs)
self.history_xs.append(save_states)
# print(self.history_xs)
def main():
dt = 0.05
simulation_time = 50 # in seconds
iteration_num = int(simulation_time / dt)
# you must be care about this matrix
# these A and B are for continuos system if you want to use discret system matrix please skip this step
tau = 0.63
A = np.array([[-1./tau, 0., 0., 0.],
[0., -1./tau, 0., 0.],
[1., 0., 0., 0.],
[0., 1., 0., 0.]])
B = np.array([[1./tau, 0.],
[0., 1./tau],
[0., 0.],
[0., 0.]])
C = np.eye(4)
D = np.zeros((4, 2))
# make simulator with coninuous matrix
init_xs = np.array([0., 0., 0., 0.])
plant_cvxopt = FirstOrderSystem(A, B, C, init_states=init_xs)
plant_scipy = FirstOrderSystem(A, B, C, init_states=init_xs)
# create system
sysc = matlab.ss(A, B, C, D)
# discrete system
sysd = matlab.c2d(sysc, dt)
Ad = sysd.A
Bd = sysd.B
# evaluation function weight
Q = np.diag([1., 1., 10., 10.])
R = np.diag([0.01, 0.01])
pre_step = 5
# make controller with discreted matrix
# please check the solver, if you want to use the scipy, set the MpcController_scipy
controller_cvxopt = MpcController_cvxopt(Ad, Bd, Q, R, pre_step,
dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]),
input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.]))
controller_scipy = MpcController_scipy(Ad, Bd, Q, R, pre_step,
dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]),
input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.]))
controller_cvxopt.initialize_controller()
controller_scipy.initialize_controller()
for i in range(iteration_num):
print("simulation time = {0}".format(i))
reference = np.array([[0., 0., -5., 7.5] for _ in range(pre_step)]).flatten()
states_cvxopt = plant_cvxopt.xs
states_scipy = plant_scipy.xs
opt_u_cvxopt = controller_cvxopt.calc_input(states_cvxopt, reference)
opt_u_scipy = controller_scipy.calc_input(states_scipy, reference)
plant_cvxopt.update_state(opt_u_cvxopt)
plant_scipy.update_state(opt_u_scipy)
history_states_cvxopt = np.array(plant_cvxopt.history_xs)
history_states_scipy = np.array(plant_scipy.history_xs)
time_history_fig = plt.figure(dpi=75)
x_fig = time_history_fig.add_subplot(411)
y_fig = time_history_fig.add_subplot(412)
v_x_fig = time_history_fig.add_subplot(413)
v_y_fig = time_history_fig.add_subplot(414)
v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 0], label="cvxopt")
v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 0], label="scipy", linestyle="dashdot")
v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
v_x_fig.set_xlabel("time [s]")
v_x_fig.set_ylabel("v_x")
v_x_fig.legend()
v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 1], label="cvxopt")
v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 1], label="scipy", linestyle="dashdot")
v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
v_y_fig.set_xlabel("time [s]")
v_y_fig.set_ylabel("v_y")
v_y_fig.legend()
x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 2], label="cvxopt")
x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 2], label="scipy", linestyle="dashdot")
x_fig.plot(np.arange(0, simulation_time+0.01, dt), [-5. for _ in range(iteration_num+1)], linestyle="dashed")
x_fig.set_xlabel("time [s]")
x_fig.set_ylabel("x")
y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 3], label ="cvxopt")
y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 3], label="scipy", linestyle="dashdot")
y_fig.plot(np.arange(0, simulation_time+0.01, dt), [7.5 for _ in range(iteration_num+1)], linestyle="dashed")
y_fig.set_xlabel("time [s]")
y_fig.set_ylabel("y")
time_history_fig.tight_layout()
plt.show()
history_us_cvxopt = np.array(controller_cvxopt.history_us)
history_us_scipy = np.array(controller_scipy.history_us)
input_history_fig = plt.figure(dpi=75)
u_1_fig = input_history_fig.add_subplot(211)
u_2_fig = input_history_fig.add_subplot(212)
u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_cvxopt[:, 0], label="cvxopt")
u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_scipy[:, 0], label="scipy", linestyle="dashdot")
u_1_fig.set_xlabel("time [s]")
u_1_fig.set_ylabel("u_x")
u_1_fig.legend()
u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_cvxopt[:, 1], label="cvxopt")
u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_scipy[:, 1], label="scipy", linestyle="dashdot")
u_2_fig.set_xlabel("time [s]")
u_2_fig.set_ylabel("u_y")
u_2_fig.legend()
input_history_fig.tight_layout()
plt.show()
if __name__ == "__main__":
main()