import numpy as np
from nutopy.ivp import exp, dexp, djexp
from nutopy.tools import *
from nutopy.errors import *
[docs]class OCP:
"""
Parameters
----------
f0fun: callable
Function defining the Lagrange cost
ffun: callable
Function defining the dynamics
Returns
-------
An Optimal Control Problem (Lagrange type) object
"""
def __init__(self, f0fun, ffun):
self.__f0 = f0fun
self.__f = ffun
[docs] def f0(self, t, x, u, *args, **kwargs):
"""
Parameters
---------
t: float
Time
x: array
State
u: array
Control
Returns
------
The value of the Lagrange cost
"""
return self.__f0(t, x, u, *args, **kwargs)
[docs] def f(self, t, x, u, *args, **kwargs):
"""
Parameters
---------
t: float
Time
x: array
State
u: array
Control
Returns
------
The value of the dynamics
"""
return self.__f(t, x, u, *args, **kwargs)
[docs]class Hamiltonian:
"""
Parameters
---------
hfun: callable
Hamiltonian function (must be tensorized up to order two)
Returns
-------
A Hamiltonian object
"""
def __init__(self, hfun, tvars=None):
if tvars is None:
if not hasattr(hfun, 'tvars'):
raise InputArgumentError('If you use a lambda function for hfun, please give tvars: Hamiltonian(hfun, tvars).')
else:
tvars = hfun.tvars
self.__val = hfun
self.__tvars = tvars
self.__tvars_args = tuple(x-3 for x in (tuple(filter(lambda x: x != 1 and x!=2 and x!=3, tvars)))) # start from 1
@property
def tvars(self):
return self.__tvars
@property
def tvars_args(self):
return self.__tvars_args
[docs] @classmethod
def fromOCP(cls, o, ufun, p0=-1.):
"""
Parameters
----------
o: OCP
Optimal control problem
ufun: callable
Dynamic feedback control
p0: float
Value of cost multiplier (default is -1, for normal case)
Returns
-------
Generates a Hamiltonian from OCP and dynamic feedback control
"""
def dhfun(t, x, dx, p, dp, *args, **kwargs):
u, du = ufun(t, (x, dx), (p, dp), *args, **kwargs)
f0, df0 = o.f0(t, (x, dx), (u, du), *args, **kwargs)
f, df = o.f(t, (x, dx), (u, du), *args, **kwargs)
h = p0*f0 + np.dot(p, f)
dh = p0*df0 + np.dot(dp, f) + np.dot(p, df)
return h, dh
def d2hfun(t, x, dx, d2x, p, dp, d2p, *args, **kwargs):
u, du, d2u = ufun(t, (x, dx, d2x), (p, dp, d2p), *args, **kwargs)
f0, df0, d2f0 = o.f0(t, (x, dx, d2x), (u, du, d2u), *args, **kwargs)
f, df, d2f = o.f(t, (x, dx, d2x), (u, du, d2u), *args, **kwargs)
h = p0*f0 + np.dot(p, f)
dh = p0*df0 + np.dot(dp, f) + np.dot(p, df)
_, dfd2x = o.f(t, (x, d2x), (u, d2u), *args, **kwargs)
d2h = p0*d2f0 + np.dot(dp, dfd2x) + np.dot(d2p, df) + np.dot(p, d2f)
return h, dh, d2h
@tensorize(dhfun, d2hfun, tvars=(2, 3), full=True)
def hfun(t, x, p, *args, **kwargs):
u = ufun(t, x, p, *args, **kwargs)
f0 = o.f0(t, x, u, *args, **kwargs)
f = o.f(t, x, u, *args, **kwargs)
h = p0*f0 + np.dot(p, f)
return h
return cls(hfun)
def __call__(self, t, x, p, *args, **kwargs):
"""
Parameters
----------
t: float
Time
x: array
State
p: array
Costate
Returns
-------
Value of Hamiltonian
"""
return self.__val(t, x, p, *args, **kwargs)
def __d2vec(self, t, x, dx, d2x, p, dp, d2p, *args, **kwargs):
n = x.shape[0]
v = np.zeros(2*n)
dv = np.zeros(2*n)
d2v = np.zeros(2*n)
d3x = np.zeros(n)
d3p = np.zeros(n)
y = dargs(lambda x : (x[0], 0., x[1], x[2]), 2, self.tvars_args, *args) # arguments and increments from parameters
for i in range(n):
d3p[i] = 1.
_, v[i], dv[i], d2v[i] = self(t, (x, d3x, dx, d2x), (p, d3p, dp, d2p), *y, **kwargs)
d3p[i] = 0.
for i in range(n):
d3x[i] =-1.
_, v[n+i], dv[n+i], d2v[n+i] = self(t, (x, d3x, dx, d2x), (p, d3p, dp, d2p), *y, **kwargs)
d3x[i] = 0.
return v, dv, d2v
def __dvec(self, t, x, dx, p, dp, *args, **kwargs):
n = x.shape[0]
v = np.zeros(2*n)
dv = np.zeros(2*n)
d2x = np.zeros(n)
d2p = np.zeros(n)
y = dargs(lambda x : (x[0], 0., x[1]), 1, self.tvars_args, *args) # arguments and increments from parameters
for i in range(n):
d2p[i] = 1.
_, v[i], dv[i] = self(t, (x, d2x, dx), (p, d2p, dp), *y, **kwargs)
d2p[i] = 0.
for i in range(n):
d2x[i] =-1.
_, v[n+i], dv[n+i] = self(t, (x, d2x, dx), (p, d2p, dp), *y, **kwargs)
d2x[i] = 0.
return v, dv
[docs] def vec(self, t, x, p, *args, **kwargs):
"""
Parameters
----------
t: float
Time
x: array
State
p: array
Costate
Returns
-------
Symplectic gradient of the Hamiltonian. Tensorized to order 1.
"""
@tensorize(self.__dvec, self.__d2vec, tvars=(2, 3)+tuple(x+3 for x in self.tvars_args), full=True)
def tmp(t, x, p, *args, **kwargs):
n = x.shape[0]
v = np.zeros(2*n)
dx = np.zeros(n)
dp = np.zeros(n)
for i in range(n):
dp[i] = 1.
_, v[i] = self(t, (x, dx), (p, dp), *args, **kwargs)
dp[i] = 0.
for i in range(n):
dx[i] =-1.
_, v[n+i] = self(t, (x, dx), (p, dp), *args, **kwargs)
dx[i] = 0.
return v
return tmp(t, x, p, *args, **kwargs)
# def __add__(self, other):
# def d2hfun(t, (x, dx, d2x), (p, dp, d2p), *args, **kwargs):
# h1, dh1, d2h1 = self(t, (x, dx, d2x), (p, dp, d2p), *args, **kwargs)
# h2, dh2, d2h2 = other(t, (x, dx, d2x), (p, dp, d2p), *args, **kwargs)
# return h1+h2, dh1+dh2, d2h1+d2h2
# def dhfun(t, (x, dx), (p, dp), *args, **kwargs):
# h1, dh1 = self(t, (x, dx), (p, dp), *args, **kwargs)
# h2, dh2 = other(t, (x, dx), (p, dp), *args, **kwargs)
# return h1+h2, dh1+dh2
# @tensorize(dhfun, d2hfun, tvars=(2, 3), full=True)
# def hfun(t, x, p, *args, **kwargs):
# h1 = self(t, x, p, *args, **kwargs)
# h2 = other(t, x, p, *args, **kwargs)
# return h1+h2
# return Hamiltonian(hfun)
# def __mul__(self, other):
# def d2hfun(t, (x, dx, d2x), (p, dp, d2p), *args, **kwargs):
# h1, dh1, d2h1 = self(t, (x, dx, d2x), (p, dp, d2p), *args, **kwargs)
# h2, dh2, d2h2 = other(t, (x, dx, d2x), (p, dp, d2p), *args, **kwargs)
# _, aux1 = self(t, (x, d2x), (p, d2p), *args, **kwargs)
# _, aux2 = other(t, (x, d2x), (p, d2p), *args, **kwargs)
# return h1*h2, dh1*h2+h1*dh2, d2h1*h2+dh1*aux2+aux1*dh2+h1*d2h2
# def dhfun(t, (x, dx), (p, dp), *args, **kwargs):
# h1, dh1 = self(t, (x, dx), (p, dp), *args, **kwargs)
# h2, dh2 = other(t, (x, dx), (p, dp), *args, **kwargs)
# return h1*h2, dh1*h2+h1*dh2
# @tensorize(dhfun, d2hfun, tvars=(2, 3), full=True)
# def hfun(t, x, p, *args, **kwargs):
# h1 = self(t, x, p, *args, **kwargs)
# h2 = other(t, x, p, *args, **kwargs)
# return h1*h2
# return Hamiltonian(hfun)
[docs]class Flow:
"""
Parameters
----------
h: Hamiltonian
Hamiltonian object defining the flow
options: dict
Dictionary of options for the IVP solver
Returns
-------
The Hamiltonian flow. The optional options are passed to the IVP solver and can be set dynamically.
See also
--------
nutopy.ivp.Options
"""
def __init__(self, h, options=None):
self.__h = h
self.options = options
@property
def h(self):
return self.__h
@property
def options(self):
return self.__options
@options.setter
def options(self, options):
self.__options = options
# Warning: ivp.djexp has no order 2 for t0, tf nor the parameters
def __d2call__(self, t0, dt0, d2t0, x0, dx0, d2x0, p0, dp0, d2p0, tf, dtf, d2tf, *args, next=False, **kwargs):
n = x0.shape[0]
y = dargs(lambda x : x, 2, self.h.tvars_args, *args)
def dynd2(t, z, dz, d2z):
x = z[0:n]
p = z[n:2*n]
dx = dz[0:n]
dp = dz[n:2*n]
d2x = d2z[0:n]
d2p = d2z[n:2*n]
dz, ddz, dd2z = self.h.vec(t, (x, dx, d2x), (p, dp, d2p), *y, **kwargs)
return dz, ddz, dd2z
z0 = np.hstack((x0, p0))
dz0 = np.hstack((dx0, dp0))
d2z0 = np.hstack((d2x0, d2p0))
ddum = np.zeros(2*n)
sol = djexp(dynd2, tf, dtf, t0, dt0, z0, d2z0, dz0, ddum, options=self.options)
if not(sol.success): raise IVPError(sol.status, sol.message)
xf, pf = sol.xf[0:n], sol.xf[n:2*n]
dxf, dpf = sol.dxf[0:n], sol.dxf[n:2*n]
d2xf, d2pf = sol.dxfd[0:n], sol.dxfd[n:2*n]
if not next: return (xf, dxf, d2xf), (pf, dpf, d2pf)
else: return (xf, dxf, d2xf), (pf, dpf, d2pf), (self, (tf, dtf), (xf, dxf, d2xf), (pf, dpf, d2pf), None, *args)
def __dcall__(self, t0, dt0, x0, dx0, p0, dp0, tf, dtf, *args, next=False, **kwargs):
n = x0.shape[0]
y = dargs(lambda x : x, 1, self.h.tvars_args, *args)
def dynd(t, z, dz):
x = z[0:n]
p = z[n:2*n]
dx = dz[0:n]
dp = dz[n:2*n]
dz, ddz = self.h.vec(t, (x, dx), (p, dp), *y, **kwargs)
return dz, ddz
z0 = np.hstack((x0, p0))
dz0 = np.hstack((dx0, dp0))
sol = dexp(dynd, tf, dtf, t0, dt0, z0, dz0, options=self.options)
if not(sol.success): raise IVPError(sol.status, sol.message)
xf, pf = sol.xf[0:n], sol.xf[n:2*n]
dxf, dpf = sol.xfd[0:n], sol.xfd[n:2*n]
if not next: return (xf, dxf), (pf, dpf)
else: return (xf, dxf), (pf, dpf), (self, (tf, dtf), (xf, dxf), (pf, dpf), None, *args)
@vectorize(vvars=(2, 3, 4))
@vectorize(vvars=(5,), next=True)
def __call__(self, t0, x0, p0, tf, *args, next=False, **kwargs):
"""
Parameters
----------
t0: float
Initial time
x0: array
Initial condition, state
p0: array
Initial condition, costate
tf: float
Final time
Returns
-------
Hamiltonian flow from (t0, x0, p0) at tf. Tensorized up to order 1 wrt. t0, x0, p0, tf. Vectorized wrt. tf, then wrt. t0, x0, p0.
"""
@tensorize(self.__dcall__, self.__d2call__, tvars=(1, 2, 3, 4)+tuple(x+4 for x in self.h.tvars_args), full=True)
def tmp(t0, x0, p0, tf, *args, next=False, **kwargs):
n = x0.shape[0]
def dyn(t, z):
x = z[0:n]
p = z[n:2*n]
dz = self.h.vec(t, x, p, *args, **kwargs)
return dz
z0 = np.hstack((x0, p0))
sol = exp(dyn, tf, t0, z0, options=self.options)
if not(sol.success): raise IVPError(sol.status, sol.message)
xf, pf = sol.xf[0:n], sol.xf[n:2*n]
if not next: return xf, pf
else: return xf, pf, (self, tf, xf, pf, None, *args)
return tmp(t0, x0, p0, tf, *args, next=next, **kwargs)
[docs]class Error(Exception):
"""
Exceptions of the module:
Unimplemented
IVPError
"""
pass
[docs]class Unimplemented(Error):
"""
Raised when trying to use a (not yet) implemented feature
"""
pass
[docs]class IVPError(Error):
"""
Parameters
----------
status : int
Termination status from IVP
message : string
Explanation from IVP
Returns
-------
An exception raised when IVP integration is not successful
"""
def __init__(self, status, message):
self.status = status
self.message = message