add iterative mpc
This commit is contained in:
Shunichi09
parent
e7263b216a
commit
bf703611bd
mpc/with_disturbance
|
|
@ -102,6 +102,7 @@ class AnimDrawer():
|
|||
"""
|
||||
self.lead_car_history_state = objects[0]
|
||||
self.follow_car_history_state = objects[1]
|
||||
self.traj = objects[2]
|
||||
|
||||
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]]
|
||||
|
|
@ -153,7 +154,7 @@ class AnimDrawer():
|
|||
self.axis.set_aspect('equal', adjustable='box')
|
||||
|
||||
# (2) set the xlim and ylim
|
||||
self.axis.set_xlim(-2, 50)
|
||||
self.axis.set_xlim(-2, 20)
|
||||
self.axis.set_ylim(-10, 10)
|
||||
|
||||
def _set_img(self):
|
||||
|
|
@ -168,11 +169,12 @@ class AnimDrawer():
|
|||
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"]
|
||||
traj_color_list = ["k", "m", "b"]
|
||||
|
||||
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
|
||||
|
|
@ -230,4 +232,6 @@ class AnimDrawer():
|
|||
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])
|
||||
self.traj_imgs[j].set_data(self.history_xs[j][:i], self.history_ys[j][:i])
|
||||
|
||||
self.traj_imgs[2].set_data(self.traj[0, :], self.traj[1, :])
|
||||
|
|
@ -0,0 +1,166 @@
|
|||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import math
|
||||
|
||||
import random
|
||||
|
||||
from traj_func import make_sample_traj
|
||||
|
||||
def calc_curvature(points):
|
||||
"""
|
||||
Parameters
|
||||
-----------
|
||||
points : numpy.ndarray, shape (2, 3)
|
||||
these points should follow subseqently
|
||||
|
||||
Returns
|
||||
----------
|
||||
curvature : float
|
||||
"""
|
||||
# Gradient 1
|
||||
diff = points[:, 0] - points[:, 1]
|
||||
Gradient_1 = -1. / (diff[1] / diff[0])
|
||||
# Gradient 2
|
||||
diff = points[:, 1] - points[:, 2]
|
||||
Gradient_2 = -1. / (diff[1] / diff[0])
|
||||
|
||||
# middle point 1
|
||||
middle_point_1 = (points[:, 0] + points[:, 1]) / 2.
|
||||
|
||||
# middle point 2
|
||||
middle_point_2 = (points[:, 1] + points[:, 2]) / 2.
|
||||
|
||||
# calc center
|
||||
c_x = (middle_point_1[1] - middle_point_2[1] - middle_point_1[0] * Gradient_1 + middle_point_2[0] * Gradient_2) / (Gradient_2 - Gradient_1)
|
||||
c_y = middle_point_1[1] - (middle_point_1[0] - c_x) * Gradient_1
|
||||
|
||||
R = math.sqrt((points[0, 0] - c_x)**2 + (points[1, 0] - c_y)**2)
|
||||
|
||||
"""
|
||||
plt.scatter(points[0, :], points[1, :])
|
||||
plt.scatter(c_x, c_y)
|
||||
|
||||
plot_points_x = []
|
||||
plot_points_y = []
|
||||
|
||||
for theta in np.arange(0, 2*math.pi, 0.01):
|
||||
plot_points_x.append(math.cos(theta)*R + c_x)
|
||||
plot_points_y.append(math.sin(theta)*R + c_y)
|
||||
|
||||
plt.plot(plot_points_x, plot_points_y)
|
||||
|
||||
plt.show()
|
||||
"""
|
||||
|
||||
return 1. / R
|
||||
|
||||
def calc_curvatures(traj_ref, predict_step, num_skip):
|
||||
"""
|
||||
Parameters
|
||||
-----------
|
||||
traj_ref : numpy.ndarray, shape (2, N)
|
||||
these points should follow subseqently
|
||||
predict_step : int
|
||||
predict step
|
||||
num_skip : int
|
||||
skip_num
|
||||
|
||||
Returns
|
||||
----------
|
||||
angles : list
|
||||
curvature : list
|
||||
"""
|
||||
|
||||
angles = []
|
||||
curvatures = []
|
||||
|
||||
for i in range(predict_step):
|
||||
# make pairs
|
||||
points = np.zeros((2, 3))
|
||||
|
||||
points[:, 0] = traj_ref[:, i]
|
||||
points[:, 1] = traj_ref[:, i + num_skip]
|
||||
points[:, 2] = traj_ref[:, i + 2 * num_skip]
|
||||
|
||||
# Gradient 1
|
||||
diff = points[:, 0] - points[:, 1]
|
||||
Gradient_1 = -1. / (diff[1] / diff[0])
|
||||
# Gradient 2
|
||||
diff = points[:, 1] - points[:, 2]
|
||||
Gradient_2 = -1. / (diff[1] / diff[0])
|
||||
|
||||
# middle point 1
|
||||
middle_point_1 = (points[:, 0] + points[:, 1]) / 2.
|
||||
|
||||
# middle point 2
|
||||
middle_point_2 = (points[:, 1] + points[:, 2]) / 2.
|
||||
|
||||
# calc center
|
||||
c_x = (middle_point_1[1] - middle_point_2[1] - middle_point_1[0] * Gradient_1 + middle_point_2[0] * Gradient_2) / (Gradient_2 - Gradient_1)
|
||||
c_y = middle_point_1[1] - (middle_point_1[0] - c_x) * Gradient_1
|
||||
|
||||
# calc R
|
||||
R = math.sqrt((points[0, 0] - c_x)**2 + (points[1, 0] - c_y)**2)
|
||||
|
||||
# add
|
||||
diff = points[:, 2] - points[:, 0]
|
||||
angles.append(math.atan2(diff[1], diff[0]))
|
||||
curvatures.append(1. / R)
|
||||
|
||||
# plot
|
||||
"""
|
||||
plt.scatter(points[0, :], points[1, :])
|
||||
plt.scatter(c_x, c_y)
|
||||
|
||||
plot_points_x = []
|
||||
plot_points_y = []
|
||||
|
||||
for theta in np.arange(0, 2*math.pi, 0.01):
|
||||
plot_points_x.append(math.cos(theta)*R + c_x)
|
||||
plot_points_y.append(math.sin(theta)*R + c_y)
|
||||
|
||||
plt.plot(plot_points_x, plot_points_y)
|
||||
|
||||
plot_points_x = []
|
||||
plot_points_y = []
|
||||
|
||||
for x in np.arange(-5, 5, 0.01):
|
||||
plot_points_x.append(x)
|
||||
plot_points_y.append(x * math.tan(angles[-1]))
|
||||
|
||||
plt.plot(plot_points_x, plot_points_y)
|
||||
|
||||
plt.xlim(-1, 10)
|
||||
plt.ylim(-1, 2)
|
||||
|
||||
plt.show()
|
||||
"""
|
||||
|
||||
return angles, curvatures
|
||||
|
||||
|
||||
def main():
|
||||
"""
|
||||
points = np.zeros((2, 3))
|
||||
points[:, 0] = np.array([1. + random.random(), random.random()])
|
||||
|
||||
points[:, 1] = np.array([random.random(), 3 + random.random()])
|
||||
|
||||
points[:, 2] = np.array([3 + random.random(), -3 + random.random()])
|
||||
|
||||
calc_cuvature(points)
|
||||
"""
|
||||
|
||||
traj_ref_xs, traj_ref_ys = make_sample_traj(1000)
|
||||
traj_ref = np.array([traj_ref_xs, traj_ref_ys])
|
||||
|
||||
calc_curvatures(traj_ref[:, 10:10 + 15 + 100 * 2], 15, 100)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -9,6 +9,7 @@ from animation import AnimDrawer
|
|||
# from control import matlab
|
||||
from coordinate_trans import coordinate_transformation_in_angle, coordinate_transformation_in_position
|
||||
from traj_func import make_sample_traj
|
||||
from func_curvature import calc_curvatures
|
||||
|
||||
class WheeledSystem():
|
||||
"""SampleSystem, this is the simulator
|
||||
|
|
@ -164,7 +165,7 @@ class SystemModel():
|
|||
Q : numpy.ndarray
|
||||
R : numpy.ndarray
|
||||
"""
|
||||
def __init__(self, tau = 0.15, dt = 0.016):
|
||||
def __init__(self, tau = 0.01, dt = 0.01):
|
||||
"""
|
||||
Parameters
|
||||
-----------
|
||||
|
|
@ -191,7 +192,7 @@ class SystemModel():
|
|||
for i in range(predict_step):
|
||||
delta_r = math.atan2(self.WHEEL_BASE, 1. / curvatures[i])
|
||||
|
||||
A12 = (V / self.WHEEL_BASE) / math.cos(delta_r)
|
||||
A12 = (V / self.WHEEL_BASE) / (math.cos(delta_r)**2)
|
||||
A22 = (1. - 1. / self.tau * self.dt)
|
||||
|
||||
Ad = np.array([[1., V * self.dt, 0.],
|
||||
|
|
@ -200,7 +201,9 @@ class SystemModel():
|
|||
|
||||
Bd = np.array([[0.], [0.], [1. / self.tau]]) * self.dt
|
||||
|
||||
W_D_0 = (V / self.WHEEL_BASE) * delta_r / (math.cos(delta_r)**2) - V * curvatures[i]
|
||||
# -v*curvature + v/L*(tan(delta_r)-delta_r*cos_delta_r_squared_inv);
|
||||
# W_D_0 = V / self.WHEEL_BASE * (delta_r / (math.cos(delta_r)**2)
|
||||
W_D_0 = -V * curvatures[i] + (V / self.WHEEL_BASE) * (math.tan(delta_r) - delta_r / (math.cos(delta_r)**2))
|
||||
|
||||
W_D = np.array([[0.], [W_D_0], [0.]]) * self.dt
|
||||
|
||||
|
|
@ -228,49 +231,17 @@ def search_nearest_point(points, base_point):
|
|||
|
||||
return index_min, points[:, index_min]
|
||||
|
||||
def calc_curvatures(traj_ref, predict_step, num_skip):
|
||||
"""
|
||||
Parameters
|
||||
-----------
|
||||
points : numpy.ndarray, shape is (2, predict_step + num_skip)
|
||||
predict_step : int
|
||||
num_skip : int
|
||||
|
||||
Returns
|
||||
-------
|
||||
angles : list
|
||||
list of angle
|
||||
curvatures : list
|
||||
list of curvature
|
||||
"""
|
||||
angles = []
|
||||
curvatures = []
|
||||
|
||||
for i in range(predict_step):
|
||||
|
||||
temp = traj_ref[:, i + num_skip] - traj_ref[:, i]
|
||||
alpha = math.atan2(temp[1], temp[0])
|
||||
|
||||
angles.append(alpha)
|
||||
|
||||
distance = np.linalg.norm(temp)
|
||||
R = distance / (2. * math.sin(alpha))
|
||||
curvatures.append(1. / R)
|
||||
|
||||
# print("curvatures = {}".format(curvatures))
|
||||
|
||||
return angles, curvatures
|
||||
|
||||
def main():
|
||||
# parameters
|
||||
dt = 0.01
|
||||
simulation_time = 10 # in seconds
|
||||
PREDICT_STEP = 15
|
||||
simulation_time = 20 # in seconds
|
||||
PREDICT_STEP = 20
|
||||
iteration_num = int(simulation_time / dt)
|
||||
|
||||
# make simulator with coninuous matrix
|
||||
init_xs_lead = np.array([0., 0., 0. ,0.])
|
||||
init_xs_follow = np.array([0., 0., 0., 0.])
|
||||
init_xs_lead = np.array([0., 0., math.pi/4, 0.])
|
||||
init_xs_follow = np.array([0., 0., math.pi/4, 0.])
|
||||
lead_car = WheeledSystem(init_states=init_xs_lead)
|
||||
follow_car = WheeledSystem(init_states=init_xs_follow)
|
||||
|
||||
|
|
@ -287,14 +258,14 @@ def main():
|
|||
|
||||
# get traj's curvature
|
||||
NUM_SKIP = 5
|
||||
angles, curvatures = calc_curvatures(traj_ref[:, index_min:index_min + PREDICT_STEP + NUM_SKIP], PREDICT_STEP, NUM_SKIP)
|
||||
angles, curvatures = calc_curvatures(traj_ref[:, index_min:index_min + PREDICT_STEP + 2 * NUM_SKIP], PREDICT_STEP, NUM_SKIP)
|
||||
|
||||
# evaluation function weight
|
||||
Q = np.diag([1., 1., 1.])
|
||||
R = np.diag([5.])
|
||||
Q = np.diag([100., 1., 1.])
|
||||
R = np.diag([0.01])
|
||||
|
||||
# System model update
|
||||
V = 4.0 # in pratical we should calc from the state
|
||||
V = 2.0 # in pratical we should calc from the state
|
||||
lead_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP)
|
||||
follow_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP)
|
||||
|
||||
|
|
@ -321,13 +292,12 @@ def main():
|
|||
# nearest point
|
||||
index_min, nearest_point = search_nearest_point(traj_ref, lead_car.xs[:2].reshape(2, 1))
|
||||
# end check
|
||||
if len(traj_ref_ys) <= index_min + 10:
|
||||
if len(traj_ref_ys) <= index_min + PREDICT_STEP + 2 * NUM_SKIP:
|
||||
print("break")
|
||||
break
|
||||
|
||||
# get traj's curvature
|
||||
NUM_SKIP = 2
|
||||
angles, curvatures = calc_curvatures(traj_ref[:, index_min:index_min + PREDICT_STEP + NUM_SKIP], PREDICT_STEP, NUM_SKIP)
|
||||
angles, curvatures = calc_curvatures(traj_ref[:, index_min:index_min + PREDICT_STEP + 2 * NUM_SKIP], PREDICT_STEP, NUM_SKIP)
|
||||
|
||||
# System model update
|
||||
V = 4.0 # in pratical we should calc from the state
|
||||
|
|
@ -341,8 +311,13 @@ def main():
|
|||
relative_car_angle = lead_states[2] - angles[0]
|
||||
relative_car_state = np.hstack((relative_car_position[1], relative_car_angle, lead_states[-1]))
|
||||
|
||||
# traj_ref
|
||||
relative_traj = coordinate_transformation_in_position(traj_ref[:, index_min:index_min + PREDICT_STEP], nearest_point)
|
||||
relative_traj = coordinate_transformation_in_angle(relative_traj, angles[0])
|
||||
relative_ref_angle = np.array(angles) - angles[0]
|
||||
|
||||
# make ref
|
||||
lead_reference = np.array([[0., 0., 0.] for i in range(PREDICT_STEP)]).flatten()
|
||||
lead_reference = np.array([[relative_traj[1, -1], relative_ref_angle[i], 0.] for i in range(PREDICT_STEP)]).flatten()
|
||||
|
||||
print("relative car state = {}".format(relative_car_state))
|
||||
print("nearest point = {}".format(nearest_point))
|
||||
|
|
@ -418,7 +393,7 @@ def main():
|
|||
plt.show()
|
||||
"""
|
||||
|
||||
animdrawer = AnimDrawer([lead_history_states, follow_history_states])
|
||||
animdrawer = AnimDrawer([lead_history_states, follow_history_states, traj_ref])
|
||||
animdrawer.draw_anim()
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@ import numpy as np
|
|||
import matplotlib.pyplot as plt
|
||||
import math
|
||||
|
||||
def make_sample_traj(NUM, dt=0.01, a=1.):
|
||||
def make_sample_traj(NUM, dt=0.01, a=5.):
|
||||
"""
|
||||
make sample trajectory
|
||||
Parameters
|
||||
|
|
|
|||
Loading…
Reference in New Issue