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