Figure 2.3

Figure 2.3:

First element of control constraint set \mathcal {U}_3(x) (shaded) and control law \kappa _3(x) (line) versus x=(\cos (\theta ),\sin (\theta )), \theta \in [-\pi ,\pi ] on the unit circle for a nonlinear system with terminal constraint.

Code for Figure 2.3

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
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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209 # [makes] circle.dat

import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import minimize
from scipy import signal
import os
from misc import gnuplotsave

# Finding the feasible region and optimal solution of the (u, u^3) example.
thmin = -1.1
thmax = 1.1

# Need to sample with higher density for this boundary.
theta = np.linspace(thmin, thmax, 1001)

# Points on unit circle.
x = np.cos(np.pi * theta)
y = np.sin(np.pi * theta)

# Weird curvy branch for numerator = 0.
numcurves = np.full((3, len(theta)), np.nan)
for i in range(len(theta)):
    r = np.roots([3, -3*x[i], -3*x[i]**2, -x[i]**3 + 4*y[i]])
    r[np.abs(np.imag(r)) > 1e-6] = np.nan
    numcurves[:,i] = np.real(r)

# Nice branch where denominator = 0.
dencurve = -x

# Need to do some magic to plot the curve properly because it's a multivalued
# function. Loop through the points, finding the next closest one at each step.
# This works because the curve is not self-intersecting and we know one of the
# endpoints. We bring the nice denominator curve along for the ride so that
# the region is easier to plot later on.
indices = np.where(~np.isnan(numcurves))
inum, jnum = indices[0], indices[1]
z = np.vstack([
    numcurves[inum, jnum],
    dencurve[jnum],
    theta[jnum]
])

zscale = np.max(z, axis=1) - np.min(z, axis=1)
z = z / zscale[:, np.newaxis]

for i in range(1, z.shape[1]):
    dist = np.sum((z[:,i:] - z[:,i-1][:,np.newaxis])**2, axis=0)
    inext = np.argmin(dist) + i
    if inext != i:
        z[:,[i, inext]] = z[:,[inext, i]]

# Rescale
z = z * zscale[:, np.newaxis]
ufeas1 = np.maximum(z[0,:], z[1,:])  # Top piece
ufeas2 = np.minimum(z[0,:], z[1,:])  # Bottom piece
thfeas = z[2,:]

# Now find the minimizer. We use a coarser gridding here.
theta = np.linspace(thmin, thmax, 241)

def cost(u, th, firstsign):
    """
    Computes optimal cost with inf if infeasible.
    """
    x = np.cos(np.pi * th)
    y = np.sin(np.pi * th)

    # Calculate b.
    bnum = 3*u**3 - 3*x*u**2 - 3*x**2*u - x**3 + 4*y
    bden = 12*(u + x)
    b = bnum/bden

    # Simulate the system.
    useq = [u, -x/2 - u/2 + firstsign*np.sqrt(b), -x/2 - u/2 - firstsign*np.sqrt(b)]
    V = 0
    for i in range(4):
        V = V + x**2 + y**2
        if i == 3:
            break
        V = V + useq[i]**2
        x = x + useq[i]
        y = y + useq[i]**3

    V = np.array(np.real(V))
    V[b < 0] = np.inf  # These points are infeasible.
    return V

# Vectorized version for meshgrid
def cost_vectorized(u_array, th_array, firstsign_array):
    """
    Vectorized version of cost function for meshgrid inputs
    """
    result = np.zeros_like(u_array)
    for i in range(u_array.shape[0]):
        for j in range(u_array.shape[1]):
            result[i,j] = cost(u_array[i,j], th_array[i,j], firstsign_array[i,j])
    return result

urange = np.linspace(-2, 2, 1001)
th, u = np.meshgrid(theta, urange)

# Calculate costs with both positive and negative firstsign
Vplus = np.zeros_like(u)
Vminus = np.zeros_like(u)

for i in range(u.shape[0]):
    for j in range(u.shape[1]):
        Vplus[i,j] = cost(u[i,j], th[i,j], 1)
        Vminus[i,j] = cost(u[i,j], th[i,j], -1)

