diff --git a/mpc/with_disturbance/animation.py b/mpc/with_disturbance/animation.py index beee54c..b5e4942 100755 --- a/mpc/with_disturbance/animation.py +++ b/mpc/with_disturbance/animation.py @@ -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]) \ No newline at end of file + 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, :]) \ No newline at end of file diff --git a/mpc/with_disturbance/func_curvature.py b/mpc/with_disturbance/func_curvature.py new file mode 100644 index 0000000..b525409 --- /dev/null +++ b/mpc/with_disturbance/func_curvature.py @@ -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() + + + + + diff --git a/mpc/with_disturbance/main_track.py b/mpc/with_disturbance/main_track.py index 48c785d..e3b0477 100644 --- a/mpc/with_disturbance/main_track.py +++ b/mpc/with_disturbance/main_track.py @@ -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__": diff --git a/mpc/with_disturbance/traj_func.py b/mpc/with_disturbance/traj_func.py index ddf6304..385a08c 100644 --- a/mpc/with_disturbance/traj_func.py +++ b/mpc/with_disturbance/traj_func.py @@ -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