frist release mix_integer_opt_path_planning and gif movie

.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

PathPlanning/MixIntegerPathPlanning/mix_integer_path_planning.py

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