1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
|
#
# MIT No Attribution
#
# Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl, KU Leuven.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy of this
# software and associated documentation files (the "Software"), to deal in the Software
# without restriction, including without limitation the rights to use, copy, modify,
# merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
# permit persons to whom the Software is furnished to do so.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
# INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
# PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
# HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
# OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
# SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#
# Car race along a track
# ----------------------
# An optimal control problem (OCP),
# solved with direct multiple-shooting.
#
# For more information see: http://labs.casadi.org/OCP
from casadi import *
N = 100 # number of control intervals
opti = Opti() # Optimization problem
# ---- decision variables ---------
X = opti.variable(2,N+1) # state trajectory
pos = X[0,:]
speed = X[1,:]
U = opti.variable(1,N) # control trajectory (throttle)
T = opti.variable() # final time
# ---- objective ---------
opti.minimize(T) # race in minimal time
# ---- dynamic constraints --------
f = lambda x,u: vertcat(x[1],u-x[1]) # dx/dt = f(x,u)
dt = T/N # length of a control interval
for k in range(N): # loop over control intervals
# Runge-Kutta 4 integration
k1 = f(X[:,k], U[:,k])
k2 = f(X[:,k]+dt/2*k1, U[:,k])
k3 = f(X[:,k]+dt/2*k2, U[:,k])
k4 = f(X[:,k]+dt*k3, U[:,k])
x_next = X[:,k] + dt/6*(k1+2*k2+2*k3+k4)
opti.subject_to(X[:,k+1]==x_next) # close the gaps
# ---- path constraints -----------
limit = lambda pos: 1-sin(2*pi*pos)/2
opti.subject_to(speed<=limit(pos)) # track speed limit
opti.subject_to(opti.bounded(0,U,1)) # control is limited
# ---- boundary conditions --------
opti.subject_to(pos[0]==0) # start at position 0 ...
opti.subject_to(speed[0]==0) # ... from stand-still
opti.subject_to(pos[-1]==1) # finish line at position 1
# ---- misc. constraints ----------
opti.subject_to(T>=0) # Time must be positive
# ---- initial values for solver ---
opti.set_initial(speed, 1)
opti.set_initial(T, 1)
# ---- solve NLP ------
opti.solver("ipopt") # set numerical backend
sol = opti.solve() # actual solve
# ---- post-processing ------
from pylab import plot, step, figure, legend, show, spy
plot(sol.value(speed),label="speed")
plot(sol.value(pos),label="pos")
plot(limit(sol.value(pos)),'r--',label="speed limit")
step(range(N),sol.value(U),'k',label="throttle")
legend(loc="upper left")
figure()
spy(sol.value(jacobian(opti.g,opti.x)))
figure()
spy(sol.value(hessian(opti.f+dot(opti.lam_g,opti.g),opti.x)[0]))
show()
|
