first release lookuptable_generator

PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py

@@ -0,0 +1,96 @@
+"""
+Lookup Table generation for model predictive trajectory generator
+
+author: Atsushi Sakai
+"""
+from matplotlib import pyplot as plt
+import numpy as np
+import math
+import model_predictive_trajectory_generator as planner
+import motion_model
+
+
+def calc_states_list():
+    maxyaw = math.radians(-30.0)
+
+    x = np.arange(1.0, 30.0, 5.0)
+    y = np.arange(0.0, 20.0, 2.0)
+    yaw = np.arange(-maxyaw, maxyaw, maxyaw)
+
+    states = []
+    for iyaw in yaw:
+        for iy in y:
+            for ix in x:
+                states.append([ix, iy, iyaw])
+    #  print(len(states))
+
+    return states
+
+
+def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable):
+
+    mind = float("inf")
+    minid = -1
+
+    for (i, table) in enumerate(lookuptable):
+
+        dx = tx - table[0]
+        dy = ty - table[1]
+        dyaw = tyaw - table[2]
+        d = math.sqrt(dx**2 + dy**2 + dyaw**2)
+        if d <= mind:
+            minid = i
+            mind = d
+
+    #  print(minid)
+
+    return lookuptable[minid]
+
+
+def generate_lookup_table():
+
+    states = calc_states_list()
+    k0 = 0.0
+
+    # x, y, yaw, s, km, kf
+    lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
+
+    for state in states:
+        bestp = search_nearest_one_from_lookuptable(
+            state[0], state[1], state[2], lookuptable)
+        #  print(bestp)
+
+        target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
+        init_p = np.matrix(
+            [math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).T
+
+        x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
+
+        if x is not None:
+            print("find good path")
+            lookuptable.append(
+                [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
+
+    print("finish lookuptable generation")
+
+    for table in lookuptable:
+        xc, yc, yawc = motion_model.generate_trajectory(
+            table[3], table[4], table[5], k0)
+        plt.plot(xc, yc, "-r")
+        xc, yc, yawc = motion_model.generate_trajectory(
+            table[3], -table[4], -table[5], k0)
+        plt.plot(xc, yc, "-r")
+
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()
+
+    print("Done")
+
+
+def main():
+    generate_lookup_table()
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py

@@ -11,10 +11,11 @@ import motion_model
 from matplotrecorder import matplotrecorder
 
 # optimization parameter
-maxiter = 1000
-h = np.matrix([0.1, 0.002, 0.002]).T  # parameter sampling distanse
+maxiter = 100
+h = np.matrix([0.1, 0.001, 0.001]).T  # parameter sampling distanse
 
 matplotrecorder.donothing = True
+show_graph = False
 
 
 def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
@@ -69,7 +70,7 @@ def selection_learning_param(dp, p, k0, target):
 
     mincost = float("inf")
     mina = 1.0
-    maxa = 5.0
+    maxa = 2.0
     da = 0.5
 
     for a in np.arange(mina, maxa, da):
@@ -108,24 +109,29 @@ def optimize_trajectory(target, k0, p):
         #  print(dc.T)
 
         cost = np.linalg.norm(dc)
-        print("cost is:" + str(cost))
         if cost <= 0.05:
-            print("cost is:" + str(cost))
-            print(p)
+            print("path is ok cost is:" + str(cost))
             break
 
         J = calc_J(target, p, h, k0)
-        dp = - np.linalg.inv(J) * dc
+        try:
+            dp = - np.linalg.inv(J) * dc
+        except np.linalg.linalg.LinAlgError:
+            print("cannot calc path LinAlgError")
+            xc, yc, yawc, p = None, None, None, None
+            break
         alpha = selection_learning_param(dp, p, k0, target)
 
         p += alpha * np.array(dp)
         #  print(p.T)
 
-        show_trajectory(target, xc, yc)
+        if show_graph:
+            show_trajectory(target, xc, yc)
+    else:
+        xc, yc, yawc, p = None, None, None, None
+        print("cannot calc path")
 
-    show_trajectory(target, xc, yc)
-
-    print("done")
+    return xc, yc, yawc, p
 
 
 def test_optimize_trajectory():
@@ -136,7 +142,9 @@ def test_optimize_trajectory():
 
     init_p = np.matrix([6.0, 0.0, 0.0]).T
 
-    optimize_trajectory(target, k0, init_p)
+    x, y, yaw, p = optimize_trajectory(target, k0, init_p)
+
+    show_trajectory(target, x, y)
     matplotrecorder.save_movie("animation.gif", 0.1)
 
     #  plt.plot(x, y, "-r")

PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py

@@ -46,7 +46,7 @@ def generate_trajectory(s, km, kf, k0):
     kk = np.array([k0, km, kf])
     t = np.arange(0.0, time, time / n)
     kp = scipy.interpolate.spline(tk, kk, t, order=2)
-    dt = time / n
+    dt = float(time / n)
 
     #  plt.plot(t, kp)
     #  plt.show()