Exercise 5: Direct methods and control by optimization

Exercise 5: Direct methods and control by optimization#

Note

  • The exercises material is divided into general information (found on this page) and the actual exercise instructions. You can download this weeks exercise instructions from here:

  • You are encouraged to prepare the homework problems 1 (indicated by a hand in the PDF file) at home and present your solution during the exercise session.

  • To get the newest version of the course material, please see Making sure your files are up to date

This weeks exercises will be slightly different since we will focus on implementing a single method, namely the direct control method we saw during the lecture.

Direct optimization is fundamentally just about translating the control problem into an optimization problem of the form:

\[\begin{split}\mathbf{z}^* & = \min\left[ J(\mathbf{z}) \right] \\ \mathbf z & = (\mathbf x_0, \mathbf u_0, \mathbf x_1, \mathbf u_1, \dots, \mathbf x_{N-1}, \mathbf u_{N-1}, t_0, t_F) \\ \mathbf z_\text{ub} \leq \mathbf z & \leq \mathbf z_\text{ub} \\ \mathbf x_k - \mathbf x_{k+1} + \frac{h_k}{2}(\mathbf f_{k+1} + \mathbf f_{k} ) & = 0\end{split}\]

That is, a minimization problem subject to both linear and non-linear constraints.

Note

You can read more about the Scipy optimizer we use here.

This optimization problem is then fed into an optimizer, which gives us an answer in the form of a vector \(\mathbf{z}^*\).

In our case, we will use the build-in non-linear optimizer scipy.optimize.minimize(). This optimizer is very powerful, but it means that a good place to begin with the optimization is to understand what sort if inputs it expects, and what outputs it gives in return. Consider the following minimization problem:

\[\begin{split}J(\mathbf z) & = z_1^2 + 3z_2^2 \\ \mathbf{z}^* & = \min\left[ J(\mathbf{z}) \right] \\ \begin{bmatrix} 0 \\ -0.5 \end{bmatrix} \leq \mathbf z & \leq \begin{bmatrix} 1 \\ 2 \end{bmatrix} \\ 1 - z_1 - 2 z_2 & \leq 0 \\ 1 - z_1^2 - z_2 & \leq 0 \\ 2 z_1 + z_2 - 1 & = 0\end{split}\]

This problem can be solved by specifying the inequality and equality constraints as dictionaries and calling minimize() as follows:

>>> import numpy as np
>>> from scipy.optimize import Bounds, minimize
>>> def J_fun(z):
...     return z[0]**2 + 3 * z[1] ** 2
... 
>>> def J_jac(z):
...     return np.array([2 * z[0], 6 * z[1]])
... 
>>> ineq_cons = {'type': 'ineq',
...             'fun': lambda x: np.array([1 - x[0] - 2 * x[1],
...                                        1 - x[0] ** 2 - x[1]]),
...             'jac': lambda x: np.array([[-1.0, -2.0],
...                                        [-2 * x[0], -1.0]])}
>>> 
>>> eq_cons = {'type': 'eq',
...             'fun': lambda x: np.array([2 * x[0] + x[1] - 1]),
...             'jac': lambda x: np.array([2.0, 1.0])}
>>> 
>>> z_lb, z_ub = [0, -0.5], [1.0, 2.0] # upper and lower bounds.
>>> z0 = np.array([0.5, 0]) # Initial guess
>>> bounds = Bounds(z_lb, z_ub)
>>> res = minimize(J_fun, z0, method='SLSQP', jac=J_jac, constraints=[eq_cons, ineq_cons], bounds=bounds)
>>> res
 message: Optimization terminated successfully
 success: True
  status: 0
     fun: 0.2307692307692308
       x: [ 4.615e-01  7.692e-02]
     nit: 2
     jac: [ 9.231e-01  4.615e-01]
    nfev: 3
    njev: 2

As shown by the output, the optimal value is res.x. The minimize function can accept any number of non-linear equality and in-equality constraints as a list (in this case it is given two), and the simple constraints as a scipy.optimize.Bounds-object.

Note that the functions specifying the equality and inequality constraints are given as function, for instance lambda x: np.array([2 * x[0] + x[1] - 1]), and then we also need to specify the Jacobian of the equality constraint, i.e.

\[\begin{split}f(\mathbf{x} ) & = 2z_1 + z_2 - 1 \\ J_x f(\mathbf{x}) & = \begin{bmatrix} 2 \\ 1 \end{bmatrix}\end{split}\]