# Find minimum between the two options
V = np.minimum(Vplus, Vminus)
firstsign = 1 - 2 * (Vplus > Vminus)  # -1 if Vminus < Vplus, otherwise +1

# Find local minima.
Vmid = V[1:-1,:]  # Slice out edges.
localmin = (Vmid < V[:-2,:]) & (Vmid < V[2:,:])
# Add edges back
localmin = np.vstack([np.zeros((1, localmin.shape[1]), dtype=bool),
                     localmin,
                     np.zeros((1, localmin.shape[1]), dtype=bool)])

ilocal = np.where(localmin)
Vlocal = V[ilocal]
thlocal = th[ilocal]
ulocal = u[ilocal]

# Find the minima in the grid of values
Vmin = np.min(V, axis=0)
imin = np.argmin(V, axis=0)
umin = urange[imin]

# Refine the minimum using scipy's minimize
for i in range(len(umin)):
    result = minimize(lambda u_val: cost(u_val, theta[i], firstsign[imin[i], i]),
                      umin[i], method='nelder-mead')
    umin[i] = result.x[0]
    Vmin[i] = result['fun']

def getdiscont(x, y, N):
    """
    Finds discontinuities in y and adds in NaNs to make a gap.
    """
    dy = np.abs(np.diff(y))
    idiscont = np.where(dy > np.mean(dy) + N*np.std(dy))[0]
    expand = lambda z: np.vstack(([z], np.append(z[:-1] + 0.5*np.diff(z), np.nan)))

    x = expand(x)
    xdc1 = x[0, idiscont]
    xdc2 = x[0, idiscont + 1]
    x[1, idiscont] = np.nan

    y = expand(y)
    ydc1 = y[0, idiscont]
    ydc2 = y[0, idiscont + 1]
    y[1, idiscont] = np.nan

    # Assemble discontinuities for plotting.
    xdc = np.vstack(([xdc1], [xdc2], [np.nan * np.ones_like(xdc2)]))
    ydc = np.vstack(([ydc1], [ydc2], [np.nan * np.ones_like(ydc2)]))

    # Package everybody up.  Recall lame numpy needs Fortran column ordering
    pieces = np.column_stack([x.flatten(order='F'), y.flatten(order='F')])
    jumps = np.column_stack([xdc.flatten(order='F'), ydc.flatten(order='F')])

    return pieces, jumps

# Get discontinuities
upieces, ujumps = getdiscont(theta, umin, 2)
Vpieces, Vjumps = getdiscont(theta, Vmin, 2)

if not os.getenv('OMIT_PLOTS') == 'true':
    # Make a plot of u.
    plt.figure()

    # Create the fill for infeasible region
    ufill = np.concatenate([ufeas1, ufeas2[::-1]])
    thfill = np.concatenate([thfeas, thfeas[::-1]])
    plt.fill(thfill, ufill, 'r', edgecolor='r')

    # Plot the rest
    plt.plot(thlocal, ulocal, 'bs')  # Local optima
    plt.plot(upieces[:,0], upieces[:,1], 'g-')  # Global optima
    plt.plot(ujumps[:,0], ujumps[:,1], 'g:')  # Jumps in global optima

    plt.legend(['Infeasible', 'Local Optima', 'Global Optima'])
    plt.xlabel(r'$\theta/\pi$')
    plt.ylabel('Input')

    # Make a plot of V.
    plt.figure()

    plt.semilogy(thlocal, Vlocal, 'sr')  # Local optima
    plt.semilogy(Vpieces[:,0], Vpieces[:,1], 'g-')  # Global optima
    plt.semilogy(Vjumps[:,0], Vjumps[:,1], 'g:')  # Jumps in global optima

    plt.xlabel(r'$\theta/\pi$')
    plt.ylabel('Cost')

    plt.show(block=False)

# Now save data.
feas = np.column_stack([thfeas, ufeas1, ufeas2])

data = {'upieces': upieces, 'ujumps': ujumps, 'Vpieces': Vpieces,
        'Vjumps': Vjumps, 'feas': feas}

gnuplotsave('circle.dat', data)