second release

.gitmodules

@@ -7,3 +7,6 @@
 [submodule "PathPlanning/LatticePlanner/matplotrecorder"]
 	path = PathPlanning/LatticePlanner/matplotrecorder
 	url = https://github.com/AtsushiSakai/matplotrecorder.git
+[submodule "PathPlanning/ModelPredictiveTrajectoryGenerator/matplotrecorder"]
+	path = PathPlanning/ModelPredictiveTrajectoryGenerator/matplotrecorder
+	url = https://github.com/AtsushiSakai/matplotrecorder

PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py

@@ -0,0 +1,82 @@
+import math
+import numpy as np
+import scipy.interpolate
+
+# motion parameter
+L = 1.0  # wheel base
+ds = 0.1  # course distanse
+v = 10.0 / 3.6  # velocity [m/s]
+
+
+class State:
+
+    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
+        self.x = x
+        self.y = y
+        self.yaw = yaw
+        self.v = v
+
+
+def pi_2_pi(angle):
+    while(angle > math.pi):
+        angle = angle - 2.0 * math.pi
+
+    while(angle < -math.pi):
+        angle = angle + 2.0 * math.pi
+
+    return angle
+
+
+def update(state, v, delta, dt, L):
+
+    state.v = v
+    state.x = state.x + state.v * math.cos(state.yaw) * dt
+    state.y = state.y + state.v * math.sin(state.yaw) * dt
+    state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
+    state.yaw = pi_2_pi(state.yaw)
+
+    return state
+
+
+def generate_trajectory(s, km, kf, k0):
+
+    n = s / ds
+    time = s / v  # [s]
+    tk = np.array([0.0, time / 2.0, time])
+    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
+
+    #  plt.plot(t, kp)
+    #  plt.show()
+
+    state = State()
+    x, y, yaw = [state.x], [state.y], [state.yaw]
+
+    for ikp in kp:
+        state = update(state, v, ikp, dt, L)
+        x.append(state.x)
+        y.append(state.y)
+        yaw.append(state.yaw)
+
+    return x, y, yaw
+
+
+def generate_last_state(s, km, kf, k0):
+
+    n = s / ds
+    time = s / v  # [s]
+    tk = np.array([0.0, time / 2.0, time])
+    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
+
+    #  plt.plot(t, kp)
+    #  plt.show()
+
+    state = State()
+
+    [update(state, v, ikp, dt, L) for ikp in kp]
+    return state.x, state.y, state.yaw

PathPlanning/ModelPredictiveTrajectoryGenerator/neuralnet_init_pram_regressor.py

@@ -0,0 +1,79 @@
+"""
+
+author: Atsushi Sakai
+"""
+from sklearn.neural_network import MLPRegressor
+from matplotlib import pyplot as plt
+import numpy as np
+import math
+import motion_model
+
+
+def calc_motion_param(maxs, ds, maxk, dk):
+    s = np.arange(1.0, maxs, ds)
+    km = np.arange(-maxk, maxk, dk)
+    kf = np.arange(-maxk, maxk, dk)
+    motion_param = []
+
+    for p1 in s:
+        for p2 in km:
+            for p3 in kf:
+                motion_param.append([p1, p2, p3])
+
+    print("motion_param size is:", len(motion_param))
+    return motion_param
+
+
+def calc_state_param(motion_param):
+    print("calc_motion_param")
+
+    state_param = []
+
+    for p in motion_param:
+        x, y, yaw = motion_model.generate_last_state(p[0], p[1], p[2], 0.0)
+        state_param.append([x, y, yaw])
+
+    print("calc_state_param done", len(state_param))
+
+    return state_param
+
+
+def learn_motion_model():
+
+    # calc motion param
+    # s, k0, km, kf
+    maxk = math.radians(45.0)
+    dk = math.radians(1.0)
+    maxs = 50.0
+    ds = 5.0
+
+    motion_param = calc_motion_param(maxs, ds, maxk, dk)
+    state_param = calc_state_param(motion_param)
+    reg = MLPRegressor(hidden_layer_sizes=(100, 100, 100))
+    reg.fit(state_param, motion_param)
+    print(reg.score(state_param, motion_param))
+
+    x = 10.0
+    y = 5.0
+    yaw = math.radians(00.0)
+
+    predict = reg.predict([[x, y, yaw]])
+    print(predict)
+    x, y, yaw = motion_model.generate_trajectory(
+        predict[0, 0], predict[0, 1], predict[0, 2], 0.0)
+
+    plt.plot(x, y, "-r", label="trajectory")
+    plt.axis("equal")
+    plt.legend()
+    plt.grid(True)
+    plt.show()
+
+    print("Done")
+
+
+def main():
+    learn_motion_model()
+
+
+if __name__ == '__main__':
+    main()