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

add ACC.py of mpc

This commit is contained in:
Shunichi09 2018-12-29 15:00:37 +09:00
parent 665c54bd27
commit ebfab5e257
6 changed files with 641 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
mpc/README.md
View File

@ -1,50 +1,95 @@
# Model Predictive Control Tool
This program is about template, function of linear model predictive control
This program is about template, generic function of linear model predictive control
# Documentation of this function
# Documentation of the MPC function
Linear model predicitive control should have state equation.
So if you want to use this function, you should model the plant as state equation.
Therefore, the parameters of this class are as following
Parameters :
**class MpcController()**
Attributes :
- A : numpy.ndarray
- system matrix
- B : numpy.ndarray
- input matrix
- Q : numpy.ndarray
- evaluation function weight
- evaluation function weight for states
- Qs : numpy.ndarray
- concatenated evaluation function weight for states
- R : numpy.ndarray
- evaluation function weight
- evaluation function weight for inputs
- Rs : numpy.ndarray
- concatenated evaluation function weight for inputs
- 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
- state_size : int
- state size of the plant
- input_size : int
- input size of the plant
- dt_input_upper : numpy.ndarray, shape(input_size, ), optional
- constraints of input dt, default is None
- dt_input_lower : numpy.ndarray, shape(input_size, ), optional
- constraints of input dt, default is None
- input_upper : numpy.ndarray, shape(input_size, ), optional
- constraints of input, default is None
- input_lower : numpy.ndarray, shape(input_size, ), optional
- constraints of input, default is None
Methods:
- initialize_controller() initialize the controller
- calc_input(states, references) calculating optimal input
More details, please look the **mpc_func_with_scipy.py** and **mpc_func_with_cvxopt.py**
We have two function, mpc_func_with_cvxopt.py and mpc_func_with_scipy.py
Both function have same variable and member function. however the solver is different.
Plese choose the right method for your environment
Both functions have same variable and member function. However the solver is different.
Plese choose the right method for your environment.
## Example
# Problem Formulation and Expected results
- example of import
```py
from mpc_func_with_scipy import MpcController as MpcController_scipy
from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
```
# Examples
## Problem Formulation
** updating soon !!
- first order system
- ACC (Adaptive cruise control)
## Expected Results
- first order system
- ACC (Adaptive cruise control)
- updating soon!!
# Usage
- for example
- for example(first order system)
```
$ python main_example.py
```
- for comparing two methods
- for example(ACC (Adaptive cruise control))
```
$ python main_ACC.py
```
- for comparing two methods of optimization solvers
```
$ python test_compare_methods.py
@ -57,6 +102,7 @@ $ python test_compare_methods.py
- matplotlib
- cvxopt
- scipy1.2.0 or more
- python-control
# Reference
I`m sorry that main references are written in Japanese

233
mpc/animation.py Executable file
View File

