Notebook source code:
examples/nlp_indirect/nlp1.ipynb
Run the notebook yourself on binder
Comparing NLP and BVP resolution from indirect method (1/2)¶
Smooth case example:
\[\int_0^1 |u|^2\,\mathrm{d}t \to \min,\]
\[\dot{q} = v,\]
\[\dot{v} = -\lambda v^2 + u,\]
with \(q\) and \(v\) fixed at \(t=0\) and \(t=1\). Denoting \(x=(q, v)\), the Lagrange cost functional is defined by
\[f^0(t, x, u, \lambda) = u^2,\]
while the dynamics is
\[f(t, x, u, \lambda) = (v, -\lambda v^2+u).\]
Denoting \(p=(p_q,p_v)\), in the normal case (\(p^0=-1/2\)), the dynamic feedback is \(u=p_v\), so the maximized Hamiltonian of the problem is
\[H(t, x, p, \lambda) = p_q v + p_v(-\lambda v^2 + u) - u^2/2.\]
Initializations¶
[1]:
import numpy as np
import matplotlib.pyplot as plt
from nutopy import nle
from nutopy.tools import *
from nutopy.ocp import *
from scipy.optimize import minimize
from scipy.optimize import NonlinearConstraint
[2]:
t0 = 0.
tf = 1.
q0 = np.array([ 0., 0., 0. ]) # we augment the system with the cost
x_target = 1.
Hamiltonian¶
[3]:
# h1
def dhfun(t, q, dq, p, dp):
p1, p2, pc = p
p1d, p2d, pcd = dp
hd = -(p1*p1d+p2*p2d)/pc+p2d+pcd*(p1**2+p2**2)/(2.*pc**2)
return hd
def d2hfun(t, q, dq, d2q, p, dp, d2p):
p1, p2, pc = p
p1d, p2d, pcd = dp
p1d0, p2d0, pcd0 = d2p
hdd = -(p1d0*p1d+p2d0*p2d)/pc+pcd*(p1*p1d0+p2*p2d0)/(2.*pc**2)-pcd*pcd0*(p1**2+p2**2)/pc**3
return hdd
@tensorize(dhfun, d2hfun, tvars=(2, 3))
def hfun(t, q, p):
"""h = hfun(t, x, p)
"""
p1, p2, pc = p
h = -(p1**2+p2**2)/(2.*pc) + p2
return h
h1 = Hamiltonian(hfun)
f = Flow(h1)
Shooting function¶
[4]:
def dshoot(p0, dp0):
(qf, dqf), (pf, dpf) = f(t0, q0, (p0, dp0), tf)
s = np.array([ qf[0] - 1., pf[1], pf[2]+1. ]) # code duplication (in order to compute dxf, shooting also needs to compute xf; accordingly, full=True
ds = np.array([ dqf[0], dpf[1], dpf[2] ])
return s, ds
@tensorize(dshoot, full=True)
def shoot(p0):
"""s = shoot(p0)
Shooting function associated with flow f"""
qf, pf = f(t0, q0, p0, tf)
s = np.array([ qf[0] - 1., pf[1], pf[2]+1. ])
return s
Solve¶
[5]:
p0 = np.array([ 0.1, 0.1, -1. ] )
nleopt = nle.Options(SolverMethod='hybrj', Display='on', TolX=1e-8)
sol = nle.solve(shoot, p0, df=shoot, options=nleopt); p0_sol = sol.x
qf, pf = f(t0, q0, p0_sol, tf)
print('NLE\t: ', '\n p0_sol =', p0_sol, '\n shoot =', shoot(p0_sol), '\n cost = ', qf[2])
Calls |f(x)| |x|
1 9.055385138137417e-01 1.009950493836208e+00
2 2.220446049250313e-16 1.414213562373095e+00
3 2.220446049250313e-16 1.414213562373095e+00
4 2.220446049250313e-16 1.414213562373095e+00
5 2.220446049250313e-16 1.414213562373095e+00
6 2.220446049250313e-16 1.414213562373095e+00
7 2.220446049250313e-16 1.414213562373095e+00
8 5.659615711335853e-02 1.374776640321359e+00
9 2.220446049250313e-16 1.414213562373095e+00
10 2.220446049250313e-16 1.414213562373095e+00
11 2.220446049250313e-16 1.414213562373095e+00
12 3.537259819584859e-03 1.411714557751623e+00
13 2.220446049250313e-16 1.414213562373095e+00
14 2.220446049250313e-16 1.414213562373095e+00
15 4.421574774478021e-04 1.413900944390497e+00
16 2.220446049250313e-16 1.414213562373095e+00
17 2.220446049250313e-16 1.414213562373095e+00
18 2.220446049250313e-16 1.414213562373095e+00
19 2.763484234036273e-05 1.414194021723683e+00
20 2.220446049250313e-16 1.414213562373095e+00
21 2.220446049250313e-16 1.414213562373095e+00
22 3.454355292364930e-06 1.414211119777153e+00
23 2.220446049250313e-16 1.414213562373095e+00
24 2.220446049250313e-16 1.414213562373095e+00
25 4.317944113374494e-07 1.414213257048372e+00
26 2.220446049250313e-16 1.414213562373095e+00
27 2.220446049250313e-16 1.414213562373095e+00
28 2.220446049250313e-16 1.414213562373095e+00
29 2.698715051430156e-08 1.414213543290298e+00
Results of the nle solver method:
xsol = [ 1. 0. -1.]
f(xsol) = [2.22044605e-16 0.00000000e+00 0.00000000e+00]
nfev = 29
njev = 2
status = 1
success = True
Successfully completed: relative error between two consecutive iterates is at most TolX.
NLE :
p0_sol = [ 1. 0. -1.]
shoot = [2.22044605e-16 0.00000000e+00 0.00000000e+00]
cost = 0.5000000000000002
NLP problem¶
[6]:
# Cost function
def F(p0):
qf, pf = f(t0, q0, p0, tf)
return qf[2]
def dF(p0, dp0):
(qf, dqf), (pf, dpf) = f(t0, q0, (p0, dp0), tf)
return dpf[2]
def grad_F(p0):
dp0 = np.eye(3)
return np.array([dF(p0, dp0[:,0]), dF(p0, dp0[:,1]), dF(p0, dp0[:,2])])
def hess_F(p0):
return np.zeros((3,3))
# Constraints: x(1, p0) - 1 = 0, pc(0) = -1
def G(p0):
qf, pf = f(t0, q0, p0, tf)
return np.array([qf[0] - 1, p0[2] + 1.])
def dG(p0, dp0):
(qf, dqf), (pf, dpf) = f(t0, q0, (p0, dp0), tf)
return np.array([dqf[0], dp0[2]])
def jac_G(p0):
dp0 = np.eye(3)
jac = np.zeros((2,3))
jac[:,0] = dG(p0, dp0[:,0])
jac[:,1] = dG(p0, dp0[:,1])
jac[:,2] = dG(p0, dp0[:,2])
return jac
nonlinear_constraint = NonlinearConstraint(G, np.array([0., 0.]), np.array([0., 0.]), jac=jac_G)
Solve¶
[7]:
p0 = np.array([ 0.1, 0.1, -1. ] )
res = minimize(F, p0, method='trust-constr', constraints=[nonlinear_constraint],
jac=grad_F, hess=hess_F,
options={'verbose': 2, 'gtol': 1e-8, 'disp': True})
print(res.x)
| niter |f evals|CG iter| obj func |tr radius | opt | c viol |
|-------|-------|-------|-------------|----------|----------|----------|
| 1 | 1 | 0 | +1.0000e-02 | 1.00e+00 | 0.00e+00 | 9.00e-01 |
| 2 | 2 | 0 | +4.1000e-01 | 1.60e+00 | 0.00e+00 | 1.00e-01 |
| 3 | 3 | 0 | +5.0500e-01 | 1.60e+00 | 0.00e+00 | 2.22e-16 |
`gtol` termination condition is satisfied.
Number of iterations: 3, function evaluations: 3, CG iterations: 0, optimality: 0.00e+00, constraint violation: 2.22e-16, execution time: 0.086 s.
[ 1. 0.1 -1. ]
/home/caillau/anaconda3/envs/dev/lib/python3.7/site-packages/scipy/optimize/_hessian_update_strategy.py:186: UserWarning: delta_grad == 0.0. Check if the approximated function is linear. If the function is linear better results can be obtained by defining the Hessian as zero instead of using quasi-Newton approximations.
'approximations.', UserWarning)