diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py index e8cf338e..cfbb750a 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py @@ -1,7 +1,7 @@ """ Model trajectory generator -author: Atsushi Sakai +author: Atsushi Sakai(@Atsushi_twi) """ import numpy as np @@ -14,7 +14,7 @@ max_iter = 100 h = np.matrix([0.5, 0.02, 0.02]).T # parameter sampling distanse cost_th = 0.1 -show_graph = False +show_animation = False def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): @@ -122,7 +122,7 @@ def optimize_trajectory(target, k0, p): p += alpha * np.array(dp) # print(p.T) - if show_graph: + if show_animation: show_trajectory(target, xc, yc) else: xc, yc, yawc, p = None, None, None, None diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py index b31d7319..6319c5a0 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py @@ -45,7 +45,8 @@ def generate_trajectory(s, km, kf, k0): 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) + fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") + kp = [fkp(ti) for ti in t] dt = float(time / n) # plt.plot(t, kp) @@ -70,7 +71,8 @@ def generate_last_state(s, km, kf, k0): 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) + fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") + kp = [fkp(ti) for ti in t] dt = time / n # plt.plot(t, kp)