@ -0,0 +1,233 @@
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as ani
import matplotlib.font_manager as fon
import sys
import math
# default setting of figures
plt.rcParams["mathtext.fontset"] = 'stix' # math fonts
plt.rcParams['xtick.direction'] = 'in' # x axis in
plt.rcParams['ytick.direction'] = 'in' # y axis in
plt.rcParams["font.size"] = 10
plt.rcParams['axes.linewidth'] = 1.0 # axis line width
plt.rcParams['axes.grid'] = True # make grid
def coordinate_transformation_in_angle(positions, base_angle):
'''
Transformation the coordinate in the angle
Parameters
-------
positions : numpy.ndarray
this parameter is composed of xs, ys
should have (2, N) shape
base_angle : float [rad]
Returns
-------
traslated_positions : numpy.ndarray
the shape is (2, N)
'''
if positions.shape[0] != 2:
raise ValueError('the input data should have (2, N)')
positions = np.array(positions)
positions = positions.reshape(2, -1)
rot_matrix = [[np.cos(base_angle), np.sin(base_angle)],
[-1*np.sin(base_angle), np.cos(base_angle)]]
rot_matrix = np.array(rot_matrix)
translated_positions = np.dot(rot_matrix, positions)
return translated_positions
def square_make_with_angles(center_x, center_y, size, angle):
'''
Create square matrix with angle line matrix(2D)
Parameters
-------
center_x : float in meters
the center x position of the square
center_y : float in meters
the center y position of the square
size : float in meters
the square's half-size
angle : float in radians
Returns
-------
square xs : numpy.ndarray
lenght is 5 (counterclockwise from right-up)
square ys : numpy.ndarray
length is 5 (counterclockwise from right-up)
angle line xs : numpy.ndarray
angle line ys : numpy.ndarray
'''
# start with the up right points
# create point in counterclockwise
square_xys = np.array([[size, 0.5 * size], [-size, 0.5 * size], [-size, -0.5 * size], [size, -0.5 * size], [size, 0.5 * size]])
trans_points = coordinate_transformation_in_angle(square_xys.T, -angle) # this is inverse type
trans_points += np.array([[center_x], [center_y]])
square_xs = trans_points[0, :]
square_ys = trans_points[1, :]
angle_line_xs = [center_x, center_x + math.cos(angle) * size]
angle_line_ys = [center_y, center_y + math.sin(angle) * size]
return square_xs, square_ys, np.array(angle_line_xs), np.array(angle_line_ys)
class AnimDrawer():
"""create animation of path and robot
Attributes
------------
cars :
anim_fig : figure of matplotlib
axis : axis of matplotlib
"""
def __init__(self, objects):
"""
Parameters
------------
objects : list of objects
"""
self.lead_car_history_state = objects[0]
self.follow_car_history_state = objects[1]
self.history_xs = [self.lead_car_history_state[:, 0], self.follow_car_history_state[:, 0]]
self.history_ys = [self.lead_car_history_state[:, 1], self.follow_car_history_state[:, 1]]
self.history_ths = [self.lead_car_history_state[:, 2], self.follow_car_history_state[:, 2]]
# setting up figure
self.anim_fig = plt.figure(dpi=150)
self.axis = self.anim_fig.add_subplot(111)
# imgs
self.object_imgs = []
self.traj_imgs = []
def draw_anim(self, interval=50):
"""draw the animation and save
Parameteres
-------------
interval : int, optional
animation's interval time, you should link the sampling time of systems
default is 50 [ms]
"""
self._set_axis()
self._set_img()
self.skip_num = 3
frame_num = int((len(self.history_xs[0])-1) / self.skip_num)
animation = ani.FuncAnimation(self.anim_fig, self._update_anim, interval=interval, frames=frame_num)
# self.axis.legend()
print('save_animation?')
shuold_save_animation = int(input())
if shuold_save_animation:
print('animation_number?')
num = int(input())
animation.save('animation_{0}.mp4'.format(num), writer='ffmpeg')
# animation.save("Sample.gif", writer = 'imagemagick') # gif保存
plt.show()
def _set_axis(self):
""" initialize the animation axies
"""
# (1) set the axis name
self.axis.set_xlabel(r'$\it{x}$ [m]')
self.axis.set_ylabel(r'$\it{y}$ [m]')
self.axis.set_aspect('equal', adjustable='box')
# (2) set the xlim and ylim
self.axis.set_xlim(-5, 50)
self.axis.set_ylim(-2, 5)
def _set_img(self):
""" initialize the imgs of animation
this private function execute the make initial imgs for animation
"""
# object imgs
obj_color_list = ["k", "k", "m", "m"]
obj_styles = ["solid", "solid", "solid", "solid"]
for i in range(len(obj_color_list)):
temp_img, = self.axis.plot([], [], color=obj_color_list[i], linestyle=obj_styles[i])
self.object_imgs.append(temp_img)
traj_color_list = ["k", "m"]
for i in range(len(traj_color_list)):
temp_img, = self.axis.plot([],[], color=traj_color_list[i], linestyle="dashed")
self.traj_imgs.append(temp_img)
def _update_anim(self, i):
"""the update animation
this function should be used in the animation functions
Parameters
------------
i : int
time step of the animation
the sampling time should be related to the sampling time of system
Returns
-----------
object_imgs : list of img
traj_imgs : list of img
"""
i = int(i * self.skip_num)
self._draw_objects(i)
self._draw_traj(i)
return self.object_imgs, self.traj_imgs,
def _draw_objects(self, i):
"""
This private function is just divided thing of
the _update_anim to see the code more clear
Parameters
------------
i : int
time step of the animation
the sampling time should be related to the sampling time of system
"""
# cars
for j in range(2):
fix_j = j * 2
object_x, object_y, angle_x, angle_y = square_make_with_angles(self.history_xs[j][i],
self.history_ys[j][i],
1.0,
self.history_ths[j][i])
self.object_imgs[fix_j].set_data([object_x, object_y])
self.object_imgs[fix_j + 1].set_data([angle_x, angle_y])
def _draw_traj(self, i):
"""
This private function is just divided thing of
the _update_anim to see the code more clear
Parameters
------------
i : int
time step of the animation
the sampling time should be related to the sampling time of system
"""
for j in range(2):
self.traj_imgs[j].set_data(self.history_xs[j][:i], self.history_ys[j][:i])

