From d733facb2847321b5a90d4374311e6ab9518dc1b Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Fri, 12 Apr 2019 00:10:53 +0900 Subject: [PATCH] add iLQR --- iLQR/animation.py | 15 ++++++---- iLQR/goal_maker.py | 69 +++++++++++++++++++++++++++++++++++++------- iLQR/ilqr.py | 4 +-- iLQR/main_dynamic.py | 28 ++++++++++++------ iLQR/main_static.py | 27 ++++++++++++----- 5 files changed, 108 insertions(+), 35 deletions(-) diff --git a/iLQR/animation.py b/iLQR/animation.py index d6970d7..92282d6 100644 --- a/iLQR/animation.py +++ b/iLQR/animation.py @@ -141,7 +141,10 @@ class AnimDrawer(): lead_history_states, lead_history_predict_states, traj_ref, history_traj_ref, history_angle_ref """ self.car_history_state = objects[0] - self.traget = objects[1] + self.target = objects[1] + + self.history_target_x = [self.target[:, 0]] + self.history_target_y = [self.target[:, 1]] self.history_xs = [self.car_history_state[:, 0]] self.history_ys = [self.car_history_state[:, 1]] @@ -155,7 +158,7 @@ class AnimDrawer(): self.car_imgs = [] self.traj_imgs = [] - def draw_anim(self, interval=50): + def draw_anim(self, interval=10): """draw the animation and save Parameteres @@ -192,8 +195,8 @@ class AnimDrawer(): self.axis.set_ylabel(r'$\it{y}$ [m]') self.axis.set_aspect('equal', adjustable='box') - LOW_MARGIN = 5 - HIGH_MARGIN = 5 + LOW_MARGIN = 2 + HIGH_MARGIN = 2 self.axis.set_xlim(np.min(self.history_xs) - LOW_MARGIN, np.max(self.history_xs) + HIGH_MARGIN) self.axis.set_ylim(np.min(self.history_ys) - LOW_MARGIN, np.max(self.history_ys) + HIGH_MARGIN) @@ -288,7 +291,7 @@ class AnimDrawer(): self.traj_imgs[0].set_data(self.history_xs[0][:i], self.history_ys[0][:i]) # goal - self.traj_imgs[-1].set_data(self.traget[0], self.traget[1]) + self.traj_imgs[-1].set_data(self.history_target_x[0][i-1], self.history_target_y[0][i-1]) # traj_ref - # self.traj_imgs[2].set_data(self.history_traj_ref[i][0, :], self.history_traj_ref[i][1, :]) \ No newline at end of file + self.traj_imgs[1].set_data(self.history_target_x[0][:i], self.history_target_y[0][:i]) \ No newline at end of file diff --git a/iLQR/goal_maker.py b/iLQR/goal_maker.py index e961c04..2112006 100644 --- a/iLQR/goal_maker.py +++ b/iLQR/goal_maker.py @@ -2,7 +2,6 @@ import numpy as np import math import matplotlib.pyplot as plt - def make_trajectory(goal_type, N): """ Parameters @@ -26,45 +25,93 @@ def make_trajectory(goal_type, N): return ref_trajectory if goal_type == "sin": + # parameters + amplitude = 2. + num_period = 2. + ref_x_trajectory = np.linspace(0., 2 * math.pi * num_period, N) + ref_y_trajectory = amplitude * np.sin(ref_x_trajectory) + + ref_xy_trajectory = np.stack((ref_x_trajectory, ref_y_trajectory)) - - - + # target of theta is always zero + ref_trajectory = np.vstack((ref_xy_trajectory, np.zeros((1, N)))) + + return ref_trajectory.T class GoalMaker(): """ Attributes ----------- - + N : int + length of reference + goal_type : str + goal type + dt : float + sampling time + ref_traj : numpy.ndarray, shape(N, STATE_SIZE) + in this case the state size is 3 """ - def __init_(self, goal_type="const", N=500, dt=0.01): + def __init__(self, N=500, goal_type="const", dt=0.01): """ + Parameters + -------------- + N : int + length of reference + goal_type : str + goal type + dt : float + sampling time """ self.N = N self.goal_type = goal_type self.dt = dt self.ref_traj = None + self.history_target = [] def preprocess(self): """preprocess of make goal - """ goal_type_list = ["const", "sin"] if self.goal_type not in goal_type_list: - raise ValueError("{0} is not in implemented goal type. please implement that!!".format(self.goal_type)) + raise KeyError("{0} is not in implemented goal type. please implement that!!".format(self.goal_type)) - self.ref_traj = make_trajectory(self.goal_type) + self.ref_traj = make_trajectory(self.goal_type, self.N) def calc_goal(self, x): - """ + """ calclate nearest goal + Parameters + ------------ + x : numpy.ndarray, shape(STATE_SIZE, ) + state of the system + + Returns + ------------ + goal : numpy.ndarray, shape(STATE_SIZE, ) """ + # search nearest point + x_dis = (self.ref_traj[:, 0]-x[0])**2 + y_dis = (self.ref_traj[:, 1]-x[1])**2 + index = np.argmin(np.sqrt(x_dis + y_dis)) + + print(index) + MARGIN = 30 + if not self.goal_type == "const": + index += MARGIN + + if index > self.N-1: + index = self.N - 1 + + goal = self.ref_traj[index] + + self.history_target.append(goal) return goal - +if __name__ == "__main__": + make_trajectory("sin", 400) \ No newline at end of file diff --git a/iLQR/ilqr.py b/iLQR/ilqr.py index 1b38b4a..ff9ece2 100644 --- a/iLQR/ilqr.py +++ b/iLQR/ilqr.py @@ -127,8 +127,8 @@ class iLQRController(): """ # total cost - R_11 = 0.01 # terminal u_v cost weight - R_22 = 0.01 # terminal u_th cost weight + R_11 = 1e-4 # terminal u_v cost weight + R_22 = 1e-4 # terminal u_th cost weight l = np.dot(us.T, np.dot(np.diag([R_11, R_22]), us)) diff --git a/iLQR/main_dynamic.py b/iLQR/main_dynamic.py index 0b7fb65..c6b22d6 100644 --- a/iLQR/main_dynamic.py +++ b/iLQR/main_dynamic.py @@ -4,9 +4,9 @@ import math from model import TwoWheeledCar from ilqr import iLQRController +from goal_maker import GoalMaker from animation import AnimDrawer - def main(): """ """ @@ -19,37 +19,47 @@ def main(): car = TwoWheeledCar(init_x) # make goal - goal_maker = + goal_maker = GoalMaker(goal_type="sin") + goal_maker.preprocess() # controller controller = iLQRController() - for iteration in range(NUM_ITERATIONS): print("iteration num = {} / {}".format(iteration, NUM_ITERATIONS)) + target = goal_maker.calc_goal(car.xs) u = controller.calc_input(car, target) car.update_state(u, dt) # update state # figures and animation history_states = np.array(car.history_xs) + history_targets = np.array(goal_maker.history_target) - time_fig = plt.figure(figsize=(3, 4)) + time_fig = plt.figure() x_fig = time_fig.add_subplot(311) y_fig = time_fig.add_subplot(312) th_fig = time_fig.add_subplot(313) time = len(history_states) - x_fig.plot(np.arange(time), history_states[:, 0]) - y_fig.plot(np.arange(time), history_states[:, 1]) - th_fig.plot(np.arange(time), history_states[:, 2]) + + x_fig.plot(np.arange(time), history_states[:, 0], "r") + x_fig.plot(np.arange(1, time), history_targets[:, 0], "b", linestyle="dashdot") + x_fig.set_ylabel("x") + + y_fig.plot(np.arange(time), history_states[:, 1], "r") + y_fig.plot(np.arange(1, time), history_targets[:, 1], "b", linestyle="dashdot") + y_fig.set_ylabel("y") + + th_fig.plot(np.arange(time), history_states[:, 2], "r") + th_fig.plot(np.arange(1, time), history_targets[:, 2], "b", linestyle="dashdot") + th_fig.set_ylabel("th") plt.show() history_states = np.array(car.history_xs) - - animdrawer = AnimDrawer([history_states, target]) + animdrawer = AnimDrawer([history_states, history_targets]) animdrawer.draw_anim() if __name__ == "__main__": diff --git a/iLQR/main_static.py b/iLQR/main_static.py index cf7a050..3cceb82 100644 --- a/iLQR/main_static.py +++ b/iLQR/main_static.py @@ -4,8 +4,10 @@ import math from model import TwoWheeledCar from ilqr import iLQRController +from goal_maker import GoalMaker from animation import AnimDrawer + def main(): """ """ @@ -18,37 +20,48 @@ def main(): car = TwoWheeledCar(init_x) # make goal - target = np.array([5., 3., 0.]) + goal_maker = GoalMaker(goal_type="const") + goal_maker.preprocess() # controller controller = iLQRController() - for iteration in range(NUM_ITERATIONS): print("iteration num = {} / {}".format(iteration, NUM_ITERATIONS)) + target = goal_maker.calc_goal(car.xs) u = controller.calc_input(car, target) car.update_state(u, dt) # update state # figures and animation history_states = np.array(car.history_xs) + history_targets = np.array(goal_maker.history_target) - time_fig = plt.figure(figsize=(3, 4)) + time_fig = plt.figure() x_fig = time_fig.add_subplot(311) y_fig = time_fig.add_subplot(312) th_fig = time_fig.add_subplot(313) time = len(history_states) - x_fig.plot(np.arange(time), history_states[:, 0]) - y_fig.plot(np.arange(time), history_states[:, 1]) - th_fig.plot(np.arange(time), history_states[:, 2]) + + x_fig.plot(np.arange(time), history_states[:, 0], "r") + x_fig.plot(np.arange(1, time), history_targets[:, 0], "b", linestyle="dashdot") + x_fig.set_ylabel("x") + + y_fig.plot(np.arange(time), history_states[:, 1], "r") + y_fig.plot(np.arange(1, time), history_targets[:, 1], "b", linestyle="dashdot") + y_fig.set_ylabel("y") + + th_fig.plot(np.arange(time), history_states[:, 2], "r") + th_fig.plot(np.arange(1, time), history_targets[:, 2], "b", linestyle="dashdot") + th_fig.set_ylabel("th") plt.show() history_states = np.array(car.history_xs) - animdrawer = AnimDrawer([history_states, target]) + animdrawer = AnimDrawer([history_states, history_targets]) animdrawer.draw_anim() if __name__ == "__main__":