Similarly we need to specify the Jacobian of the objective function $nabla E$. To make this feasible, our strategy will be to specify all equality and in-equalty constraints as symbolic objects using sympy and then compute the Jacobians explicitly. That means that in your implementation:

  • \(x_k, u_k, t_0, \dots\) and all other variables are symbolic (sympy) variables

  • \(\mathbf z\) is therefore a list of symbolic expressions corresponding

  • \(\mathbf z_\text{lb}, \mathbf z_\text{ub}\) are lists of numbers

  • \(\mathbf z_0\) (the initial guess) is a list of numbers

  • \(J(\mathbf{z})\), i.e. the objective function we minimize, is a symbolic expression depending on \(\mathbf{z}\)

  • The collocation constraints are defined as lists of symbolic expressions Ieq = [..., c_eq, ...] with the convention that \(c_\text{eq} = 0\).

  • Any non-linear inequality constraints are also defined as lists of symbolic expressions Iineq = [..., c_ineq, ...] with the convention that \(c_\text{ineq} = 0\).

Your job is in other words to define the above variables (transcribe the problem) and then feeding it into the optimizer will happen automatically in the code.

Classes and functions#

irlc.ex05.direct.collocate(model, N=25, optimizer_options=None, guess=None, verbose=True)[source]#

Performs collocation by discretizing the model using a grid-size of N and optimize to find the optimal solution. The ‘model’ should be a ControlModel instance, optimizer_options contains options for the optimizer, and guess is a dictionary used to initialize the optimizer containing keys:

guess = {'t0': Start time (float),
         'tF': Terminal time (float),
         'x': A *function* which takes time as input and return a guess for x(t),
         'u': A *function* which takes time as input and return a guess for u(t),
        }

So for instance

guess['x'](0.5)

will return the state \(\mathbf x(0.5)\) as a numpy ndarray.

The overall structure of the optimization procedure is as follows:

  1. Define the following variables. They will all be lists:

    • z: Variables to be optimized over. Each element z[k] is a symbolic variable. This will allow us to compute derivatives.

    • z0: A list of numbers representing the initial guess. Computed using ‘guess’ (above)

    • z_lb, z_ub: Lists of numbers representting the upper/lower bounds on z. Use bound-methods in irlc.ex03.control_model.ControlModel to get these.

  2. Create a symbolic expression representing the cost-function J This is defined using the symbolic variables similar to the toy-problem we saw last week. This allows us to compute derivatives of the cost

  3. Create symbolic expressions representing all constraints The lists Iineq and Ieq contains lists of constraints. The solver will ensure that for any i:

    Ieq[i] == 0

    and:

    Iineq[i] <= 0

    This allows us to just specify each element in ‘eqC’ and ‘ineqC’ as a single symbolic expression. Once more, we use symbolic expressions so derivatives can be computed automatically. The most important constraints are in ‘eqC’, as these must include the collocation-constraints (see algorithm in notes)

  4. Compile all symbolic expressions into a format useful for the optimizer The optimizer accepts numpy functions, so we turn all symbolic expressions and derivatives into numpy (similar to the example last week). It is then fed into the optimizer and, fingers crossed, the optimizer spits out a value ‘z*’, which represents the optimal values.

  5. Unpack z: The value ‘z*’ then has to be unpacked and turned into function u*(t) and x*(t) (as in the notes). These functions can then be put into the solution-dictionary and used to initialize the next guess (or assuming we terminate, these are simply our solution).

Parameters:

  • model (ControlModel) – A irlc.ex03.control_model.ControlModel instance

  • N – The number of collocation knot points \(N\)

  • optimizer_options – Options for the scipy optimizer. You can ignore this.

  • guess (dict) – A dictionary containing the initial guess. See the online documentation.

  • verbose – Whether to print out extra details during the run. Useful only for debugging.

Returns:

A dictionary containing the solution. It is compatible with the guess datastructure .

irlc.ex05.direct.trapezoid_interpolant(ts, xs, fs, t_new=None)[source]#

This function implements (Her24, eq. (15.26)) to evaluate \(\mathbf{x}(t)\) at a point \(t =\) t_new.

The other inputs represent the output of the direct optimization procedure. I.e., ts is a list of length \(N+1\) corresponding to \(t_k\), xs is a list of \(\mathbf x_k\), and fs is a list corresponding to \(\mathbf f_k\). To implement the method, you should first determine which \(k\) the new time point t_new corresponds to, i.e. where \(t_k \leq t_\text{new} < t_{k+1}\).

Parameters:

  • ts (list) – List of time points [.., t_k, ..]

  • xs (list) – List of numpy ndarrays [.., x_k, ...]

  • fs (list) – List of numpy ndarrays [.., f_k, ...]

  • t_new – The time point we should evaluate the function in.

Returns:

The state evaluated at time t_new, i.e. \(\mathbf x(t_\text{new})\).

Solutions to selected exercises#

