Skip to content
Snippets Groups Projects
Commit 3e83635a authored by Olivier Cots's avatar Olivier Cots
Browse files

add -1/2

parent 26b11cd9
No related branches found
No related tags found
No related merge requests found
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
# Exercice 3: Introduction to indirect multiple shooting: the Bang-Singular-Bang case on a turnpike problem # Exercice 3: Introduction to indirect multiple shooting: the Bang-Singular-Bang case on a turnpike problem
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
* Author: Olivier Cots * Author: Olivier Cots
* Date: March 2021 * Date: March 2021
------ ------
* Back: [Exercice 2](../exercices/ex2_implement_simple_shooting.ipynb) - * Back: [Exercice 2](../exercices/ex2_implement_simple_shooting.ipynb) -
[Correction](../corrections/ex2_cor_implement_simple_shooting.ipynb) [Correction](../corrections/ex2_cor_implement_simple_shooting.ipynb)
------ ------
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## I) Description of the optimal control problem ## I) Description of the optimal control problem
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We consider the following optimal control problem: We consider the following optimal control problem:
$$ $$
\left\{ \left\{
\begin{array}{l} \begin{array}{l}
\displaystyle J(u) := \displaystyle \int_0^{t_f} x^2(t) \, \mathrm{d}t \longrightarrow \min \\[1.0em] \displaystyle J(u) := \displaystyle \int_0^{t_f} x^2(t) \, \mathrm{d}t \longrightarrow \min \\[1.0em]
\dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em] \dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em]
x(0) = 1, \quad x(t_f) = 1/2. x(0) = 1, \quad x(t_f) = 1/2.
\end{array} \end{array}
\right. \right.
$$ $$
To this optimal control problem is associated the stationnary optimization problem To this optimal control problem is associated the stationnary optimization problem
$$ $$
\min_{(x, u)} \{~ x^2 ~ | ~ (x, u) \in \mathrm{R} \times [-1, 1],~ f(x,u) = u = 0\}. \min_{(x, u)} \{~ x^2 ~ | ~ (x, u) \in \mathrm{R} \times [-1, 1],~ f(x,u) = u = 0\}.
$$ $$
The static solution is thus $(x^*, u^*) = (0, 0)$. This solution may be seen as the static pair $(x, u)$ which minimizes the cost $J(u)$ under The static solution is thus $(x^*, u^*) = (0, 0)$. This solution may be seen as the static pair $(x, u)$ which minimizes the cost $J(u)$ under
the constraint $u \in [-1, 1]$. the constraint $u \in [-1, 1]$.
It is well known that this problem is what we call a *turnpike* optimal control problem. It is well known that this problem is what we call a *turnpike* optimal control problem.
Hence, if the final time $t_f$ is long enough the solution is of the following form: Hence, if the final time $t_f$ is long enough the solution is of the following form:
starting from $x(0)=1$, reach as fast as possible the static solution, stay at the static solution as long as possible before reaching starting from $x(0)=1$, reach as fast as possible the static solution, stay at the static solution as long as possible before reaching
the target $x(t_f)=1/2$. In this case, the optimal control would be the target $x(t_f)=1/2$. In this case, the optimal control would be
$$ $$
u(t) = \left\{ u(t) = \left\{
\begin{array}{lll} \begin{array}{lll}
-1 & \text{if} & t \in [0, t_1], \\[0.5em] -1 & \text{if} & t \in [0, t_1], \\[0.5em]
\phantom{-}0 & \text{if} & t \in (t_1, t_2], \\[0.5em] \phantom{-}0 & \text{if} & t \in (t_1, t_2], \\[0.5em]
+1 & \text{if} & t \in (t_2, t_f], +1 & \text{if} & t \in (t_2, t_f],
\end{array} \end{array}
\right. \right.
$$ $$
with $0 < t_1 < t_2 < t_f$. We say that the control is *Bang-Singular-Bang*. A Bang arc corresponds to $u \in \{-1, 1\}$ while a singular control corresponds to $u \in (-1, 1)$. Since the optimal control law is discontinuous, then to solve this optimal control problem by indirect methods and find the *switching times* $t_1$ and $t_2$, we need to implement what we call a *multiple shooting method*. In the next section we introduce a regularization technique to force the control to be in the set $(-1,1)$ and to be smooth. In this context, we will be able to implement a simple shooting method and determine the structure of the optimal control law. Thanks to the simple shooting method, we will have the structure of the optimal control law together with an approximation of the switching times that we will use as initial guess for the multiple shooting method that we present in the last section. with $0 < t_1 < t_2 < t_f$. We say that the control is *Bang-Singular-Bang*. A Bang arc corresponds to $u \in \{-1, 1\}$ while a singular control corresponds to $u \in (-1, 1)$. Since the optimal control law is discontinuous, then to solve this optimal control problem by indirect methods and find the *switching times* $t_1$ and $t_2$, we need to implement what we call a *multiple shooting method*. In the next section we introduce a regularization technique to force the control to be in the set $(-1,1)$ and to be smooth. In this context, we will be able to implement a simple shooting method and determine the structure of the optimal control law. Thanks to the simple shooting method, we will have the structure of the optimal control law together with an approximation of the switching times that we will use as initial guess for the multiple shooting method that we present in the last section.
<div class="alert alert-warning"> <div class="alert alert-warning">
**Main goal** **Main goal**
Find the switching times $t_1$ and $t_2$ by multiple shooting. Find the switching times $t_1$ and $t_2$ by multiple shooting.
</div> </div>
Steps: Steps:
1. Regularize the problem and solve the regularized problem by simple shooting. 1. Regularize the problem and solve the regularized problem by simple shooting.
2. Determine the structure of the non-regularized optimal control problem, that is the structure Bang-Singular-Bang, and find a good approximation of the switching times and of the initial co-vector. 2. Determine the structure of the non-regularized optimal control problem, that is the structure Bang-Singular-Bang, and find a good approximation of the switching times and of the initial co-vector.
3. Solve the non-regularized optimal control problem by multiple shooting. 3. Solve the non-regularized optimal control problem by multiple shooting.
**_Remark 1._** See this [page](../lecture/lecture_simple_shooting.ipynb) for a general presentation of the simple shooting method. **_Remark 1._** See this [page](../lecture/lecture_simple_shooting.ipynb) for a general presentation of the simple shooting method.
**_Remark 2._** In this particular example, the singular control does not depend on the costate $p$ since it is constant. This happens in low dimension. This could be taken into consideration to simplify the definition of the multiple shooting method. However, to stay general, we will not consider this particular property in this notebook. **_Remark 2._** In this particular example, the singular control does not depend on the costate $p$ since it is constant. This happens in low dimension. This could be taken into consideration to simplify the definition of the multiple shooting method. However, to stay general, we will not consider this particular property in this notebook.
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## II) Regularization and simple shooting ## II) Regularization and simple shooting
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We make the following regularization: We make the following regularization:
$$ $$
\left\{ \left\{
\begin{array}{l} \begin{array}{l}
\displaystyle J(u) := \displaystyle \int_0^{t_f} (x^2(t) - \varepsilon\ln(1-u^2(t))) \, \mathrm{d}t \longrightarrow \min \\[1.0em] \displaystyle J(u) := \displaystyle \int_0^{t_f} (x^2(t) - \varepsilon\ln(1-u^2(t))) \, \mathrm{d}t \longrightarrow \min \\[1.0em]
\dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em] \dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em]
x(0) = 1, \quad x(t_f) = 1/2. x(0) = 1, \quad x(t_f) = 1/2.
\end{array} \end{array}
\right. \right.
$$ $$
Our goal is to determine the structure of the optimal control problem when $(\varepsilon, t_f) = (0, 2)$. The problem is simpler to solver when $\varepsilon$ is bigger and $t_f$ is smaller. It is also smooth whenever $\varepsilon>0$. Hence, we will start by solving the problem for $(\varepsilon, t_f) = (1, 1)$. In a second step, we will decrease the *penalization term* $\varepsilon$ and in a final step, we will increase the final time $t_f$ to the final value $2$. Our goal is to determine the structure of the optimal control problem when $(\varepsilon, t_f) = (0, 2)$. The problem is simpler to solver when $\varepsilon$ is bigger and $t_f$ is smaller. It is also smooth whenever $\varepsilon>0$. Hence, we will start by solving the problem for $(\varepsilon, t_f) = (1, 1)$. In a second step, we will decrease the *penalization term* $\varepsilon$ and in a final step, we will increase the final time $t_f$ to the final value $2$.
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Preliminaries ### Preliminaries
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# import packages # import packages
import nutopy as nt import nutopy as nt
import nutopy.tools as tools import nutopy.tools as tools
import nutopy.ocp as ocp import nutopy.ocp as ocp
import numpy as np import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
#plt.rcParams['figure.figsize'] = [10, 5] #plt.rcParams['figure.figsize'] = [10, 5]
plt.rcParams['figure.dpi'] = 150 plt.rcParams['figure.dpi'] = 150
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Finite differences function for scalar functionnal # Finite differences function for scalar functionnal
# Return f'(x).dx # Return f'(x).dx
def finite_diff(fun, x, dx, *args, **kwargs): def finite_diff(fun, x, dx, *args, **kwargs):
v_eps = np.finfo(float).eps v_eps = np.finfo(float).eps
t = np.sqrt(v_eps) * np.sqrt(np.maximum(1.0, np.abs(x))) / np.sqrt(np.maximum(1.0, np.abs(dx))) t = np.sqrt(v_eps) * np.sqrt(np.maximum(1.0, np.abs(x))) / np.sqrt(np.maximum(1.0, np.abs(dx)))
j = (fun(x + t*dx, *args, **kwargs) - fun(x, *args, **kwargs)) / t j = (fun(x + t*dx, *args, **kwargs) - fun(x, *args, **kwargs)) / t
return j return j
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Parameters # Parameters
t0 = 0.0 t0 = 0.0
x0 = np.array([1.0]) x0 = np.array([1.0])
xf_target = np.array([0.5]) xf_target = np.array([0.5])
e_init = 1.0 e_init = 1.0
e_final = 0.002 # e_final = 0.002 #
tf_init = 1.0 # With this value the problem is simpler to solver since the trajectory stay tf_init = 1.0 # With this value the problem is simpler to solver since the trajectory stay
# less time around the static solution # less time around the static solution
tf_final = 2.0 tf_final = 2.0
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Maximized Hamiltonian and its derivatives ### Maximized Hamiltonian and its derivatives
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
The pseudo-Hamiltonian is (in the normal case) The pseudo-Hamiltonian is (in the normal case)
$$ $$
H(x,p,u,\varepsilon) = pu - x^2 + \varepsilon ln(1-u^2). H(x,p,u,\varepsilon) = pu - x^2 + \varepsilon ln(1-u^2).
$$ $$
Note that we put the parameter $\varepsilon$ into the arguments of the pseudo-Hamiltonian since we will vary it. Note that we put the parameter $\varepsilon$ into the arguments of the pseudo-Hamiltonian since we will vary it.
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 1:_** **_Question 1:_**
Give the maximizing control $u[p, \varepsilon]$, that is the control in feedback form solution of the maximization condition. Give the maximizing control $u[p, \varepsilon]$, that is the control in feedback form solution of the maximization condition.
</div> </div>
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
**Answer 1:** To complete here (double-click on the line to complete) **Answer 1:** To complete here (double-click on the line to complete)
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 2:_** **_Question 2:_**
Complete the code of the maximizing control $u[p, \varepsilon]$ and its derivative with respect to $p$, that is $\frac{\partial u}{\partial p}[p, \varepsilon]$. Complete the code of the maximizing control $u[p, \varepsilon]$ and its derivative with respect to $p$, that is $\frac{\partial u}{\partial p}[p, \varepsilon]$.
</div> </div>
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# ---------------------------- # ----------------------------
# Answer 2 to complete here # Answer 2 to complete here
# ---------------------------- # ----------------------------
# #
# Control in feedback form u[p,e] and its partial derivative wrt. p. # Control in feedback form u[p,e] and its partial derivative wrt. p.
# #
@tools.vectorize(vvars=(1,)) @tools.vectorize(vvars=(1,))
def ufun(p, e): def ufun(p, e):
# u = 0 ### TO COMPLETE # u = 0 ### TO COMPLETE
u = (-e + np.sqrt(e**2+p**2))/p u = (-e + np.sqrt(e**2+p**2))/p
return u return u
def dufun(p, e): def dufun(p, e):
# du = 0 ### TO COMPLETE # du = 0 ### TO COMPLETE
s = np.sqrt(e**2+p**2) s = np.sqrt(e**2+p**2)
du = 1.0/s + (e - s)/p**2 du = 1.0/s + (e - s)/p**2
return du return du
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We give next the maximized Hamiltonian with its derivatives. This permits us to define the flow of the associated Hamiltonian vector field. We give next the maximized Hamiltonian with its derivatives. This permits us to define the flow of the associated Hamiltonian vector field.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Definition of the maximized Hamiltonian and its derivatives # Definition of the maximized Hamiltonian and its derivatives
# The second derivative d2hfun is computed by finite differences for a part # The second derivative d2hfun is computed by finite differences for a part
def dhfun(t, x, dx, p, dp, e): def dhfun(t, x, dx, p, dp, e):
# dh = dh_x dx + dh_p dp # dh = dh_x dx + dh_p dp
u = ufun(p, e) u = ufun(p, e)
du = dufun(p, e) du = dufun(p, e)
hd = (u+p*du+2.0*e*u*du/(u**2-1.0))*dp - 2.0*x*dx hd = (u+p*du+2.0*e*u*du/(u**2-1.0))*dp - 2.0*x*dx
return hd return hd
def d2hfun(t, x, dx, d2x, p, dp, d2p, e): def d2hfun(t, x, dx, d2x, p, dp, d2p, e):
# d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p # d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p
d2h_xx = -2.0*dx*d2x # dh_xx dx d2x d2h_xx = -2.0*dx*d2x # dh_xx dx d2x
dh_p = lambda p: dhfun(t, x, 0.0, p, dp, e) # dh_px = 0 so we can put dx = 0 dh_p = lambda p: dhfun(t, x, 0.0, p, dp, e) # dh_px = 0 so we can put dx = 0
d2h_pp = finite_diff(dh_p, p, d2p) # dh_pp dp d2p d2h_pp = finite_diff(dh_p, p, d2p) # dh_pp dp d2p
hdd = d2h_xx + d2h_pp hdd = d2h_xx + d2h_pp
return hdd return hdd
@tools.tensorize(dhfun, d2hfun, tvars=(2, 3)) @tools.tensorize(dhfun, d2hfun, tvars=(2, 3))
def hfun(t, x, p, e): def hfun(t, x, p, e):
u = ufun(p, e) u = ufun(p, e)
h = p*u - x**2 + e*(np.log(1.0-u**2)) h = p*u - x**2 + e*(np.log(1.0-u**2))
return h return h
h = ocp.Hamiltonian(hfun) # The Hamiltonian object h = ocp.Hamiltonian(hfun) # The Hamiltonian object
f = ocp.Flow(h) # The flow associated to the Hamiltonian object is f = ocp.Flow(h) # The flow associated to the Hamiltonian object is
# the exponential mapping with its derivative # the exponential mapping with its derivative
# that can be used to define the Jacobian of the # that can be used to define the Jacobian of the
# shooting function # shooting function
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Shooting function and its derivative ### Shooting function and its derivative
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
The shooting function is The shooting function is
$$ $$
S(p_0, \varepsilon, t_f) = \pi_x(z(t_f, 1, p_0, \varepsilon)) - 1/2, S(p_0, \varepsilon, t_f) = \pi_x(z(t_f, 1, p_0, \varepsilon)) - 1/2,
$$ $$
where $z(t_f, x_0, p_0, \varepsilon)$ is the solution of the associated Hamiltonian system where $z(t_f, x_0, p_0, \varepsilon)$ is the solution of the associated Hamiltonian system
with the initial condition $z(0) = (x_0, p_0)$. Note that the Hamiltonian system depends on $\varepsilon$. We put $\varepsilon$ and $t_f$ into with the initial condition $z(0) = (x_0, p_0)$. Note that the Hamiltonian system depends on $\varepsilon$. We put $\varepsilon$ and $t_f$ into
the arguments of the shooting function since we will vary them. the arguments of the shooting function since we will vary them.
<div class="alert alert-warning"> <div class="alert alert-warning">
**Procedure** **Procedure**
First solve $S=0$ for $(\varepsilon, t_f) = (1,1)$ then decrease $\varepsilon$ to $0.01$, and finish by increasing $t_f$ to 2. First solve $S=0$ for $(\varepsilon, t_f) = (1,1)$ then decrease $\varepsilon$ to $0.01$, and finish by increasing $t_f$ to 2.
</div> </div>
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 3:_** **_Question 3:_**
Complete the code of the shooting function. Complete the code of the shooting function.
</div> </div>
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# ---------------------------- # ----------------------------
# Answer 3 to complete here # Answer 3 to complete here
# ---------------------------- # ----------------------------
# #
# Definition of the shooting function and its partial derivative wrt. p0 against the vector dp0 # Definition of the shooting function and its partial derivative wrt. p0 against the vector dp0
# #
def shoot(p0, e, tf): def shoot(p0, e, tf):
# s = 0 ### TO COMPLETE: use the flow f, the parameters t0, x0 and xf_target # s = 0 ### TO COMPLETE: use the flow f, the parameters t0, x0 and xf_target
xf, _ = f(t0, x0, p0, tf, e) # We use the flow to get z(tf, x0, p0) xf, _ = f(t0, x0, p0, tf, e) # We use the flow to get z(tf, x0, p0)
s = xf - xf_target # x(tf, x0, p0) - xf_target s = xf - xf_target # x(tf, x0, p0) - xf_target
return s return s
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
def dshoot(p0, dp0, e, tf): def dshoot(p0, dp0, e, tf):
# ds = 0 ### TO COMPLETE
(xf, dxf), _ = f(t0, x0, (p0, dp0), tf, e) (xf, dxf), _ = f(t0, x0, (p0, dp0), tf, e)
ds = dxf ds = dxf
return ds return ds
shoot = nt.tools.tensorize(dshoot, tvars=(1,))(shoot) shoot = nt.tools.tensorize(dshoot, tvars=(1,))(shoot)
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Function to plot the solution # Function to plot the solution
def plotSolution(p0, e, tf): def plotSolution(p0, e, tf):
N = 200 N = 200
tspan = list(np.linspace(t0, tf, N+1)) tspan = list(np.linspace(t0, tf, N+1))
xf, pf = f(t0, x0, p0, tspan, e) xf, pf = f(t0, x0, p0, tspan, e)
u = ufun(pf, e) u = ufun(pf, e)
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(511); ax.plot(tspan, xf); ax.set_xlabel('t'); ax.set_ylabel('$x$'); ax.axhline(0, color='k') ax = fig.add_subplot(511); ax.plot(tspan, xf); ax.set_xlabel('t'); ax.set_ylabel('$x$'); ax.axhline(0, color='k')
ax = fig.add_subplot(513); ax.plot(tspan, pf); ax.set_xlabel('t'); ax.set_ylabel('$p$'); ax.axhline(0, color='k') ax = fig.add_subplot(513); ax.plot(tspan, pf); ax.set_xlabel('t'); ax.set_ylabel('$p$'); ax.axhline(0, color='k')
ax = fig.add_subplot(515); ax.plot(tspan, u); ax.set_xlabel('t'); ax.set_ylabel('$u$'); ax.axhline(0, color='k') ax = fig.add_subplot(515); ax.plot(tspan, u); ax.set_xlabel('t'); ax.set_ylabel('$u$'); ax.axhline(0, color='k')
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Resolution of the regularized problem ### Resolution of the regularized problem
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Shooting for (tf, e) = (tf_init, e_init) # Shooting for (tf, e) = (tf_init, e_init)
p0_guess = np.array([0.1]) p0_guess = np.array([0.1])
nlefun = lambda p0: shoot(p0, e_init, tf_init) nlefun = lambda p0: shoot(p0, e_init, tf_init)
sol_nle = nt.nle.solve(nlefun, p0_guess, df=nlefun) sol_nle = nt.nle.solve(nlefun, p0_guess, df=nlefun)
``` ```
%% Output %% Output
Calls |f(x)| |x| Calls |f(x)| |x|
1 9.225527956247914e-01 1.000000000000000e-01 1 9.225527956247914e-01 1.000000000000000e-01
2 1.328510039876105e-01 2.933843238918098e+00 2 1.328510039876105e-01 2.933843238918098e+00
3 7.295873671084141e-02 2.551952326284172e+00 3 7.295873671084141e-02 2.551952326284172e+00
4 3.048709252432280e-02 2.086745717585143e+00 4 3.048709252432280e-02 2.086745717585143e+00
5 4.420525056492819e-03 2.223849330434452e+00 5 4.420525056492819e-03 2.223849330434452e+00
6 2.283580968987509e-04 2.206487218725369e+00 6 2.283580968987509e-04 2.206487218725369e+00
7 1.831090794324197e-06 2.205541459926580e+00 7 1.831090794324197e-06 2.205541459926580e+00
8 7.509122212923103e-10 2.205548983174077e+00 8 7.509122212923103e-10 2.205548983174077e+00
9 2.775557561562891e-15 2.205548980090132e+00 9 2.775557561562891e-15 2.205548980090132e+00
Results of the nle solver method: Results of the nle solver method:
xsol = [-2.20554898] xsol = [-2.20554898]
f(xsol) = [-2.77555756e-15] f(xsol) = [-2.77555756e-15]
nfev = 9 nfev = 9
njev = 1 njev = 1
status = 1 status = 1
success = True success = True
Successfully completed: relative error between two consecutive iterates is at most TolX. Successfully completed: relative error between two consecutive iterates is at most TolX.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Plot solution for (tf, e) = (tf_init, e_init) # Plot solution for (tf, e) = (tf_init, e_init)
plotSolution(sol_nle.x, e_init, tf_init) plotSolution(sol_nle.x, e_init, tf_init)
``` ```
%% Output %% Output
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Definition of the homotopic function and its first order derivative # Definition of the homotopic function and its first order derivative
# This function is used to solve S=0 for different values of e=epsilon and tf. # This function is used to solve S=0 for different values of e=epsilon and tf.
def dhomfun(p0, dp0, e, de, tf, dtf): def dhomfun(p0, dp0, e, de, tf, dtf):
# #
(xf, dxf), (pf, dpf) = f(t0, x0, (p0, dp0), tf, e) (xf, dxf), (pf, dpf) = f(t0, x0, (p0, dp0), tf, e)
# #
s = xf - xf_target s = xf - xf_target
# #
ds_p0 = dxf ds_p0 = dxf
ds_tf = ufun(pf, e) * dtf # dS_tf dtf = u dtf ds_tf = ufun(pf, e) * dtf # dS_tf dtf = u dtf
# #
fun = lambda e: float(f(t0, x0, p0, tf, e)[0]) fun = lambda e: float(f(t0, x0, p0, tf, e)[0])
ds_e = finite_diff(fun, e, de) # dS_e de ds_e = finite_diff(fun, e, de) # dS_e de
# #
ds = ds_p0 + ds_e + ds_tf ds = ds_p0 + ds_e + ds_tf
return s, ds return s, ds
@tools.tensorize(dhomfun, tvars=(1, 2, 3), full=True) @tools.tensorize(dhomfun, tvars=(1, 2, 3), full=True)
def homfun(p0, e, tf): def homfun(p0, e, tf):
xf, pf = f(t0, x0, p0, tf, e) # We use the flow to get z(tf, x0, p0, e) xf, pf = f(t0, x0, p0, tf, e) # We use the flow to get z(tf, x0, p0, e)
s = xf - xf_target # x(tf, x0, p0) - xf_target s = xf - xf_target # x(tf, x0, p0) - xf_target
return s return s
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Making the penalization smaller: homotopy on e # Making the penalization smaller: homotopy on e
p0 = sol_nle.x p0 = sol_nle.x
sol_path_e = nt.path.solve(homfun, p0, e_init, e_final, args=tf_init, df=homfun) sol_path_e = nt.path.solve(homfun, p0, e_init, e_final, args=tf_init, df=homfun)
``` ```
%% Output %% Output
Calls |f(x,pars)| |x| Homotopic param Arclength s det A(s) Dot product Calls |f(x,pars)| |x| Homotopic param Arclength s det A(s) Dot product
1 2.22044605e-16 2.20554898009e+00 1.00000000000e+00 0.00000000e+00 -3.99453543e-01 0.00000000e+00 1 2.22044605e-16 2.20554898009e+00 1.00000000000e+00 0.00000000e+00 -3.99453543e-01 0.00000000e+00
2 2.22044605e-16 2.19703976141e+00 9.93456169329e-01 1.07344549e-02 -4.02221666e-01 9.99999991e-01 2 2.22044605e-16 2.19703976141e+00 9.93456169329e-01 1.07344549e-02 -4.02221666e-01 9.99999991e-01
3 3.33066907e-16 2.17562949518e+00 9.76983031043e-01 3.77485955e-02 -4.09360819e-01 9.99999942e-01 3 3.33066907e-16 2.17562949518e+00 9.76983031043e-01 3.77485955e-02 -4.09360819e-01 9.99999942e-01
4 2.22044605e-16 2.13582370908e+00 9.46324313715e-01 8.79925768e-02 -4.23336260e-01 9.99999773e-01 4 2.22044605e-16 2.13582370908e+00 9.46324313715e-01 8.79925768e-02 -4.23336260e-01 9.99999773e-01
5 0.00000000e+00 2.04380511707e+00 8.75274200125e-01 2.04248946e-01 -4.59644793e-01 9.99998415e-01 5 0.00000000e+00 2.04380511707e+00 8.75274200125e-01 2.04248946e-01 -4.59644793e-01 9.99998415e-01
6 1.11022302e-16 1.92916643462e+00 7.86347624598e-01 3.49335047e-01 -5.14727915e-01 9.99996190e-01 6 1.11022302e-16 1.92916643462e+00 7.86347624598e-01 3.49335047e-01 -5.14727915e-01 9.99996190e-01
7 2.22044605e-16 1.79444755857e+00 6.81092176321e-01 5.20296827e-01 -5.99279037e-01 9.99990735e-01 7 2.22044605e-16 1.79444755857e+00 6.81092176321e-01 5.20296827e-01 -5.99279037e-01 9.99990735e-01
8 1.11022302e-16 1.66966322809e+00 5.82619647458e-01 6.79256037e-01 -7.06991902e-01 9.99985113e-01 8 1.11022302e-16 1.66966322809e+00 5.82619647458e-01 6.79256037e-01 -7.06991902e-01 9.99985113e-01
9 1.11022302e-16 1.56620772374e+00 5.00008221016e-01 8.11648417e-01 -8.30695653e-01 9.99981810e-01 9 1.11022302e-16 1.56620772374e+00 5.00008221016e-01 8.11648417e-01 -8.30695653e-01 9.99981810e-01
10 3.33066907e-16 1.46593779252e+00 4.18824463927e-01 9.40663687e-01 -9.99327396e-01 9.99971770e-01 10 3.33066907e-16 1.46593779252e+00 4.18824463927e-01 9.40663687e-01 -9.99327396e-01 9.99971770e-01
11 0.00000000e+00 1.37678846132e+00 3.45480162266e-01 1.05610658e+00 -1.21575495e+00 9.99967683e-01 11 0.00000000e+00 1.37678846132e+00 3.45480162266e-01 1.05610658e+00 -1.21575495e+00 9.99967683e-01
12 5.55111512e-17 1.28960342151e+00 2.72548876780e-01 1.16977396e+00 -1.52984851e+00 9.99968620e-01 12 5.55111512e-17 1.28960342151e+00 2.72548876780e-01 1.16977396e+00 -1.52984851e+00 9.99968620e-01
13 1.11022302e-16 1.23722039409e+00 2.28228170219e-01 1.23839109e+00 -1.79572280e+00 9.99995132e-01 13 1.11022302e-16 1.23722039409e+00 2.28228170219e-01 1.23839109e+00 -1.79572280e+00 9.99995132e-01
14 7.77156117e-16 1.19422849087e+00 1.91735175890e-01 1.29478295e+00 -2.07744292e+00 9.99999997e-01 14 7.77156117e-16 1.19422849087e+00 1.91735175890e-01 1.29478295e+00 -2.07744292e+00 9.99999997e-01
15 8.88178420e-16 1.15761257447e+00 1.60763287717e-01 1.34274112e+00 -2.37832445e+00 9.99993905e-01 15 8.88178420e-16 1.15761257447e+00 1.60763287717e-01 1.34274112e+00 -2.37832445e+00 9.99993905e-01
16 8.88178420e-16 1.12621332813e+00 1.34479524463e-01 1.38368932e+00 -2.69537295e+00 9.99976683e-01 16 8.88178420e-16 1.12621332813e+00 1.34479524463e-01 1.38368932e+00 -2.69537295e+00 9.99976683e-01
17 6.66133815e-16 1.09882012933e+00 1.11936744078e-01 1.41916572e+00 -3.03058070e+00 9.99950610e-01 17 6.66133815e-16 1.09882012933e+00 1.11936744078e-01 1.41916572e+00 -3.03058070e+00 9.99950610e-01
18 9.43689571e-16 1.07488689408e+00 9.26897526120e-02 1.44987824e+00 -3.38245451e+00 9.99920746e-01 18 9.43689571e-16 1.07488689408e+00 9.26897526120e-02 1.44987824e+00 -3.38245451e+00 9.99920746e-01
19 3.10862447e-15 1.05162281210e+00 7.45308292960e-02 1.47939067e+00 -3.79520044e+00 9.99862606e-01 19 3.10862447e-15 1.05162281210e+00 7.45308292960e-02 1.47939067e+00 -3.79520044e+00 9.99862606e-01
20 4.44089210e-16 1.03348997369e+00 6.08568585866e-02 1.50210166e+00 -4.18199975e+00 9.99863622e-01 20 4.44089210e-16 1.03348997369e+00 6.08568585866e-02 1.50210166e+00 -4.18199975e+00 9.99863622e-01
21 7.77156117e-16 1.01743621699e+00 4.91774488730e-02 1.52195467e+00 -4.58879415e+00 9.99842046e-01 21 7.77156117e-16 1.01743621699e+00 4.91774488730e-02 1.52195467e+00 -4.58879415e+00 9.99842046e-01
22 2.88657986e-15 1.00351364714e+00 3.94278240330e-02 1.53895177e+00 -5.00712133e+00 9.99833210e-01 22 2.88657986e-15 1.00351364714e+00 3.94278240330e-02 1.53895177e+00 -5.00712133e+00 9.99833210e-01
23 1.11022302e-15 9.91189285569e-01 3.11338118442e-02 1.55380731e+00 -5.44570207e+00 9.99823057e-01 23 1.11022302e-15 9.91189285569e-01 3.11338118442e-02 1.55380731e+00 -5.44570207e+00 9.99823057e-01
24 7.21644966e-16 9.80356390822e-01 2.41377538128e-02 1.56670310e+00 -5.90269325e+00 9.99819071e-01 24 7.21644966e-16 9.80356390822e-01 2.41377538128e-02 1.56670310e+00 -5.90269325e+00 9.99819071e-01
25 1.11022302e-16 9.70835442114e-01 1.82431398575e-02 1.57790126e+00 -6.37956456e+00 9.99817622e-01 25 1.11022302e-16 9.70835442114e-01 1.82431398575e-02 1.57790126e+00 -6.37956456e+00 9.99817622e-01
26 8.99280650e-15 9.62692462871e-01 1.34115980809e-02 1.58736986e+00 -6.86424447e+00 9.99827368e-01 26 8.99280650e-15 9.62692462871e-01 1.34115980809e-02 1.58736986e+00 -6.86424447e+00 9.99827368e-01
27 4.77395901e-15 9.55675551884e-01 9.42082903289e-03 1.59544235e+00 -7.36122521e+00 9.99834372e-01 27 4.77395901e-15 9.55675551884e-01 9.42082903289e-03 1.59544235e+00 -7.36122521e+00 9.99834372e-01
28 1.32116540e-14 9.49853651601e-01 6.24486269889e-03 1.60207427e+00 -7.85282352e+00 9.99851794e-01 28 1.32116540e-14 9.49853651601e-01 6.24486269889e-03 1.60207427e+00 -7.85282352e+00 9.99851794e-01
29 4.19664303e-14 9.45389069838e-01 3.90225115046e-03 1.60711617e+00 -8.30049315e+00 9.99886223e-01 29 4.19664303e-14 9.45389069838e-01 3.90225115046e-03 1.60711617e+00 -8.30049315e+00 9.99886223e-01
30 1.88737914e-15 9.41931793716e-01 2.15059352872e-03 1.61099191e+00 -8.71023410e+00 9.99909978e-01 30 1.88737914e-15 9.41931793716e-01 2.15059352872e-03 1.61099191e+00 -8.71023410e+00 9.99909978e-01
31 6.66133815e-16 9.41628939496e-01 2.00000000000e-03 1.61133013e+00 -8.74973421e+00 9.99999182e-01 31 6.66133815e-16 9.41628939496e-01 2.00000000000e-03 1.61133013e+00 -8.74973421e+00 9.99999182e-01
Results of the path solver method: Results of the path solver method:
xf = [-0.94162894] xf = [-0.94162894]
parsf = 0.002 parsf = 0.002
|f(xf,parsf)| = 6.661338147750939e-16 |f(xf,parsf)| = 6.661338147750939e-16
steps = 31 steps = 31
status = 1 status = 1
success = True success = True
Homotopy successfully completed. Homotopy successfully completed.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Plot solution for (tf, e) = (tf_init, e_final) # Plot solution for (tf, e) = (tf_init, e_final)
plotSolution(sol_path_e.xf, e_final, tf_init) plotSolution(sol_path_e.xf, e_final, tf_init)
``` ```
%% Output %% Output
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Making the time bigger: homotopy on tf # Making the time bigger: homotopy on tf
p0 = sol_path_e.xf # sol is coming from last homotopy p0 = sol_path_e.xf # sol is coming from last homotopy
pathfun = lambda p0, tf, e: homfun(p0, e, tf) # invert order of arguments pathfun = lambda p0, tf, e: homfun(p0, e, tf) # invert order of arguments
sol_path_tf = nt.path.solve(pathfun, p0, tf_init, tf_final, args=e_final, df=pathfun) sol_path_tf = nt.path.solve(pathfun, p0, tf_init, tf_final, args=e_final, df=pathfun)
``` ```
%% Output %% Output
Calls |f(x,pars)| |x| Homotopic param Arclength s det A(s) Dot product Calls |f(x,pars)| |x| Homotopic param Arclength s det A(s) Dot product
1 2.22044605e-16 9.41628939496e-01 1.00000000000e+00 0.00000000e+00 4.01455484e+00 0.00000000e+00 1 2.22044605e-16 9.41628939496e-01 1.00000000000e+00 0.00000000e+00 4.01455484e+00 0.00000000e+00
2 8.88178420e-16 9.44075168781e-01 1.00970881514e+00 1.00122571e-02 4.08611419e+00 9.99990245e-01 2 8.88178420e-16 9.44075168781e-01 1.00970881514e+00 1.00122571e-02 4.08611419e+00 9.99990245e-01
3 6.66133815e-16 9.52110673458e-01 1.04297202177e+00 4.42326128e-02 4.35312513e+00 9.99884749e-01 3 6.66133815e-16 9.52110673458e-01 1.04297202177e+00 4.42326128e-02 4.35312513e+00 9.99884749e-01
4 8.88178420e-16 9.61556842283e-01 1.08526416490e+00 8.75675217e-02 4.75042396e+00 9.99811995e-01 4 8.88178420e-16 9.61556842283e-01 1.08526416490e+00 8.75675217e-02 4.75042396e+00 9.99811995e-01
5 5.55111512e-17 9.71896630561e-01 1.13674411999e+00 1.40076814e-01 5.34799409e+00 9.99719724e-01 5 5.55111512e-17 9.71896630561e-01 1.13674411999e+00 1.40076814e-01 5.34799409e+00 9.99719724e-01
6 5.55111512e-16 9.81245191618e-01 1.18988053242e+00 1.94030671e-01 6.14816857e+00 9.99701750e-01 6 5.55111512e-16 9.81245191618e-01 1.18988053242e+00 1.94030671e-01 6.14816857e+00 9.99701750e-01
7 2.22044605e-15 9.89208695747e-01 1.24261341184e+00 2.47362758e-01 7.21580053e+00 9.99709964e-01 7 2.22044605e-15 9.89208695747e-01 1.24261341184e+00 2.47362758e-01 7.21580053e+00 9.99709964e-01
8 7.21644966e-16 9.95584144595e-01 1.29272773597e+00 2.97882063e-01 8.62227225e+00 9.99745289e-01 8 7.21644966e-16 9.95584144595e-01 1.29272773597e+00 2.97882063e-01 8.62227225e+00 9.99745289e-01
9 1.22124533e-15 1.00051725716e+00 1.33956581293e+00 3.44980041e-01 1.04922584e+01 9.99787850e-01 9 1.22124533e-15 1.00051725716e+00 1.33956581293e+00 3.44980041e-01 1.04922584e+01 9.99787850e-01
10 1.88737914e-15 1.00422155325e+00 1.38281120954e+00 3.88384407e-01 1.30075715e+01 9.99831827e-01 10 1.88737914e-15 1.00422155325e+00 1.38281120954e+00 3.88384407e-01 1.30075715e+01 9.99831827e-01
11 2.33146835e-15 1.00693858544e+00 1.42255002654e+00 4.28216425e-01 1.64500266e+01 9.99872114e-01 11 2.33146835e-15 1.00693858544e+00 1.42255002654e+00 4.28216425e-01 1.64500266e+01 9.99872114e-01
12 4.88498131e-15 1.00888895880e+00 1.45901756959e+00 4.64736370e-01 2.12529132e+01 9.99906881e-01 12 4.88498131e-15 1.00888895880e+00 1.45901756959e+00 4.64736370e-01 2.12529132e+01 9.99906881e-01
13 1.22124533e-14 1.01026195185e+00 1.49256266733e+00 4.98309736e-01 2.80968027e+01 9.99935247e-01 13 1.22124533e-14 1.01026195185e+00 1.49256266733e+00 4.98309736e-01 2.80968027e+01 9.99935247e-01
14 4.94049246e-15 1.01121117641e+00 1.52358925456e+00 5.29350951e-01 3.80663163e+01 9.99957193e-01 14 4.94049246e-15 1.01121117641e+00 1.52358925456e+00 5.29350951e-01 3.80663163e+01 9.99957193e-01
15 6.59472477e-14 1.01185675334e+00 1.55253698600e+00 5.58305944e-01 5.29258454e+01 9.99973207e-01 15 6.59472477e-14 1.01185675334e+00 1.55253698600e+00 5.58305944e-01 5.29258454e+01 9.99973207e-01
16 8.80406859e-14 1.01228937436e+00 1.57986254022e+00 5.85634959e-01 7.56112643e+01 9.99984175e-01 16 8.80406859e-14 1.01228937436e+00 1.57986254022e+00 5.85634959e-01 7.56112643e+01 9.99984175e-01
17 1.68531855e-13 1.01257542227e+00 1.60602989124e+00 6.11803893e-01 1.11142150e+02 9.99991198e-01 17 1.68531855e-13 1.01257542227e+00 1.60602989124e+00 6.11803893e-01 1.11142150e+02 9.99991198e-01
18 2.80664381e-13 1.01276214622e+00 1.63150346762e+00 6.37278163e-01 1.68372602e+02 9.99995395e-01 18 2.80664381e-13 1.01276214622e+00 1.63150346762e+00 6.37278163e-01 1.68372602e+02 9.99995395e-01
19 2.86659585e-13 1.01288241210e+00 1.65674705661e+00 6.62522043e-01 2.63490945e+02 9.99997737e-01 19 2.86659585e-13 1.01288241210e+00 1.65674705661e+00 6.62522043e-01 2.63490945e+02 9.99997737e-01
20 2.96984659e-13 1.01295868532e+00 1.68222816553e+00 6.88003269e-01 4.27333674e+02 9.99998957e-01 20 2.96984659e-13 1.01295868532e+00 1.68222816553e+00 6.88003269e-01 4.27333674e+02 9.99998957e-01
21 1.11244347e-13 1.01300613907e+00 1.70842815072e+00 7.14203298e-01 7.21468310e+02 9.99999552e-01 21 1.11244347e-13 1.01300613907e+00 1.70842815072e+00 7.14203298e-01 7.21468310e+02 9.99999552e-01
22 4.27813340e-12 1.01303494245e+00 1.73585852757e+00 7.41633690e-01 1.27569212e+03 9.99999821e-01 22 4.27813340e-12 1.01303494245e+00 1.73585852757e+00 7.41633690e-01 1.27569212e+03 9.99999821e-01
23 2.57732169e-11 1.01305187229e+00 1.76508420787e+00 7.70859375e-01 2.38160109e+03 9.99999935e-01 23 2.57732169e-11 1.01305187229e+00 1.76508420787e+00 7.70859375e-01 2.38160109e+03 9.99999935e-01
24 1.20242538e-10 1.01306141530e+00 1.79675602397e+00 8.02531193e-01 4.74536748e+03 9.99999978e-01 24 1.20242538e-10 1.01306141530e+00 1.79675602397e+00 8.02531193e-01 4.74536748e+03 9.99999978e-01
25 6.23208152e-10 1.01306651002e+00 1.83165732709e+00 8.37432497e-01 1.02379807e+04 9.99999994e-01 25 6.23208152e-10 1.01306651002e+00 1.83165732709e+00 8.37432497e-01 1.02379807e+04 9.99999994e-01
26 3.66588537e-09 1.01306904469e+00 1.87077329303e+00 8.76548463e-01 2.43881436e+04 9.99999998e-01 26 3.66588537e-09 1.01306904469e+00 1.87077329303e+00 8.76548463e-01 2.43881436e+04 9.99999998e-01
27 2.69244553e-08 1.01307019491e+00 1.91539837246e+00 9.21173542e-01 6.58870660e+04 1.00000000e+00 27 2.69244553e-08 1.01307019491e+00 1.91539837246e+00 9.21173542e-01 6.58870660e+04 1.00000000e+00
28 2.69732114e-07 1.01307065731e+00 1.96730883161e+00 9.73084001e-01 2.09516626e+05 1.00000000e+00 28 2.69732114e-07 1.01307065731e+00 1.96730883161e+00 9.73084001e-01 2.09516626e+05 1.00000000e+00
29 2.15117862e-06 1.01307076680e+00 2.00000000000e+00 1.00577516e+00 4.36713283e+05 1.00000000e+00 29 2.15117862e-06 1.01307076680e+00 2.00000000000e+00 1.00577516e+00 4.36713283e+05 1.00000000e+00
Results of the path solver method: Results of the path solver method:
xf = [-1.01307077] xf = [-1.01307077]
parsf = 2.0 parsf = 2.0
|f(xf,parsf)| = 2.1511786191807936e-06 |f(xf,parsf)| = 2.1511786191807936e-06
steps = 29 steps = 29
status = 1 status = 1
success = True success = True
Homotopy successfully completed. Homotopy successfully completed.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Plot solution for (tf, e) = (tf_final, e_final) # Plot solution for (tf, e) = (tf_final, e_final)
plotSolution(sol_path_tf.xf, e_final, tf_final) plotSolution(sol_path_tf.xf, e_final, tf_final)
``` ```
%% Output %% Output
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## III) Resolution of the optimal control problem by multiple shooting ## III) Resolution of the optimal control problem by multiple shooting
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We come back to the original optimal control problem: We come back to the original optimal control problem:
$$ $$
\left\{ \left\{
\begin{array}{l} \begin{array}{l}
\displaystyle J(u) := \displaystyle \int_0^{t_f} x^2(t) \, \mathrm{d}t \longrightarrow \min \\[1.0em] \displaystyle J(u) := \displaystyle \int_0^{t_f} x^2(t) \, \mathrm{d}t \longrightarrow \min \\[1.0em]
\dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em] \dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em]
x(0) = 1, \quad x(t_f) = 1/2. x(0) = 1, \quad x(t_f) = 1/2.
\end{array} \end{array}
\right. \right.
$$ $$
We have determined that the optimal control follows the strategy: We have determined that the optimal control follows the strategy:
$$ $$
u(t) = \left\{ u(t) = \left\{
\begin{array}{lll} \begin{array}{lll}
-1 & \text{if} & t \in [0, t_1], \\[0.5em] -1 & \text{if} & t \in [0, t_1], \\[0.5em]
\phantom{-}0 & \text{if} & t \in (t_1, t_2], \\[0.5em] \phantom{-}0 & \text{if} & t \in (t_1, t_2], \\[0.5em]
+1 & \text{if} & t \in (t_2, t_f], +1 & \text{if} & t \in (t_2, t_f],
\end{array} \end{array}
\right. \right.
$$ $$
with $0 < t_1 < t_2 < t_f=2$. with $0 < t_1 < t_2 < t_f=2$.
<div class="alert alert-warning"> <div class="alert alert-warning">
**Goal** **Goal**
The goal is to find the values of the switching times $t_1$ and $t_2$ together with the initial covector $p_0$ (see Remark 2). The goal is to find the values of the switching times $t_1$ and $t_2$ together with the initial covector $p_0$ (see Remark 2).
</div> </div>
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Maximized Hamiltonian and its derivatives ### Maximized Hamiltonian and its derivatives
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We define first the three control laws $u \equiv \{-1, 0, 1\}$. We define first the three control laws $u \equiv \{-1, 0, 1\}$.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Controls in feedback form # Controls in feedback form
@tools.vectorize(vvars=(1, 2, 3)) @tools.vectorize(vvars=(1, 2, 3))
def uplus(t, x, p): def uplus(t, x, p):
u = +1.0 u = +1.0
return u return u
@tools.vectorize(vvars=(1, 2, 3)) @tools.vectorize(vvars=(1, 2, 3))
def uminus(t, x, p): def uminus(t, x, p):
u = -1.0 u = -1.0
return u return u
@tools.vectorize(vvars=(1, 2, 3)) @tools.vectorize(vvars=(1, 2, 3))
def using(t, x, p): def using(t, x, p):
u = 0.0 u = 0.0
return u return u
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
The pseudo-Hamiltonian is The pseudo-Hamiltonian is
$$ $$
H(x,p,u) = pu - x^2. H(x,p,u) = pu - x^2.
$$ $$
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 4:_** **_Question 4:_**
Complete the code of the Hamiltonian for $u \equiv +1$, with its derivatives. Complete the code of the Hamiltonian for $u \equiv +1$, with its derivatives.
</div> </div>
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# ---------------------------- # ----------------------------
# Answer 4 to complete here # Answer 4 to complete here
# ---------------------------- # ----------------------------
# #
# Definition of the Hamiltonian and its derivatives for u = 1 # Definition of the Hamiltonian and its derivatives for u = 1
# #
def dhfunplus(t, x, dx, p, dp): def dhfunplus(t, x, dx, p, dp):
# dh = dh_x dx + dh_p dp # dh = dh_x dx + dh_p dp
# hd = 0 ### TO COMPLETE: use uplus # hd = 0 ### TO COMPLETE: use uplus
u = uplus(t, x, p) u = uplus(t, x, p)
hd = u*dp - 2.0*x*dx hd = u*dp - 2.0*x*dx
return hd return hd
def d2hfunplus(t, x, dx, d2x, p, dp, d2p): def d2hfunplus(t, x, dx, d2x, p, dp, d2p):
# d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p # d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p
# hdd = 0 ### TO COMPLETE # hdd = 0 ### TO COMPLETE
hdd = -2.0 * d2x * dx hdd = -2.0 * d2x * dx
return hdd return hdd
@tools.tensorize(dhfunplus, d2hfunplus, tvars=(2, 3)) @tools.tensorize(dhfunplus, d2hfunplus, tvars=(2, 3))
def hfunplus(t, x, p): def hfunplus(t, x, p):
# h = 0 ### TO COMPLETE: use uplus # h = 0 ### TO COMPLETE: use uplus
u = uplus(t, x, p) u = uplus(t, x, p)
h = p*u - x**2 h = p*u - x**2
return h return h
hplus = ocp.Hamiltonian(hfunplus) hplus = ocp.Hamiltonian(hfunplus)
fplus = ocp.Flow(hplus) fplus = ocp.Flow(hplus)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We give the Hamiltonians for $u=-1$ and $u=0$ with their derivatives. We give the Hamiltonians for $u=-1$ and $u=0$ with their derivatives.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Definition of the Hamiltonian and its derivatives for u = -1 # Definition of the Hamiltonian and its derivatives for u = -1
def dhfunminus(t, x, dx, p, dp): def dhfunminus(t, x, dx, p, dp):
# dh = dh_x dx + dh_p dp # dh = dh_x dx + dh_p dp
u = uminus(t, x, p) u = uminus(t, x, p)
hd = u*dp - 2.0*x*dx hd = u*dp - 2.0*x*dx
return hd return hd
def d2hfunminus(t, x, dx, d2x, p, dp, d2p): def d2hfunminus(t, x, dx, d2x, p, dp, d2p):
# d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p # d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p
hdd = -2.0 * d2x * dx hdd = -2.0 * d2x * dx
return hdd return hdd
@tools.tensorize(dhfunminus, d2hfunminus, tvars=(2, 3)) @tools.tensorize(dhfunminus, d2hfunminus, tvars=(2, 3))
def hfunminus(t, x, p): def hfunminus(t, x, p):
u = uminus(t, x, p) u = uminus(t, x, p)
h = p*u - x**2 h = p*u - x**2
return h return h
hminus = ocp.Hamiltonian(hfunminus) hminus = ocp.Hamiltonian(hfunminus)
fminus = ocp.Flow(hminus) fminus = ocp.Flow(hminus)
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Definition of the Hamiltonian and its derivatives for u = 0 # Definition of the Hamiltonian and its derivatives for u = 0
def dhfunsing(t, x, dx, p, dp): def dhfunsing(t, x, dx, p, dp):
# dh = dh_x dx + dh_p dp # dh = dh_x dx + dh_p dp
u = using(t, x, p) u = using(t, x, p)
hd = u*dp - 2.0*x*dx hd = u*dp - 2.0*x*dx
return hd return hd
def d2hfunsing(t, x, dx, d2x, p, dp, d2p): def d2hfunsing(t, x, dx, d2x, p, dp, d2p):
# d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p # d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p
hdd = -2.0 * d2x * dx hdd = -2.0 * d2x * dx
return hdd return hdd
@tools.tensorize(dhfunsing, d2hfunsing, tvars=(2, 3)) @tools.tensorize(dhfunsing, d2hfunsing, tvars=(2, 3))
def hfunsing(t, x, p): def hfunsing(t, x, p):
u = using(t, x, p) u = using(t, x, p)
h = p*u - x**2 h = p*u - x**2
return h return h
hsing = ocp.Hamiltonian(hfunsing) hsing = ocp.Hamiltonian(hfunsing)
fsing = ocp.Flow(hsing) fsing = ocp.Flow(hsing)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Shooting function ### Shooting function
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
The multiple shooting function is given by The multiple shooting function is given by
$$ $$
S(p_0, t_1, t_2) := S(p_0, t_1, t_2) :=
\begin{pmatrix} \begin{pmatrix}
x(t_1, t_0, x_0, p_0, u_-) \\ x(t_1, t_0, x_0, p_0, u_-) \\
p(t_1, t_0, x_0, p_0, u_-) \\ p(t_1, t_0, x_0, p_0, u_-) \\
x(t_f, t_2, x_2, p_2, u_+) x(t_f, t_2, x_2, p_2, u_+) - 1/2
\end{pmatrix}, \end{pmatrix},
$$ $$
where $z_2 := (x_2, p_2) = z(t_2, t_1, x_1, p_1, u_0)$, $z_1 := (x_1, p_1) = z(t_1, t_0, x_0, p_0, u_-)$ and where z(t, s, a, b, u) is the solution at time $t$ of the Hamiltonian system associated to the control u starting at time $s$ at the initial condition $z(s) = (a,b)$. where $z_2 := (x_2, p_2) = z(t_2, t_1, x_1, p_1, u_0)$, $z_1 := (x_1, p_1) = z(t_1, t_0, x_0, p_0, u_-)$ and where z(t, s, a, b, u) is the solution at time $t$ of the Hamiltonian system associated to the control u starting at time $s$ at the initial condition $z(s) = (a,b)$.
We have introduced the notation $u_-$ for $u\equiv -1$, $u_0$ for $u\equiv 0$ and $u_+$ for $u\equiv +1$. We have introduced the notation $u_-$ for $u\equiv -1$, $u_0$ for $u\equiv 0$ and $u_+$ for $u\equiv +1$.
**_Remark:_** We know that $(x_2, p_2)=(0,0)$. **_Remark:_** We know that $(x_2, p_2)=(0,0)$.
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 5:_** **_Question 5:_**
Complete the code of the multiple shooting function. Complete the code of the multiple shooting function.
</div> </div>
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# ---------------------------- # ----------------------------
# Answer 5 to complete here # Answer 5 to complete here
# ---------------------------- # ----------------------------
# #
# Multiple shooting function # Multiple shooting function
# #
tf = tf_final # we set the final time to the value tf_final tf = tf_final # we set the final time to the value tf_final
def shoot_multiple(y): def shoot_multiple(y):
p0 = y[0] p0 = y[0]
t1 = y[1] t1 = y[1]
t2 = y[2] t2 = y[2]
# s = np.zeros([3]) ### TO COMPLETE: use fminus, fsin, fplus, t0, t1, t2, tf, x0, xf_target # s = np.zeros([3]) ### TO COMPLETE: use fminus, fsin, fplus, t0, t1, t2, tf, x0, xf_target
x1, p1 = fminus(t0, x0, p0, t1) # on [ 0, t1] x1, p1 = fminus(t0, x0, p0, t1) # on [ 0, t1]
x2, p2 = (np.array([0.0]), np.array([0.0])) # fsing(t1, x1, p1, t2) # on [t1, t2] x2, p2 = (np.array([0.0]), np.array([0.0])) # fsing(t1, x1, p1, t2) # on [t1, t2]
xf, pf = fplus(t2, x2, p2, tf) # on [t2, tf] xf, pf = fplus(t2, x2, p2, tf) # on [t2, tf]
s = np.zeros([3]) s = np.zeros([3])
s[0] = x1 s[0] = x1
s[1] = p1 s[1] = p1
s[2] = xf - xf_target # x(tf, x0, p0) - xf_target s[2] = xf - xf_target # x(tf, x0, p0) - xf_target
return s return s
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Resolution of the shooting function ### Resolution of the shooting function
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 6:_** **_Question 6:_**
Give initial guesses for the times $t_1$ and $t_2$ according to the solution of the regularized problem. Give initial guesses for the times $t_1$ and $t_2$ according to the solution of the regularized problem.
</div> </div>
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# ---------------------------- # ----------------------------
# Answer 6 to complete here # Answer 6 to complete here
# ---------------------------- # ----------------------------
# #
# Initial guess for the Newton solver # Initial guess for the Newton solver
t1_guess = 0.9 # to update t1_guess = 0.9 # to update
t2_guess = 1.6 # to update t2_guess = 1.6 # to update
p0_guess = sol_path_tf.xf # from previous homotopy p0_guess = sol_path_tf.xf # from previous homotopy
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Resolution of the shooting function # Resolution of the shooting function
y_guess = np.array([float(p0_guess), t1_guess, t2_guess]) y_guess = np.array([float(p0_guess), t1_guess, t2_guess])
sol_nle_mul = nt.nle.solve(shoot_multiple, y_guess) sol_nle_mul = nt.nle.solve(shoot_multiple, y_guess)
``` ```
%% Output %% Output
Calls |f(x)| |x| Calls |f(x)| |x|
1 1.432908241330024e-01 2.096738509815576e+00 1 1.432908241330024e-01 2.096738509815576e+00
2 9.999998053989939e-03 2.066422027958497e+00 2 9.999998053989939e-03 2.066422027958497e+00
3 1.506856008950395e-05 2.061545503561054e+00 3 1.506856008950395e-05 2.061545503561054e+00
4 1.103251344504288e-11 2.061552812803479e+00 4 1.103251344504288e-11 2.061552812803479e+00
5 3.854837484910665e-16 2.061552812808831e+00 5 3.854837484910665e-16 2.061552812808831e+00
Results of the nle solver method: Results of the nle solver method:
xsol = [-1. 1. 1.5] xsol = [-1. 1. 1.5]
f(xsol) = [ 1.38777878e-17 3.47378376e-16 -1.66533454e-16] f(xsol) = [ 1.38777878e-17 3.47378376e-16 -1.66533454e-16]
nfev = 5 nfev = 5
njev = 1 njev = 1
status = 1 status = 1
success = True success = True
Successfully completed: relative error between two consecutive iterates is at most TolX. Successfully completed: relative error between two consecutive iterates is at most TolX.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# function to plot solution # function to plot solution
def plotSolutionBSB(p0, t1, t2, tf): def plotSolutionBSB(p0, t1, t2, tf):
N = 20 N = 20
tspan1 = list(np.linspace(t0, t1, N+1)) tspan1 = list(np.linspace(t0, t1, N+1))
tspan2 = list(np.linspace(t1, t2, N+1)) tspan2 = list(np.linspace(t1, t2, N+1))
tspanf = list(np.linspace(t2, tf, N+1)) tspanf = list(np.linspace(t2, tf, N+1))
x1, p1 = fminus(t0, x0, p0, tspan1) # on [ 0, t1] x1, p1 = fminus(t0, x0, p0, tspan1) # on [ 0, t1]
x2, p2 = fsing(t1, x1[-1], p1[-1], tspan2) # on [t1, t2] x2, p2 = fsing(t1, x1[-1], p1[-1], tspan2) # on [t1, t2]
xf, pf = fplus(t2, x2[-1], p2[-1], tspanf) # on [t2, tf] xf, pf = fplus(t2, x2[-1], p2[-1], tspanf) # on [t2, tf]
u1 = uminus(tspan1, x1, p1) u1 = uminus(tspan1, x1, p1)
u2 = using(tspan2, x2, p2) u2 = using(tspan2, x2, p2)
uf = uplus(tspanf, xf, pf) uf = uplus(tspanf, xf, pf)
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(511); ax.plot(tspan1, x1); ax.plot(tspan2, x2); ax.plot(tspanf, xf); ax = fig.add_subplot(511); ax.plot(tspan1, x1); ax.plot(tspan2, x2); ax.plot(tspanf, xf);
ax.set_xlabel('t'); ax.set_ylabel('$x$'); ax.axhline(0, color='k') ax.set_xlabel('t'); ax.set_ylabel('$x$'); ax.axhline(0, color='k')
ax = fig.add_subplot(513); ax.plot(tspan1, p1); ax.plot(tspan2, p2); ax.plot(tspanf, pf); ax = fig.add_subplot(513); ax.plot(tspan1, p1); ax.plot(tspan2, p2); ax.plot(tspanf, pf);
ax.set_xlabel('t'); ax.set_ylabel('$p$'); ax.axhline(0, color='k') ax.set_xlabel('t'); ax.set_ylabel('$p$'); ax.axhline(0, color='k')
ax = fig.add_subplot(515); ax.plot(tspan1, u1); ax.plot(tspan2, u2); ax.plot(tspanf, uf); ax = fig.add_subplot(515); ax.plot(tspan1, u1); ax.plot(tspan2, u2); ax.plot(tspanf, uf);
ax.set_xlabel('t'); ax.set_ylabel('$u$'); ax.axhline(0, color='k') ax.set_xlabel('t'); ax.set_ylabel('$u$'); ax.axhline(0, color='k')
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# plot solution # plot solution
p0 = sol_nle_mul.x[0] p0 = sol_nle_mul.x[0]
t1 = sol_nle_mul.x[1] t1 = sol_nle_mul.x[1]
t2 = sol_nle_mul.x[2] t2 = sol_nle_mul.x[2]
plotSolutionBSB(p0, t1, t2, tf) plotSolutionBSB(p0, t1, t2, tf)
``` ```
%% Output %% Output
......
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
# Exercice 3: Introduction to indirect multiple shooting: the Bang-Singular-Bang case on a turnpike problem # Exercice 3: Introduction to indirect multiple shooting: the Bang-Singular-Bang case on a turnpike problem
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
* Author: Olivier Cots * Author: Olivier Cots
* Date: March 2021 * Date: March 2021
------ ------
* Back: [Exercice 2](../exercices/ex2_implement_simple_shooting.ipynb) - * Back: [Exercice 2](../exercices/ex2_implement_simple_shooting.ipynb) -
[Correction](../corrections/ex2_cor_implement_simple_shooting.ipynb) [Correction](../corrections/ex2_cor_implement_simple_shooting.ipynb)
------ ------
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## I) Description of the optimal control problem ## I) Description of the optimal control problem
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We consider the following optimal control problem: We consider the following optimal control problem:
$$ $$
\left\{ \left\{
\begin{array}{l} \begin{array}{l}
\displaystyle J(u) := \displaystyle \int_0^{t_f} x^2(t) \, \mathrm{d}t \longrightarrow \min \\[1.0em] \displaystyle J(u) := \displaystyle \int_0^{t_f} x^2(t) \, \mathrm{d}t \longrightarrow \min \\[1.0em]
\dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em] \dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em]
x(0) = 1, \quad x(t_f) = 1/2. x(0) = 1, \quad x(t_f) = 1/2.
\end{array} \end{array}
\right. \right.
$$ $$
To this optimal control problem is associated the stationnary optimization problem To this optimal control problem is associated the stationnary optimization problem
$$ $$
\min_{(x, u)} \{~ x^2 ~ | ~ (x, u) \in \mathrm{R} \times [-1, 1],~ f(x,u) = u = 0\}. \min_{(x, u)} \{~ x^2 ~ | ~ (x, u) \in \mathrm{R} \times [-1, 1],~ f(x,u) = u = 0\}.
$$ $$
The static solution is thus $(x^*, u^*) = (0, 0)$. This solution may be seen as the static pair $(x, u)$ which minimizes the cost $J(u)$ under The static solution is thus $(x^*, u^*) = (0, 0)$. This solution may be seen as the static pair $(x, u)$ which minimizes the cost $J(u)$ under
the constraint $u \in [-1, 1]$. the constraint $u \in [-1, 1]$.
It is well known that this problem is what we call a *turnpike* optimal control problem. It is well known that this problem is what we call a *turnpike* optimal control problem.
Hence, if the final time $t_f$ is long enough the solution is of the following form: Hence, if the final time $t_f$ is long enough the solution is of the following form:
starting from $x(0)=1$, reach as fast as possible the static solution, stay at the static solution as long as possible before reaching starting from $x(0)=1$, reach as fast as possible the static solution, stay at the static solution as long as possible before reaching
the target $x(t_f)=1/2$. In this case, the optimal control would be the target $x(t_f)=1/2$. In this case, the optimal control would be
$$ $$
u(t) = \left\{ u(t) = \left\{
\begin{array}{lll} \begin{array}{lll}
-1 & \text{if} & t \in [0, t_1], \\[0.5em] -1 & \text{if} & t \in [0, t_1], \\[0.5em]
\phantom{-}0 & \text{if} & t \in (t_1, t_2], \\[0.5em] \phantom{-}0 & \text{if} & t \in (t_1, t_2], \\[0.5em]
+1 & \text{if} & t \in (t_2, t_f], +1 & \text{if} & t \in (t_2, t_f],
\end{array} \end{array}
\right. \right.
$$ $$
with $0 < t_1 < t_2 < t_f$. We say that the control is *Bang-Singular-Bang*. A Bang arc corresponds to $u \in \{-1, 1\}$ while a singular control corresponds to $u \in (-1, 1)$. Since the optimal control law is discontinuous, then to solve this optimal control problem by indirect methods and find the *switching times* $t_1$ and $t_2$, we need to implement what we call a *multiple shooting method*. In the next section we introduce a regularization technique to force the control to be in the set $(-1,1)$ and to be smooth. In this context, we will be able to implement a simple shooting method and determine the structure of the optimal control law. Thanks to the simple shooting method, we will have the structure of the optimal control law together with an approximation of the switching times that we will use as initial guess for the multiple shooting method that we present in the last section. with $0 < t_1 < t_2 < t_f$. We say that the control is *Bang-Singular-Bang*. A Bang arc corresponds to $u \in \{-1, 1\}$ while a singular control corresponds to $u \in (-1, 1)$. Since the optimal control law is discontinuous, then to solve this optimal control problem by indirect methods and find the *switching times* $t_1$ and $t_2$, we need to implement what we call a *multiple shooting method*. In the next section we introduce a regularization technique to force the control to be in the set $(-1,1)$ and to be smooth. In this context, we will be able to implement a simple shooting method and determine the structure of the optimal control law. Thanks to the simple shooting method, we will have the structure of the optimal control law together with an approximation of the switching times that we will use as initial guess for the multiple shooting method that we present in the last section.
<div class="alert alert-warning"> <div class="alert alert-warning">
**Main goal** **Main goal**
Find the switching times $t_1$ and $t_2$ by multiple shooting. Find the switching times $t_1$ and $t_2$ by multiple shooting.
</div> </div>
Steps: Steps:
1. Regularize the problem and solve the regularized problem by simple shooting. 1. Regularize the problem and solve the regularized problem by simple shooting.
2. Determine the structure of the non-regularized optimal control problem, that is the structure Bang-Singular-Bang, and find a good approximation of the switching times and of the initial co-vector. 2. Determine the structure of the non-regularized optimal control problem, that is the structure Bang-Singular-Bang, and find a good approximation of the switching times and of the initial co-vector.
3. Solve the non-regularized optimal control problem by multiple shooting. 3. Solve the non-regularized optimal control problem by multiple shooting.
**_Remark 1._** See this [page](../lecture/lecture_simple_shooting.ipynb) for a general presentation of the simple shooting method. **_Remark 1._** See this [page](../lecture/lecture_simple_shooting.ipynb) for a general presentation of the simple shooting method.
**_Remark 2._** In this particular example, the singular control does not depend on the costate $p$ since it is constant. This happens in low dimension. This could be taken into consideration to simplify the definition of the multiple shooting method. However, to stay general, we will not consider this particular property in this notebook. **_Remark 2._** In this particular example, the singular control does not depend on the costate $p$ since it is constant. This happens in low dimension. This could be taken into consideration to simplify the definition of the multiple shooting method. However, to stay general, we will not consider this particular property in this notebook.
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## II) Regularization and simple shooting ## II) Regularization and simple shooting
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We make the following regularization: We make the following regularization:
$$ $$
\left\{ \left\{
\begin{array}{l} \begin{array}{l}
\displaystyle J(u) := \displaystyle \int_0^{t_f} (x^2(t) - \varepsilon\ln(1-u^2(t))) \, \mathrm{d}t \longrightarrow \min \\[1.0em] \displaystyle J(u) := \displaystyle \int_0^{t_f} (x^2(t) - \varepsilon\ln(1-u^2(t))) \, \mathrm{d}t \longrightarrow \min \\[1.0em]
\dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em] \dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em]
x(0) = 1, \quad x(t_f) = 1/2. x(0) = 1, \quad x(t_f) = 1/2.
\end{array} \end{array}
\right. \right.
$$ $$
Our goal is to determine the structure of the optimal control problem when $(\varepsilon, t_f) = (0, 2)$. The problem is simpler to solver when $\varepsilon$ is bigger and $t_f$ is smaller. It is also smooth whenever $\varepsilon>0$. Hence, we will start by solving the problem for $(\varepsilon, t_f) = (1, 1)$. In a second step, we will decrease the *penalization term* $\varepsilon$ and in a final step, we will increase the final time $t_f$ to the final value $2$. Our goal is to determine the structure of the optimal control problem when $(\varepsilon, t_f) = (0, 2)$. The problem is simpler to solver when $\varepsilon$ is bigger and $t_f$ is smaller. It is also smooth whenever $\varepsilon>0$. Hence, we will start by solving the problem for $(\varepsilon, t_f) = (1, 1)$. In a second step, we will decrease the *penalization term* $\varepsilon$ and in a final step, we will increase the final time $t_f$ to the final value $2$.
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Preliminaries ### Preliminaries
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# import packages # import packages
import nutopy as nt import nutopy as nt
import nutopy.tools as tools import nutopy.tools as tools
import nutopy.ocp as ocp import nutopy.ocp as ocp
import numpy as np import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
#plt.rcParams['figure.figsize'] = [10, 5] #plt.rcParams['figure.figsize'] = [10, 5]
plt.rcParams['figure.dpi'] = 150 plt.rcParams['figure.dpi'] = 150
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Finite differences function for scalar functionnal # Finite differences function for scalar functionnal
# Return f'(x).dx # Return f'(x).dx
def finite_diff(fun, x, dx, *args, **kwargs): def finite_diff(fun, x, dx, *args, **kwargs):
v_eps = np.finfo(float).eps v_eps = np.finfo(float).eps
t = np.sqrt(v_eps) * np.sqrt(np.maximum(1.0, np.abs(x))) / np.sqrt(np.maximum(1.0, np.abs(dx))) t = np.sqrt(v_eps) * np.sqrt(np.maximum(1.0, np.abs(x))) / np.sqrt(np.maximum(1.0, np.abs(dx)))
j = (fun(x + t*dx, *args, **kwargs) - fun(x, *args, **kwargs)) / t j = (fun(x + t*dx, *args, **kwargs) - fun(x, *args, **kwargs)) / t
return j return j
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Parameters # Parameters
t0 = 0.0 t0 = 0.0
x0 = np.array([1.0]) x0 = np.array([1.0])
xf_target = np.array([0.5]) xf_target = np.array([0.5])
e_init = 1.0 e_init = 1.0
e_final = 0.002 # e_final = 0.002 #
tf_init = 1.0 # With this value the problem is simpler to solver since the trajectory stay tf_init = 1.0 # With this value the problem is simpler to solver since the trajectory stay
# less time around the static solution # less time around the static solution
tf_final = 2.0 tf_final = 2.0
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Maximized Hamiltonian and its derivatives ### Maximized Hamiltonian and its derivatives
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
The pseudo-Hamiltonian is (in the normal case) The pseudo-Hamiltonian is (in the normal case)
$$ $$
H(x,p,u,\varepsilon) = pu - x^2 + \varepsilon ln(1-u^2). H(x,p,u,\varepsilon) = pu - x^2 + \varepsilon ln(1-u^2).
$$ $$
Note that we put the parameter $\varepsilon$ into the arguments of the pseudo-Hamiltonian since we will vary it. Note that we put the parameter $\varepsilon$ into the arguments of the pseudo-Hamiltonian since we will vary it.
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 1:_** **_Question 1:_**
Give the maximizing control $u[p, \varepsilon]$, that is the control in feedback form solution of the maximization condition. Give the maximizing control $u[p, \varepsilon]$, that is the control in feedback form solution of the maximization condition.
</div> </div>
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
**Answer 1:** To complete here (double-click on the line to complete) **Answer 1:** To complete here (double-click on the line to complete)
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 2:_** **_Question 2:_**
Complete the code of the maximizing control $u[p, \varepsilon]$ and its derivative with respect to $p$, that is $\frac{\partial u}{\partial p}[p, \varepsilon]$. Complete the code of the maximizing control $u[p, \varepsilon]$ and its derivative with respect to $p$, that is $\frac{\partial u}{\partial p}[p, \varepsilon]$.
</div> </div>
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# ---------------------------- # ----------------------------
# Answer 2 to complete here # Answer 2 to complete here
# ---------------------------- # ----------------------------
# #
# Control in feedback form u[p,e] and its partial derivative wrt. p. # Control in feedback form u[p,e] and its partial derivative wrt. p.
# #
@tools.vectorize(vvars=(1,)) @tools.vectorize(vvars=(1,))
def ufun(p, e): def ufun(p, e):
u = 0 ### TO COMPLETE u = 0 ### TO COMPLETE
return u return u
def dufun(p, e): def dufun(p, e):
du = 0 ### TO COMPLETE du = 0 ### TO COMPLETE
return du return du
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We give next the maximized Hamiltonian with its derivatives. This permits us to define the flow of the associated Hamiltonian vector field. We give next the maximized Hamiltonian with its derivatives. This permits us to define the flow of the associated Hamiltonian vector field.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Definition of the maximized Hamiltonian and its derivatives # Definition of the maximized Hamiltonian and its derivatives
# The second derivative d2hfun is computed by finite differences for a part # The second derivative d2hfun is computed by finite differences for a part
def dhfun(t, x, dx, p, dp, e): def dhfun(t, x, dx, p, dp, e):
# dh = dh_x dx + dh_p dp # dh = dh_x dx + dh_p dp
u = ufun(p, e) u = ufun(p, e)
du = dufun(p, e) du = dufun(p, e)
hd = (u+p*du+2.0*e*u*du/(u**2-1.0))*dp - 2.0*x*dx hd = (u+p*du+2.0*e*u*du/(u**2-1.0))*dp - 2.0*x*dx
return hd return hd
def d2hfun(t, x, dx, d2x, p, dp, d2p, e): def d2hfun(t, x, dx, d2x, p, dp, d2p, e):
# d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p # d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p
d2h_xx = -2.0*dx*d2x # dh_xx dx d2x d2h_xx = -2.0*dx*d2x # dh_xx dx d2x
dh_p = lambda p: dhfun(t, x, 0.0, p, dp, e) # dh_px = 0 so we can put dx = 0 dh_p = lambda p: dhfun(t, x, 0.0, p, dp, e) # dh_px = 0 so we can put dx = 0
d2h_pp = finite_diff(dh_p, p, d2p) # dh_pp dp d2p d2h_pp = finite_diff(dh_p, p, d2p) # dh_pp dp d2p
hdd = d2h_xx + d2h_pp hdd = d2h_xx + d2h_pp
return hdd return hdd
@tools.tensorize(dhfun, d2hfun, tvars=(2, 3)) @tools.tensorize(dhfun, d2hfun, tvars=(2, 3))
def hfun(t, x, p, e): def hfun(t, x, p, e):
u = ufun(p, e) u = ufun(p, e)
h = p*u - x**2 + e*(np.log(1.0-u**2)) h = p*u - x**2 + e*(np.log(1.0-u**2))
return h return h
h = ocp.Hamiltonian(hfun) # The Hamiltonian object h = ocp.Hamiltonian(hfun) # The Hamiltonian object
f = ocp.Flow(h) # The flow associated to the Hamiltonian object is f = ocp.Flow(h) # The flow associated to the Hamiltonian object is
# the exponential mapping with its derivative # the exponential mapping with its derivative
# that can be used to define the Jacobian of the # that can be used to define the Jacobian of the
# shooting function # shooting function
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Shooting function and its derivative ### Shooting function and its derivative
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
The shooting function is The shooting function is
$$ $$
S(p_0, \varepsilon, t_f) = \pi_x(z(t_f, 1, p_0, \varepsilon)) - 1/2, S(p_0, \varepsilon, t_f) = \pi_x(z(t_f, 1, p_0, \varepsilon)) - 1/2,
$$ $$
where $z(t_f, x_0, p_0, \varepsilon)$ is the solution of the associated Hamiltonian system where $z(t_f, x_0, p_0, \varepsilon)$ is the solution of the associated Hamiltonian system
with the initial condition $z(0) = (x_0, p_0)$. Note that the Hamiltonian system depends on $\varepsilon$. We put $\varepsilon$ and $t_f$ into with the initial condition $z(0) = (x_0, p_0)$. Note that the Hamiltonian system depends on $\varepsilon$. We put $\varepsilon$ and $t_f$ into
the arguments of the shooting function since we will vary them. the arguments of the shooting function since we will vary them.
<div class="alert alert-warning"> <div class="alert alert-warning">
**Procedure** **Procedure**
First solve $S=0$ for $(\varepsilon, t_f) = (1,1)$ then decrease $\varepsilon$ to $0.01$, and finish by increasing $t_f$ to 2. First solve $S=0$ for $(\varepsilon, t_f) = (1,1)$ then decrease $\varepsilon$ to $0.01$, and finish by increasing $t_f$ to 2.
</div> </div>
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 3:_** **_Question 3:_**
Complete the code of the shooting function. Complete the code of the shooting function.
</div> </div>
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# ---------------------------- # ----------------------------
# Answer 3 to complete here # Answer 3 to complete here
# ---------------------------- # ----------------------------
# #
# Definition of the shooting function and its partial derivative wrt. p0 against the vector dp0 # Definition of the shooting function and its partial derivative wrt. p0 against the vector dp0
# #
def shoot(p0, e, tf): def shoot(p0, e, tf):
s = 0 ### TO COMPLETE: use the flow f, the parameters t0, x0 and xf_target s = 0 ### TO COMPLETE: use the flow f, the parameters t0, x0 and xf_target
return s return s
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
def dshoot(p0, dp0, e, tf): def dshoot(p0, dp0, e, tf):
(xf, dxf), _ = f(t0, x0, (p0, dp0), tf, e) (xf, dxf), _ = f(t0, x0, (p0, dp0), tf, e)
ds = dxf ds = dxf
return ds return ds
shoot = nt.tools.tensorize(dshoot, tvars=(1,))(shoot) shoot = nt.tools.tensorize(dshoot, tvars=(1,))(shoot)
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Function to plot the solution # Function to plot the solution
def plotSolution(p0, e, tf): def plotSolution(p0, e, tf):
N = 200 N = 200
tspan = list(np.linspace(t0, tf, N+1)) tspan = list(np.linspace(t0, tf, N+1))
xf, pf = f(t0, x0, p0, tspan, e) xf, pf = f(t0, x0, p0, tspan, e)
u = ufun(pf, e) u = ufun(pf, e)
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(511); ax.plot(tspan, xf); ax.set_xlabel('t'); ax.set_ylabel('$x$'); ax.axhline(0, color='k') ax = fig.add_subplot(511); ax.plot(tspan, xf); ax.set_xlabel('t'); ax.set_ylabel('$x$'); ax.axhline(0, color='k')
ax = fig.add_subplot(513); ax.plot(tspan, pf); ax.set_xlabel('t'); ax.set_ylabel('$p$'); ax.axhline(0, color='k') ax = fig.add_subplot(513); ax.plot(tspan, pf); ax.set_xlabel('t'); ax.set_ylabel('$p$'); ax.axhline(0, color='k')
ax = fig.add_subplot(515); ax.plot(tspan, u); ax.set_xlabel('t'); ax.set_ylabel('$u$'); ax.axhline(0, color='k') ax = fig.add_subplot(515); ax.plot(tspan, u); ax.set_xlabel('t'); ax.set_ylabel('$u$'); ax.axhline(0, color='k')
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Resolution of the regularized problem ### Resolution of the regularized problem
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Shooting for (tf, e) = (tf_init, e_init) # Shooting for (tf, e) = (tf_init, e_init)
p0_guess = np.array([0.1]) p0_guess = np.array([0.1])
nlefun = lambda p0: shoot(p0, e_init, tf_init) nlefun = lambda p0: shoot(p0, e_init, tf_init)
sol_nle = nt.nle.solve(nlefun, p0_guess, df=nlefun) sol_nle = nt.nle.solve(nlefun, p0_guess, df=nlefun)
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Plot solution for (tf, e) = (tf_init, e_init) # Plot solution for (tf, e) = (tf_init, e_init)
plotSolution(sol_nle.x, e_init, tf_init) plotSolution(sol_nle.x, e_init, tf_init)
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Definition of the homotopic function and its first order derivative # Definition of the homotopic function and its first order derivative
# This function is used to solve S=0 for different values of e=epsilon and tf. # This function is used to solve S=0 for different values of e=epsilon and tf.
def dhomfun(p0, dp0, e, de, tf, dtf): def dhomfun(p0, dp0, e, de, tf, dtf):
# #
(xf, dxf), (pf, dpf) = f(t0, x0, (p0, dp0), tf, e) (xf, dxf), (pf, dpf) = f(t0, x0, (p0, dp0), tf, e)
# #
s = xf - xf_target s = xf - xf_target
# #
ds_p0 = dxf ds_p0 = dxf
ds_tf = ufun(pf, e) * dtf # dS_tf dtf = u dtf ds_tf = ufun(pf, e) * dtf # dS_tf dtf = u dtf
# #
fun = lambda e: float(f(t0, x0, p0, tf, e)[0]) fun = lambda e: float(f(t0, x0, p0, tf, e)[0])
ds_e = finite_diff(fun, e, de) # dS_e de ds_e = finite_diff(fun, e, de) # dS_e de
# #
ds = ds_p0 + ds_e + ds_tf ds = ds_p0 + ds_e + ds_tf
return s, ds return s, ds
@tools.tensorize(dhomfun, tvars=(1, 2, 3), full=True) @tools.tensorize(dhomfun, tvars=(1, 2, 3), full=True)
def homfun(p0, e, tf): def homfun(p0, e, tf):
xf, pf = f(t0, x0, p0, tf, e) # We use the flow to get z(tf, x0, p0, e) xf, pf = f(t0, x0, p0, tf, e) # We use the flow to get z(tf, x0, p0, e)
s = xf - xf_target # x(tf, x0, p0) - xf_target s = xf - xf_target # x(tf, x0, p0) - xf_target
return s return s
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Making the penalization smaller: homotopy on e # Making the penalization smaller: homotopy on e
p0 = sol_nle.x p0 = sol_nle.x
sol_path_e = nt.path.solve(homfun, p0, e_init, e_final, args=tf_init, df=homfun) sol_path_e = nt.path.solve(homfun, p0, e_init, e_final, args=tf_init, df=homfun)
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Plot solution for (tf, e) = (tf_init, e_final) # Plot solution for (tf, e) = (tf_init, e_final)
plotSolution(sol_path_e.xf, e_final, tf_init) plotSolution(sol_path_e.xf, e_final, tf_init)
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Making the time bigger: homotopy on tf # Making the time bigger: homotopy on tf
p0 = sol_path_e.xf # sol is coming from last homotopy p0 = sol_path_e.xf # sol is coming from last homotopy
pathfun = lambda p0, tf, e: homfun(p0, e, tf) # invert order of arguments pathfun = lambda p0, tf, e: homfun(p0, e, tf) # invert order of arguments
sol_path_tf = nt.path.solve(pathfun, p0, tf_init, tf_final, args=e_final, df=pathfun) sol_path_tf = nt.path.solve(pathfun, p0, tf_init, tf_final, args=e_final, df=pathfun)
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Plot solution for (tf, e) = (tf_final, e_final) # Plot solution for (tf, e) = (tf_final, e_final)
plotSolution(sol_path_tf.xf, e_final, tf_final) plotSolution(sol_path_tf.xf, e_final, tf_final)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## III) Resolution of the optimal control problem by multiple shooting ## III) Resolution of the optimal control problem by multiple shooting
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We come back to the original optimal control problem: We come back to the original optimal control problem:
$$ $$
\left\{ \left\{
\begin{array}{l} \begin{array}{l}
\displaystyle J(u) := \displaystyle \int_0^{t_f} x^2(t) \, \mathrm{d}t \longrightarrow \min \\[1.0em] \displaystyle J(u) := \displaystyle \int_0^{t_f} x^2(t) \, \mathrm{d}t \longrightarrow \min \\[1.0em]
\dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em] \dot{x}(t) = f(x(t), u(t)) := \displaystyle u(t), \quad |u(t)| \le 1, \quad t \in [0, t_f] \text{ a.e.}, \\[1.0em]
x(0) = 1, \quad x(t_f) = 1/2. x(0) = 1, \quad x(t_f) = 1/2.
\end{array} \end{array}
\right. \right.
$$ $$
We have determined that the optimal control follows the strategy: We have determined that the optimal control follows the strategy:
$$ $$
u(t) = \left\{ u(t) = \left\{
\begin{array}{lll} \begin{array}{lll}
-1 & \text{if} & t \in [0, t_1], \\[0.5em] -1 & \text{if} & t \in [0, t_1], \\[0.5em]
\phantom{-}0 & \text{if} & t \in (t_1, t_2], \\[0.5em] \phantom{-}0 & \text{if} & t \in (t_1, t_2], \\[0.5em]
+1 & \text{if} & t \in (t_2, t_f], +1 & \text{if} & t \in (t_2, t_f],
\end{array} \end{array}
\right. \right.
$$ $$
with $0 < t_1 < t_2 < t_f=2$. with $0 < t_1 < t_2 < t_f=2$.
<div class="alert alert-warning"> <div class="alert alert-warning">
**Goal** **Goal**
The goal is to find the values of the switching times $t_1$ and $t_2$ together with the initial covector $p_0$ (see Remark 2). The goal is to find the values of the switching times $t_1$ and $t_2$ together with the initial covector $p_0$ (see Remark 2).
</div> </div>
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Maximized Hamiltonian and its derivatives ### Maximized Hamiltonian and its derivatives
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We define first the three control laws $u \equiv \{-1, 0, 1\}$. We define first the three control laws $u \equiv \{-1, 0, 1\}$.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Controls in feedback form # Controls in feedback form
@tools.vectorize(vvars=(1, 2, 3)) @tools.vectorize(vvars=(1, 2, 3))
def uplus(t, x, p): def uplus(t, x, p):
u = +1.0 u = +1.0
return u return u
@tools.vectorize(vvars=(1, 2, 3)) @tools.vectorize(vvars=(1, 2, 3))
def uminus(t, x, p): def uminus(t, x, p):
u = -1.0 u = -1.0
return u return u
@tools.vectorize(vvars=(1, 2, 3)) @tools.vectorize(vvars=(1, 2, 3))
def using(t, x, p): def using(t, x, p):
u = 0.0 u = 0.0
return u return u
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
The pseudo-Hamiltonian is The pseudo-Hamiltonian is
$$ $$
H(x,p,u) = pu - x^2. H(x,p,u) = pu - x^2.
$$ $$
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 4:_** **_Question 4:_**
Complete the code of the Hamiltonian for $u \equiv +1$, with its derivatives. Complete the code of the Hamiltonian for $u \equiv +1$, with its derivatives.
</div> </div>
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# ---------------------------- # ----------------------------
# Answer 4 to complete here # Answer 4 to complete here
# ---------------------------- # ----------------------------
# #
# Definition of the Hamiltonian and its derivatives for u = 1 # Definition of the Hamiltonian and its derivatives for u = 1
# #
def dhfunplus(t, x, dx, p, dp): def dhfunplus(t, x, dx, p, dp):
# dh = dh_x dx + dh_p dp # dh = dh_x dx + dh_p dp
hd = 0 ### TO COMPLETE: use uplus hd = 0 ### TO COMPLETE: use uplus
return hd return hd
def d2hfunplus(t, x, dx, d2x, p, dp, d2p): def d2hfunplus(t, x, dx, d2x, p, dp, d2p):
# d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p # d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p
hdd = 0 ### TO COMPLETE hdd = 0 ### TO COMPLETE
return hdd return hdd
@tools.tensorize(dhfunplus, d2hfunplus, tvars=(2, 3)) @tools.tensorize(dhfunplus, d2hfunplus, tvars=(2, 3))
def hfunplus(t, x, p): def hfunplus(t, x, p):
h = 0 ### TO COMPLETE: use uplus h = 0 ### TO COMPLETE: use uplus
return h return h
hplus = ocp.Hamiltonian(hfunplus) hplus = ocp.Hamiltonian(hfunplus)
fplus = ocp.Flow(hplus) fplus = ocp.Flow(hplus)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
We give the Hamiltonians for $u=-1$ and $u=0$ with their derivatives. We give the Hamiltonians for $u=-1$ and $u=0$ with their derivatives.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Definition of the Hamiltonian and its derivatives for u = -1 # Definition of the Hamiltonian and its derivatives for u = -1
def dhfunminus(t, x, dx, p, dp): def dhfunminus(t, x, dx, p, dp):
# dh = dh_x dx + dh_p dp # dh = dh_x dx + dh_p dp
u = uminus(t, x, p) u = uminus(t, x, p)
hd = u*dp - 2.0*x*dx hd = u*dp - 2.0*x*dx
return hd return hd
def d2hfunminus(t, x, dx, d2x, p, dp, d2p): def d2hfunminus(t, x, dx, d2x, p, dp, d2p):
# d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p # d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p
hdd = -2.0 * d2x * dx hdd = -2.0 * d2x * dx
return hdd return hdd
@tools.tensorize(dhfunminus, d2hfunminus, tvars=(2, 3)) @tools.tensorize(dhfunminus, d2hfunminus, tvars=(2, 3))
def hfunminus(t, x, p): def hfunminus(t, x, p):
u = uminus(t, x, p) u = uminus(t, x, p)
h = p*u - x**2 h = p*u - x**2
return h return h
hminus = ocp.Hamiltonian(hfunminus) hminus = ocp.Hamiltonian(hfunminus)
fminus = ocp.Flow(hminus) fminus = ocp.Flow(hminus)
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Definition of the Hamiltonian and its derivatives for u = 0 # Definition of the Hamiltonian and its derivatives for u = 0
def dhfunsing(t, x, dx, p, dp): def dhfunsing(t, x, dx, p, dp):
# dh = dh_x dx + dh_p dp # dh = dh_x dx + dh_p dp
u = using(t, x, p) u = using(t, x, p)
hd = u*dp - 2.0*x*dx hd = u*dp - 2.0*x*dx
return hd return hd
def d2hfunsing(t, x, dx, d2x, p, dp, d2p): def d2hfunsing(t, x, dx, d2x, p, dp, d2p):
# d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p # d2h = dh_xx dx d2x + dh_xp dp d2x + dh_px dx d2p + dh_pp dp d2p
hdd = -2.0 * d2x * dx hdd = -2.0 * d2x * dx
return hdd return hdd
@tools.tensorize(dhfunsing, d2hfunsing, tvars=(2, 3)) @tools.tensorize(dhfunsing, d2hfunsing, tvars=(2, 3))
def hfunsing(t, x, p): def hfunsing(t, x, p):
u = using(t, x, p) u = using(t, x, p)
h = p*u - x**2 h = p*u - x**2
return h return h
hsing = ocp.Hamiltonian(hfunsing) hsing = ocp.Hamiltonian(hfunsing)
fsing = ocp.Flow(hsing) fsing = ocp.Flow(hsing)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Shooting function ### Shooting function
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
The multiple shooting function is given by The multiple shooting function is given by
$$ $$
S(p_0, t_1, t_2) := S(p_0, t_1, t_2) :=
\begin{pmatrix} \begin{pmatrix}
x(t_1, t_0, x_0, p_0, u_-) \\ x(t_1, t_0, x_0, p_0, u_-) \\
p(t_1, t_0, x_0, p_0, u_-) \\ p(t_1, t_0, x_0, p_0, u_-) \\
x(t_f, t_2, x_2, p_2, u_+) x(t_f, t_2, x_2, p_2, u_+) - 1/2
\end{pmatrix}, \end{pmatrix},
$$ $$
where $z_2 := (x_2, p_2) = z(t_2, t_1, x_1, p_1, u_0)$, $z_1 := (x_1, p_1) = z(t_1, t_0, x_0, p_0, u_-)$ and where z(t, s, a, b, u) is the solution at time $t$ of the Hamiltonian system associated to the control u starting at time $s$ at the initial condition $z(s) = (a,b)$. where $z_2 := (x_2, p_2) = z(t_2, t_1, x_1, p_1, u_0)$, $z_1 := (x_1, p_1) = z(t_1, t_0, x_0, p_0, u_-)$ and where z(t, s, a, b, u) is the solution at time $t$ of the Hamiltonian system associated to the control u starting at time $s$ at the initial condition $z(s) = (a,b)$.
We have introduced the notation $u_-$ for $u\equiv -1$, $u_0$ for $u\equiv 0$ and $u_+$ for $u\equiv +1$. We have introduced the notation $u_-$ for $u\equiv -1$, $u_0$ for $u\equiv 0$ and $u_+$ for $u\equiv +1$.
**_Remark:_** We know that $(x_2, p_2)=(0,0)$. **_Remark:_** We know that $(x_2, p_2)=(0,0)$.
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 5:_** **_Question 5:_**
Complete the code of the multiple shooting function. Complete the code of the multiple shooting function.
</div> </div>
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# ---------------------------- # ----------------------------
# Answer 5 to complete here # Answer 5 to complete here
# ---------------------------- # ----------------------------
# #
# Multiple shooting function # Multiple shooting function
# #
tf = tf_final # we set the final time to the value tf_final tf = tf_final # we set the final time to the value tf_final
def shoot_multiple(y): def shoot_multiple(y):
p0 = y[0] p0 = y[0]
t1 = y[1] t1 = y[1]
t2 = y[2] t2 = y[2]
s = np.zeros([3]) ### TO COMPLETE: use fminus, fsin, fplus, t0, t1, t2, tf, x0, xf_target s = np.zeros([3]) ### TO COMPLETE: use fminus, fsin, fplus, t0, t1, t2, tf, x0, xf_target
return s return s
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Resolution of the shooting function ### Resolution of the shooting function
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
<div class="alert alert-info"> <div class="alert alert-info">
**_Question 6:_** **_Question 6:_**
Give initial guesses for the times $t_1$ and $t_2$ according to the solution of the regularized problem. Give initial guesses for the times $t_1$ and $t_2$ according to the solution of the regularized problem.
</div> </div>
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# ---------------------------- # ----------------------------
# Answer 6 to complete here # Answer 6 to complete here
# ---------------------------- # ----------------------------
# #
# Initial guess for the Newton solver # Initial guess for the Newton solver
t1_guess = 0.0 # to update t1_guess = 0.0 # to update
t2_guess = 0.0 # to update t2_guess = 0.0 # to update
p0_guess = sol_path_tf.xf # from previous homotopy p0_guess = sol_path_tf.xf # from previous homotopy
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# Resolution of the shooting function # Resolution of the shooting function
y_guess = np.array([float(p0_guess), t1_guess, t2_guess]) y_guess = np.array([float(p0_guess), t1_guess, t2_guess])
sol_nle_mul = nt.nle.solve(shoot_multiple, y_guess) sol_nle_mul = nt.nle.solve(shoot_multiple, y_guess)
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# function to plot solution # function to plot solution
def plotSolutionBSB(p0, t1, t2, tf): def plotSolutionBSB(p0, t1, t2, tf):
N = 20 N = 20
tspan1 = list(np.linspace(t0, t1, N+1)) tspan1 = list(np.linspace(t0, t1, N+1))
tspan2 = list(np.linspace(t1, t2, N+1)) tspan2 = list(np.linspace(t1, t2, N+1))
tspanf = list(np.linspace(t2, tf, N+1)) tspanf = list(np.linspace(t2, tf, N+1))
x1, p1 = fminus(t0, x0, p0, tspan1) # on [ 0, t1] x1, p1 = fminus(t0, x0, p0, tspan1) # on [ 0, t1]
x2, p2 = fsing(t1, x1[-1], p1[-1], tspan2) # on [t1, t2] x2, p2 = fsing(t1, x1[-1], p1[-1], tspan2) # on [t1, t2]
xf, pf = fplus(t2, x2[-1], p2[-1], tspanf) # on [t2, tf] xf, pf = fplus(t2, x2[-1], p2[-1], tspanf) # on [t2, tf]
u1 = uminus(tspan1, x1, p1) u1 = uminus(tspan1, x1, p1)
u2 = using(tspan2, x2, p2) u2 = using(tspan2, x2, p2)
uf = uplus(tspanf, xf, pf) uf = uplus(tspanf, xf, pf)
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(511); ax.plot(tspan1, x1); ax.plot(tspan2, x2); ax.plot(tspanf, xf); ax = fig.add_subplot(511); ax.plot(tspan1, x1); ax.plot(tspan2, x2); ax.plot(tspanf, xf);
ax.set_xlabel('t'); ax.set_ylabel('$x$'); ax.axhline(0, color='k') ax.set_xlabel('t'); ax.set_ylabel('$x$'); ax.axhline(0, color='k')
ax = fig.add_subplot(513); ax.plot(tspan1, p1); ax.plot(tspan2, p2); ax.plot(tspanf, pf); ax = fig.add_subplot(513); ax.plot(tspan1, p1); ax.plot(tspan2, p2); ax.plot(tspanf, pf);
ax.set_xlabel('t'); ax.set_ylabel('$p$'); ax.axhline(0, color='k') ax.set_xlabel('t'); ax.set_ylabel('$p$'); ax.axhline(0, color='k')
ax = fig.add_subplot(515); ax.plot(tspan1, u1); ax.plot(tspan2, u2); ax.plot(tspanf, uf); ax = fig.add_subplot(515); ax.plot(tspan1, u1); ax.plot(tspan2, u2); ax.plot(tspanf, uf);
ax.set_xlabel('t'); ax.set_ylabel('$u$'); ax.axhline(0, color='k') ax.set_xlabel('t'); ax.set_ylabel('$u$'); ax.axhline(0, color='k')
``` ```
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# plot solution # plot solution
p0 = sol_nle_mul.x[0] p0 = sol_nle_mul.x[0]
t1 = sol_nle_mul.x[1] t1 = sol_nle_mul.x[1]
t2 = sol_nle_mul.x[2] t2 = sol_nle_mul.x[2]
plotSolutionBSB(p0, t1, t2, tf) plotSolutionBSB(p0, t1, t2, tf)
``` ```
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment