add animation

mpc/with_disturbance/animation.py

@@ -84,6 +84,42 @@ def square_make_with_angles(center_x, center_y, size, angle):
     return square_xs, square_ys, np.array(angle_line_xs), np.array(angle_line_ys)
 
 
+def circle_make_with_angles(center_x, center_y, radius, angle):
+    '''
+    Create circle matrix with angle line matrix
+    
+    Parameters
+    -------
+    center_x : float
+        the center x position of the circle
+    center_y : float
+        the center y position of the circle
+    radius : float
+    angle : float [rad]
+    
+    Returns
+    -------
+    circle xs : numpy.ndarray
+    circle ys : numpy.ndarray
+    angle line xs : numpy.ndarray
+    angle line ys : numpy.ndarray
+    '''
+
+    point_num = 100 # 分解能
+
+    circle_xs = []
+    circle_ys = []
+
+    for i in range(point_num + 1):
+        circle_xs.append(center_x + radius * math.cos(i*2*math.pi/point_num))
+        circle_ys.append(center_y + radius * math.sin(i*2*math.pi/point_num))
+
+    angle_line_xs = [center_x, center_x + math.cos(angle) * radius]
+    angle_line_ys = [center_y, center_y + math.sin(angle) * radius]
+
+    return np.array(circle_xs), np.array(circle_ys), np.array(angle_line_xs), np.array(angle_line_ys)
+
+
 class AnimDrawer():
     """create animation of path and robot
     
@@ -99,22 +135,29 @@ class AnimDrawer():
         Parameters
         ------------
         objects : list of objects
+
+        Notes
+        ---------
+        lead_history_states, lead_history_predict_states, traj_ref, history_traj_ref, history_angle_ref
         """
         self.lead_car_history_state = objects[0]
-        self.follow_car_history_state = objects[1]
+        self.lead_car_history_predict_state = objects[1]
         self.traj = objects[2]
+        self.history_traj_ref = objects[3]
+        self.history_angle_ref = objects[4]
         
-        self.history_xs = [self.lead_car_history_state[:, 0], self.follow_car_history_state[:, 0]]
+        self.history_xs = [self.lead_car_history_state[:, 0]]
-        self.history_ys = [self.lead_car_history_state[:, 1], self.follow_car_history_state[:, 1]]
+        self.history_ys = [self.lead_car_history_state[:, 1]]
-        self.history_ths = [self.lead_car_history_state[:, 2], self.follow_car_history_state[:, 2]]
+        self.history_ths = [self.lead_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.car_imgs = []
         self.traj_imgs = []
+        self.predict_imgs = []
 
     def draw_anim(self, interval=50):
         """draw the animation and save
@@ -153,10 +196,12 @@ class AnimDrawer():
         self.axis.set_ylabel(r'$\it{y}$ [m]')
         self.axis.set_aspect('equal', adjustable='box')
 
-        # (2) set the xlim and ylim        
+        LOW_MARGIN = 5
-        self.axis.set_xlim(-2, 20)
+        HIGH_MARGIN = 5
-        self.axis.set_ylim(-10, 10)         
 
+        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)
+        
     def _set_img(self):
         """ initialize the imgs of animation
             this private function execute the make initial imgs for animation
@@ -167,14 +212,22 @@ class AnimDrawer():
 
         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)
+            self.car_imgs.append(temp_img)
         
-        traj_color_list = ["k", "m", "b"]
+        traj_color_list = ["k", "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)
 
+        temp_img, = self.axis.plot([],[], ".", color="m")
+        self.traj_imgs.append(temp_img)
+
+        # predict
+        for _ in range(2 * len(self.history_angle_ref[0])):
+            temp_img, = self.axis.plot([],[], color="g", linewidth=0.5) # point
+            # temp_img, = self.axis.plot([],[], ".", color="g", linewidth=0.5) # point
+            self.predict_imgs.append(temp_img)
     
     def _update_anim(self, i):
         """the update animation
@@ -193,12 +246,24 @@ class AnimDrawer():
         """
         i = int(i * self.skip_num)
 
-        self._draw_objects(i)
+        # self._draw_set_axis(i)
+        self._draw_car(i)
         self._draw_traj(i)
+        # self._draw_prediction(i)
 
-        return self.object_imgs, self.traj_imgs,
+        return self.car_imgs, self.traj_imgs, self.predict_imgs, 
-        
+
-    def _draw_objects(self, i):
+    def _draw_set_axis(self, i):
+        """
+        """
+        # (2) set the xlim and ylim
+        LOW_MARGIN = 20
+        HIGH_MARGIN = 20
+        OVER_LOOK = 50
+        self.axis.set_xlim(np.min(self.history_xs[0][i : i + OVER_LOOK]) - LOW_MARGIN, np.max(self.history_xs[0][i : i + OVER_LOOK]) + HIGH_MARGIN)
+        self.axis.set_ylim(np.min(self.history_ys[0][i : i + OVER_LOOK]) - LOW_MARGIN, np.max(self.history_ys[0][i : i + OVER_LOOK]) + HIGH_MARGIN)         
+
+    def _draw_car(self, i):
         """
         This private function is just divided thing of
         the _update_anim to see the code more clear
@@ -210,16 +275,14 @@ class AnimDrawer():
             the sampling time should be related to the sampling time of system
         """
         # cars
-        for j in range(2):
+        object_x, object_y, angle_x, angle_y = square_make_with_angles(self.history_xs[0][i],
-            fix_j = j * 2
+                                                                        self.history_ys[0][i], 
-            object_x, object_y, angle_x, angle_y = square_make_with_angles(self.history_xs[j][i],
+                                                                        5.0,
-                                                                           self.history_ys[j][i], 
+                                                                        self.history_ths[0][i])
-                                                                           1.0,
+
-                                                                           self.history_ths[j][i])
+        self.car_imgs[0].set_data([object_x, object_y])
+        self.car_imgs[1].set_data([angle_x, angle_y])
 
-            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
@@ -231,7 +294,31 @@ class AnimDrawer():
             time step of the animation
             the sampling time should be related to the sampling time of system
         """
-        for j in range(2):
+        # car
-            self.traj_imgs[j].set_data(self.history_xs[j][:i], self.history_ys[j][:i])
+        self.traj_imgs[0].set_data(self.history_xs[0][:i], self.history_ys[0][:i])
 
-        self.traj_imgs[2].set_data(self.traj[0, :], self.traj[1, :])
+        # all traj_ref
+        self.traj_imgs[1].set_data(self.traj[0, :], self.traj[1, :])
+
+        # traj_ref
+        # self.traj_imgs[2].set_data(self.history_traj_ref[i][0, :], self.history_traj_ref[i][1, :])
+
+    def _draw_prediction(self, i):
+        """draw prediction
+
+        Parameters
+        ------------
+        i : int
+            time step of the animation
+            the sampling time should be related to the sampling time of system
+        """
+
+        for j in range(0, len(self.history_angle_ref[0]), 4):
+            fix_j = j * 2
+            object_x, object_y, angle_x, angle_y =\
+                 circle_make_with_angles(self.lead_car_history_predict_state[i][j, 0],
+                 self.lead_car_history_predict_state[i][j, 1], 1.,
+                 self.lead_car_history_predict_state[i][j, 2])
+
+            self.predict_imgs[fix_j].set_data(object_x, object_y)
+            self.predict_imgs[fix_j + 1].set_data(angle_x, angle_y)

mpc/with_disturbance/func_curvature.py

@@ -138,6 +138,22 @@ def calc_curvatures(traj_ref, predict_step, num_skip):
 
     return angles, curvatures
 
+def calc_ideal_vel(traj_ref, dt):
+    """
+    Parameters
+    ------------
+    traj_ref : numpy.ndarray, shape (2, N)
+        these points should follow subseqently
+    dt : float
+        sampling time of system
+    """
+    # end point and start point
+    diff = traj_ref[:, -1] - traj_ref[:, 0] 
+    distance = np.sqrt(np.sum(diff**2))
+
+    V = distance / (dt * traj_ref.shape[1])
+
+    return V
 
 def main():
     """

mpc/with_disturbance/iterative_MPC.py

@@ -11,11 +11,11 @@ class IterativeMpcController():
     """
     Attributes
     ------------
-    A : numpy.ndarray
+    Ad_s : list of numpy.ndarray
         system matrix
-    B : numpy.ndarray
+    Bd_s : list of numpy.ndarray
         input matrix
-    W_D : numpy.ndarray
+    W_D_s : list of numpy.ndarray
         distubance matrix in state equation
     Q : numpy.ndarray
         evaluation function weight for states
@@ -107,7 +107,7 @@ class IterativeMpcController():
 
             A_factorials.append(temp_mat) # after we use this factorials
             
-        print("phi_mat = \n{0}".format(self.phi_mat))
+        # print("phi_mat = \n{0}".format(self.phi_mat))
 
         self.gamma_mat = copy.deepcopy(self.Bd_s[0])
         gammma_mat_temp = copy.deepcopy(self.Bd_s[0])
@@ -117,7 +117,7 @@ class IterativeMpcController():
             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))
+        # print("gamma_mat = \n{0}".format(self.gamma_mat))
 
         self.theta_mat = copy.deepcopy(self.gamma_mat)
 
@@ -127,25 +127,25 @@ class IterativeMpcController():
 
             self.theta_mat = np.hstack((self.theta_mat, temp_mat))
 
-        print("theta_mat = \n{0}".format(self.theta_mat))
+        # print("theta_mat = \n{0}".format(self.theta_mat))
         
         # disturbance
-        print("A_factorials_mat = \n{0}".format(A_factorials))
+        # print("A_factorials_mat = \n{0}".format(A_factorials))
         A_factorials_mat = np.array(A_factorials).reshape(-1, self.state_size)
-        print("A_factorials_mat = \n{0}".format(A_factorials_mat))
+        # print("A_factorials_mat = \n{0}".format(A_factorials_mat))
 
         eye = np.eye(self.state_size)
         self.dist_mat = np.vstack((eye, A_factorials_mat[:-self.state_size, :]))
         base_mat = copy.deepcopy(self.dist_mat)
 
-        print("base_mat = \n{0}".format(base_mat))
+        # print("base_mat = \n{0}".format(base_mat))
 
         for i in range(self.pre_step - 1):
             temp_mat = np.zeros_like(A_factorials_mat)
             temp_mat[int((i + 1)*self.state_size): , :] = base_mat[:-int((i + 1)*self.state_size) , :]
             self.dist_mat = np.hstack((self.dist_mat, temp_mat))
 
-        print("dist_mat = \n{0}".format(self.dist_mat))
+        # print("dist_mat = \n{0}".format(self.dist_mat))
         
         W_Ds = copy.deepcopy(self.W_D_s[0])
 
@@ -154,7 +154,7 @@ class IterativeMpcController():
         
         self.dist_mat = np.dot(self.dist_mat, W_Ds)
 
-        print("dist_mat = \n{0}".format(self.dist_mat))
+        # print("dist_mat = \n{0}".format(self.dist_mat))
 
         # evaluation function weight
         diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)])
@@ -163,8 +163,8 @@ class IterativeMpcController():
         self.Qs = np.diag(diag_Qs.flatten())
         self.Rs = np.diag(diag_Rs.flatten())
 
-        print("Qs = \n{0}".format(self.Qs))
+        # print("Qs = \n{0}".format(self.Qs))
-        print("Rs = \n{0}".format(self.Rs))
+        # print("Rs = \n{0}".format(self.Rs))
 
         # constraints
         # about dt U
@@ -175,7 +175,7 @@ class IterativeMpcController():
                 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))
+            # print("F = \n{0}".format(self.F))
 
             for i in range(self.pre_step - 1):
                 temp_F = copy.deepcopy(temp_F)
@@ -195,9 +195,9 @@ class IterativeMpcController():
 
             self.f = np.array([temp_f for _ in range(self.pre_step)]).flatten()
 
-            print("F = \n{0}".format(self.F))
+            # print("F = \n{0}".format(self.F))
-            print("F1 = \n{0}".format(self.F1))
+            # print("F1 = \n{0}".format(self.F1))
-            print("f = \n{0}".format(self.f))
+            # print("f = \n{0}".format(self.f))
 
         # about dt_u
         if self.dt_input_lower is not None:
@@ -217,8 +217,8 @@ class IterativeMpcController():
 
             self.omega = np.array([temp_omega for _ in range(self.pre_step)]).flatten()
 
-            print("W = \n{0}".format(self.W))
+            # print("W = \n{0}".format(self.W))
-            print("omega = \n{0}".format(self.omega))
+            # print("omega = \n{0}".format(self.omega))
 
         # about state
         print("check the matrix!! if you think rite, plese push enter")
@@ -235,8 +235,9 @@ class IterativeMpcController():
 
         References
         ------------
-        opt_input : numpy.ndarray, shape(input_length, )
+        opt_u : numpy.ndarray, shape(input_length, )
             optimal input
+        all_opt_u  : numpy.ndarray, shape(PREDICT_STEP, input_length)
         """
         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))