Solution to problem 1

Part a+b: The dimensions are obviously the same. When \(N=3\) the states are \(x_0, x_1, x_2\) and the actions are \(u_0, u_1, u_2\). Since these are all 1-dimensional there are 8 dimensions because:

\[\mathbf z = \begin{bmatrix} x_0 & u_0 & x_1 & u_1 & x_2 & u_2 & t_0 & t_F \end{bmatrix}\]

The constraints are therefore:

\[\begin{split}\mathbf z_{\text{lb} } & = \begin{bmatrix} 0 \\ -\infty \\ -\infty \\ -\infty \\ \frac{\pi}{2} \\ -\infty \\ 0 \\ 0 \end{bmatrix}, \quad \mathbf z_{\text{ub} } = \begin{bmatrix} 0 \\ \infty \\ \infty \\ \infty \\ \frac{\pi}{2} \\ \infty \\ 0 \\ 10 \end{bmatrix}\end{split}\]

Part c: The two collocation constraints are, for \(k=0, 1\) and using that \(f_k = u_k + \cos(x_k)\),

\[\mathbf x_{k+1} - \mathbf x_k = \frac{t_F - t_0}{4} \left(u_{k+1} + \cos(x_{k+1} ) + u_{k} + \cos(x_{k} ) \right)\]

Applying the constraints gives the desired result.

Part d: According to trapezoid collocation, the cost function will take the general form:

\[E(u_0, u_1, u_2) = \frac{c_0 + c_1}{2} + \frac{c_1 + c_2}{2} = \frac{c_0 + c_2}{2} + c_1.\]

where:

\[c_k = \frac{t_F - t_0}{2}\left( (2x_k - \frac{\pi}{2})^2 + \frac{\pi^2}{4} (2u_k + 2\cos(x_k) + 1 )^2 \right)\]

If we insert the constraints, \(x_0=u_0=u_2=0\) and \(x_2=\frac{\pi}{2}\) and simplify we get the expression in the problem.

Part e: The first step is to simplify the two constraints by (i) forming an equation where they are added together (ii) forming an equation by subtracting them from each other. Doing this gives two new equations:

\[\begin{split}\frac{\pi}{2} & = \frac{t_F}{4}(2 u_1 + 2\cos x_1 + 1) \\ 2x_1 - \frac{\pi}{2} & = \frac{t_F}{4}\end{split}\]

We observe that the right-hand side of the first equation corresponds to the second term in the cost-function and the left-hand side of the second corresponds to the first term of the cost-function. Thus, we can use these two equations to eliminate all terms in the cost-function except \(t_F\). This means that

\[\begin{split}E & = \frac{c_0 + c_2}{2} + c_1 \\ \text{where} \quad \frac{c_0 + c_2}{2} & = \frac{3\pi^2}{4} t_F \\ \text{and} \quad c_1 & = \frac{t_F}{2} \left[ \left( \frac{t_F}{4} \right)^2 + \frac{\pi^2}{4} \left( \frac{4 \frac{\pi}{2} }{ t_F} \right)^2 \right]\end{split}\]

Applying a bit of algebra and collecting terms gives us:

\[E = \frac{t_F^3}{32} + t_F \frac{3\pi^2}{4} + \frac{\pi^4}{2}\frac{1}{t_F}\]

To get the value of \(t_F\) that minimize this expression we just set the derivative equal to zero: \(E'(t_F)=0\). If we do that we get:

\[\frac{3}{32}t_F^2 + \frac{3\pi^2}{4} - \frac{\pi^4}{2} \frac{1}{t_F^2} = 0\]

Whenever you encounter something like this it is a good idea to see if it can be re-written as a polynomial. So we multiply by \(t_F^2\) and get:

\[\frac{3}{32}t_F^4 + \frac{3\pi^2}{4} t_f^2 - \frac{\pi^4}{2} = 0\]

Polynomials with only even coefficients have the nice property they can be simplified by introducing \(y=t_F^2\). This gives us a second-order polynomial in \(y\) which we can easily solve using high-school math. After carefully simplifying the expression we get:

\[y = 4\pi^2 \left( \frac{2\sqrt{3} }{3} - 1 \right)\]

Thus, the final answer is:

\[t_F = 2\pi \sqrt{ \frac{2\sqrt{3} }{3} - 1 }.\]

By using the constraint equations we could potentially recover \(u_1\) and \(x_1\), but that is left as an exercise.

Problem 5.1

Warning

I make a small mistake in the first video (a numpy ndarray is not flattened correctly when I define z0). The problem is corrected in the video for problem 5.4 below.

Problem 5.2

Problem 5.3

Problem 5.4

Problem 5.5

Contents