add iterative mpc

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])
+            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, :])

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()
+
+
+
+
+

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
+    simulation_time = 20 # in seconds
-    PREDICT_STEP = 15
+    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_lead = np.array([0., 0., math.pi/4, 0.])
-    init_xs_follow = np.array([0., 0., 0., 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.])
+    Q = np.diag([100., 1., 1.])
-    R = np.diag([5.])
+    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 + 2 * NUM_SKIP], PREDICT_STEP, NUM_SKIP)
-        angles, curvatures = calc_curvatures(traj_ref[:, index_min:index_min + PREDICT_STEP + 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__":

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