def loss_quadratic(m, z, v=None, W=None):
""" Quadratic cost function
Simple quadratic loss with W as weight matrix, ignoring variance
Parameters
----------
m : dx1 ndarray[float | casadi.Sym]
The mean of the input Gaussian
v : dxd ndarray[float | casadi.Sym]
z: dx1 ndarray[float | casadi.Sym]
The target-state [optional]
W: dxd ndarray[float | casadi.Sym]
The weighting matrix factor for the cost-function (scaling)
Returns
-------
L: float
The quadratic loss
"""
D = np.shape(m)[0]
if W is None:
W = SX.eye(D)
l_var = 0
if not v is None:
l_var = trace(mtimes(W, v))
return mtimes((m - z).T, mtimes(W, m - z)) + l_var
def loss_sat(m, v, z, W=None):
""" Saturating cost function
Parameters
----------
m : dx1 ndarray[float | casadi.Sym]
The mean of the input Gaussian
v : dxd ndarray[float | casadi.Sym]
z: dx1 ndarray[float | casadi.Sym]
The target-state [optional]
W: dxd ndarray[float | casadi.Sym]
The weighting matrix factor for the cost-function (scaling)
Returns
-------
L: float
The expected loss under the saturating cost function
Warning: Solving the Matlab system W/(eye(D)+SW) via inversion. Can be instable
TO-DO: Should be fixed
"""
D = np.shape(m)[0]
if W is None:
W = SX.eye(D)
SW = mtimes(v, W)
G = SX.eye(D) + SW
inv_G = inv(G)
iSpW = mtimes(W, inv_G)
L = 1 - exp(mtimes(-(m - z).T, mtimes(iSpW, (m - z) / 2))) / sqrt(det(G))
return Function("l_sat", [m, v], [L]) # convert into MX function
def construct_upd_z(self):
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
self._construct_upd_z_nlp()
x_i = struct_symSX(self.q_i_struct)
x_j = struct_symSX(self.q_ij_struct)
l_i = struct_symSX(self.q_i_struct)
l_ij = struct_symSX(self.q_ij_struct)
t = SX.sym('t')
T = SX.sym('T')
rho = SX.sym('rho')
par = struct_symSX(self.par_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t/T
# put symbols in SX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A([par])[0]
b = b([par])[0]
# build KKT system
E = rho*SX.eye(A.shape[1])
l, x = vertcat([l_i.cat, l_ij.cat]), vertcat([x_i.cat, x_j.cat])
f = -(l + rho*x)
G = vertcat([horzcat([E, A.T]),
horzcat([A, SX.zeros(A.shape[0], A.shape[0])])])
h = vertcat([-f, b])
z = solve(G, h)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, compile_time = self.father.create_function(
'upd_z', inp, out, self.options)
self.problem_upd_z = prob
# set dimensions
ocp.dims.N = N
# set cost
ocp.cost.cost_type = 'EXTERNAL'
ocp.cost.cost_type_e = 'EXTERNAL'
W_u = 1e-3
theta = model.x[1]
ocp.model.cost_expr_ext_cost = tanh(theta)**2 + .5 * (model.x[0]**2 +
W_u * model.u**2)
ocp.model.cost_expr_ext_cost_e = tanh(theta)**2 + .5 * model.x[0]**2
custom_hess_u = W_u
J = horzcat(SX.eye(2), SX(2, 2))
print(DM(J.sparsity()))
# diagonal matrix with second order terms of outer loss function.
D = SX.sym('D', Sparsity.diag(2))
D[0, 0] = 1
[hess_tan, grad_tan] = hessian(tanh(theta)**2, theta)
D[1, 1] = if_else(theta == 0, hess_tan, grad_tan / theta)
custom_hess_x = J.T @ D @ J
zeros = SX(1, nx)
cost_expr_ext_cost_custom_hess = blockcat(custom_hess_u, zeros, zeros.T,
custom_hess_x)
cost_expr_ext_cost_custom_hess_e = custom_hess_x
def get_ls_factor(n_uncertain, n_samples, pc_order, lamb=0.0):
# Uncertain parameter design
sobol_design = sobol_seq.i4_sobol_generate(n_uncertain, n_samples,
ceil(np.log2(n_samples)))
sobol_samples = np.transpose(sobol_design)
for i in range(n_uncertain):
sobol_samples[i, :] = norm(loc=0., scale=1).ppf(sobol_samples[i, :])
# Polynomial function definition
x = SX.sym('x')
he0fcn = Function('He0fcn', [x], [1.])
he1fcn = Function('He1fcn', [x], [x])
he2fcn = Function('He2fcn', [x], [x**2 - 1])
he3fcn = Function('He3fcn', [x], [x**3 - 3 * x])
he4fcn = Function('He4fcn', [x], [x**4 - 6 * x**2 + 3])
he5fcn = Function('He5fcn', [x], [x**5 - 10 * x**3 + 15 * x])
he6fcn = Function('He6fcn', [x], [x**6 - 15 * x**4 + 45 * x**2 - 15])
he7fcn = Function('He7fcn', [x], [x**7 - 21 * x**5 + 105 * x**3 - 105 * x])
he8fcn = Function('He8fcn', [x],
[x**8 - 28 * x**6 + 210 * x**4 - 420 * x**2 + 105])
he9fcn = Function('He9fcn', [x],
[x**9 - 36 * x**7 + 378 * x**5 - 1260 * x**3 + 945 * x])
he10fcn = Function(
'He10fcn', [x],
[x**10 - 45 * x**8 + 640 * x**6 - 3150 * x**4 + 4725 * x**2 - 945])
helist = [
he0fcn, he1fcn, he2fcn, he3fcn, he4fcn, he5fcn, he6fcn, he7fcn, he8fcn,
he9fcn, he10fcn
]
# Calculation of factor for least-squares
xu = SX.sym("xu", n_uncertain)
exps = (p for p in product(range(pc_order + 1), repeat=n_uncertain)
if sum(p) <= pc_order)
next(exps)
exps = list(exps)
psi = SX.ones(
int(
factorial(n_uncertain + pc_order) /
(factorial(n_uncertain) * factorial(pc_order))))
for i in range(len(exps)):
for j in range(n_uncertain):
psi[i + 1] *= helist[exps[i][j]](xu[j])
psi_fcn = Function('PSIfcn', [xu], [psi])
nparameter = SX.size(psi)[0]
psi_matrix = SX.zeros(n_samples, nparameter)
for i in range(n_samples):
psi_a = psi_fcn(sobol_samples[:, i])
for j in range(SX.size(psi)[0]):
psi_matrix[i, j] = psi_a[j]
psi_t_psi = mtimes(psi_matrix.T, psi_matrix) + lamb * DM.eye(nparameter)
chol_psi_t_psi = chol(psi_t_psi)
inv_chol_psi_t_psi = solve(chol_psi_t_psi, SX.eye(nparameter))
inv_psi_t_psi = mtimes(inv_chol_psi_t_psi, inv_chol_psi_t_psi.T)
ls_factor = mtimes(inv_psi_t_psi, psi_matrix.T)
ls_factor = DM(ls_factor)
# Calculation of expectations for variance function
n_sample_expectation_vector = 100000
x_sample = np.random.multivariate_normal(np.zeros(n_uncertain),
np.eye(n_uncertain),
n_sample_expectation_vector)
psi_squared_sum = DM.zeros(SX.size(psi)[0])
for i in range(n_sample_expectation_vector):
psi_squared_sum += psi_fcn(x_sample[i, :])**2
expectation_vector = psi_squared_sum / n_sample_expectation_vector
return ls_factor, expectation_vector, psi_fcn
