mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
first release mix_integer_path_planning
This commit is contained in:
Atsushi Sakai
@@ -4,85 +4,9 @@ Mix Integer Optimization based path planner
|
||||
author: Atsushi Sakai
|
||||
"""
|
||||
|
||||
"""
|
||||
|
||||
function control(is, gs, ob)
|
||||
|
||||
nob = length(ob[:,1])
|
||||
|
||||
model = Model(solver=solver)
|
||||
@variable(model, w[1:2,t=1:T])
|
||||
@variable(model, v[1:2,t=1:T])
|
||||
@variable(model, s[1:2,t=1:T])
|
||||
@variable(model, -u_max <= u[1:2,t=1:T] <= u_max)
|
||||
@variable(model, o[1:4*nob,t=1:T], Bin)
|
||||
|
||||
@constraint(model, s[:,1] .== is)
|
||||
|
||||
obj = []
|
||||
for i in 1:T
|
||||
@constraint(model, s[:,i] - gs .<= w[:,i])
|
||||
@constraint(model, -s[:,i] + gs .<= w[:,i])
|
||||
@constraint(model, u[:,i] .<= v[:,i])
|
||||
@constraint(model, -u[:,i] .<= v[:,i])
|
||||
push!(obj, q'*w[1:end,i]+r'*v[1:2,i])
|
||||
|
||||
|
||||
# obstable avoidanse
|
||||
for io in 1:nob
|
||||
start_ind = 1+(io-1)*4
|
||||
@constraint(model, sum(o[start_ind:start_ind+3, i]) <= 3)
|
||||
@constraint(model, s[1,i] <= ob[io, 1] + M * o[start_ind, i])
|
||||
@constraint(model, -s[1,i] <= -ob[io, 2] + M * o[start_ind+1, i])
|
||||
@constraint(model, s[2,i] <= ob[io, 3] + M * o[start_ind+2, i])
|
||||
@constraint(model, -s[2,i] <= -ob[io, 4] + M * o[start_ind+3, i])
|
||||
end
|
||||
end
|
||||
|
||||
for i in 1:T-1
|
||||
@constraint(model, s[:,i+1] .== A*s[:,i]+B*u[:,i])
|
||||
end
|
||||
|
||||
@objective(model, Min, sum(obj))
|
||||
|
||||
status = solve(model)
|
||||
|
||||
u_vec = getvalue(u)
|
||||
s_vec = getvalue(s)
|
||||
|
||||
return s_vec, u_vec
|
||||
end
|
||||
|
||||
|
||||
function main()
|
||||
|
||||
for i=1:10000
|
||||
|
||||
if sqrt((gs[1]-s[1])^2+(gs[2]-s[2])^2) <= 0.1
|
||||
println("Goal!!!")
|
||||
break
|
||||
end
|
||||
|
||||
s = A*s+B*u_p[:,1] # simulation
|
||||
|
||||
push!(h_sx, s[1])
|
||||
push!(h_sy, s[2])
|
||||
|
||||
end
|
||||
|
||||
plt.cla()
|
||||
plot_obstacle(ob)
|
||||
plt.plot(gs[1],gs[2],"*r")
|
||||
plt.plot(h_sx,h_sy,"-b")
|
||||
plt.axis("equal")
|
||||
plt.grid(true)
|
||||
plt.show()
|
||||
|
||||
println(PROGRAM_FILE," Done!!")
|
||||
end
|
||||
"""
|
||||
|
||||
import cvxpy
|
||||
import math
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
@@ -93,11 +17,11 @@ B = np.matrix([[1.0, 1.0],
|
||||
[0.0, 1.0]])
|
||||
q = np.matrix([[1.0],
|
||||
[1.0]])
|
||||
r = np.matrix([[1.0],
|
||||
[1.0]])
|
||||
r = np.matrix([[0.1],
|
||||
[0.1]])
|
||||
|
||||
u_max = 0.1
|
||||
T = 50
|
||||
T = 30
|
||||
M = 10000.0
|
||||
|
||||
|
||||
@@ -110,52 +34,46 @@ def plot_obstacle(ob):
|
||||
|
||||
def control(s1, gs, ob):
|
||||
|
||||
# w = cvxpy.Variable(2, T)
|
||||
# v = cvxpy.Variable(2, T)
|
||||
w = cvxpy.Variable(2, T)
|
||||
v = cvxpy.Variable(2, T)
|
||||
s = cvxpy.Variable(2, T)
|
||||
u = cvxpy.Variable(2, T)
|
||||
# ob = 2
|
||||
# o = cvxpy.Bool(4 * ob, T)
|
||||
nob = len(ob)
|
||||
o = cvxpy.Bool(4 * nob, T)
|
||||
|
||||
constraints = [-u_max <= u, u <= u_max]
|
||||
constraints = [cvxpy.abs(u) <= u_max]
|
||||
constraints.append(s[:, 0] == s1)
|
||||
|
||||
constraints.append(s[:, 1] == 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 = [s]
|
||||
# for i in range(T)
|
||||
obj.append(t * q.T * w[:, t] + r.T * v[:, t])
|
||||
|
||||
# @constraint(model, s[:, i] - gs . <= w[:, i])
|
||||
# @constraint(model, -s[:, i] + gs . <= w[:, i])
|
||||
# @constraint(model, u[:, i] . <= v[:, i])
|
||||
# @constraint(model, -u[:, i] . <= v[:, i])
|
||||
# push!(obj, q'*w[1:end,i]+r' * v[1:2, i])
|
||||
# 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])
|
||||
|
||||
# # obstable avoidanse
|
||||
# for io in 1:
|
||||
# nob
|
||||
# start_ind = 1 + (io - 1) * 4
|
||||
for t in range(T - 1):
|
||||
constraints.append(s[:, t + 1] == A * s[:, t] + B * u[:, t])
|
||||
|
||||
# @constraint(model, sum(o[start_ind:start_ind + 3, i]) <= 3)
|
||||
# @constraint(model, s[1, i] <= ob[io, 1] + M * o[start_ind, i])
|
||||
# @constraint(model, -s[1, i] <= -ob[io, 2] + M * o[start_ind + 1, i])
|
||||
# @constraint(model, s[2, i] <= ob[io, 3] + M * o[start_ind + 2, i])
|
||||
# @constraint(model, -s[2, i] <= -ob[io, 4] + M * o[start_ind + 3, i])
|
||||
# end
|
||||
# end
|
||||
|
||||
for i in range(T - 1):
|
||||
constraints.append(s[:, 1] == s1)
|
||||
|
||||
# @constraint(model, s[:, i + 1] . == A * s[:, i] + B * u[:, i])
|
||||
|
||||
objective = cvxpy.Minimize(cvxpy.sum_squares(s))
|
||||
objective = cvxpy.Minimize(sum(obj))
|
||||
|
||||
prob = cvxpy.Problem(objective, constraints)
|
||||
|
||||
prob.solve()
|
||||
prob.solve(solver=cvxpy.GUROBI)
|
||||
|
||||
s_p = []
|
||||
u_p = np.matrix([[0.1], [0.1]])
|
||||
s_p = s.value
|
||||
u_p = u.value
|
||||
print("status:" + prob.status)
|
||||
|
||||
return s_p, u_p
|
||||
|
||||
@@ -168,24 +86,28 @@ def main():
|
||||
|
||||
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(i)
|
||||
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[1, :], s_p[2, :], "xb")
|
||||
# plt.plot(s_p[1, :], s_p[2, :], "xb")
|
||||
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")
|
||||
|
||||
Reference in New Issue
Block a user