-
Notifications
You must be signed in to change notification settings - Fork 0
/
race-car-direct-multiple-shooting.py
163 lines (147 loc) · 5.19 KB
/
race-car-direct-multiple-shooting.py
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
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
# 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: https://github.com/casadi/casadi/blob/master/docs/examples/python/race_car.py
#
# -------------------------------------------------------------------------
#
# .///////////////////*,
# ///////%////////%////////////,
# //////////////////////////(///(#///,
# ,/////////, ,///////&/#//////*
# ,////////, ,%//////(/////,
# ,////////. ,///////////*
# ////////, /////%////,
# ////(///* /////////,
# ,//////// ,////////.
# ////////, ////(///,
# ////////, ,*/////////,, *////////
# //////////,/////////////////////*, ,//////&////,
# ,/////(////,//////////////////(///////////////////(//////
# ,////////////////////,..,*///////////////(%%#//////////,
# ,///////////(////// ,////////////////*,
# //////////////,
# ,,,,,
#
#
# -------------------------------------------------------------------------
#
# We must control u(t) to ensure the race car stays on the track.
#
# System Dynamics:
# d[p(t)v(t)]/dt=[v(t), u(t)−v(t)]
#
# p(t) - position (m)
# v(t) - velocity (m/s)
# T - time to finish track
#
# Encode the the task definition in a continuous-time optimal control problem (OCP)
#
# minimize T
# x,u
# subject to
# dynamic constraints
# xdot = f(x(t),u(t)), t \in [0,T],
# boundary condition: start at position 0
# p(t0) = 0,
# boundary condition: start with zero speed
# v(t0) = 0,
# boundary condition: the finish line is at position 1
# p(T) = 1,
# path constraint: throttle is limited
# 0 <= u(t) < 1,
# path constraint: speed limit varying along the track
# v(t) <= L(p(t))
#
# The following is a multiple-shooting transcription of the original OCP
#
# minimize T
# x_k, u_j,
# k \in [1,N+1],
# j \in [1, N]
#
# subject to
# (1) dynamic constraints a.k.a gap closing
# x_{k+1} = F(x_k,u_k), k \in [1,N],
# (2) boundary condition: start at position 0
# p(t0) = 0,
# (3) boundary condition: start with zero speed
# v(t0) = 0,
# (4) boundary condition: the finish line is at position 1
# p(T) = 1,
# (5) path constraint: throttle is limited
# 0 <= u(t) < 1,
# (6) path constraint: speed limit varying along the track
# v(t) <= L(p(t))
#
from casadi import (
Opti,
vertcat,
sin,
jacobian,
hessian,
dot,
pi,
)
from matplotlib.pyplot import (
plot,
step,
figure,
legend,
show,
spy
)
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
Xk, Uk = X[:,k], U[:,k]
k1 = f(Xk, Uk)
k2 = f(Xk+dt/2*k1, Uk)
k3 = f(Xk+dt/2*k2, Uk)
k4 = f(Xk+dt*k3, Uk)
x_next = Xk + 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) # start 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 ------
plot(sol.value(speed),label="speed")
plot(sol.value(pos),label="pos")
plot(limit(sol.value(pos)).full(),'r--',label="speed limit")
step(range(N),sol.value(U),'k',label="throttle")
legend(loc="upper left")
# Uncomment to see jacobian & hessian sparsity
# 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()