def test_tau_max_from_actuators(value, threshold):
ocp = prepare_test_ocp(with_actuator=True)
x = [DM.zeros((6, 1)), DM.zeros((6, 1))]
u = [DM.ones((3, 1)) * value, DM.ones((3, 1)) * value]
penalty_type = ConstraintFcn.TORQUE_MAX_FROM_ACTUATORS
penalty = Constraint(penalty_type)
if threshold and threshold < 0:
with pytest.raises(
ValueError,
match="min_torque cannot be negative in tau_max_from_actuators"
):
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []),
min_torque=threshold),
else:
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []),
min_torque=threshold)
val = []
for i in range(len(ocp.nlp[0].g[0])):
val.append(ocp.nlp[0].g[0][i]["val"])
for res in val:
if threshold:
np.testing.assert_almost_equal(
res,
np.repeat([value + threshold, value - threshold],
3)[:, np.newaxis])
else:
np.testing.assert_almost_equal(
res,
np.repeat([value + 5, value - 10], 3)[:, np.newaxis])
def create_quadratic_cost(self, par_dict):
self.L = DM(0)
self.V = DM(0)
if 'x_ref' not in par_dict:
par_dict['x_ref'] = DM.zeros(self.model.n_x)
if 'u_ref' not in par_dict:
par_dict['u_ref'] = DM.zeros(self.model.n_u)
if 'Q' in par_dict:
self.L += mtimes(
mtimes((self.model.x - par_dict['x_ref']).T, par_dict['Q']),
(self.model.x - par_dict['x_ref']))
if 'R' in par_dict:
self.L += mtimes(
mtimes((self.model.u - par_dict['u_ref']).T, par_dict['R']),
(self.model.u - par_dict['u_ref']))
if 'Qv' in par_dict:
self.V += mtimes(
mtimes((self.model.x - par_dict['x_ref']).T, par_dict['Qv']),
(self.model.x - par_dict['x_ref']))
if 'Rv' in par_dict:
self.V += mtimes(mtimes(self.model.x.T, par_dict['Rv']),
self.model.x)
def test_tau_max_from_actuators(value, threshold):
ocp = prepare_test_ocp(with_actuator=True)
t = [0]
x = [DM.zeros((6, 1)), DM.zeros((6, 1))]
u = [DM.ones((3, 1)) * value, DM.ones((3, 1)) * value]
penalty_type = ConstraintFcn.TORQUE_MAX_FROM_Q_AND_QDOT
penalty = Constraint(penalty_type, min_torque=threshold)
if threshold and threshold < 0:
with pytest.raises(
ValueError,
match="min_torque cannot be negative in tau_max_from_actuators"
):
get_penalty_value(ocp, penalty, t, x, u, [])
return
else:
res = get_penalty_value(ocp, penalty, t, x, u, [])
if threshold:
np.testing.assert_almost_equal(
res,
np.repeat([value + threshold, value - threshold], 3)[:,
np.newaxis])
else:
np.testing.assert_almost_equal(
res,
np.repeat([value + 5, value - 10], 3)[:, np.newaxis])
def _sample_parameters(self):
n_samples = self.n_samples
n_uncertain = self.n_uncertain
mean = vertcat(self.socp.p_unc_mean,
self.socp.uncertain_initial_conditions_mean)
if self.socp.n_p_unc > 0 and self.socp.n_uncertain_initial_condition > 0:
covariance = diagcat(self.socp.p_unc_cov,
self.socp.uncertain_initial_conditions_cov)
elif self.socp.n_p_unc > 0:
covariance = self.socp.p_unc_cov
elif self.socp.n_uncertain_initial_condition > 0:
covariance = self.socp.uncertain_initial_conditions_cov
else:
raise ValueError("No uncertainties found n_p_unc = {}, "
"n_uncertain_initial_condition={}".format(
self.socp.n_p_unc,
self.socp.n_uncertain_initial_condition))
dist = self.socp.p_unc_dist + self.socp.uncertain_initial_conditions_distribution
for d in dist:
if not d == 'normal':
raise NotImplementedError(
'Distribution "{}" not implemented, only "normal" is available.'
.format(d))
sampled_epsilon = sample_parameter_normal_distribution_with_sobol(
DM.zeros(mean.shape), DM.eye(covariance.shape[0]), n_samples)
sampled_parameters = SX.zeros(n_uncertain, n_samples)
for s in range(n_samples):
sampled_parameters[:, s] = mean + mtimes(sampled_epsilon[:, s].T,
chol(covariance)).T
return sampled_parameters
def sample_parameter_normal_distribution_with_sobol(mean,
covariance,
n_samples=1):
"""Sample parameter using Sobol sampling with a normal distribution.
:param mean:
:param covariance:
:param n_samples:
:return:
"""
if isinstance(mean, list):
mean = vertcat(*mean)
n_uncertain = mean.size1()
# Uncertain parameter design
sobol_design = sobol_seq.i4_sobol_generate(n_uncertain, n_samples,
math.ceil(np.log2(n_samples)))
sobol_samples = DM(sobol_design.T)
for i in range(n_uncertain):
sobol_samples[i, :] = norm(loc=0., scale=1).ppf(sobol_samples[i, :])
unscaled_sample = DM.zeros(n_uncertain, n_samples)
for i in range(n_samples):
unscaled_sample[:, i] = mean + mtimes(
chol(covariance).T, sobol_samples[:, i])
return unscaled_sample
def blockdiag(*matrices_list):
"""Receives a list of matrices and return a block diagonal.
:param DM|MX|SX matrices_list: list of matrices
"""
size_1 = sum([m.size1() for m in matrices_list])
size_2 = sum([m.size2() for m in matrices_list])
matrix_types = [type(m) for m in matrices_list]
if SX in matrix_types and MX in matrix_types:
raise ValueError(
"Can not mix MX and SX types. Types give: {}".format(matrix_types))
if SX in matrix_types:
matrix = SX.zeros(size_1, size_2)
elif MX in matrix_types:
matrix = MX.zeros(size_1, size_2)
else:
matrix = DM.zeros(size_1, size_2)
index_1 = 0
index_2 = 0
for m in matrices_list:
matrix[index_1:index_1 + m.size1(), index_2:index_2 + m.size2()] = m
index_1 += m.size1()
index_2 += m.size2()
return matrix
def include_algebraic(self,
var,
alg=None,
y_min=None,
y_max=None,
y_guess=None):
if y_min is None:
y_min = -DM.inf(var.numel())
if y_max is None:
y_max = DM.inf(var.numel())
if isinstance(y_min, list):
y_min = vertcat(*y_min)
if isinstance(y_max, list):
y_max = vertcat(*y_max)
if not var.numel() == y_min.numel():
raise ValueError(
"Given 'var' and 'y_min' does not have the same size, {}!={}".
format(var.numel(), y_min.numel()))
if not var.numel() == y_max.numel():
raise ValueError(
"Given 'var' and 'y_max' does not have the same size, {}!={}".
format(var.numel(), y_max.numel()))
if self.y_guess is not None:
if y_guess is None:
self.y_guess = vertcat(self.y_guess, DM.zeros(var.numel()))
else:
self.y_guess = vertcat(self.y_guess, y_guess)
self.model.include_algebraic(var, alg)
self.y_min = vertcat(self.y_min, y_min)
self.y_max = vertcat(self.y_max, y_max)
def create_initial_guess(self, p=None, theta=None):
"""Create an initial guess for the optimal control problem using problem.x_0, problem.y_guess, problem.u_guess,
and a given p and theta (for p_opt and theta_opt) if they are given.
If y_guess or u_guess are None the initial guess uses a vector of zeros of appropriate size.
:param p: Optimization parameters
:param theta: Optimization theta
:return:
"""
x_init = repmat(self.problem.x_0,
(self.degree + 1) * self.finite_elements)
if self.problem.y_guess is not None:
y_init = repmat(self.problem.y_guess,
self.degree * self.finite_elements)
else:
y_init = repmat(DM.zeros(self.model.n_y),
self.degree * self.finite_elements)
if self.model.n_u_par > 0:
if self.problem.u_guess is not None:
u_init = repmat(self.problem.u_guess,
self.degree_control * self.finite_elements)
else:
u_init = repmat(DM.zeros(self.model.n_u),
self.degree_control * self.finite_elements)
else:
u_init = []
eta_init = DM.zeros(self.problem.n_eta, 1)
p_opt_init = DM.zeros(self.problem.n_p_opt, 1)
theta_opt_init = DM.zeros(
self.problem.n_theta_opt * self.finite_elements, 1)
if p is not None:
for k, ind in enumerate(self.problem.get_p_opt_indices()):
p_opt_init[k] = p[ind]
if theta is not None:
for el in range(self.finite_elements):
for k, ind in enumerate(self.problem.get_theta_opt_indices()):
theta_opt_init[k + el *
self.problem.n_theta_opt] = theta[el][ind]
return vertcat(x_init, y_init, u_init, eta_init, p_opt_init,
theta_opt_init)
def include_time_chance_inequality(self,
ineq,
prob,
rhs=None,
when='default'):
r"""Include time dependent chance inequality.
Prob[ineq(..., t) <= rhs] <= prob, for t \in [t_0, t_f]
The inequality is concatenated to "g_stochastic_ineq"
:param ineq: inequality
:param list|DM rhs: Right-hand size of the inequality
:param list|DM prob: Chance/probability of the constraint being satisfied
:param str when: Can be 'default', 'end', 'start'.
'start' - the constraint will be evaluated at the start of every finite element
'end' - the constraint will be evaluated at the end of every finite element
'default' - will be evaluated at each collocation point of every finite element.
For the multiple shooting, the constraint will be evaluated at the end of each
finite element
"""
if isinstance(ineq, list):
ineq = vertcat(*ineq)
if isinstance(rhs, list):
rhs = vertcat(*rhs)
if isinstance(prob, list):
prob = vertcat(*prob)
if isinstance(rhs, (float, int)):
rhs = vertcat(rhs)
if isinstance(prob, (float, int)):
prob = vertcat(prob)
if rhs is None:
rhs = DM.zeros(ineq.shape)
if not ineq.numel() == rhs.numel():
raise ValueError(
'Given inequality does not have the same dimensions of the provided "rhs". '
'Size of ineq: {}, size of rhs: {}'.format(
ineq.shape, rhs.shape))
if not ineq.numel() == rhs.numel():
raise ValueError(
'Given inequality does not have the same dimensions of the provided "prob". '
'Size of ineq: {}, size of prob: {}'.format(
ineq.shape, prob.shape))
self.g_stochastic_ineq = vertcat(self.g_stochastic_ineq, ineq)
self.g_stochastic_rhs = vertcat(self.g_stochastic_rhs, rhs)
self.g_stochastic_prob = vertcat(self.g_stochastic_prob, prob)
def include_state(self,
var,
ode=None,
x_0=None,
x_min=None,
x_max=None,
h_initial=None,
x_0_sym=None,
suppress=False):
if x_0 is not None:
x_0 = vertcat(x_0)
if x_min is None:
x_min = -DM.inf(var.numel())
if x_max is None:
x_max = DM.inf(var.numel())
if x_0 is None and h_initial is None and not suppress:
raise Exception('No initial condition given')
var = vertcat(var)
x_min = vertcat(x_min)
x_max = vertcat(x_max)
if not var.numel() == x_max.numel():
raise ValueError('Size of "x" and "x_max" differ. x.numel()={} '
'and x_max.numel()={}'.format(
var.numel(), x_max.numel()))
if not var.numel() == x_min.numel():
raise ValueError('Size of "x" and "x_min" differ. x.numel()={} '
'and x_min.numel()={}'.format(
var.numel(), x_min.numel()))
if not var.numel() == x_min.numel():
raise ValueError('Size of "x" and "x_0" differ. x.numel()={} '
'and x_0.numel()={}'.format(
var.numel(), x_0.numel()))
x_0_sym = self.model.include_state(var, ode, x_0_sym)
if x_0 is not None:
self.x_0 = vertcat(self.x_0, x_0)
h_initial = x_0_sym - var
else:
x_0 = DM.zeros(var.shape)
self.x_0 = vertcat(self.x_0, x_0)
if h_initial is not None:
self.h_initial = vertcat(self.h_initial, h_initial)
self.x_min = vertcat(self.x_min, x_min)
self.x_max = vertcat(self.x_max, x_max)
def get_states(self, t_k, y_k, u_k):
"""Get states out of a measurement.
:param DM t_k: time of the measurement
:param DM y_k: measurements
:param DM u_k: controls
:return: DM
"""
if self.estimator is None:
x_k = y_k[:self.solution_method.model.n_x]
cov_x_k = DM.zeros(x_k.shape)
else:
x_k, cov_x_k = self.estimator.estimate(t_k, y_k, u_k)
if self.include_cost_in_state_vector:
x_k = vertcat(x_k, 0)
return x_k, cov_x_k
def calculate_optimal_control(self):
dd_h_dudu, d_h_du = hessian(self.problem.H, self.model.u)
if is_equal(dd_h_dudu, DM.zeros(self.model.n_u, self.model.n_u)):
# TODO: Implement the case where the controls are linear on the Hamiltonian ("Bang-Bang" control)
raise Exception(
'The Hamiltonian "H" is not strictly convex with respect to the control "u". '
+ 'The obtained hessian d^2 H/du^2 = 0')
# if not ddH_dudu.is_constant():
# raise NotImplementedError('The Hessian of the Hamiltonian with respect to "u" is not constant,
# this case has not been implemented')
u_opt = -mtimes(inv(dd_h_dudu), substitute(d_h_du, self.model.u, 0))
for i in range(self.model.n_u):
if not self.problem.u_min[i] == -inf:
u_opt[i] = fmax(u_opt[i], self.problem.u_min[i])
if not self.problem.u_max[i] == inf:
u_opt[i] = fmin(u_opt[i], self.problem.u_max[i])
return u_opt
def runge_kutta_4th_order(
x0=DM.zeros(n_states, 1), p=None, n_iter=iterations):
if n_iter < 1:
raise Exception(
"The given number of Runge Kutta iterations is less than one, given {}"
.format(n_iter))
if p is None:
p = []
x_f = x0
h = (t_f - t_0) / n_iter
t = t_0
for it in range(n_iter):
k1 = h * f(t, x0, p)
k2 = h * f(t + 0.5 * h, x0 + 0.5 * k1, p)
k3 = h * f(t + 0.5 * h, x0 + 0.5 * k2, p)
k4 = h * f(t + h, x0 + k3, p)
x_f = x0 + 1 / 6. * k1 + 1 / 3. * k2 + 1 / 3. * k3 + 1 / 6. * k4
x0 = x_f
t += h
return {'xf': x_f, 'zf': DM([])}
def include_equality(self, expr, rhs=None):
"""Include a equality with the following form
expr = rhs
:param expr: expression, this is the only term that should contain symbolic variables
:param rhs: right hand side, by default it is a vector of zeros with same size of expr. If the 'expr' size is
greater than one but a scalar is passed as 'rhs', a vector of 'rhs' with size of 'expr' will be used as
right hand side. (default = [0]*size)
"""
if isinstance(expr, list):
expr = vertcat(expr)
if expr.size2() > 1:
raise Exception(
"Given expression is not a vector, number of columns is {}".
format(expr.size2()))
if rhs is None:
rhs = DM.zeros(expr.shape)
else:
rhs = vertcat(rhs)
if rhs.numel() == 1 and expr.numel() > 1:
rhs = repmat(rhs, expr.numel())
if not expr.shape == rhs.shape:
msg = "Expression and the right hand side does not have the same size: " \
"expr.shape={}, rhs.shape=={}".format(expr.shape, rhs.shape)
raise ValueError(msg)
# check for if rhs have 'x's and 'p's
if depends_on(rhs, vertcat(self.x, self.p)):
raise ValueError(
"Right-hand side cannot contain variables from the optimization problem. "
"RHS = {}".format(rhs))
self.g = vertcat(self.g, expr)
self.g_lb = vertcat(self.g_lb, rhs)
self.g_ub = vertcat(self.g_ub, rhs)
def custom_with_bounds(ocp, nlp, t, x, u, p):
my_values = DM.zeros((12, 1)) + x[0]
return -10, my_values, 10
def create_initial_guess_with_simulation(self, u=None, p=None, theta=None):
"""Create an initial guess for the optimal control problem using by simulating with a given control u,
and a given p and theta (for p_opt and theta_opt) if they are given.
If no u is given the value of problem.u_guess is used, or problem.last_u, then a vector of zeros of appropriate
size is used.
If no p or theta is given, an vector of zeros o appropriate size is used.
:param u: Control initial guess
:param p: Optimization parameters
:param theta: Optimization theta
:return:
"""
x_init = []
y_init = []
u_init = []
# Simulation
if u is None:
if self.problem.u_guess is not None:
u = self.problem.u_guess
elif self.problem.last_u is not None:
u = self.problem.last_u
else:
u = DM.zeros(self.model.n_u)
if self.model.n_u_par > 0:
u = vec(horzcat(*[u] * self.degree_control))
else:
u = []
x_0 = self.problem.x_0
y_guess = self.problem.y_guess
x_init.append([x_0])
for el in range(self.finite_elements):
el_x = []
el_y = []
# get DAE system and remove tau
dae_sys = self.model.get_dae_system()
dae_sys.convert_from_tau_to_time(t_k=self.time_breakpoints[el],
t_kp1=self.time_breakpoints[el +
1])
# Prepare for loop
t_init = self.time_breakpoints[el]
p_el = vertcat(p, theta[el], u)
for t in [self.time_breakpoints[el + 1]]:
res = dae_sys.simulate(x_0=x_0,
t_0=t_init,
t_f=t,
p=p_el,
y_0=y_guess)
el_x.append(res['xf'])
el_y.append(res['zf'])
t_init = t
x_0 = res['xf']
x_init.append(el_x)
y_init.append(el_y)
u_init.append(u)
x_init = self.vectorize(x_init)
y_init = self.vectorize(y_init)
u_init = self.vectorize(u_init)
# Other variables
eta_init = DM.zeros(self.problem.n_eta, 1)
p_opt_init = DM.zeros(self.problem.n_p_opt, 1)
theta_opt_init = DM.zeros(
self.problem.n_theta_opt * self.finite_elements, 1)
if p is not None:
for k, ind in enumerate(self.problem.get_p_opt_indices()):
p_opt_init[k] = p[ind]
if theta is not None:
for el in range(self.finite_elements):
for k, ind in enumerate(self.problem.get_theta_opt_indices()):
theta_opt_init[k + el *
self.problem.n_theta_opt] = theta[el][ind]
return vertcat(x_init, y_init, u_init, eta_init, p_opt_init,
theta_opt_init)
def __init__(self, model, x_0, **kwargs):
"""
Plant which uses a SystemModel.simulate to obtain the measurements.
:param SystemModel model: simulation model
:param DM x_0: initial condition
:param DM t_s: (default: 1) sampling time
:param DM u: (default: 0) initial control
:param DM y_guess: initial guess for algebraic variables for simulation
:param DM t_0: (default: 0) initial time
:param dict integrator_options: integrator options
:param bool has_noise: Turn on/off the process/measurement noise
:param DM r_n: Measurement noise covariance matrix
:param DM r_v: Process noise covariance matrix
:param noise_seed: Seed for the random number generator used to create noise. Use the same seed for the
repeatability in the experiments.
"""
Plant.__init__(self)
self.model = model
self.name = self.model.name
self.x = x_0
self.u = DM.zeros(self.model.n_u)
self.y_guess = None
self.t = 0.
self.t_s = 1.
self.c_matrix = None
self.d_matrix = None
self.p = None
self.theta = None
# Noise
self.has_noise = False
self.r_n = DM(0.)
self.r_v = DM(0.)
self.noise_seed = None
# Options
self.integrator_options = None
self.verbosity = 1
self.dataset = DataSet(name='Plant')
if 't_0' in kwargs:
self.t = kwargs['t_0']
for (k, v) in kwargs.items():
setattr(self, k, v)
if self.c_matrix is None:
self.c_matrix = DM.eye(self.model.n_x + self.model.n_y)
if self.d_matrix is None:
self.d_matrix = DM(0.)
self._initialize_dataset()
if self.has_noise:
if self.noise_seed is not None:
numpy.random.seed(self.noise_seed)
def custom_with_bounds(pn):
my_values = DM.zeros((12, 1)) + x[0]
return -10, my_values, 10
def custom(pn, mult):
my_values = DM.zeros((12, 1)) + pn.x[0] * mult
return my_values
def call_solver(self,
initial_guess=None,
p=None,
theta=None,
x_0=None,
last_u=None,
initial_guess_dict=None):
if self.opt_problem is None:
self.create_optimization_problem()
# initial conditions
if x_0 is None:
x_0 = self.problem.x_0
if isinstance(x_0, list):
x_0 = vertcat(x_0)
if not vertcat(x_0).numel() == self.model.n_x:
raise Exception(
'Size of given x_0 (or obtained from problem.x_0) is different from model.n_x, '
'x_0.numel() = {}, model.n_x = {}'.format(
vertcat(x_0).numel(), self.model.n_x))
# parameters
if p is None:
if self.problem.n_p_opt == self.model.n_p:
p = DM.zeros(self.problem.n_p_opt)
elif self.problem.model.n_p > 0:
raise Exception(
"A parameter 'p' of size {} should be given".format(
self.problem.model.n_p))
if isinstance(p, list):
p = DM(p)
# theta
if theta is None:
if self.problem.n_theta_opt == self.model.n_theta:
theta = create_constant_theta(0, self.problem.n_theta_opt,
self.finite_elements)
elif self.problem.model.n_theta > 0:
raise Exception(
"A parameter 'theta' of size {} should be given".format(
self.problem.model.n_theta))
# Prepare NLP parameter vector
theta_vector, par_x_0, par_last_u = [], [], []
if theta is not None:
theta_vector = vertcat(
*[theta[i] for i in range(self.finite_elements)])
if self.initial_condition_as_parameter:
par_x_0 = x_0
# last control
if not self.last_control_as_parameter and last_u is not None:
raise warnings.warn(
'solution_method.last_control_as_parameter is False, but last_u was passed. last_u will'
' be ignored.')
else:
if last_u is not None:
if isinstance(last_u, list):
last_u = vertcat(*last_u)
elif self.problem.last_u is not None:
last_u = self.problem.last_u
elif self.last_control_as_parameter and last_u is None:
raise Exception(
'last_control_as_parameter is True, but no "last_u" was passed and the "ocp.last_u" is '
'None.')
if self.last_control_as_parameter:
par_last_u = last_u
par = vertcat(p, theta_vector, par_x_0, par_last_u)
if initial_guess_dict is None:
if initial_guess is None:
if self.initial_guess_heuristic == 'simulation':
initial_guess = self.discretizer.create_initial_guess_with_simulation(
p=p, theta=theta)
elif self.initial_guess_heuristic == 'problem_info':
initial_guess = self.discretizer.create_initial_guess(
p, theta)
else:
raise ValueError(
'initial_guess_heuristic did not recognized, available options: "simulation" and '
'"problem_info". Given: {}'.format(
self.initial_guess_heuristic))
args = dict(initial_guess=initial_guess, p=par)
else:
args = dict(initial_guess=initial_guess_dict['x'],
p=par,
lam_x=initial_guess_dict['lam_x'],
lam_g=initial_guess_dict['lam_g'])
sol = self.opt_problem.solve(**args)
return sol, p, theta, x_0, last_u
nl['lbg'][:3 * (N_pred + 1):3] = [h_lb] * (N_pred + 1)
nl['ubg'][:3 * (N_pred + 1):3] = [h_ub] * (N_pred + 1)
nl['lbg'][1:3 * (N_pred + 1):3] = [-inf] * (N_pred + 1)
nl['ubg'][1:3 * (N_pred + 1):3] = [inf] * (N_pred + 1)
nl['lbg'][2:3 * (N_pred + 1):3] = [eta_lb] * (N_pred + 1)
nl['ubg'][2:3 * (N_pred + 1):3] = [eta_ub] * (N_pred + 1)
nl['lbg'][-1] = -inf
nl['ubg'][-1] = 500
print(nl['ubg'])
nl['p'] = [0, 0, 49, 0.9, 0, 48]
nl['x0'] = [0 for i in range(N_pred)]
sol = solver(**nl)
temp_xn = DM.zeros(3, 1)
temp_xn[0, 0] = sol['g'][3 * (N_pred)]
temp_xn[1, 0] = sol['g'][3 * (N_pred) + 1]
temp_xn[2, 0] = sol['g'][3 * (N_pred) + 2]
print(temp_xn)
print(temp_xn - nl['p'][3:6])
print((temp_xn - nl['p'][3:6]).T @ P_f @ (temp_xn - nl['p'][3:6]))
print(sol['g'][3 * (N_pred + 1)])
# print(type(float((sol['g'][2]))))
print(sol)
# print(sol['x'].shape)
eta_out = []
h_out = []
t = [T * i for i in range(N_pred)]
def _priori_update(self, x_mean, x_cov, u, p, theta):
x_samples = self._get_sampled_states(x_mean, x_cov)
# Perform predictions via simulation
simulation_results = []
x_cal_x_k_at_k_minus_1 = []
y_alg_cal_x_k_at_k_minus_1 = []
for s in range(self.n_samples):
x_0_i = DM(x_samples[s])
simulation_results_i = self.model.simulate(x_0=x_0_i,
t_0=self.t,
t_f=self.t + self.t_s,
u=u,
p=p,
theta=theta,
y_0=self.y_guess)
simulation_results.append(simulation_results_i)
x_cal_x_k_at_k_minus_1.append(
simulation_results_i.final_condition()[0])
y_alg_cal_x_k_at_k_minus_1.append(
simulation_results[s].final_condition()[1])
# fit the polynomial for x
a_x = []
x_hat_k_minus = []
for i in range(self.model.n_x):
x_i_vector = vertcat(
*[x_cal_x_k_at_k_minus_1[s][i] for s in range(self.n_samples)])
a_x.append(mtimes(self._ls_factor, x_i_vector))
# get the mean for x
for i in range(self.model.n_x):
x_hat_k_minus.append(a_x[i][0])
x_hat_k_minus = vertcat(*x_hat_k_minus)
# get the covariance for x
p_k_minus = DM.zeros(self.model.n_x, self.model.n_x)
for i in range(self.model.n_x):
for j in range(self.model.n_x):
p_k_minus[i, j] = sum(
[a_x[i][k] * a_x[j][k]
for k in range(1, self.n_samples)]) + self.r_v[i, j]
# calculate the measurement for each sample
y_cal_k_at_k_minus_1 = []
for s in range(self.n_samples):
y_cal_k_at_k_minus_1.append(
self._get_measurement_from_prediction(
x_cal_x_k_at_k_minus_1[s], y_alg_cal_x_k_at_k_minus_1[s],
u))
n_meas = y_cal_k_at_k_minus_1[0].numel()
# find the measurements estimate
a_meas = []
y_meas_hat_k_minus = []
for i in range(n_meas):
y_meas_i_vector = vertcat(
*[y_cal_k_at_k_minus_1[s][i] for s in range(self.n_samples)])
a_meas.append(mtimes(self._ls_factor, y_meas_i_vector))
# get the mean for the measurement
for i in range(n_meas):
y_meas_hat_k_minus.append(a_meas[i][0])
y_meas_hat_k_minus = vertcat(*y_meas_hat_k_minus)
# get the covariance for the meas
p_yk_yk = DM.zeros(n_meas, n_meas)
for i in range(n_meas):
for j in range(n_meas):
p_yk_yk[i, j] = sum([
a_meas[i][k] * a_meas[j][k]
for k in range(1, self.n_samples)
])
p_yk_yk = p_yk_yk + self.r_n
# get cross-covariance
p_xk_yk = DM.zeros(self.model.n_x, n_meas)
for i in range(self.model.n_x):
for j in range(n_meas):
p_xk_yk[i, j] = sum([
a_x[i][k] * a_meas[j][k] for k in range(1, self.n_samples)
])
# k_gain = mtimes(p_xk_yk, inv(p_yk_yk))
k_gain = solve(p_yk_yk.T, p_xk_yk.T).T
return x_hat_k_minus, p_k_minus, y_meas_hat_k_minus, p_yk_yk, k_gain
def custom_no_mult(ocp, nlp, t, x, u, p):
my_values = DM.zeros((12, 1)) + x[0]
return my_values
def objective(x,y,p,N,params):
nPrimal = x.numel() #number of primal sln
nDual = y['lam_g'].numel() #number of dual
nParam = p.numel() #number of parameters
#Loading initial states and controls
data = spio.loadmat('CstrDistXinit.mat', squeeze_me = True)
Xinit = data['Xinit']
xf = Xinit[0:84]
u_opt = Xinit[84:]
#Model parameters
NT = params['dist']['NT'] #Stages in column
Uf = 0.3 #Feed rate to CSTR F_0
_,state,xdot,inputs = ColCSTR_model(Uf,params)
sf = Function('sf',[state,inputs],[xdot])
params['model']['sf'] = sf
params['model']['xdot_val_rf_ss'] = xf
params['model']['x'] = x
params['model']['u_opt'] = u_opt
nx = params['prob']['nx']
nu = params['prob']['nu']
nk = params['prob']['nk']
tf = params['prob']['tf']
ns = params['prob']['ns']
h = params['prob']['h']
#Preparing collocation matrices
_, C, D, d = collocationSetup()
params['prob']['d'] = d
colloc = {'C':C,'D':D, 'h':h}
params['colloc'] = colloc
#NLP variable vector
V = MX() #Decision variables (control + state)
obj = 0 #Objective function
cons = MX() #Nonlinear Constraints
delta_time = 1
alpha = 1
beta = 1
gamma = 1
params['weight']['delta_time'] = delta_time
params['weight']['alpha'] = alpha
params['weight']['beta'] = beta
params['weight']['gamma'] = gamma
#Initial states and Controls
data_init = spio.loadmat('CstrDistXinit.mat', squeeze_me = True)
xf = data_init['Xinit'][0:84]
u_opt = data_init['Xinit'][84:89]
#"Lift" Initial conditions
X0 = MX.sym('X0', nx)
V = vertcat(V,X0) #Decision variables
cons = vertcat(cons, X0 - x[0:nx,0]) #Nonlinear constraints
cons_x0 = X0 - x[0:nx,0]
#Formulating the NLP
Xk = X0
data = spio.loadmat('Qmax.mat', squeeze_me = True)
params['Qmax'] = data['Qmax']
ssoftc = 0
for i in range(0,N):
obj, cons, V, Xk, params, ssoftc = itPredHorizon_pf(Xk, V, cons, obj, params, i, ssoftc)
V = vertcat(V[:])
cons = vertcat(cons[:])
#Objective function and constraint functions
f = Function('f', [V], [obj], ['V'], ['objective'])
c = Function('c', [V], [cons], ['V'], ['constraint'])
cx0 = Function('cx0',[X0], [cons_x0], ['X0'], ['constraint'])
#Constructing Lagrangian
lag_expr = obj + mtimes(transpose(y['lam_g']),cons)
g = Function('g',[V],[jacobian(obj,V),obj])
lagr = Function('lagr', [V], [lag_expr], ['V'], ['lag_expr'])
[H,gg] = hessian(lag_expr,V)
H = Function('H',[V],[H,gg])
[Hobj,gobj] = hessian(obj,V)
Hobj = Function('Hobj', [V], [Hobj,gobj])
J = Function('J',[V],[jacobian(cons,V),cons])
Jp = Function('Jp',[X0],[jacobian(cons_x0,X0),cons_x0])
#Evaluating functions at current point
f = f(x)
g = g(x)
g = g[0]
g = transpose(g)
H = H(x)
H = H[0]
Lxp = H[0:nPrimal,0:nParam]
J = J(x)
J = J[0]
Jtemp = DM.zeros((nDual,nParam))
cp = Jp(x[0:nParam])
cp = cp[0]
Jtemp[0:nParam, 0:nParam] = cp.full()
cp = Jtemp.sparse()
cst = c(x)
#Evaluation of objective function used for Greshgorin bound
Hobj = Hobj(x)
Hobj = Hobj[0].sparse()
f = f.full()
g = g.sparse()
Lxp = Lxp.sparse()
cst = cst.full()
#Equality constraint
Jeq = J
dpe = cp
return f, g, H, Lxp, cst, J, cp, Jeq, dpe, Hobj
def custom_with_mult(ocp, nlp, t, x, u, p, mult):
my_values = DM.zeros((12, 1)) + x[0] * mult
return my_values
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
