add iLQR
This commit is contained in:
Shunichi09
parent
b013192976
commit
80e8a112cd
233
IOC/animation.py
233
IOC/animation.py
|
|
@ -1,233 +0,0 @@
|
||||||
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 = 1
|
|
||||||
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])
|
|
||||||
246
IOC/main_ACC.py
246
IOC/main_ACC.py
|
|
@ -1,246 +0,0 @@
|
||||||
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()
|
|
||||||
|
|
||||||
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()
|
|
||||||
|
|
@ -1,3 +0,0 @@
|
||||||
import numpy as np
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
|
|
||||||
|
|
@ -1,184 +0,0 @@
|
||||||
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
|
|
||||||
-----------
|
|
||||||
xs : numpy.ndarray
|
|
||||||
system states
|
|
||||||
A : numpy.ndarray
|
|
||||||
system matrix
|
|
||||||
B : numpy.ndarray
|
|
||||||
control matrix
|
|
||||||
C : numpy.ndarray
|
|
||||||
observation matrix
|
|
||||||
history_xs : 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
|
|
||||||
D : 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 : 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]
|
|
||||||
"""
|
|
||||||
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))
|
|
||||||
|
|
||||||
# save
|
|
||||||
save_states = copy.deepcopy(self.xs)
|
|
||||||
self.history_xs.append(save_states)
|
|
||||||
|
|
||||||
def main():
|
|
||||||
dt = 0.05
|
|
||||||
simulation_time = 30 # 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 = 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., 1., 1.])
|
|
||||||
R = np.diag([1., 1.])
|
|
||||||
pre_step = 10
|
|
||||||
|
|
||||||
# make controller with discreted matrix
|
|
||||||
# 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., -5., 7.5] for _ in range(pre_step)]).flatten()
|
|
||||||
states = plant.xs
|
|
||||||
opt_u = controller.calc_input(states, reference)
|
|
||||||
plant.update_state(opt_u, dt=dt)
|
|
||||||
|
|
||||||
history_states = np.array(plant.history_xs)
|
|
||||||
|
|
||||||
time_history_fig = plt.figure()
|
|
||||||
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[:, 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()
|
|
||||||
plt.show()
|
|
||||||
|
|
||||||
history_us = np.array(controller.history_us)
|
|
||||||
input_history_fig = plt.figure()
|
|
||||||
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[:, 0])
|
|
||||||
u_1_fig.set_xlabel("time [s]")
|
|
||||||
u_1_fig.set_ylabel("u_x")
|
|
||||||
|
|
||||||
u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us[:, 1])
|
|
||||||
u_2_fig.set_xlabel("time [s]")
|
|
||||||
u_2_fig.set_ylabel("u_y")
|
|
||||||
input_history_fig.tight_layout()
|
|
||||||
plt.show()
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
23
IOC/memo.md
23
IOC/memo.md
|
|
@ -1,23 +0,0 @@
|
||||||
# ラプラス近似
|
|
||||||
参考
|
|
||||||
- http://triadsou.hatenablog.com/entry/20090217/1234865055
|
|
||||||
|
|
||||||
# iterative LQR
|
|
||||||
これは概要
|
|
||||||
https://people.eecs.berkeley.edu/~pabbeel/cs287-fa12/slides/LQR.pdf
|
|
||||||
|
|
||||||
こっちのほうはがわかりやすいかも
|
|
||||||
https://katefvision.github.io/katefSlides/RECITATIONtrajectoryoptimization_katef.pdf
|
|
||||||
|
|
||||||
直感的に説明してくれているやつ
|
|
||||||
|
|
||||||
https://medium.com/@jonathan_hui/rl-lqr-ilqr-linear-quadratic-regulator-a5de5104c750
|
|
||||||
|
|
||||||
これはsergey先生のやつ
|
|
||||||
http://rll.berkeley.edu/deeprlcoursesp17/docs/week_2_lecture_2_optimal_control.pdf
|
|
||||||
|
|
||||||
Iterative LQRはおそらく、終端状態が固定（目標値になる）と仮定して解く問題っぽい
|
|
||||||
これはMPCみたいにしないとダメだわ
|
|
||||||
結局やっていることとしては、ある時間までの有限時間最適化問題なんだけど
|
|
||||||
そのときに各タイムステップのモデルが変わっても大丈夫的な話をしている気がする
|
|
||||||
非線形かつ時変の問題を解法できるようなイメージ
|
|
||||||
|
|
@ -1,256 +0,0 @@
|
||||||
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 for states
|
|
||||||
Qs : numpy.ndarray
|
|
||||||
concatenated evaluation function weight for states
|
|
||||||
R : numpy.ndarray
|
|
||||||
evaluation function weight for inputs
|
|
||||||
Rs : numpy.ndarray
|
|
||||||
concatenated evaluation function weight for inputs
|
|
||||||
pre_step : int
|
|
||||||
prediction step
|
|
||||||
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 for states
|
|
||||||
R : numpy.ndarray
|
|
||||||
evaluation function weight for inputs
|
|
||||||
pre_step : int
|
|
||||||
prediction step
|
|
||||||
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)
|
|
||||||
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
|
|
||||||
|
|
||||||
# about mpc matrix
|
|
||||||
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.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, 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))
|
|
||||||
|
|
||||||
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)
|
|
||||||
|
|
||||||
ub = np.array(b).flatten()
|
|
||||||
|
|
||||||
# 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 :
|
|
||||||
opt_result = solvers.qp(P, q, G=A, h=b)
|
|
||||||
|
|
||||||
opt_dt_us = list(opt_result['x'])
|
|
||||||
|
|
||||||
opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
|
|
||||||
|
|
||||||
# save
|
|
||||||
self.history_us.append(opt_u)
|
|
||||||
|
|
||||||
return opt_u
|
|
||||||
|
|
@ -1,262 +0,0 @@
|
||||||
import numpy as np
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import math
|
|
||||||
import copy
|
|
||||||
|
|
||||||
from scipy.optimize import minimize
|
|
||||||
from scipy.optimize import LinearConstraint
|
|
||||||
|
|
||||||
class MpcController():
|
|
||||||
"""
|
|
||||||
Attributes
|
|
||||||
------------
|
|
||||||
A : numpy.ndarray
|
|
||||||
system matrix
|
|
||||||
B : numpy.ndarray
|
|
||||||
input matrix
|
|
||||||
Q : numpy.ndarray
|
|
||||||
evaluation function weight for states
|
|
||||||
Qs : numpy.ndarray
|
|
||||||
concatenated evaluation function weight for states
|
|
||||||
R : numpy.ndarray
|
|
||||||
evaluation function weight for inputs
|
|
||||||
Rs : numpy.ndarray
|
|
||||||
concatenated evaluation function weight for inputs
|
|
||||||
pre_step : int
|
|
||||||
prediction step
|
|
||||||
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 for states
|
|
||||||
R : numpy.ndarray
|
|
||||||
evaluation function weight for inputs
|
|
||||||
pre_step : int
|
|
||||||
prediction step
|
|
||||||
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)
|
|
||||||
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.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, 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) )
|
|
||||||
|
|
||||||
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)
|
|
||||||
|
|
||||||
ub = np.array(b).flatten()
|
|
||||||
|
|
||||||
def optimized_func(dt_us):
|
|
||||||
"""
|
|
||||||
"""
|
|
||||||
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]
|
|
||||||
|
|
||||||
# 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 :
|
|
||||||
opt_result = minimize(optimized_func, init_dt_us, constraints=[linear_cons])
|
|
||||||
|
|
||||||
opt_dt_us = opt_result.x
|
|
||||||
|
|
||||||
opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
|
|
||||||
|
|
||||||
# save
|
|
||||||
self.history_us.append(opt_u)
|
|
||||||
|
|
||||||
return opt_u
|
|
||||||
|
|
@ -1,211 +0,0 @@
|
||||||
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()
|
|
||||||
|
|
@ -4,7 +4,7 @@ import math
|
||||||
import copy
|
import copy
|
||||||
|
|
||||||
# from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
|
# from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
|
||||||
from iterative_MPC import IterativeMpcController
|
from extended_MPC import IterativeMpcController
|
||||||
from animation import AnimDrawer
|
from animation import AnimDrawer
|
||||||
# from control import matlab
|
# from control import matlab
|
||||||
from coordinate_trans import coordinate_transformation_in_angle, coordinate_transformation_in_position
|
from coordinate_trans import coordinate_transformation_in_angle, coordinate_transformation_in_position
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue