diff --git a/.gitmodules b/.gitmodules index 3374bce8..5102c642 100644 --- a/.gitmodules +++ b/.gitmodules @@ -16,3 +16,6 @@ [submodule "PathPlanning/AStar/matplotrecorder"] path = PathPlanning/AStar/matplotrecorder url = https://github.com/AtsushiSakai/matplotrecorder +[submodule "PathPlanning/MixIntegerPathPlanning/matplotrecorder"] + path = PathPlanning/MixIntegerPathPlanning/matplotrecorder + url = https://github.com/AtsushiSakai/matplotrecorder diff --git a/PathPlanning/MixIntegerPathPlanning/animation.gif b/PathPlanning/MixIntegerPathPlanning/animation.gif new file mode 100644 index 00000000..8079f3ee Binary files /dev/null and b/PathPlanning/MixIntegerPathPlanning/animation.gif differ diff --git a/PathPlanning/MixIntegerPathPlanning/matplotrecorder b/PathPlanning/MixIntegerPathPlanning/matplotrecorder new file mode 160000 index 00000000..adb95ae9 --- /dev/null +++ b/PathPlanning/MixIntegerPathPlanning/matplotrecorder @@ -0,0 +1 @@ +Subproject commit adb95ae92b0b7384fed7c1b39dd41e3ab86bad78 diff --git a/PathPlanning/MixIntegerPathPlanning/mix_integer_path_planning.py b/PathPlanning/MixIntegerPathPlanning/mix_integer_path_planning.py deleted file mode 100644 index a3718139..00000000 --- a/PathPlanning/MixIntegerPathPlanning/mix_integer_path_planning.py +++ /dev/null @@ -1,119 +0,0 @@ -""" -Mix Integer Optimization based path planner - -author: Atsushi Sakai -""" - - -import cvxpy -import math -import numpy as np -import matplotlib.pyplot as plt - -# parameter -A = np.matrix([[1.0, 0.0], - [0.0, 1.0]]) -B = np.matrix([[1.0, 1.0], - [0.0, 1.0]]) -q = np.matrix([[1.0], - [1.0]]) -r = np.matrix([[0.1], - [0.1]]) - -u_max = 0.1 -T = 30 -M = 10000.0 - - -def plot_obstacle(ob): - for i in range(len(ob)): - x = [ob[i, 0], ob[i, 1], ob[i, 1], ob[i, 0], ob[i, 0]] - y = [ob[i, 2], ob[i, 2], ob[i, 3], ob[i, 3], ob[i, 2]] - plt.plot(x, y, "-g") - - -def control(s1, gs, ob): - - w = cvxpy.Variable(2, T) - v = cvxpy.Variable(2, T) - s = cvxpy.Variable(2, T) - u = cvxpy.Variable(2, T) - nob = len(ob) - o = cvxpy.Bool(4 * nob, T) - - constraints = [cvxpy.abs(u) <= u_max] - constraints.append(s[:, 0] == s1) - - obj = [] - for t in range(T): - constraints.append(s[:, t] - gs <= w[:, t]) - constraints.append(-s[:, t] + gs <= w[:, t]) - constraints.append(u[:, t] <= v[:, t]) - constraints.append(-u[:, t] <= v[:, t]) - - obj.append(t * q.T * w[:, t] + r.T * v[:, t]) - - # obstable avoidanse - for io in range(nob): - ind = io * 4 - constraints.append(sum(o[ind:ind + 4, t]) <= 3) - constraints.append(s[0, t] <= ob[io, 0] + M * o[ind + 0, t]) - constraints.append(-s[0, t] <= -ob[io, 1] + M * o[ind + 1, t]) - constraints.append(s[1, t] <= ob[io, 2] + M * o[ind + 2, t]) - constraints.append(-s[1, t] <= -ob[io, 3] + M * o[ind + 3, t]) - - for t in range(T - 1): - constraints.append(s[:, t + 1] == A * s[:, t] + B * u[:, t]) - - objective = cvxpy.Minimize(sum(obj)) - - prob = cvxpy.Problem(objective, constraints) - - prob.solve(solver=cvxpy.GUROBI) - - s_p = s.value - u_p = u.value - print("status:" + prob.status) - - return s_p, u_p - - -def main(): - print(__file__ + " start!!") - - s = np.matrix([10.0, 5.0]).T # init state - gs = np.matrix([5.0, 7.0]).T # goal state - - ob = np.matrix([[7.0, 8.0, 3.0, 8.0], - [5.5, 6.0, 6.0, 10.0]]) # [xmin xmax ymin ymax] - # ob = np.matrix([[7.0, 8.0, 3.0, 8.0]]) - - h_sx = [] - h_sy = [] - - for i in range(10000): - print("time:", i) - s_p, u_p = control(s, gs, ob) - - s = A * s + B * u_p[:, 0] # simulation - - if(math.sqrt((gs[0] - s[0]) ** 2 + (gs[1] - s[1]) ** 2) <= 0.1): - print("Goal!!!") - break - - h_sx.append(s[0, 0]) - h_sy.append(s[1, 0]) - - plt.cla() - plt.plot(gs[0], gs[1], "*r") - plot_obstacle(ob) - plt.plot(s_p[0, :], s_p[1, :], "xb") - plt.plot(h_sx, h_sy, "-b") - plt.plot(s[0], s[1], "or") - plt.axis("equal") - plt.grid(True) - plt.pause(0.0001) - - -if __name__ == '__main__': - main()