def RBF(n): if n == 1: sX = 1 sY = n n_features = self.n_features X = cs.SX.sym('X', sX, n_features) Y = cs.SX.sym('Y', sY, n_features) length_scale = cs.SX.sym('l', 1, n_features) X_ = X / cs.repmat(length_scale, sX, 1) Y_ = Y / cs.repmat(length_scale, sY, 1) dist = cs.SX.zeros((sX, sY)) for i in xrange(0, sX): for j in xrange(0, sY): dist[i, j] = cs.sum2((X_[i, :] - Y_[j, :])**2) K = cs.exp(-.5 * dist) self.RBF1 = cs.Function('RBF1', [X, Y, length_scale], [K]) else: sX = 1 sY = n n_features = self.n_features X = cs.SX.sym('X', sX, n_features) Y = self.model.X_train_ length_scale = cs.SX.sym('l', 1, n_features) X_ = X / cs.repmat(length_scale, sX, 1) Y_ = Y / cs.repmat(length_scale, sY, 1) dist = cs.SX.zeros((sX, sY)) for i in xrange(0, sX): for j in xrange(0, sY): dist[i, j] = cs.sum2((X_[i, :] - Y_[j, :])**2) K = cs.exp(-.5 * dist) self.RBFn = cs.Function('RBFn', [X, length_scale], [K])
def generate_cost_function(self, p_0, u_0, p_all, q_all, mu_perf, sigma_perf, k_ff_safe, k_fb_safe, sigma_safe, k_fb_perf=None, k_ff_perf=None, gp_pred_sigma_perf=None, custom_cost_func=None, eps_noise=0.0): # Generate cost function if custom_cost_func is None: cost = 0 if self.n_perf > 1: n_cost_deviation = np.minimum(self.n_perf, self.n_safe) for i in range(1, n_cost_deviation): cost += mtimes(mu_perf[i, :] - p_all[i, :], mtimes(.1 * self.wx_cost, (mu_perf[i, :] - p_all[i, :]).T)) for i in range(self.n_perf): cost -= sqrt(sum2(gp_pred_sigma_perf[i, :] + eps_noise)) else: for i in range(self.n_safe): cost -= sqrt(sum2(sigma_safe[i, :] + eps_noise)) else: if self.n_perf > 1: cost = custom_cost_func(p_0, u_0, p_all, q_all, k_ff_safe, k_fb_safe, sigma_safe, mu_perf, sigma_perf, gp_pred_sigma_perf, k_fb_perf, k_ff_perf) else: cost = custom_cost_func(p_0, u_0, p_all, q_all, k_ff_safe, k_fb_safe, sigma_safe) return cost
def run_ipopt_ssmevaluator(ssm, n_s, n_u, linearize_mean): casadi_ssm = CasadiSSMEvaluator(ssm, linearize_mean) x = cas.MX.sym("x", (n_s, 1)) y = cas.MX.sym("y", (n_u, 1)) if linearize_mean: mu, sigma, mu_jac = casadi_ssm(x, y) f = cas.sum1(cas.sum2(mu)) + cas.sum1(cas.sum2(sigma)) + cas.sum1( cas.sum2(mu_jac)) else: mu, sigma = casadi_ssm(x, y) f = cas.sum1(cas.sum2(mu)) + cas.sum1(cas.sum2(sigma)) x = cas.vertcat(x, y) options = { "ipopt": { "hessian_approximation": "limited-memory", "max_iter": 2, "derivative_test": "first-order" } } solver = cas.nlpsol("solver", "ipopt", {"x": x, "f": f}, options) with capture_stdout() as out: res = solver(x0=np.random.randn(5, 1)) return str(type(ssm)), linearize_mean, out[0]
def quadrature(fcn, a, b, N=200): # Simpson's rule N += 1 if N % 2 != 0 else 0 fcnmap = fcn.map(N + 1) xvals = cas.linspace(a, b, N + 1) fvals = fcnmap(xvals) return (b - a) / N / 3 * (fvals[0] + 4 * cas.sum2(fvals[1::2]) + 2 * cas.sum2(fvals[2:-2:2]) + fvals[-1])
def maha(a1, b1, Q1, N): """Calculate the Mahalanobis distance Copyright (c) 2018, Eric Bradford """ aQ = ca.mtimes(a1, Q1) bQ = ca.mtimes(b1, Q1) K1 = ca.repmat(ca.sum2(aQ * a1), 1, N) \ + ca.repmat(ca.transpose(ca.sum2(bQ * b1)), N, 1) \ - 2 * ca.mtimes(aQ, ca.transpose(b1)) return K1
def casadi_sum(x, axis=None, out=None): assert out is None if axis == 0: return cs.sum1(x) elif axis == 1: return cs.sum2(x) elif axis is None: return cs.sum1(cs.sum2(x)) else: raise Exception("Invalid argument for sum")
def _unscaled_dist(x, y): """ calculate the squared distance between two sets of datapoints Source: https://github.com/SheffieldML/GPy/blob/devel/GPy/kern/src/stationary.py """ n_x, _ = np.shape(x) n_y, _ = np.shape(y) x1sq = sum2(x**2) x2sq = sum2(y**2) r2 = -2 * mtimes(x, y.T) + repmat(x1sq, 1, n_y) + repmat(x2sq.T, n_x, 1) return sqrt(r2)
def gp_pred(x, kern, beta=None, x_train=None, k_inv_training=None, pred_var=True): """ """ n_pred, _ = np.shape(x) if beta is None: pred_mu = SX.zeros(n_pred, 1) else: k_star = kern(x, y=x_train) pred_mu = mtimes(k_star, beta) if pred_var: pred_sigm = kern(x, diag_only=True) if not beta is None: pred_sigm = pred_sigm - sum2( mtimes(k_star, k_inv_training) * k_star) return pred_mu, pred_sigm return pred_mu
def predict(Xd): Xc = cs.SX.sym('control', self.n_control, 1) coeffs = self.find_linearmodels(self.linear_models, Xd) Xc_leaf = cs.vertcat(1, Xc).T y_pred = cs.sum2(coeffs * Xc_leaf) predict = cs.Function('predict', [Xc], [y_pred]) self.predict_ = predict
def finalize_objective_value(j_dict): val = j_dict["val"] if j_dict["target"] is not None: nan_idx = np.isnan(j_dict["target"]) j_dict["target"][nan_idx] = 0 val -= j_dict["target"] if np.any(nan_idx): val[np.where(nan_idx)] = 0 if j_dict["objective"].quadratic: val = val ** 2 return sum1(sum2(j_dict["objective"].weight * val * j_dict["dt"]))
def predict(coeffs): Xc = cs.SX.sym('control', self.n_control, 1) # coeffs = self.find_linearmodels(Xd) Xc_ = cs.vertcat(1, Xc).T y_pred = cs.sum2(coeffs * Xc_) y_pred = 0.5 * (y_pred + 1) * ( self.normalizer_y.data_max_ - self.normalizer_y.data_min_) + self.normalizer_y.data_min_ predict = cs.Function('predict', [Xc], [y_pred]) self.predict_ = predict
def nllh_casf(self, grad=True, hess=False): """ Creates a casadi function computing the negative log likelihood (without const terms) of the data dependent on hyperparameters v. :param grad: Whether the function should compute the gradient, too :param hess: Whether the function should compute the hessian, too :return: A casadi function taking a value of v as input and returning the neg log likelihood, gradient, and hessian is desired """ vshape = np.atleast_2d(self.v).shape v = cas.MX.sym("v", vshape[0], vshape[1]) phi_x = self.phi_cas(self.x, v) sinv = self.sinv0 + self.beta * cas.mtimes(phi_x.T, phi_x) mean = cas.solve( sinv, cas.mtimes(self.sinv0, self.mean0) + self.beta * cas.mtimes(phi_x.T, self.y)) y_pred = cas.mtimes(mean.T, phi_x.T).T sigma = 1 / self.beta + cas.sum2(phi_x * cas.solve(sinv, phi_x.T).T) y_true = self.y y_diff = y_true - y_pred llht = cas.sum2(y_diff * y_diff) / sigma llh = cas.sum1(llht) if hess: H, llh_grad = cas.hessian(llh, v) elif grad: llh_grad = cas.gradient(llh, v) res = [llh] if grad: res += [llh_grad] if hess: res += [H] f = cas.Function("f_mu", [v], res) return f
def RationalQuadratic(n): if n == 1: sX = 1 sY = n X = cs.SX.sym('X', sX, self.n_features) Y = cs.SX.sym('Y', sY, self.n_features) alpha = cs.SX.sym('a') length_scale = cs.SX.sym('l') dist = cs.SX.zeros((sX, sY)) for i in xrange(0, sX): for j in xrange(0, sY): dist[i, j] = cs.sum2((X[i, :] - Y[j, :])**2) K = (1 + dist / (2 * alpha * length_scale**2))**(-alpha) self.RationalQuadratic1 = cs.Function( 'RQ1', [X, Y, alpha, length_scale], [K]) else: sX = 1 sY = n X = cs.SX.sym('X', sX, self.n_features) Y = self.model.X_train_ alpha = cs.SX.sym('a') length_scale = cs.SX.sym('l') dist = cs.SX.zeros((sX, sY)) for i in xrange(0, sX): for j in xrange(0, sY): dist[i, j] = cs.sum2( (X[i, :] - (Y[j, :].reshape(1, -1)))**2) K = (1 + dist / (2 * alpha * length_scale**2))**(-alpha) self.RationalQuadraticn = cs.Function('RQn', [X, alpha, length_scale], [K])
def _initialize_variables(self, pvars=None): core_variables = { 'x' : (self.nk, self.d+1, self.nx), 'v' : (self.nf, self.nv), 'a' : (self.nk, self.d), 'h' : (self.nf), } self.var = VariableHandler(core_variables) # Initialize default variable bounds self.var.x_lb[:] = 0. self.var.x_ub[:] = 100. self.var.x_in[:] = 1. # Initialize EFM bounds. EFMs are nonnegative. self.var.v_lb[:] = 0. self.var.v_ub[:] = 1. self.var.v_in[:] = 0. # Activity polynomial. self.var.a_lb[:] = 0. self.var.a_ub[:] = np.inf self.var.a_in[:] = 1. # Stage stepsize control (in hours) self.var.h_lb[:] = .1 self.var.h_ub[:] = 10. self.var.h_in[:] = 1. # Maintain compatibility with codes using a symbolic final time self.var.tf_sx = sum([self.var.h_sx[i] * self.stage_breakdown[i] for i in range(self.nf)]) # We also want the v_sx variable to represent a fraction of the overall # efm, so we'll add a constraint saying the sum of the variable must # equal 1. if self.nf > 1: self.add_constraint(cs.sum2(self.var.v_sx[:]), np.ones(self.nf), np.ones(self.nf), 'Sum(v_sx) == 1') elif self.nf == 1: self.add_constraint(cs.sum1(self.var.v_sx[:]), np.ones(self.nf), np.ones(self.nf), 'Sum(v_sx) == 1') if pvars is None: pvars = {} self.pvar = VariableHandler(pvars)
def sum(x, axis: int = None): """ Sum of array elements over a given axis. See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.sum.html """ if not is_casadi_type(x): return _onp.sum(x, axis=axis) else: if axis == 0: return _cas.sum1(x).T elif axis == 1: return _cas.sum2(x) elif axis is None: return sum(sum(x, axis=0), axis=0) else: raise ValueError("CasADi types can only be up to 2D, so `axis` must be None, 0, or 1.")
def _find_discontinuity_for_plotting(self, time, values): tolerance = 1.2 if self.max_delta_t is None: delta_t = sum2(time) / time.numel() else: delta_t = self.max_delta_t out_time = time[0] out_values = values[:, 0] for i in range(1, time.shape[1]): delta_t_comparison = time[i] - time[i - 1] if delta_t_comparison > (1 + tolerance) * delta_t: # include NaN out_time = horzcat(out_time, DM.nan()) out_values = horzcat(out_values, DM.nan(values.shape[0], 1)) out_time = horzcat(out_time, time[i]) out_values = horzcat(out_values, values[:, i]) if self.max_delta_t is None: delta_t = delta_t_comparison return out_time, out_values
def loadGPModel(name, model, xscaler, yscaler, kernel='RBF'): """ GP mean and variance as casadi.SX variable """ X = model.X_train_ x = cs.SX.sym('x', 1, X.shape[1]) # mean if kernel == 'RBF': K1 = CasadiRBF(x, X, model) K2 = CasadiConstant(x, X, model) K = K1 + K2 elif kernel == 'Matern': K = CasadiMatern(x, X, model) else: raise NotImplementedError y_mu = cs.mtimes(K, model.alpha_) + model._y_train_mean y_mu = y_mu * yscaler.scale_ + yscaler.mean_ # variance L_inv = solve_triangular(model.L_.T,np.eye(model.L_.shape[0])) K_inv = L_inv.dot(L_inv.T) if kernel == 'RBF': K1_ = CasadiRBF(x, x, model) K2_ = CasadiConstant(x, x, model) K_ = K1_ + K2_ elif kernel == 'Matern': K_ = CasadiMatern(x, x, model) y_var = cs.diag(K_) - cs.sum2(cs.mtimes(K, K_inv)*K) y_var = cs.fmax(y_var, 0) y_std = cs.sqrt(y_var) y_std *= yscaler.scale_ gpmodel = cs.Function(name, [x], [y_mu, y_std]) return gpmodel
def _squared_exponential_kernel_cs(self, x_1, x_2): """ Symbolic implementation of the anisotropic squared exponential kernel :param x_1: Array of m points (m x d). :param x_2: Array of n points (m x d). :return: Covariance matrix (m x n). """ # Length scale parameter len_scale = self.params['l'] if 'l' in self.params.keys() else 1.0 # Vertical variation parameter sigma_f = self.params['sigma_f'] if 'sigma_f' in self.params.keys( ) else 1.0 if x_1.shape != x_2.shape and x_2.shape[0] == 1: tiling_ones = cs.MX.ones(x_1.shape[0], 1) d = x_1 - cs.mtimes(tiling_ones, x_2) dist = cs.sum2(d**2 / cs.mtimes(tiling_ones, cs.MX(len_scale**2).T)) else: d = x_1 - x_2 dist = cs.sum1(d**2 / cs.MX(len_scale**2)) return sigma_f * cs.SX.exp(-.5 * dist)
def _k_lin(x, y=None, variances=None, diag_only=False): """ Evaluate the Linear kernel function symbolically using Casadi """ n_x, dim_x = np.shape(x) if variances is None: variances = np.ones((dim_x, )) if diag_only: var = repmat(variances.reshape(1, -1), n_x) ret = sum2(var * x**2) return ret var_x = sqrt(repmat(variances.reshape(1, -1), n_x)) if y is None: var_y = var_x y = x else: n_y, _ = np.shape(y) var_y = sqrt(repmat(variances.reshape(1, -1), n_y)) return mtimes(x * var_x, (y * var_y).T)
def add_constraints(self, stage, opti): # Obtain the discretised system f = stage._ode() if stage.is_free_time(): opti.subject_to(self.T >= 0) ps = [] tau_root = [0] + self.tau # Construct polynomial basis for j in range(self.degree + 1): # Construct Lagrange polynomials to get the polynomial basis at the collocation point p = np.poly1d([1]) for r in range(self.degree + 1): if r != j: p *= np.poly1d([1, -tau_root[r] ]) / (tau_root[j] - tau_root[r]) ps.append(hcat(p.coef[::-1])) poly = vcat(ps) ps_z = [] # Construct polynomial basis for z cfr "2.5 Continuous Output for Optimal Control" from Rien's thesis for j in range(1, self.degree + 1): # Construct Lagrange polynomials to get the polynomial basis at the collocation point p_z = np.poly1d([1]) for r in range(1, self.degree + 1): if r != j: p_z *= np.poly1d([1, -tau_root[r] ]) / (tau_root[j] - tau_root[r]) ps_z.append(hcat(p_z.coef[::-1])) poly_z = vcat(ps_z) self.q = 0 # Make time-grid for roots for k in range(self.N): dt = (self.control_grid[k + 1] - self.control_grid[k]) / self.M tr = [] for i in range(self.M): tr.append([ self.integrator_grid[k][i] + dt * self.tau[j] for j in range(self.degree) ]) self.tr.append(tr) for k in range(self.N): dt = (self.control_grid[k + 1] - self.control_grid[k]) / self.M S = 1 / repmat(hcat([dt**i for i in range(self.degree + 1)]), self.degree + 1, 1) S_z = 1 / repmat(hcat([dt**i for i in range(self.degree)]), self.degree, 1) self.Z.append(self.Zc[k][0] @ poly_z[:, 0]) for i in range(self.M): self.xk.append(self.Xc[k][i][:, 0]) self.poly_coeff.append(self.Xc[k][i] @ (poly * S)) self.poly_coeff_z.append(self.Zc[k][i] @ (poly_z * S_z)) self.zk.append(self.Zc[k][i] @ poly_z[:, 0]) for j in range(self.degree): Pidot_j = self.Xc[k][i] @ self.C[:, j] / dt res = f(x=self.Xc[k][i][:, j + 1], u=self.U[k], z=self.Zc[k][i][:, j], p=self.P, t=self.tr[k][i][j]) # Collocation constraints opti.subject_to(Pidot_j == res["ode"]) self.q = self.q + res["quad"] * dt * self.B[j] if stage.nz: opti.subject_to(0 == res["alg"]) for c, meta, _ in stage._constraints["integrator_roots"]: opti.subject_to(self.eval_at_integrator_root( stage, c, k, i, j), meta=meta) # Continuity constraints x_next = self.X[k + 1] if i == self.M - 1 else self.Xc[k][i + 1][:, 0] opti.subject_to(self.Xc[k][i] @ self.D == x_next) for c, meta, _ in stage._constraints["integrator"]: opti.subject_to(self.eval_at_integrator(stage, c, k, i), meta=meta) for c, meta, _ in stage._constraints["inf"]: self.add_inf_constraints(stage, opti, c, k, i, meta) for c, meta, _ in stage._constraints[ "control"]: # for each constraint expression # Add it to the optimizer, but first make x,u concrete. opti.subject_to(self.eval_at_control(stage, c, k), meta=meta) self.Z.append(self.Zc[-1][-1] @ sum2(poly_z)) for c, meta, _ in stage._constraints["control"] + stage._constraints[ "integrator"]: # for each constraint expression # Add it to the optimizer, but first make x,u concrete. opti.subject_to(self.eval_at_control(stage, c, -1), meta=meta) for c, meta, _ in stage._constraints[ "point"]: # Append boundary conditions to the end opti.subject_to(self.eval(stage, c), meta=meta)
A_opt = opti.variable(1, N) opti.subject_to(casadi.vec(A_opt) >= 0) # elements nonnegative opti.subject_to(casadi.vec(A_opt) <= 1) # elements bounded by 1 opti.subject_to(A_opt @ np.ones(N) == 1) # rows sum to 1 opti.set_initial(A_opt, np.array([int(j == 0) for j in range(N)])) ck_new = {} for k in np.ndindex(*[K] * n): ck = np.array([agents[j].c_k[k] for j in range(N)]) # ck_sum = casadi.sum1(ck)*np.ones(N) # can try abs sum also ck_new[k] = A_opt @ ck # Sparse Metric -- Average element penalization sparsemetric = casadi.sum1(casadi.sum2(elt_metric(A_opt))) / A_opt.numel() # opti.subject_to(sparsemetric < 0.25) e_plan = 0 for k in np.ndindex(*[K] * n): e_plan += lambd[k] * (ck_new[k] - mu[k])**2 # readjust sparsemetric range to be [0, e_plan] opti.minimize(e_plan * (1 + sparsemetric)) p_opts = {} s_opts = {'print_level': 0} opti.solver('ipopt', p_opts, s_opts) sol = opti.solve() A = sol.value(A_opt).round(r)
def test_sum(self): self.message("sum") D=DM([[1,2,3],[4,5,6],[7,8,9]]) self.checkarray(c.sum1(D),array([[12,15,18]]),'sum()') self.checkarray(c.sum2(D),array([[6,15,24]]).T,'sum()')
def minimize_acceleration(u): return casadi.sum2(casadi.sum1(casadi.mtimes(u, casadi.transpose(u))))
def test_ipopt_ssmevaluator_multistep_ahead(before_test_casadissm): p_0, q_0, ssm, k_fb, k_ff, L_mu, L_sigm, c_safety, a, b = before_test_casadissm T = 3 n_u, n_s = np.shape(k_fb) u_0 = .2 * np.random.randn(n_u, 1) k_fb_0 = np.random.randn( T - 1, n_s * n_u) # np.zeros((T-1,n_s*n_u))# np.random.randn(T-1,n_s*n_u) k_ff = np.random.randn(T - 1, n_u) # k_fb_ctrl = np.zeros((n_u,n_s))#np.random.randn(n_u,n_s) u_0_cas = MX.sym("u_0", (n_u, 1)) k_fb_cas_0 = MX.sym("k_fb", (T - 1, n_u * n_s)) k_ff_cas = MX.sym("k_ff", (T - 1, n_u)) ssm_forward = ssm.get_forward_model_casadi(True) p_new_cas, q_new_cas, pred_sigm_all = reach_cas.multi_step_reachability( p_0, u_0, k_fb_cas_0, k_ff_cas, ssm_forward, L_mu, L_sigm, c_safety, a, b) h_mat_safe = np.hstack((np.eye(n_s, 1), -np.eye(n_s, 1))).T h_safe = np.array([300, 300]).reshape((2, 1)) h_mat_obs = np.copy(h_mat_safe) h_obs = np.array([300, 300]).reshape((2, 1)) g = [] lbg = [] ubg = [] for i in range(T): p_i = p_new_cas[i, :].T q_i = q_new_cas[i, :].reshape((n_s, n_s)) g_state = lin_ellipsoid_safety_distance(p_i, q_i, h_mat_obs, h_obs, c_safety=2.0) g = vertcat(g, g_state) lbg += [-cas.inf] * 2 ubg += [0] * 2 x_safe = vertcat(u_0_cas, k_ff_cas.reshape((-1, 1))) params_safe = vertcat(k_fb_cas_0.reshape((-1, 1))) f_safe = sum1(sum2(p_new_cas)) + sum1(sum2(q_new_cas)) + sum1( sum2(pred_sigm_all)) k_ff_cas_all = MX.sym("k_ff_single", (T, n_u)) k_fb_cas_all = MX.sym("k_fb_all", (T - 1, n_s * n_u)) k_fb_cas_all_inp = [ k_fb_cas_all[i, :].reshape((n_u, n_s)) for i in range(T - 1) ] ssm_forward1 = ssm.get_forward_model_casadi(True) mu_multistep, sigma_multistep, sigma_pred_perf = prop_casadi.multi_step_taylor_symbolic( p_0, ssm_forward1, k_ff_cas_all, k_fb_cas_all_inp, a=a, b=b) x_perf = vertcat(k_ff_cas_all.reshape((-1, 1))) params_perf = vertcat(k_fb_cas_all.reshape((-1, 1))) f_perf = sum1(sum2(mu_multistep)) + sum1(sum2(sigma_multistep)) + sum1( sum2(sigma_pred_perf)) f_both = f_perf + f_safe x_both = vertcat(x_safe, x_perf) params_both = vertcat(params_safe, params_perf) options = { "ipopt": { "hessian_approximation": "limited-memory", "max_iter": 1 }, 'error_on_fail': False } #raise NotImplementedError("""Need to parse output to get fail/pass signal! # Either 'Maximun Number of Iterations..' or 'Optimal solution found' are result of # a successful run""") #safe only n_x = np.shape(x_safe)[0] n_p = np.shape(params_safe)[0] solver = cas.nlpsol("solver", "ipopt", { "x": x_safe, "f": f_safe, "p": params_safe, "g": g }, options) with capture_stdout() as out: solver(x0=np.random.randn(n_x, 1), p=np.random.randn(n_p, 1), lbg=lbg, ubg=ubg) opt_sol_found = solver.stats() if not opt_sol_found: max_numb_exceeded = parse_solver_output_pass(out) if not max_numb_exceeded: pytest.fail( "Neither optimal solution found, nor maximum number of iterations exceeded. Sth. is wrong" ) n_x = np.shape(x_perf)[0] n_p = np.shape(params_perf)[0] solver = cas.nlpsol("solver", "ipopt", { "x": x_perf, "f": f_perf, "p": params_perf }, options) with capture_stdout() as out: solver(x0=np.random.randn(n_x, 1), p=np.random.randn(n_p, 1)) opt_sol_found = solver.stats() if not opt_sol_found: max_numb_exceeded = parse_solver_output_pass(out) if not max_numb_exceeded: pytest.fail( "Neither optimal solution found, nor maximum number of iterations exceeded. Sth. is wrong" ) #both n_x = np.shape(x_both)[0] n_p = np.shape(params_both)[0] solver = cas.nlpsol("solver", "ipopt", { "x": x_both, "f": f_both, "p": params_both }, options) with capture_stdout() as out: solver(x0=np.random.randn(n_x, 1), p=np.random.randn(n_p, 1)) opt_sol_found = solver.stats() if not opt_sol_found: max_numb_exceeded = parse_solver_output_pass(out) if not max_numb_exceeded: pytest.fail( "Neither optimal solution found, nor maximum number of iterations exceeded. Sth. is wrong" )
def Sum(matrix): """ the equivalent to np.sum(matrix) """ return ca.sum1(ca.sum2(matrix))
def __construct_solver(self): """ Construct periodic NLP and solver. """ # system variables and dimensions x = self.__vars['x'] u = self.__vars['u'] variables_entry = (ct.entry('x', shape=(self.__nx, ), repeat=self.__N), ct.entry('u', shape=(self.__nu, ), repeat=self.__N)) if 'us' in self.__vars: variables_entry += (ct.entry('us', shape=(self.__ns, ), repeat=self.__N), ) # nlp variables + bounds w = ct.struct_symMX([variables_entry]) self.__lbw = w(-np.inf) self.__ubw = w(np.inf) # prepare dynamics and path constraints entry constraints_entry = (ct.entry('dyn', shape=(self.__nx, ), repeat=self.__N), ) if self.__h is not None: constraints_entry += (ct.entry('h', shape=self.__h.size1_out(0), repeat=self.__N), ) if self.__gnl is not None: constraints_entry += (ct.entry('g', shape=self.__gnl.size1_out(0), repeat=self.__N), ) # create general constraints structure g_struct = ct.struct_symMX([ constraints_entry, ]) # create symbolic constraint expressions map_args = collections.OrderedDict() map_args['x0'] = ct.horzcat(*w['x']) map_args['p'] = ct.horzcat(*w['u']) # evaluate function dynamics F_constr = ct.horzsplit( self.__F.map(self.__N, self.__parallelization)(**map_args)['xf']) # generate constraints constr = collections.OrderedDict() constr['dyn'] = [ a - b for a, b in zip(F_constr, w['x', 1:] + [w['x', 0]]) ] if 'us' in self.__vars: map_args['us'] = ct.horzcat(*w['us']) if self.__h is not None: constr['h'] = ct.horzsplit( self.__h.map(self.__N, self.__parallelization)(*map_args.values())) if self.__gnl is not None: constr['g'] = ct.horzsplit( self.__gnl.map(self.__N, self.__parallelization)(*map_args.values())) # interleaving of constraints repeated_constr = list( itertools.chain.from_iterable(zip(*constr.values()))) # fill in constraint structure self.__g = g_struct(ca.vertcat(*repeated_constr)) # constraint bounds self.__lbg = g_struct(np.zeros(self.__g.shape)) self.__ubg = g_struct(np.zeros(self.__g.shape)) if self.__h is not None: self.__ubg['h', :] = np.inf # nlp cost cost_map_fun = self.__cost.map(self.__N, self.__parallelization) f = ca.sum2(cost_map_fun(map_args['x0'], map_args['p'])) # add phase fixing cost self.__construct_phase_fixing_cost() alpha = ca.MX.sym('alpha') x0star = ca.MX.sym('x0star', self.__nx, 1) f += self.__phase_fix_fun(alpha, x0star, w['x', 0]) # add slack regularization # if 'us' in self.__vars: # f += self.__reg_slack*ct.mtimes(ct.vertcat(*w['us']).T,ct.vertcat(*w['us'])) # NLP parameters p = ca.vertcat(alpha, x0star) self.__w = w self.__g_fun = ca.Function('g_fun', [w, p], [self.__g]) # create IP-solver prob = {'f': f, 'g': self.__g, 'x': w, 'p': p} opts = {'ipopt': {'linear_solver': 'ma57'}, 'expand': False} if Logger.logger.getEffectiveLevel() > 10: opts['ipopt']['print_level'] = 0 opts['print_time'] = 0 opts['ipopt']['sb'] = 'yes' self.__solver = ca.nlpsol('solver', 'ipopt', prob, opts) # create SQP-solver prob['lbg'] = self.__lbg prob['ubg'] = self.__ubg self.__sqp_solver = sqp_method.Sqp(prob) return None
def sum_column(matrix): """ the equivalent to np.sum(matrix, axis=1) """ return ca.sum2(matrix)
def __get_all_penalties(self, nlp, penalties): def format_target(target_in): target_out = [] if target_in is not None: if len(target_in.shape) == 2: target_out = target_in[:, penalty.node_idx.index(idx)] elif len(target_in.shape) == 3: target_out = target_in[:, :, penalty.node_idx.index(idx)] else: raise NotImplementedError( "penalty target with dimension != 2 or 3 is not implemented yet" ) return target_out def get_x_and_u_at_idx(_penalty, _idx): if _penalty.transition: ocp = self.ocp _x = horzcat(ocp.nlp[_penalty.phase_pre_idx].X[-1][:, 0], ocp.nlp[_penalty.phase_post_idx].X[0][:, 0]) _u = horzcat(ocp.nlp[_penalty.phase_pre_idx].U[-1][:, 0], ocp.nlp[_penalty.phase_post_idx].U[0][:, 0]) else: if _penalty.integrate: _x = nlp.X[_idx] _u = nlp.U[_idx][:, 0] if _idx < len(nlp.U) else [] else: _x = nlp.X[_idx][:, 0] _u = nlp.U[_idx][:, 0] if _idx < len(nlp.U) else [] if _penalty.derivative or _penalty.explicit_derivative: _x = horzcat(_x, nlp.X[_idx + 1][:, 0]) _u = horzcat( _u, nlp.U[_idx + 1][:, 0] if _idx + 1 < len(nlp.U) else []) return _x, _u param = self.ocp.cx(self.ocp.v.parameters_in_list.cx) out = self.ocp.cx() for penalty in penalties: if not penalty: continue if penalty.multi_thread: if penalty.target is not None and len( penalty.target.shape) != 2: raise NotImplementedError( "multi_thread penalty with target shape != [n x m] is not implemented yet" ) target = penalty.target if penalty.target is not None else [] x = nlp.cx() u = nlp.cx() for idx in penalty.node_idx: x_tp, u_tp = get_x_and_u_at_idx(penalty, idx) x = horzcat(x, x_tp) u = horzcat(u, u_tp) if (penalty.derivative or penalty.explicit_derivative or penalty.node[0] == Node.ALL ) and nlp.control_type == ControlType.CONSTANT: u = horzcat(u, u[:, -1]) p = reshape( penalty.weighted_function(x, u, param, penalty.weight, target, penalty.dt), -1, 1) else: p = self.ocp.cx() for idx in penalty.node_idx: target = format_target(penalty.target) if np.isnan(np.sum(target)): continue if not nlp: x = [] u = [] else: x, u = get_x_and_u_at_idx(penalty, idx) p = vertcat( p, penalty.weighted_function(x, u, param, penalty.weight, target, penalty.dt)) out = vertcat(out, sum2(p)) return out
def fit(cls, x, y, k=3, monotonicity=0, curvature=0, num_test_points=100, epsilon=1e-7, delta=1e-4, interior_pts=None): """ fit() returns a tck tuple like scipy.interpolate.splrep, but adjusts the weights to meet the desired constraints to the curvature of the spline curve. :param monotonicity: - is an integer, magnitude is ignored - if positive, causes spline to be monotonically increasing - if negative, causes spline to be monotonically decreasing - if 0, leaves spline monotonicity unconstrained :param curvature: - is an integer, magnitude is ignored - if positive, causes spline curvature to be positive (convex) - if negative, causes spline curvature to be negative (concave) - if 0, leaves spline curvature unconstrained :param num_test_points: - sets the number of points that the constraints will be applied at across the range of the spline :param epsilon: - offset of monotonicity and curvature constraints from zero, ensuring strict monotonicity - if epsilon is set to less than the tolerance of the solver, errors will result :param delta: - amount the first and last knots are extended outside the range of the splined points - ensures that the spline evaluates correctly at the first and last nodes, as well as the distance delta beyond these nodes :param interior_pts: - optional list of interior knots to use :returns: A tuple of spline knots, weights, and order. """ x = np.asarray(x) y = np.asarray(y) N = len(x) if interior_pts is None: # Generate knots: This algorithm is based on the Fitpack algorithm by p.dierckx # The original code lives here: http://www.netlib.org/dierckx/ if k % 2 == 1: interior_pts = x[k // 2 + 1:-k // 2] else: interior_pts = (x[k // 2 + 1:-k // 2] + x[k // 2:-k // 2 - 1]) / 2 t = np.concatenate( (np.full(k + 1, x[0] - delta), interior_pts, np.full(k + 1, x[-1] + delta))) num_knots = len(t) # Casadi Variable Symbols c = SX.sym('c', num_knots) x_sym = SX.sym('x') # Casadi Representation of Spline Function & Derivatives expr = cls(t, c, k)(x_sym) free_vars = [c, x_sym] bspline = Function('bspline', free_vars, [expr]) J = jacobian(expr, x_sym) # bspline_prime = Function('bspline_prime', free_vars, [J]) H = jacobian(J, x_sym) bspline_prime_prime = Function('bspline_prime_prime', free_vars, [H]) # Objective Function xpt = SX.sym('xpt') ypt = SX.sym('ypt') sq_diff = Function('sq_diff', [xpt, ypt], [ (ypt - bspline(c, xpt))**2]) sq_diff = sq_diff.map(N, 'serial') f = sum2(sq_diff(SX(x), SX(y))) # Setup Curvature Constraints delta_c_max = np.full(num_knots - 1, inf) delta_c_min = np.full(num_knots - 1, -inf) max_slope_slope = np.full(num_test_points, inf) min_slope_slope = np.full(num_test_points, -inf) if monotonicity != 0: if monotonicity < 0: delta_c_max = np.full(num_knots - 1, -epsilon) else: delta_c_min = np.full(num_knots - 1, epsilon) if curvature != 0: if curvature < 0: max_slope_slope = np.full(num_test_points, -epsilon) else: min_slope_slope = np.full(num_test_points, epsilon) monotonicity_constraints = vertcat(*[ c[i + 1] - c[i] for i in range(num_knots - 1)]) x_linspace = np.linspace(x[0], x[-1], num_test_points) curvature_constraints = vertcat(*[ bspline_prime_prime(c, SX(x)) for x in x_linspace]) g = vertcat(monotonicity_constraints, curvature_constraints) lbg = np.concatenate((delta_c_min, min_slope_slope)) ubg = np.concatenate((delta_c_max, max_slope_slope)) # Perform mini-optimization problem to calculate the the values of c nlp = {'x': c, 'f': f, 'g': g} my_solver = "ipopt" solver = nlpsol("solver", my_solver, nlp, {'print_time': 0, 'expand': True, 'ipopt': {'print_level': 0}}) sol = solver(lbg=lbg, ubg=ubg) stats = solver.stats() return_status = stats['return_status'] if return_status not in ['Solve_Succeeded', 'Solved_To_Acceptable_Level', 'SUCCESS']: raise Exception("Spline fitting failed with status {}".format(return_status)) # Return the new tck tuple return (t, np.array(sol['x']).ravel(), k)
def __get_all_penalties(self, nlp: NonLinearProgram, penalties): """ Parse the penalties of the full ocp to a Ipopt-friendly one Parameters ---------- nlp: NonLinearProgram The nonlinear program to parse the penalties from penalties: The penalties to parse Returns ------- """ def format_target(target_in: np.array) -> np.array: """ Format the target of a penalty to a numpy array Parameters ---------- target_in: np.array The target of the penalty Returns ------- np.array The target of the penalty formatted to a numpy array """ if len(target_in.shape) == 2: target_out = target_in[:, penalty.node_idx.index(idx)] elif len(target_in.shape) == 3: target_out = target_in[:, :, penalty.node_idx.index(idx)] else: raise NotImplementedError( "penalty target with dimension != 2 or 3 is not implemented yet" ) return target_out def get_x_and_u_at_idx(_penalty, _idx): if _penalty.transition: ocp = self.ocp _x = vertcat(ocp.nlp[_penalty.phase_pre_idx].X[-1], ocp.nlp[_penalty.phase_post_idx].X[0][:, 0]) _u = vertcat(ocp.nlp[_penalty.phase_pre_idx].U[-1], ocp.nlp[_penalty.phase_post_idx].U[0]) elif _penalty.multinode_constraint: ocp = self.ocp _x = vertcat( ocp.nlp[_penalty.phase_first_idx].X[_penalty.node_idx[0]], ocp.nlp[_penalty.phase_second_idx].X[_penalty.node_idx[1]] [:, 0], ) # Make an exception to the fact that U is not available for the last node mod_u0 = 1 if _penalty.first_node == Node.END else 0 mod_u1 = 1 if _penalty.second_node == Node.END else 0 _u = vertcat( ocp.nlp[_penalty.phase_first_idx].U[_penalty.node_idx[0] - mod_u0], ocp.nlp[_penalty.phase_second_idx].U[_penalty.node_idx[1] - mod_u1], ) elif _penalty.integrate: _x = nlp.X[_idx] _u = nlp.U[_idx][:, 0] if _idx < len(nlp.U) else [] else: _x = nlp.X[_idx][:, 0] _u = nlp.U[_idx][:, 0] if _idx < len(nlp.U) else [] if _penalty.derivative or _penalty.explicit_derivative: _x = horzcat(_x, nlp.X[_idx + 1][:, 0]) _u = horzcat( _u, nlp.U[_idx + 1][:, 0] if _idx + 1 < len(nlp.U) else []) if _penalty.integration_rule == IntegralApproximation.TRAPEZOIDAL: _x = horzcat(_x, nlp.X[_idx + 1][:, 0]) if nlp.control_type == ControlType.LINEAR_CONTINUOUS: _u = horzcat( _u, nlp.U[_idx + 1][:, 0] if _idx + 1 < len(nlp.U) else []) if _penalty.integration_rule == IntegralApproximation.TRUE_TRAPEZOIDAL: if nlp.control_type == ControlType.LINEAR_CONTINUOUS: _u = horzcat( _u, nlp.U[_idx + 1][:, 0] if _idx + 1 < len(nlp.U) else []) return _x, _u param = self.ocp.cx(self.ocp.v.parameters_in_list.cx) out = self.ocp.cx() for penalty in penalties: if not penalty: continue if penalty.multi_thread: if penalty.target is not None and len( penalty.target[0].shape) != 2: raise NotImplementedError( "multi_thread penalty with target shape != [n x m] is not implemented yet" ) target = penalty.target if penalty.target is not None else [] x = nlp.cx() u = nlp.cx() for idx in penalty.node_idx: x_tp, u_tp = get_x_and_u_at_idx(penalty, idx) x = horzcat(x, x_tp) u = horzcat(u, u_tp) if (penalty.derivative or penalty.explicit_derivative or penalty.node[0] == Node.ALL ) and nlp.control_type == ControlType.CONSTANT: u = horzcat(u, u[:, -1]) p = reshape( penalty.weighted_function(x, u, param, penalty.weight, target, penalty.dt), -1, 1) else: p = self.ocp.cx() for idx in penalty.node_idx: if penalty.target is None: target = [] elif (penalty.integration_rule == IntegralApproximation.TRAPEZOIDAL or penalty.integration_rule == IntegralApproximation.TRUE_TRAPEZOIDAL): target0 = format_target(penalty.target[0]) target1 = format_target(penalty.target[1]) target = np.vstack((target0, target1)).T else: target = format_target(penalty.target[0]) if np.isnan(np.sum(target)): continue if not nlp: x = [] u = [] else: x, u = get_x_and_u_at_idx(penalty, idx) p = vertcat( p, penalty.weighted_function(x, u, param, penalty.weight, target, penalty.dt)) out = vertcat(out, sum2(p)) return out
def init_solver(self, cost_func=None): """ Generate the exloration NLP in casadi Parameters ---------- T: int, optional the safempc horizon """ u_0 = MX.sym("init_control", (self.n_u, 1)) k_ff_all = MX.sym("feed-forward control", (self.T - 1, self.n_u)) g = [] lbg = [] ubg = [] g_name = [] p_0 = MX.sym("initial state", (self.n_s, 1)) k_fb_safe_ctrl = MX.sym("Feedback term", (self.n_u, self.n_s)) p_all, q_all, gp_sigma_pred_safe_all = cas_multistep(p_0, u_0, k_fb_safe_ctrl, k_ff_all, self.gp.get_forward_model_casadi(True), self.l_mu, self.l_sigma, self.beta_safety, self.a, self.b, self.lin_trafo_gp_input) # generate open_loop trajectory function [vertcat(x_0,u_0)],[f_x]) self.f_multistep_eval = cas.Function("safe_multistep", [p_0, u_0, k_fb_safe_ctrl, k_ff_all], [p_all, q_all]) g_safe, lbg_safe, ubg_safe, g_names_safe = self.generate_safety_constraints( p_all, q_all, u_0, k_fb_safe_ctrl, k_ff_all) g = vertcat(g, g_safe) lbg += lbg_safe ubg += ubg_safe g_name += g_names_safe if cost_func is None: cost = -sum1(sum2(gp_sigma_pred_safe_all)) else: cost = cost_func(p_all, q_all, gp_sigma_pred_safe_all, u_0, k_ff_all) opt_vars = vertcat(p_0, u_0, k_ff_all.reshape((-1, 1))) opt_params = vertcat(k_fb_safe_ctrl.reshape((-1, 1))) prob = {'f': cost, 'x': opt_vars, 'p': opt_params, 'g': g} opt = {'error_on_fail': False, 'ipopt': {'hessian_approximation': 'exact', "max_iter": 120, "expect_infeasible_problem": "no", \ 'acceptable_tol': 1e-4, "acceptable_constr_viol_tol": 1e-5, "bound_frac": 0.5, "start_with_resto": "no", "required_infeasibility_reduction": 0.85, "acceptable_iter": 8}} # ipopt # opt = {'qpsol':'qpoases','max_iter':120,'hessian_approximation':'exact'}#,"c1":5e-4} #sqpmethod #,'hessian_approximation':'limited-memory' # opt = {'max_iter':120,'qpsol':'qpoases'} self.solver = cas.nlpsol('solver', 'ipopt', prob, opt) self.lbg = lbg self.ubg = ubg