add iLQR

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
+        LOW_MARGIN = 2
-        HIGH_MARGIN = 5
+        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, :])
+        self.traj_imgs[1].set_data(self.history_target_x[0][:i], self.history_target_y[0][:i])

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)

iLQR/ilqr.py

@@ -127,8 +127,8 @@ class iLQRController():
         """
 
         # total cost
-        R_11 = 0.01 # terminal u_v cost weight
+        R_11 = 1e-4 # terminal u_v cost weight
-        R_22 = 0.01 # terminal u_th 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))
 

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])
+    x_fig.plot(np.arange(time), history_states[:, 0], "r")
-    th_fig.plot(np.arange(time), history_states[:, 2])
+    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, history_targets])
-    animdrawer = AnimDrawer([history_states, target])
     animdrawer.draw_anim()
 
 if __name__ == "__main__":

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])
+    x_fig.plot(np.arange(time), history_states[:, 0], "r")
-    th_fig.plot(np.arange(time), history_states[:, 2])
+    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__":