Figure 8.9:

Direct single shooting solution for \eqref {eq:vdp} without path constraints.

Code for Figure 8.9

Text of the GNU GPL.

main.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
# [makes] single_shooting.mat
# Solves the following minimization problem using direct single shooting
#  minimize       1/2*integral{t=0 until t=T}(x1^2 + x2^2 + u^2) dt
#  subject to     dot(x1) = (1-x2^2)*x1 - x2 + u,   x1(0)=0, x1(T)=0
#                 dot(x2) = x1,                     x2(0)=1, x2(T)=0
#  with T=10
#
# This example can be found in CasADi's example collection in MATLAB/Octave,
# Python and C++ formats.
#
# Joel Andersson, UW Madison 2017
import numpy as np
import casadi
from scipy.io import savemat

# States and control
x1 = casadi.SX.sym('x1')
x2 = casadi.SX.sym('x2')
u = casadi.SX.sym('u')

# Model equations
x1_dot = (1 - x2**2) * x1 - x2 + u
x2_dot = x1

# Objective function (integral term)
quad = x1**2 + x2**2 + u**2

# Problem structure
ocp = dict(x=casadi.vertcat(x1, x2), u=u,
           ode=casadi.vertcat(x1_dot, x2_dot), quad=quad)

# Problem data
data = dict(T=10.0, x0=np.array([0.0, 1.0]), xN=np.array([0.0, 0.0]),
            u_min=-1.0, u_max=1.0, u_guess=0.0)

# Solver options
opts = dict(N=50, verbose=True)

# Problem independent code from here on

# Problem dimensions
N = opts['N']
nx = ocp['x'].numel()
nu = ocp['u'].numel()

# Time grid
tgrid = np.linspace(0, data['T'], N + 1)

# Continuous-time dynamics as a function
f = casadi.Function('f', [ocp['x'], ocp['u']], [ocp['ode'], ocp['quad']],
                    ['x', 'p'], ['ode', 'quad'])

# RK4 integrator that takes a single step
dt = data['T'] / N
x0sym = casadi.MX.sym('x0', nx)
psym = casadi.MX.sym('p', nu)
k1, k1q = f(x0sym, psym)
k2, k2q = f(x0sym + dt / 2 * k1, psym)
k3, k3q = f(x0sym + dt / 2 * k2, psym)
k4, k4q = f(x0sym + dt * k3, psym)
xf = x0sym + dt * (k1 + 2 * k2 + 2 * k3 + k4) / 6
qf = dt * (k1q + 2 * k2q + 2 * k3q + k4q) / 6
F = casadi.Function('RK4', [x0sym, psym], [xf, qf], ['x0', 'p'], ['xf', 'qf'])

# Start with an empty NLP
w = []
lbw = []
ubw = []
w0 = []
g = []
J = 0

# Expressions corresponding to the trajectories we want to plot
x_plot = []
u_plot = []

# Initial conditions
xk = casadi.DM(data['x0'])
x_plot.append(xk)

# Loop over all times
for k in range(N):
    # Declare local control
    uk = casadi.MX.sym('u{}'.format(k), nu)
    w.append(uk)
    lbw.append(data['u_min'])
    ubw.append(data['u_max'])
    w0.append(data['u_guess'])
    u_plot.append(uk)

    # Simulate the system forward in time
    Fk = F(x0=xk, p=uk)

    # Add contribution to objective function
    J = J + Fk['qf']

    # Update x
    xk = Fk['xf']
    x_plot.append(xk)

# Enforce terminal conditions
g.append(xk - data['xN'])

# Concatenate variables and constraints
w = casadi.vertcat(*w)
lbw = casadi.vertcat(*lbw)
ubw = casadi.vertcat(*ubw)
w0 = casadi.vertcat(*w0)
g = casadi.vertcat(*g)

# Formulate the NLP solver object
nlp = dict(x=w, g=g, f=J)
solver = casadi.nlpsol('solver', 'ipopt', nlp)

# Create a function that maps the NLP decision variable to the x and u trajectories
traj = casadi.Function('traj', [w],
                       [casadi.horzcat(*x_plot), casadi.horzcat(*u_plot)],
                       ['w'], ['x', 'u'])

# Solve the NLP and extract solution
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=0, ubg=0)
x_opt, u_opt = traj(sol['x'])
x_opt = np.array(x_opt)
u_opt = np.array(u_opt)

savemat('single_shooting.mat', {'tgrid': tgrid, 'x_opt': x_opt, 'u_opt': u_opt})