@@ -277,11 +278,19 @@ class IterativeMpcController():
         opt_dt_us = list(opt_result['x'])
         
         opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
+
+        # calc all predit u
+        all_opt_u = [copy.deepcopy(opt_u)]
+        temp_u = copy.deepcopy(opt_u)
+
+        for i in range(1, self.pre_step):
+            temp_u += opt_dt_us[i * self.input_size: (i + 1) * self.input_size]
+            all_opt_u.append(copy.deepcopy(temp_u))
         
         # save
         self.history_us.append(opt_u)
 
-        return opt_u
+        return opt_u, np.array(all_opt_u)
 
     def update_system_model(self, system_model):
         """update system model

mpc/with_disturbance/main_track.py

@@ -9,7 +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
+from func_curvature import calc_curvatures, calc_ideal_vel
 
 class WheeledSystem():
     """SampleSystem, this is the simulator
@@ -21,6 +21,11 @@ class WheeledSystem():
         system states, [x, y, phi, beta]
     history_xs : list
         time history of state
+    tau : float
+        time constant of tire
+    FRONT_WHEEL_BASE : float
+    REAR_WHEEL_BASE : float
+    predict_xs : 
     """
     def __init__(self, init_states=None):
         """
@@ -41,6 +46,7 @@ class WheeledSystem():
             self.xs = copy.deepcopy(init_states)
 
         self.history_xs = [init_states]
