Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def __cost_saturation_lf(self, x, x_ref, covar_x, P):
""" Terminal Cost function: Expected Value of Saturating Cost
"""
Nx = ca.MX.size1(P)
# Create symbols
P_s = ca.SX.sym('P', Nx, Nx)
x_s = ca.SX.sym('x', Nx)
covar_x_s = ca.SX.sym('covar_z', Nx, Nx)
Z_x = ca.SX.eye(Nx) + 2 * covar_x_s @ P_s
cost_x = ca.Function('cost_x', [x_s, P_s, covar_x_s],
[1 - ca.exp(-(x_s.T @ ca.solve(Z_x.T, P_s.T).T @ x_s))
/ ca.sqrt(ca.det(Z_x))])
return cost_x(x - x_ref, P, covar_x)
def cost_saturation_l(x, x_ref, covar_x, u, delta_u, Q, R_u, R_du, K):
""" Stage Cost function: Expected Value of Saturating Cost
"""
Nx = ca.MX.size1(Q)
Nu = ca.MX.size1(R_u)
# Create symbols
Q_s = ca.SX.sym('Q', Nx, Nx)
R_s = ca.SX.sym('Q', Nu, Nu)
K_s = ca.SX.sym('K', ca.MX.size(K))
x_s = ca.SX.sym('x', Nx)
u_s = ca.SX.sym('x', Nu)
covar_x_s = ca.SX.sym('covar_z', Nx, Nx)
covar_u_s = ca.SX.sym('covar_u', ca.MX.size(R_u))
covar_u = ca.Function('covar_u', [covar_x_s, K_s],
[K_s @ covar_x_s @ K_s.T])
Z_x = ca.SX.eye(Nx) + 2 * covar_x_s @ Q_s
Z_u = ca.SX.eye(Nu) + 2 * covar_u_s @ R_s
cost_x = ca.Function('cost_x', [x_s, Q_s, covar_x_s],
[1 - ca.exp(-(x_s.T @ ca.solve(Z_x.T, Q_s.T).T @ x_s))
/ ca.sqrt(ca.det(Z_x))])
cost_u = ca.Function('cost_u', [u_s, R_s, covar_u_s],
[1 - ca.exp(-(u_s.T @ ca.solve(Z_u.T, R_s.T).T @ u_s))
/ ca.sqrt(ca.det(Z_u))])
def codgen_model(self, model, opts):
# from casadi import * # syntax valid only for the entire module
import casadi
# x
if model.x==None:
x = casadi.SX.sym('x', 0, 1)
else:
x = model.x
# xdot
if model.xdot==None:
xdot = casadi.SX.sym('xdot', 0, 1)
else:
xdot = model.xdot
# u
if model.u==None:
u = casadi.SX.sym('u', 0, 1)
else:
u = model.u
# z
if model.z==None:
z = casadi.SX.sym('z', 0, 1)
else:
z = model.z
# fun
fun = model.ode_expr
def define_position(self, expr):
self.define_signal('position', expr)
self.n_pos = self.signal_length['position']
self._position = SX.sym('position', self.n_pos)
return self._position
var: The estimated variance
"""
if log:
X = ca.log(X)
Y = ca.log(Y)
inputmean = ca.log(inputmean)
Ny = len(invK)
N, Nx = ca.MX.size(X)
mean = ca.MX.zeros(Ny, 1)
var = ca.MX.zeros(Ny, 1)
# Casadi symbols
x_s = ca.SX.sym('x', Nx)
z_s = ca.SX.sym('z', Nx)
ell_s = ca.SX.sym('ell', Nx)
sf2_s = ca.SX.sym('sf2')
invK_s = ca.SX.sym('invK', N, N)
Y_s = ca.SX.sym('Y', N)
m_s = ca.SX.sym('m')
ks_s = ca.SX.sym('ks', N)
kss_s = ca.SX.sym('kss')
ksT_invK_s = ca.SX.sym('ksT_invK', 1, N)
alpha_s = ca.SX.sym('alpha', N)
covSE = ca.Function('covSE', [x_s, z_s, ell_s, sf2_s],
[covSEard(x_s, z_s, ell_s, sf2_s)])
ksT_invK_func = ca.Function('ksT_invK', [ks_s, invK_s],
[ca.mtimes(ks_s.T, invK_s)])
def generate_cost_function(self):
initial_state = cd.SX.sym('initial_state', self._controller.model.number_of_states, 1)
state_reference = cd.SX.sym('state_reference', self._controller.model.number_of_states, 1)
input_reference = cd.SX.sym('input_reference', self._controller.model.number_of_inputs, 1)
static_casadi_parameters = cd.vertcat(initial_state, state_reference, input_reference)
constraint_weights = cd.SX.sym('constraint_weights', self._controller.number_of_constraints, 1)
input_all_steps = cd.SX.sym('input_all_steps', self._controller.model.number_of_inputs * self._controller.horizon + \
self._controller.model.number_of_states * (self._controller.horizon-1), 1)
cost = cd.SX.sym('cost', 1, 1)
cost = 0
current_init_state = initial_state
for i in range(1, self._controller.horizon + 1):
current_input = input_all_steps[(i - 1) * self._controller.model.number_of_inputs:i * self._controller.model.number_of_inputs]
next_state_bar = self._controller.model.get_next_state(current_init_state,current_input)
cost = cost + self._controller.stage_cost(next_state_bar, current_input, i, state_reference, input_reference)
cost = cost + self._controller.generate_cost_constraints(next_state_bar, constraint_weights)
# add a soft constraint for the continuity
weight_continuity = 1000
if i > 1 :
def pendulum_model():
""" Nonlinear inverse pendulum model. """
M = 1 # mass of the cart [kg]
m = 0.1 # mass of the ball [kg]
g = 9.81 # gravity constant [m/s^2]
l = 0.8 # length of the rod [m]
p = SX.sym('p') # horizontal displacement [m]
theta = SX.sym('theta') # angle with the vertical [rad]
v = SX.sym('v') # horizontal velocity [m/s]
omega = SX.sym('omega') # angular velocity [rad/s]
F = SX.sym('F') # horizontal force [N]
ode_rhs = vertcat(v,
omega,
(- l*m*sin(theta)*omega**2 + F + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta)**2),
(- l*m*cos(theta)*sin(theta)*omega**2 + F*cos(theta) + g*m*sin(theta) + M*g*sin(theta))/(l*(M + m - m*cos(theta)**2)))
nx = 4
# for IRK
xdot = SX.sym('xdot', nx, 1)
z = SX.sym('z',0,1)
return (Function('pendulum', [vertcat(p, theta, v, omega), F], [ode_rhs], ['x', 'u'], ['xdot']),
nx, # number of states
1, # number of controls
Function('impl_pendulum', [vertcat(p, theta, v, omega), F, xdot, z], [ode_rhs-xdot],
['x', 'u','xdot','z'], ['rhs']))
def initialize(self):
"""
Python version: 3.5
Created by: Thomas Herrmann
Created on: 01.04.2020
Documentation: Initialization of necessary optimization variables (symbolic CasADi expressions)
and states including limits.
"""
self.temp_inv_n = ca.SX.sym('temp_inv_n')
self.temp_inv_s = self.pars["temp_inv_max"] - 30
self.temp_inv = self.temp_inv_s * self.temp_inv_n
# Define limits and initial guess
self.temp_min = self.pars["T_env"] / self.temp_inv_s
self.temp_max = self.pars["temp_inv_max"] / self.temp_inv_s
self.temp_guess = self.pars["T_env"] / self.temp_inv_s
self.get_thermal_resistance()
def generate_cost_function(self):
initial_state = cd.SX.sym('initial_state', self._controller.model.number_of_states, 1)
state_reference = cd.SX.sym('state_reference', self._controller.model.number_of_states, 1)
input_reference = cd.SX.sym('input_reference', self._controller.model.number_of_inputs, 1)
static_casadi_parameters = cd.vertcat(initial_state, state_reference, input_reference)
constraint_weights = cd.SX.sym('constraint_weights', self._controller.number_of_constraints, 1)
input_all_steps = cd.SX.sym('input_all_steps', self._controller.model.number_of_inputs * self._controller.horizon + \
self._controller.model.number_of_states * (self._controller.horizon-1), 1)
cost = cd.SX.sym('cost', 1, 1)
cost = 0
current_init_state = initial_state
for i in range(1, self._controller.horizon + 1):
current_input = input_all_steps[(i - 1) * self._controller.model.number_of_inputs:i * self._controller.model.number_of_inputs]
next_state_bar = self._controller.model.get_next_state(current_init_state,current_input)
cost = cost + self._controller.stage_cost(next_state_bar, current_input, i, state_reference, input_reference)
cost = cost + self._controller.generate_cost_constraints(next_state_bar, constraint_weights)
# add a soft constraint for the continuity
def cost_l(x, x_ref, covar_x, u, delta_u, Q, R_u, R_du, K, s=1):
""" Stage cost function: Expected Value of Quadratic Cost
"""
Q_s = ca.SX.sym('Q', ca.MX.size(Q))
R_s = ca.SX.sym('R', ca.MX.size(R_u))
K_s = ca.SX.sym('K', ca.MX.size(K))
x_s = ca.SX.sym('x', ca.MX.size(x))
u_s = ca.SX.sym('u', ca.MX.size(u))
covar_x_s = ca.SX.sym('covar_x', ca.MX.size(covar_x))
covar_u_s = ca.SX.sym('covar_u', ca.MX.size(R_u))
sqnorm_x = ca.Function('sqnorm_x', [x_s, Q_s],
[ca.mtimes(x_s.T, ca.mtimes(Q_s, x_s))])
sqnorm_u = ca.Function('sqnorm_u', [u_s, R_s],
[ca.mtimes(u_s.T, ca.mtimes(R_s, u_s))])
covar_u = ca.Function('covar_u', [covar_x_s, K_s],
[ca.mtimes(K_s, ca.mtimes(covar_x_s, K_s.T))])
trace_u = ca.Function('trace_u', [R_s, covar_u_s],
[s * ca.trace(ca.mtimes(R_s, covar_u_s))])
trace_x = ca.Function('trace_x', [Q_s, covar_x_s],
[s * ca.trace(ca.mtimes(Q_s, covar_x_s))])