247
mpc/main_ACC.py Normal file
View File

@ -0,0 +1,247 @@
import numpy as np
import matplotlib.pyplot as plt
import math
import copy
from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
from animation import AnimDrawer
from control import matlab
class TwoWheeledSystem():
"""SampleSystem, this is the simulator
Attributes
-----------
xs : numpy.ndarray
system states, [x, y, theta]
history_xs : list
time history of state
"""
def __init__(self, init_states=None):
"""
Palameters
-----------
init_state : float, optional, shape(3, )
initial state of system default is None
"""
self.xs = np.zeros(3)
if init_states is not None:
self.xs = copy.deepcopy(init_states)
self.history_xs = [init_states]
def update_state(self, us, dt=0.01):
"""
Palameters
------------
u : numpy.ndarray
inputs of system in some cases this means the reference
dt : float in seconds, optional
sampling time of simulation, default is 0.01 [s]
"""
# for theta 1, theta 1 dot, theta 2, theta 2 dot
k0 = [0.0 for _ in range(3)]
k1 = [0.0 for _ in range(3)]
k2 = [0.0 for _ in range(3)]
k3 = [0.0 for _ in range(3)]
functions = [self._func_x_1, self._func_x_2, self._func_x_3]
# solve Runge-Kutta
for i, func in enumerate(functions):
k0[i] = dt * func(self.xs[0], self.xs[1], self.xs[2], us[0], us[1])
for i, func in enumerate(functions):
k1[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1])
for i, func in enumerate(functions):
k2[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1])
for i, func in enumerate(functions):
k3[i] = dt * func(self.xs[0] + k2[0], self.xs[1] + k2[1], self.xs[2] + k2[2], us[0], us[1])
self.xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6.
self.xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6.
self.xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6.
# save
save_states = copy.deepcopy(self.xs)
self.history_xs.append(save_states)
print(self.xs)
def _func_x_1(self, y_1, y_2, y_3, u_1, u_2):
"""
Parameters
------------
y_1 : float
y_2 : float
y_3 : float
u_1 : float
system input
u_2 : float
system input
"""
y_dot = math.cos(y_3) * u_1
return y_dot
def _func_x_2(self, y_1, y_2, y_3, u_1, u_2):
"""
Parameters
------------
y_1 : float
y_2 : float
y_3 : float
u_1 : float
system input
u_2 : float
system input
"""
y_dot = math.sin(y_3) * u_1
return y_dot
def _func_x_3(self, y_1, y_2, y_3, u_1, u_2):
"""
Parameters
------------
y_1 : float
y_2 : float
y_3 : float
u_1 : float
system input
u_2 : float
system input
"""
y_dot = u_2
return y_dot
def main():
dt = 0.05
simulation_time = 10 # 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
# lineared car system
V = 5.0
A = np.array([[0., V], [0., 0.]])
B = np.array([[0.], [1.]])
C = np.eye(2)
D = np.zeros((2, 1))
# make simulator with coninuous matrix
init_xs_lead = np.array([5., 0., 0.])
init_xs_follow = np.array([0., 0., 0.])
lead_car = TwoWheeledSystem(init_states=init_xs_lead)
follow_car = TwoWheeledSystem(init_states=init_xs_follow)
# 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.])
R = np.diag([5.])
pre_step = 15
# make controller with discreted matrix
# please check the solver, if you want to use the scipy, set the MpcController_scipy
lead_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_step,
dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]),
input_upper=np.array([30.]), input_lower=np.array([-30.]))
follow_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_step,
dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]),
input_upper=np.array([30.]), input_lower=np.array([-30.]))
lead_controller.initialize_controller()
follow_controller.initialize_controller()
# reference
lead_reference = np.array([[0., 0.] for _ in range(pre_step)]).flatten()
for i in range(iteration_num):
print("simulation time = {0}".format(i))
# make lead car's move
if i > int(iteration_num / 3):
lead_reference = np.array([[4., 0.] for _ in range(pre_step)]).flatten()
lead_states = lead_car.xs
lead_opt_u = lead_controller.calc_input(lead_states[1:], lead_reference)
lead_opt_u = np.hstack((np.array([V]), lead_opt_u))
# make follow car
follow_reference = np.array([lead_states[1:] for _ in range(pre_step)]).flatten()
follow_states = follow_car.xs
follow_opt_u = follow_controller.calc_input(follow_states[1:], follow_reference)
follow_opt_u = np.hstack((np.array([V]), follow_opt_u))
lead_car.update_state(lead_opt_u, dt=dt)
follow_car.update_state(follow_opt_u, dt=dt)
# figures and animation
lead_history_states = np.array(lead_car.history_xs)
follow_history_states = np.array(follow_car.history_xs)
time_history_fig = plt.figure()
x_fig = time_history_fig.add_subplot(311)
y_fig = time_history_fig.add_subplot(312)
theta_fig = time_history_fig.add_subplot(313)
car_traj_fig = plt.figure()
traj_fig = car_traj_fig.add_subplot(111)
traj_fig.set_aspect('equal')
x_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 0], label="lead")
x_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 0], label="follow")
x_fig.set_xlabel("time [s]")
x_fig.set_ylabel("x")
x_fig.legend()
y_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 1], label="lead")
y_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 1], label="follow")
y_fig.plot(np.arange(0, simulation_time+0.01, dt), [4. for _ in range(iteration_num+1)], linestyle="dashed")
y_fig.set_xlabel("time [s]")
y_fig.set_ylabel("y")
y_fig.legend()
theta_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 2], label="lead")
theta_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 2], label="follow")
theta_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
theta_fig.set_xlabel("time [s]")
theta_fig.set_ylabel("theta")
theta_fig.legend()
time_history_fig.tight_layout()
time_history_fig.legend()
traj_fig.plot(lead_history_states[:, 0], lead_history_states[:, 1], label="lead")
traj_fig.plot(follow_history_states[:, 0], follow_history_states[:, 1], label="follow")
traj_fig.set_xlabel("x")
traj_fig.set_ylabel("y")
traj_fig.legend()
plt.show()
lead_history_us = np.array(lead_controller.history_us)
follow_history_us = np.array(follow_controller.history_us)
input_history_fig = plt.figure()
u_1_fig = input_history_fig.add_subplot(111)
u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_us[:, 0], label="lead")
u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_us[:, 0], label="follow")
u_1_fig.set_xlabel("time [s]")
u_1_fig.set_ylabel("u_omega")
input_history_fig.tight_layout()
plt.show()
animdrawer = AnimDrawer([lead_history_states, follow_history_states])
animdrawer.draw_anim()
if __name__ == "__main__":
main()