+        self.history_predict_xs = []
 
     def update_state(self, us, dt=0.01):
         """
@@ -51,7 +57,6 @@ class WheeledSystem():
         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(self.NUM_STATE)]
         k1 = [0.0 for _ in range(self.NUM_STATE)]
         k2 = [0.0 for _ in range(self.NUM_STATE)]
@@ -80,7 +85,50 @@ class WheeledSystem():
         # save
         save_states = copy.deepcopy(self.xs)
         self.history_xs.append(save_states)
-        print(self.xs)
+        # print(self.xs)
+
+    def predict_state(self, us, dt=0.01):
+        """make predict state by using optimal input made by MPC
+        Paramaters
+        -----------
+        us : array-like, shape(2, N)
+            optimal input made by MPC
+        dt : float in seconds, optional
+            sampling time of simulation, default is 0.01 [s]
+        """
+
+        xs = copy.deepcopy(self.xs)
+        predict_xs = [copy.deepcopy(xs)]
+
+        for i in range(us.shape[1]):
+            k0 = [0.0 for _ in range(self.NUM_STATE)]
+            k1 = [0.0 for _ in range(self.NUM_STATE)]
+            k2 = [0.0 for _ in range(self.NUM_STATE)]
+            k3 = [0.0 for _ in range(self.NUM_STATE)]
+
+            functions = [self._func_x_1, self._func_x_2, self._func_x_3, self._func_x_4]
+
+            # solve Runge-Kutta
+            for i, func in enumerate(functions):
+                k0[i] = dt * func(xs[0], xs[1], xs[2], xs[3], us[0, i], us[1, i])
+
+            for i, func in enumerate(functions):
+                k1[i] = dt * func(xs[0] + k0[0]/2., xs[1] + k0[1]/2., xs[2] + k0[2]/2., xs[3] + k0[3]/2., us[0, i], us[1, i])
+            
+            for i, func in enumerate(functions):
+                k2[i] = dt * func(xs[0] + k1[0]/2., xs[1] + k1[1]/2., xs[2] + k1[2]/2., xs[3] + k1[3]/2., us[0, i], us[1, i])
+            
+            for i, func in enumerate(functions):
+                k3[i] =  dt * func(xs[0] + k2[0], xs[1] + k2[1], xs[2] + k2[2], xs[3] + k2[3], us[0, i], us[1, i])
+            
+            xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6.
+            xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6.
+            xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6.
+            xs[3] += (k0[3] + 2. * k1[3] + 2. * k2[3] + k3[3]) / 6.
+
+            predict_xs.append(copy.deepcopy(xs))
+
+        self.history_predict_xs.append(np.array(predict_xs))
 
     def _func_x_1(self, y_1, y_2, y_3, y_4, u_1, u_2):
         """
