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 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 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