16
mpc/main_example.py
View File

@ -12,7 +12,7 @@ class FirstOrderSystem():
Attributes
-----------
states : float
xs : numpy.ndarray
system states
A : numpy.ndarray
system matrix
@ -20,7 +20,7 @@ class FirstOrderSystem():
control matrix
C : numpy.ndarray
observation matrix
history_state : list
history_xs : list
time history of state
"""
def __init__(self, A, B, C, D=None, init_states=None):
@ -33,7 +33,7 @@ class FirstOrderSystem():
control matrix
C : numpy.ndarray
observation matrix
C : numpy.ndarray
D : numpy.ndarray
directly matrix
init_state : float, optional
initial state of system default is None
@ -59,8 +59,8 @@ class FirstOrderSystem():
"""calculating input
Parameters
------------
u : float
input of system in some cases this means the reference
u : numpy.ndarray
inputs of system in some cases this means the reference
dt : float in seconds, optional
sampling time of simulation, default is 0.01 [s]
"""
@ -77,13 +77,11 @@ class FirstOrderSystem():
# 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
@ -135,7 +133,7 @@ def main():
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)
plant.update_state(opt_u, dt=dt)
history_states = np.array(plant.history_xs)

93
mpc/mpc_func_with_cvxopt.py
View File

@ -14,41 +14,52 @@ class MpcController():
B : numpy.ndarray
input matrix
Q : numpy.ndarray
evaluation function weight
evaluation function weight for states
Qs : numpy.ndarray
concatenated evaluation function weight for states
R : numpy.ndarray
evaluation function weight
evaluation function weight for inputs
Rs : numpy.ndarray
concatenated evaluation function weight for inputs
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_
state_size : int
state size of the plant
input_size : int
input size of the plant
dt_input_upper : numpy.ndarray, shape(input_size, ), optional
constraints of input dt, default is None
dt_input_lower : numpy.ndarray, shape(input_size, ), optional
constraints of input dt, default is None
input_upper : numpy.ndarray, shape(input_size, ), optional
constraints of input, default is None
input_lower : numpy.ndarray, shape(input_size, ), optional
constraints of input, default is None
"""
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):
"""
Parameters
------------
A : numpy.ndarray
system matrix
B : numpy.ndarray
input matrix
Q : numpy.ndarray
evaluation function weight
evaluation function weight for states
R : numpy.ndarray
evaluation function weight
evaluation function weight for inputs
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
dt_input_upper : numpy.ndarray, shape(input_size, ), optional
constraints of input dt, default is None
dt_input_lower : numpy.ndarray, shape(input_size, ), optional
constraints of input dt, default is None
input_upper : numpy.ndarray, shape(input_size, ), optional
constraints of input, default is None
input_lower : numpy.ndarray, shape(input_size, ), optional
constraints of input, default is None
history_us : list
time history of optimal input us(numpy.ndarray)
"""
self.A = np.array(A)
self.B = np.array(B)
@ -73,7 +84,8 @@ class MpcController():
self.dt_input_upper = dt_input_upper
self.input_upper = input_upper
self.input_lower = input_lower
# about mpc matrix
self.W = None
self.omega = None
self.F = None
@ -82,6 +94,7 @@ class MpcController():
def initialize_controller(self):
"""
make matrix to calculate optimal control input
"""
A_factorials = [self.A]
@ -187,22 +200,22 @@ class MpcController():
"""calculate optimal input
Parameters
-----------
states : numpy.array
the size should have (state length * 1)
references :
the size should have (state length * pre_step)
states : numpy.ndarray, shape(state length, )
current state of system
references : numpy.ndarray, shape(state length * pre_step, )
reference of the system, you should set this value as reachable goal
References
------------
opt_input : numpy.ndarray
optimal input, size is (1, input_length)
opt_input : numpy.ndarray, shape(input_length, )
optimal input
"""
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) )
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
@ -220,9 +233,8 @@ 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)
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)
@ -232,26 +244,13 @@ class MpcController():
# 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)
"""
return opt_u

86
mpc/mpc_func_with_scipy.py
View File

@ -15,41 +15,52 @@ class MpcController():
B : numpy.ndarray
input matrix
Q : numpy.ndarray
evaluation function weight
evaluation function weight for states
Qs : numpy.ndarray
concatenated evaluation function weight for states
R : numpy.ndarray
evaluation function weight
evaluation function weight for inputs
Rs : numpy.ndarray
concatenated evaluation function weight for inputs
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_
state_size : int
state size of the plant
input_size : int
input size of the plant
dt_input_upper : numpy.ndarray, shape(input_size, ), optional
constraints of input dt, default is None
dt_input_lower : numpy.ndarray, shape(input_size, ), optional
constraints of input dt, default is None
input_upper : numpy.ndarray, shape(input_size, ), optional
constraints of input, default is None
input_lower : numpy.ndarray, shape(input_size, ), optional
constraints of input, default is None
"""
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):
"""
Parameters
------------
A : numpy.ndarray
system matrix
B : numpy.ndarray
input matrix
Q : numpy.ndarray
evaluation function weight
evaluation function weight for states
R : numpy.ndarray
evaluation function weight
evaluation function weight for inputs
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
dt_input_upper : numpy.ndarray, shape(input_size, ), optional
constraints of input dt, default is None
dt_input_lower : numpy.ndarray, shape(input_size, ), optional
constraints of input dt, default is None
input_upper : numpy.ndarray, shape(input_size, ), optional
constraints of input, default is None
input_lower : numpy.ndarray, shape(input_size, ), optional
constraints of input, default is None
history_us : list
time history of optimal input us(numpy.ndarray)
"""
self.A = np.array(A)
self.B = np.array(B)
@ -188,15 +199,15 @@ class MpcController():
"""calculate optimal input
Parameters
-----------
states : numpy.array
the size should have (state length * 1)
references :
the size should have (state length * pre_step)
states : numpy.ndarray, shape(state length, )
current state of system
references : numpy.ndarray, shape(state length * pre_step, )
reference of the system, you should set this value as reachable goal
References
------------
opt_input : numpy.ndarray
optimal input, size is (1, input_length)
opt_input : numpy.ndarray, shape(input_length, )
optimal input
"""
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))
@ -221,9 +232,8 @@ 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)
ub = np.array(b).flatten()
# print(np.dot(self.F1, self.history_us[-1].reshape(-1, 1)))
def optimized_func(dt_us):
"""
@ -240,25 +250,13 @@ class MpcController():
# constraint
if self.W is not None or self.F is not None :
# print("consider constraint!")
opt_result = minimize(optimized_func, init_dt_us, constraints=[linear_cons])
opt_dt_us = 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)
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)
"""
return opt_u