@@ -184,6 +232,7 @@ class SystemModel():
         """
         calc next predict systemo models
         V : float
+            current speed of car
         curvatures : list
             this is the next curvature's list
         predict_step : int
@@ -236,52 +285,53 @@ def main():
     # parameters
     dt = 0.01
     simulation_time = 20 # in seconds
-    PREDICT_STEP = 15
+    PREDICT_STEP = 30
     iteration_num = int(simulation_time / dt)
 
     # make simulator with coninuous matrix
-    init_xs_lead = np.array([0., 0., math.pi/4, 0.])
+    init_xs_lead = np.array([0., 0., math.pi/6, 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)
 
     # make system model
     lead_car_system_model = SystemModel()
-    follow_car_system_model = SystemModel()
 
     # reference
+    history_traj_ref = []
+    history_angle_ref = []
     traj_ref_xs, traj_ref_ys = make_sample_traj(int(simulation_time/dt))
     traj_ref = np.array([traj_ref_xs, traj_ref_ys])
-
+    
     # nearest point
     index_min, nearest_point = search_nearest_point(traj_ref, lead_car.xs[:2].reshape(2, 1))
 
     # get traj's curvature
-    NUM_SKIP = 5
+    NUM_SKIP = 3
-    MARGIN = 10
+    MARGIN = 20
     angles, curvatures = calc_curvatures(traj_ref[:, index_min + MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN], PREDICT_STEP, NUM_SKIP)
 
+    # save traj ref
+    history_traj_ref.append(traj_ref[:, index_min + MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN])
+    history_angle_ref.append(angles)
+
+    # print(history_traj_ref)
+    # input()
+
     # evaluation function weight
-    Q = np.diag([100., 1., 1.])
+    Q = np.diag([100., 1000., 1.])
-    R = np.diag([0.01])
+    R = np.diag([0.1])
 
     # System model update
-    V = 3.0 # in pratical we should calc from the state
+    V = calc_ideal_vel(traj_ref, dt) # 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)
 
     # make controller with discreted matrix
     lead_controller = IterativeMpcController(lead_car_system_model, Q, R, PREDICT_STEP,
-                               dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]),
+                               dt_input_upper=np.array([1 * dt]), dt_input_lower=np.array([-1 * dt]),
-                               input_upper=np.array([30.]), input_lower=np.array([-30.]))
+                               input_upper=np.array([1.]), input_lower=np.array([-1.]))
 
-    follow_controller = IterativeMpcController(follow_car_system_model, Q, R, PREDICT_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.]))
 
     # initialize
     lead_controller.initialize_controller()
-    follow_controller.initialize_controller()
     
     for i in range(iteration_num):
         print("simulation time = {0}".format(i))
@@ -292,6 +342,7 @@ 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 + PREDICT_STEP + 2 * NUM_SKIP + MARGIN:
             print("break")
@@ -300,8 +351,12 @@ def main():
         # get traj's curvature
         angles, curvatures = calc_curvatures(traj_ref[:, index_min+MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN], PREDICT_STEP, NUM_SKIP)
 
+        # save
+        history_traj_ref.append(traj_ref[:, index_min + MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN])
+        history_angle_ref.append(angles)
+
         # System model update
-        V = 4.0 # in pratical we should calc from the state
+        V = calc_ideal_vel(traj_ref, dt) # in pratical we should calc from the state
         lead_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP)
 
         # transformation
@@ -318,7 +373,7 @@ def main():
         relative_ref_angle = np.array(angles) - angles[0]
 
         # make ref
-        lead_reference = np.array([[relative_traj[1, -1], relative_ref_angle[i], 0.] for i in range(PREDICT_STEP)]).flatten()
+        lead_reference = np.array([[relative_traj[1, -1], relative_ref_angle[-1], 0.] for i in range(PREDICT_STEP)]).flatten()
 
         print("relative car state = {}".format(relative_car_state))
         print("nearest point = {}".format(nearest_point))
@@ -327,19 +382,27 @@ def main():
         # update system matrix
         lead_controller.update_system_model(lead_car_system_model)
 
-        lead_opt_u = lead_controller.calc_input(relative_car_state, lead_reference)
+        lead_opt_u, all_opt_u = lead_controller.calc_input(relative_car_state, lead_reference)
 
         lead_opt_u = np.hstack((np.array([V]), lead_opt_u))
 
+        all_opt_u = np.stack((np.ones(PREDICT_STEP)*V, all_opt_u.flatten()))
+
         print("opt_u = {}".format(lead_opt_u))
-        # input()
+        print("all_opt_u = {}".format(all_opt_u))
         
+        # predict
+        lead_car.predict_state(all_opt_u, dt=dt)
+
+        # update
         lead_car.update_state(lead_opt_u, dt=dt)
-        follow_car.update_state(lead_opt_u, dt=dt)
+
+        # print(lead_car.history_predict_xs)
+        # input()
 
     # figures and animation
     lead_history_states = np.array(lead_car.history_xs)
-    follow_history_states = np.array(follow_car.history_xs)
+    lead_history_predict_states = lead_car.history_predict_xs
 
     """
     time_history_fig = plt.figure()
@@ -394,7 +457,7 @@ def main():
     plt.show()
     """
 
-    animdrawer = AnimDrawer([lead_history_states, follow_history_states, traj_ref])
+    animdrawer = AnimDrawer([lead_history_states, lead_history_predict_states, traj_ref, history_traj_ref, history_angle_ref])
     animdrawer.draw_anim()
 
 if __name__ == "__main__":

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=5.):
+def make_sample_traj(NUM, dt=0.01, a=30.):
     """
     make sample trajectory
     Parameters
@@ -21,10 +21,9 @@ def make_sample_traj(NUM, dt=0.01, a=5.):
     traj_ys = []
 
     for i in range(NUM):
-        traj_xs.append(dt * i)
+        traj_xs.append(i * 0.1)
         traj_ys.append(a * math.sin(dt * i / DELAY))
 
-
     plt.plot(traj_xs, traj_ys)
     plt.show()