def make_horseshoe_list(LENE, LEPE, TEPE, TENE, strength, strength_prev):
PE_filament = cas.vertcat(LEPE, TEPE, strength)
LE_filament = cas.vertcat(LENE, LEPE, strength - strength_prev)
NE_filament = cas.vertcat(TENE, LENE, strength)
filament_list = cas.horzcat(PE_filament, LE_filament, NE_filament)
return filament_list
def initial_node_variables_for_standard_path(t,
options,
model,
formulation,
ret={}):
trajectory_type = options['type']
number_of_nodes = model.architecture.number_of_nodes
parent_map = model.architecture.parent_map
kite_nodes = model.architecture.kite_nodes
level_siblings = get_all_level_siblings(options, model)
ua_norm = options['ua_norm']
kite_dof = model.kite_dof
[height_list, radius, ehat_tether, ehat_side,
ehat_up] = get_orbit_cone_parameters(options, model, ret['l_t'])
for node in range(1, number_of_nodes):
parent = parent_map[node]
if parent == 0:
parent_position = np.zeros((3, 1))
else:
grandparent = parent_map[parent]
parent_position = ret['q' + str(parent) + str(grandparent)]
if not node in kite_nodes:
ret['q' + str(node) + str(parent)] = get_tether_node_position(
options, parent_position, node, ret['l_t'])
ret['dq' + str(node) + str(parent)] = np.zeros((3, 1))
else:
if parent == 0:
height = height_list[0]
else:
height = height_list[1]
omega_norm = ua_norm / radius
omega_vector = ehat_tether * omega_norm
psi = get_azimuthal_angle(t, level_siblings, node, parent,
omega_norm)
ehat_radial = ehat_side * np.cos(psi) + ehat_up * np.sin(psi)
tether_vector = ehat_radial * radius + ehat_tether * height
position = parent_position + tether_vector
ehat_tangential = -1. * ehat_side * np.sin(psi) + ehat_up * np.cos(
psi)
velocity = ua_norm * ehat_tangential
ehat1 = -1. * ehat_tangential
ehat3 = ehat_tether
ehat2 = vect_op.normed_cross(ehat3, ehat1)
dcm = cas.horzcat(ehat1, ehat2, ehat3)
dcm_column = cas.reshape(dcm, (9, 1))
ret['q' + str(node) + str(parent)] = position
ret['dq' + str(node) + str(parent)] = velocity
if int(kite_dof) == 6:
ret['omega' + str(node) + str(parent)] = omega_vector
ret['r' + str(node) + str(parent)] = dcm_column
return ret
def animation_snapshot(axes,
plot_dict,
index,
cosmetics,
init_colors=bool(False),
plot_kites=bool(True)):
dims = ['xy', 'xz', 'yz']
# figure limits
q_limits = tools.get_q_limits(plot_dict, cosmetics)
architecture = plot_dict['architecture']
parent_map = architecture.parent_map
kite_nodes = architecture.kite_nodes
for dim in dims:
ax = 'ax_' + dim
axes[ax].clear()
# plot system
trajectory.plot_trajectory_instant(axes[ax],
axes['ax2'],
plot_dict,
index,
cosmetics,
dim,
init_colors=init_colors,
plot_kites=plot_kites)
# plot trajectories
counter = 0
alph = cosmetics['trajectory']['alpha']
for n in kite_nodes:
if init_colors == True:
local_color = 'k'
elif init_colors == False:
local_color = cosmetics['trajectory']['colors'][counter]
else:
local_color = init_colors
parent = parent_map[n]
vertically_stacked_kite_locations = cas.horzcat(
plot_dict['xd']['q' + str(n) + str(parent)][0],
plot_dict['xd']['q' + str(n) + str(parent)][1],
plot_dict['xd']['q' + str(n) + str(parent)][2])
for dim in dims:
ax = 'ax_' + dim
tools.make_side_plot(axes[ax],
vertically_stacked_kite_locations,
dim,
local_color,
alpha=alph)
counter += 1
# change axes limits
for dim in dims:
ax = 'ax_' + dim
xdim = dim[0]
ydim = dim[1]
axes[ax].set_xlim(q_limits[xdim])
axes[ax].set_ylim(q_limits[ydim])
axes[ax].set_aspect('equal', adjustable='box')
axes[ax].set_xlabel(xdim + ' [m]')
axes[ax].set_ylabel(ydim + ' [m]')
# flip x-axis to get "upstream" view
axes['ax_yz'].invert_xaxis()
# move axes out of way of three-view
axes['ax_yz'].yaxis.set_label_position("right")
axes['ax_yz'].yaxis.tick_right()
axes['ax_yz'].xaxis.set_label_position("top")
axes['ax_yz'].xaxis.tick_top()
axes['ax_xz'].xaxis.set_label_position("top")
axes['ax_xz'].xaxis.tick_top()
plt.tight_layout(w_pad=-11, h_pad=1, rect=[0, 0, 1, 0.95]) #pad=2.5)
return None
def __construct_sensitivity_funcs(self):
""" Construct functions for NLP sensitivity evaluations
"""
# system variables
x, u = self.__vars['x'], self.__vars['u']
nx = x.shape[0]
wk = ca.vertcat(x, u)
if 'us' in self.__vars:
us = self.__vars['us']
uhat = ca.vertcat(u, us)
wk = ca.vertcat(wk, us)
else:
uhat = u
# dynamics sensitivities
if type(self.__F) != list:
x_next = self.__F(x0=x,
p=u)['xf'] # symbolic integrator evaluation
self.__jac_Fx = ca.Function('jac_Fx', [x, u],
[ca.jacobian(x_next, x)]).map(self.__N)
self.__jac_Fu = ca.Function(
'jac_Fu', [x, u], [ca.jacobian(x_next, uhat)]).map(self.__N)
else:
x_map = ca.MX.sym('x_map', self.__nx, self.__N)
u_map = ca.MX.sym('u_map', self.__nu, self.__N)
x_next = [
self.__F[k](x0=x_map[:, k], p=u_map[:, k])['xf']
for k in range(self.__N)
]
jac_Fx = [
ca.jacobian(x_next[k],
x_map)[:, k * self.__nx:(k + 1) * self.__nx]
for k in range(self.__N)
]
jac_Fu = [
ca.jacobian(x_next[k],
u_map)[:, k * self.__nu:(k + 1) * self.__nu]
for k in range(self.__N)
]
if 'us' in self.__vars:
jac_Fu = [
ct.horzcat(jac_Fu[k], ca.MX.zeros(self.__nx, self.__ns))
for k in range(self.__N)
]
self.__jac_Fx = ca.Function('jac_Fx', [x_map, u_map],
[ct.horzcat(*jac_Fx)])
self.__jac_Fu = ca.Function('jac_Fu', [x_map, u_map],
[ct.horzcat(*jac_Fu)])
# constraints sensitivities
if self.__h is not None:
if type(self.__h) != list:
if 'us' in self.__vars:
constr = self.__h(x, u,
us) # symbolic constraint evaluation
self.__jac_h = ca.Function('jac_h', [x, u, us],
[ca.jacobian(constr, wk)]).map(
self.__N)
else:
constr = self.__h(x, u)
self.__jac_h = ca.Function('jac_h', [x, u],
[ca.jacobian(constr, wk)]).map(
self.__N)
else:
x_map = ca.MX.sym('x_map', self.__nx, self.__N)
u_map = ca.MX.sym('u_map', self.__nu, self.__N)
if 'us' in self.__vars:
us_map = ca.MX.sym('us_map', self.__ns, self.__N)
constr = [
self.__h[k](x_map[:, k], u_map[:, k], us_map[:, k])
for k in range(self.__N)
]
jac_hx = [
ca.jacobian(constr[k],
x_map)[:,
k * self.__nx:(k + 1) * self.__nx]
for k in range(self.__N)
]
jac_hu = [
ca.jacobian(constr[k],
u_map)[:,
k * self.__nu:(k + 1) * self.__nu]
for k in range(self.__N)
]
jac_hus = [
ca.jacobian(constr[k],
us_map)[:,
k * self.__ns:(k + 1) * self.__ns]
for k in range(self.__N)
]
jac_h = [
ct.horzcat(jac_hx[k], jac_hu[k], jac_hus[k])
for k in range(self.__N)
]
self.__jac_h = ca.Function('jac_h', [x_map, u_map, us_map],
[ct.horzcat(*jac_h)])
else:
constr = [
self.__h[k](x_map[:, k], u_map[:, k])
for k in range(self.__N)
]
jac_hx = [
ca.jacobian(constr[k],
x_map)[:,
k * self.__nx:(k + 1) * self.__nx]
for k in range(self.__N)
]
jac_hu = [
ca.jacobian(constr[k],
u_map)[:,
k * self.__nu:(k + 1) * self.__nu]
for k in range(self.__N)
]
jac_h = [
ct.horzcat(jac_hx[k], jac_hu[k])
for k in range(self.__N)
]
self.__jac_h = ca.Function('jac_h', [x_map, u_map],
[ct.horzcat(*jac_h)])
if self.__gnl is not None:
if type(self.__gnl) == list:
constr = [
self.__gnl[k](x_map[:, k], u_map[:, k], us_map[:, k])
for k in range(self.__N)
]
jac_gx = [
ca.jacobian(constr[k],
x_map)[:, k * self.__nx:(k + 1) * self.__nx]
for k in range(self.__N)
]
jac_gu = [
ca.jacobian(constr[k],
u_map)[:, k * self.__nu:(k + 1) * self.__nu]
for k in range(self.__N)
]
jac_gus = [
ca.jacobian(constr[k],
us_map)[:, k * self.__ns:(k + 1) * self.__ns]
for k in range(self.__N)
]
jac_g = [
ct.horzcat(jac_gx[k], jac_gu[k], jac_gus[k])
for k in range(self.__N)
]
self.__jac_g = ca.Function('jac_g', [x_map, u_map, us_map],
[ct.horzcat(*jac_g)])
else:
constr = self.__gnl(x, u, us)
self.__jac_g = ca.Function('jac_g', [x, u, us],
[ca.jacobian(constr, wk)]).map(
self.__N)
return None
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:
if type(self.__h) is not list:
constraints_entry += (ct.entry('h',
shape=self.__h.size1_out(0),
repeat=self.__N), )
else:
# TODO: allow for changing dimension of h
constraints_entry += (ct.entry('h',
shape=self.__h[0].size1_out(0),
repeat=self.__N), )
if self.__gnl is not None:
if type(self.__gnl) is not list:
constraints_entry += (ct.entry('g',
shape=self.__gnl.size1_out(0),
repeat=self.__N), )
else:
# TODO: allow for changing dimension of g
constraints_entry += (ct.entry(
'g', shape=self.__gnl[0].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
if type(self.__F) == list:
F_constr = [
self.__F[i](x0=w['x', i], p=w['u', i])['xf']
for i in range(len(self.__F))
]
else:
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:
if type(self.__h) == list:
constr['h'] = [
self.__h[i](*[[*map_args.values()][j][:, i]
for j in range(len(map_args))])
for i in range(len(self.__h))
]
else:
constr['h'] = ct.horzsplit(
self.__h.map(self.__N,
self.__parallelization)(*map_args.values()))
if self.__gnl is not None:
if type(self.__gnl) == list:
constr['g'] = [
self.__gnl[i](*[[*map_args.values()][j][:, i]
for j in range(len(map_args))])
for i in range(len(self.__gnl))
]
else:
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
if type(self.__cost) == list:
f = sum([
self.__cost[k](w['x', k], w['u', k]) for k in range(self.__N)
])
else:
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 __construct_solver(self):
""" Construct periodic MPC solver
"""
# system variables and dimensions
x = self.__vars['x']
u = self.__vars['u']
# NLP parameters
if self.__type == 'economic':
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape=(self.__nx, 1)),
ct.entry('xN', shape=(self.__nx, 1))
])
# reassign for brevity
x0 = self.__p['x0']
xN = self.__p['xN']
if self.__type == 'tracking':
ref_vars = (ct.entry('x', shape=(self.__nx, ),
repeat=self.__N + 1),
ct.entry('u', shape=(self.__nu, ), repeat=self.__N))
if 'us' in self.__vars:
ref_vars += (ct.entry('us',
shape=(self.__ns, ),
repeat=self.__N), )
# reference trajectory
wref = ct.struct_symMX([ref_vars])
nw = self.__nx + self.__nu + self.__ns
tuning = ct.struct_symMX([ # tracking tuning
ct.entry('H', shape=(nw, nw), repeat=self.__N),
ct.entry('q', shape=(nw, 1), repeat=self.__N)
])
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape=(self.__nx, )),
ct.entry('wref', struct=wref),
ct.entry('tuning', struct=tuning)
])
# reassign for brevity
x0 = self.__p['x0']
wref = self.__p.prefix['wref']
tuning = self.__p.prefix['tuning']
xN = wref['x', -1]
# NLP variables
variables_entry = (ct.entry('x',
shape=(self.__nx, ),
repeat=self.__N + 1),
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), )
self.__wref = ct.struct_symMX([variables_entry
]) # structure of reference
if 'usc' in self.__vars:
variables_entry += (ct.entry('usc',
shape=(self.__nsc, ),
repeat=self.__N), )
# nlp variables + bounds
w = ct.struct_symMX([variables_entry])
# variable bounds are implemented as inequalities
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.__gnl is not None:
constraints_entry += (ct.entry('g',
shape=self.__gnl.size1_out(0),
repeat=self.__N), )
if self.__h is not None:
constraints_entry += (ct.entry('h',
shape=self.__h.size1_out(0),
repeat=self.__N), )
# terminal constraint
self.__nx_term = self.__p_operator.size1_out(0)
# create general constraints structure
g_struct = ct.struct_symMX([
ct.entry('init', shape=(self.__nx, 1)), constraints_entry,
ct.entry('term', shape=(self.__nx_term, 1))
])
# create symbolic constraint expressions
map_args = collections.OrderedDict()
map_args['x0'] = ct.horzcat(*w['x', :-1])
map_args['p'] = ct.horzcat(*w['u'])
F_constr = ct.horzsplit(self.__F.map(self.__N)(**map_args)['xf'])
# generate constraints
constr = collections.OrderedDict()
constr['dyn'] = [a - b for a, b in zip(F_constr, w['x', 1:])]
if 'us' in self.__vars:
map_args['us'] = ct.horzcat(*w['us'])
if self.__gnl is not None:
constr['g'] = ct.horzsplit(
self.__gnl.map(self.__N)(*map_args.values()))
if 'usc' in self.__vars:
map_args['usc'] = ct.horzcat(*w['usc'])
if self.__h is not None:
constr['h'] = ct.horzsplit(
self.__h.map(self.__N)(*map_args.values()))
repeated_constr = list(
itertools.chain.from_iterable(zip(*constr.values())))
term_constraint = self.__p_operator(w['x', -1] - xN)
self.__g = g_struct(
ca.vertcat(w['x', 0] - x0, *repeated_constr, term_constraint))
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
for i in self.__h_us_idx + self.__h_x_idx: # rm constraints the only depend on x at k = 0
self.__lbg['h', 0, i] = -np.inf
# nlp cost
cost_map = self.__cost.map(self.__N)
if self.__type == 'economic':
cost_args = [ct.horzcat(*w['x', :-1]), ct.horzcat(*w['u'])]
elif self.__type == 'tracking':
if self.__ns != 0:
cost_args_w = ct.horzcat(*[
ct.vertcat(w['x', k], w['u', k], w['us', k])
for k in range(self.__N)
])
cost_args_w_ref = ct.horzcat(*[
ct.vertcat(wref['x', k], wref['u', k], wref['us', k])
for k in range(self.__N)
])
else:
cost_args_w = ct.horzcat(*[
ct.vertcat(w['x', k], w['u', k]) for k in range(self.__N)
])
cost_args_w_ref = ct.horzcat(*[
ct.vertcat(wref['x', k], wref['u', k])
for k in range(self.__N)
])
cost_args = [
cost_args_w, cost_args_w_ref,
ct.horzcat(*tuning['H']),
ct.horzcat(*tuning['q'])
]
if self.__options['hessian_approximation'] == 'gauss_newton':
if 'usc' not in self.__vars:
hess_gn = ct.diagcat(*tuning['H'],
ca.DM.zeros(self.__nx, self.__nx))
else:
hess_block = list(
itertools.chain.from_iterable(
zip(tuning['H'],
[ca.DM.zeros(self.__nsc, self.__nsc)] *
self.__N)))
hess_gn = ct.diagcat(*hess_block,
ca.DM.zeros(self.__nx, self.__nx))
J = ca.sum2(cost_map(*cost_args))
# add cost on slacks
if 'usc' in self.__vars:
J += ca.sum2(ct.mtimes(self.__scost.T, ct.horzcat(*w['usc'])))
# create solver
prob = {'f': J, 'g': self.__g, 'x': w, 'p': self.__p}
self.__w = w
self.__g_fun = ca.Function('g_fun', [self.__w, self.__p], [self.__g])
# create IPOPT-solver instance if needed
if self.__options['ipopt_presolve']:
opts = {
'ipopt': {
'linear_solver': 'ma57',
'print_level': 0
},
'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 hessian approximation function
if self.__options['hessian_approximation'] == 'gauss_newton':
lam_g = ca.MX.sym('lam_g', self.__g.shape) # will not be used
hess_approx = ca.Function('hess_approx',
[self.__w, self.__p, lam_g], [hess_gn])
elif self.__options['hessian_approximation'] == 'exact':
hess_approx = 'exact'
# create sqp solver
prob['lbg'] = self.__lbg
prob['ubg'] = self.__ubg
sqp_opts = {
'hessian_approximation': hess_approx,
'max_iter': self.__options['max_iter']
}
self.__sqp_solver = sqp_method.Sqp(prob, sqp_opts)
def get_sensitivities(self):
"""
Extract NLP sensitivities evaluated at the solution.
"""
# solution
wsol = self.__w(self.__sol['x'])
# extract multipliers
lam_g = self.__g(self.__sol['lam_g'])
self.__lam_g = lam_g
map_args = collections.OrderedDict()
map_args['x'] = ct.horzcat(*wsol['x'])
map_args['u'] = ct.horzcat(*wsol['u'])
map_args['lam_g'] = ct.horzcat(*lam_g['dyn'])
if 'us' in self.__vars:
map_args['us'] = ct.horzcat(*wsol['us'])
map_args['lam_s'] = ct.horzcat(*lam_g['g'])
# sensitivity dict
S = {}
# dynamics sensitivities
S['A'] = np.split(self.__jac_Fx(map_args['x'], map_args['u']).full(),
self.__N,
axis=1)
S['B'] = np.split(self.__jac_Fu(map_args['x'], map_args['u']).full(),
self.__N,
axis=1)
# extract active constraints
if self.__h is not None:
# add slacks to function args if necessary
if 'us' in self.__vars:
args = [map_args['x'], map_args['u'], map_args['us']]
else:
args = [map_args['x'], map_args['u']]
# compute gradient of constraints
mu_s = lam_g['h']
S['C'] = np.split(self.__jac_h(*args).full(), self.__N, axis=1)
if type(self.__h) != list:
S['e'] = np.split(self.__h(*args).full(), self.__N, axis=1)
else:
S['e'] = [
self.__h[k](*[args[j][:, k] for j in range(len(args))])
for k in range(self.__N)
]
if 'g' in lam_g.keys():
lam_s = lam_g['g']
S['G'] = np.split(self.__jac_g(*args).full(), self.__N, axis=1)
if type(self.__gnl) == list:
S['r'] = [
self.__gnl[k](
*[args[j][:, k] for j in range(len(args))])
for k in range(self.__N)
]
else:
S['r'] = np.split(self.__gnl(*args).full(),
self.__N,
axis=1)
else:
S['G'] = None
S['r'] = None
# retrieve active set
C_As = []
self.__indeces_As = []
for k in range(self.__N):
C_active = []
index_active = []
for i in range(mu_s[k].shape[0]):
if np.abs(mu_s[k][i].full()) > self.__mu_tresh:
C_active.append(S['C'][k][i, :])
index_active.append(i)
if len(C_active) > 0:
C_As.append(ca.horzcat(*C_active).full().T)
else:
C_As.append(None)
self.__indeces_As.append(index_active)
S['C_As'] = C_As
else:
S['C'] = None
S['C_As'] = None
S['G'] = None
S['e'] = None
S['r'] = None
self.__indeces_As = None
# compute hessian of lagrangian
H = self.__sol['S']['H']
M = self.__nx + self.__nu
if 'us' in self.__vars:
M += self.__ns
S['H'] = [
H[i * M:(i + 1) * M, i * M:(i + 1) * M] for i in range(self.__N)
]
# compute cost function gradient
if self.__h is not None:
S['q'] = [
-ca.mtimes(lam_g['h', i].T, S['C'][i]) for i in range(self.__N)
]
else:
S['q'] = [
np.zeros((1, self.__nx + self.__nu)) for i in range(self.__N)
]
return S
def __process_interpolation_Variables(interpolation_variables, configurations,
model):
"""Create casadi functions mapping interpolation variables to states
:type interpolation_variables: dict
:param interpolation_variables: variables defining the interpolation
:type configurations: dict
:param configurations: parameters defining a given configuration
:type model: awebox.model_dir.model
:param model: system model
:rtype: dict, casadi.struct_symSX
"""
# initialize dict
rotation_matrix = {}
rotation_matrix['q00'] = np.eye(3)
rotation_matrix['dq00'] = np.zeros([3, 3])
kite_nodes = model.architecture.kite_nodes
parent_nodes = model.architecture.parent_map
parent_nodes[0] = 0
number_of_nodes = model.architecture.number_of_nodes
kite_dof = model.kite_dof
trajectory_type = model.options['trajectory']['type']
conf_0 = configurations['conf_0']
conf_f = configurations['conf_f']
l_s = configurations['l_s']
l_i = configurations['l_i']
## generate casadi expressions
# generate symbolic variables
sinterp = __get_sstruct(interpolation_variables)
sstates = {}
sstates['var'] = {}
sstates['var']['q00'] = 0.
sstates['dvar'] = {}
sstates['dvar']['q00'] = 0.
sstates['ddvar'] = {}
sstates['ddvar']['q00'] = 0.
# generate dict to store functions in
sstates_functions = {}
# generate casadi functions: inteprolation variables -> states
l_t = sinterp['var', 'l_t']
for node in range(1, number_of_nodes):
parent_node = parent_nodes[node]
node_str = str(node) + str(parent_node)
grandparent_node = parent_nodes[parent_node]
parent_str = str(parent_node) + str(grandparent_node)
grandgrandparent_node = parent_nodes[grandparent_node]
grandparent_str = str(grandparent_node) + str(grandgrandparent_node)
if (node == 1) and (
node not in kite_nodes
): # first node parameterized with main tether length
a = cas.mtimes((conf_f['q' + node_str] - conf_0['q' + node_str]).T,
(conf_f['q' + node_str] - conf_0['q' + node_str]))
if a == 0:
e_t = vect_op.normalize(conf_0['q' + node_str])
else:
b = 2 * cas.mtimes(
conf_0['q' + node_str].T,
(conf_f['q' + node_str] - conf_0['q' + node_str]))
c = cas.mtimes(conf_0['q' + node_str].T,
conf_0['q' + node_str]) - l_t**2
D = b**2 - 4 * a * c
x1 = (-b + np.sqrt(D)) / (2 * a)
x2 = (-b - np.sqrt(D)) / (2 * a)
#if x2 >= 0:
# s = x2
#else:
# s = x1
s = x1
e_t = 1. / l_t * (
conf_0['q' + node_str] + s *
(conf_f['q' + node_str] - conf_0['q' + node_str]))
sstates['var']['q' + node_str] = l_t * e_t
rotation_matrix = compute_rotation_matrices(
sinterp.prefix['var'], node_str, parent_str, rotation_matrix)
else:
if (node in kite_nodes):
if node == 1:
tether_length = l_t
else:
tether_length = l_s
Phi = sinterp['var', 'Phi' + node_str]
Omega = sinterp['var', 'Omega' + node_str]
parent = sstates['var']['q' + parent_str]
grandparent = sstates['var']['q' + grandparent_str]
radius = tether_length * np.sin(Phi)
l_x = tether_length * np.cos(Phi)
# define axis of rotation
if node != 1:
axis_of_rot = parent - grandparent
axis_of_rot = vect_op.normalize(axis_of_rot)
else:
inclination = sinterp['var', 'inclination' + node_str]
axis_of_rot = np.zeros([3, 1])
axis_of_rot[0] = np.cos(inclination)
axis_of_rot[2] = np.sin(inclination)
e_hat_x = axis_of_rot
e_hat_y = vect_op.normed_cross(e_hat_x, vect_op.zhat())
e_hat_z = vect_op.normed_cross(e_hat_y, e_hat_x)
e_hat_r = e_hat_z * np.sin(Omega) + e_hat_y * np.cos(Omega)
sstates['var']['q' + node_str] = sstates['var'][
'q' + parent_str] + e_hat_r * radius + e_hat_x * l_x
else:
tether_length = l_i
rotation_matrix = compute_rotation_matrices(
sinterp.prefix['var'], node_str, parent_str,
rotation_matrix)
tether_vector = vect_op.xhat()
tether_vector = cas.mtimes(rotation_matrix['q' + node_str],
tether_vector)
sstates['var']['q' + node_str] = sstates['var'][
'q' + parent_str] + tether_vector * tether_length
sstates['dvar']['q' + node_str] = cas.mtimes(
cas.jacobian(sstates['var']['q' + node_str], sinterp['var']),
sinterp['dvar'])
# create rotational kinematics
if int(kite_dof) == 6:
# iterate over all kite ndoes
if node in kite_nodes:
# get node strings
parent = parent_nodes[node]
node_str = str(node) + str(parent)
grandparent = parent_nodes[parent]
parent_str = str(parent) + str(grandparent)
grandgrandparent = parent_nodes[grandparent]
grandparent_str = str(grandparent) + str(grandgrandparent)
# compute dcm matrix for node
e_hat_1, e_hat_2, e_hat_3 = __get_kite_axis(
sstates, node_str, parent_str, grandparent_str,
trajectory_type)
dcm = ct.horzcat(e_hat_1, e_hat_2, e_hat_3)
dcm_column = ct.reshape(dcm, (9, 1))
# compute rotation around axis
omega_norm = vect_op.norm(
sstates['dvar']['q' + node_str]) / radius
if trajectory_type == 'lift_mode':
omega_vector = vect_op.normalize(axis_of_rot) * omega_norm
elif trajectory_type == 'nominal_landing':
omega_vector = cas.DM([0, 0, 0])
# put in state dict
sstates['omega' + node_str] = omega_vector
sstates['r' + node_str] = dcm_column
# generate functions
sstates_functions['q' + node_str] = cas.Function(
'q' + node_str, [sinterp], [sstates['var']['q' + node_str]])
sstates_functions['dq' + node_str] = cas.Function(
'dq' + node_str, [sinterp], [sstates['dvar']['q' + node_str]])
if int(kite_dof) == 6 and node in kite_nodes:
sstates_functions['r' + node_str] = cas.Function(
'r' + node_str, [sinterp], [sstates['r' + node_str]])
sstates_functions['omega' + node_str] = cas.Function(
'omega' + node_str, [sinterp], [sstates['omega' + node_str]])
return sstates_functions, sinterp
def get_outputs(options, atmos, wind, variables, outputs, parameters,
architecture):
parent_map = architecture.parent_map
kite_nodes = architecture.kite_nodes
xd = variables['xd']
elevation_angle = indicators.get_elevation_angle(xd)
for n in kite_nodes:
parent = parent_map[n]
# get relevant variables for kite n
q = xd['q' + str(n) + str(parent)]
dq = xd['dq' + str(n) + str(parent)]
coeff = xd['coeff' + str(n) + str(parent)]
# wind parameters
rho_infty = atmos.get_density(q[2])
uw_infty = wind.get_velocity(q[2])
# apparent air velocity
if options['induction_model'] == 'actuator':
ua = actuator_disk_flow.get_kite_effective_velocity(
options, variables, wind, n, parent, architecture)
else:
ua = uw_infty - dq
# relative air speed
ua_norm = vect_op.smooth_norm(ua, epsilon=1e-8)
# ua_norm = mtimes(ua.T, ua) ** 0.5
# in kite body:
if parent > 0:
grandparent = parent_map[parent]
qparent = xd['q' + str(parent) + str(grandparent)]
else:
qparent = np.array([0., 0., 0.])
ehat_r = (q - qparent) / vect_op.norm(q - qparent)
ehat_t = vect_op.normed_cross(ua, ehat_r)
ehat_s = vect_op.normed_cross(ehat_t, ua)
# roll angle
psi = coeff[1]
ehat_l = cas.cos(psi) * ehat_s + cas.sin(psi) * ehat_t
ehat_span = cas.cos(psi) * ehat_t - cas.sin(psi) * ehat_s
ehat_chord = ua / ua_norm
# implicit direct cosine matrix (for plotting only)
r = cas.horzcat(ehat_chord, ehat_span, ehat_l)
# lift and drag coefficients
CL = coeff[0]
CD = parameters['theta0', 'aero', 'CD0'] + 0.02 * CL**2
# lift and drag force
f_lift = CL * 1. / 2. * rho_infty * cas.mtimes(
ua.T, ua) * parameters['theta0', 'geometry', 's_ref'] * ehat_l
f_drag = CD * 1. / 2. * rho_infty * ua_norm * parameters['theta0',
'geometry',
's_ref'] * ua
f_side = cas.DM(np.zeros((3, 1)))
f_aero = f_lift + f_drag
m_aero = cas.DM(np.zeros((3, 1)))
CA = CD
CN = CL
CY = cas.DM(0.)
aero_coefficients = {}
aero_coefficients['CD'] = CD
aero_coefficients['CL'] = CL
aero_coefficients['CA'] = CA
aero_coefficients['CN'] = CN
aero_coefficients['CY'] = CY
outputs = indicators.collect_kite_aerodynamics_outputs(
options, atmos, ua, ua_norm, aero_coefficients, f_aero, f_lift,
f_drag, f_side, m_aero, ehat_chord, ehat_span, r, q, n, outputs,
parameters)
outputs = indicators.collect_environmental_outputs(
atmos, wind, q, n, outputs)
outputs = indicators.collect_aero_validity_outputs(
options, xd, ua, n, parent, outputs, parameters)
outputs = indicators.collect_local_performance_outputs(
options, atmos, wind, variables, CL, CD, elevation_angle, ua, n,
parent, outputs, parameters)
outputs = indicators.collect_power_balance_outputs(
variables, n, outputs, architecture)
return outputs
def plot_trajectory_contents(ax,
plot_dict,
cosmetics,
side,
init_colors=bool(False),
plot_kites=bool(True),
label=None):
# read in inputs
model_options = plot_dict['options']['model']
kite_nodes = plot_dict['architecture'].kite_nodes
parent_map = plot_dict['architecture'].parent_map
body_cross_sections_per_meter = cosmetics['trajectory'][
'body_cross_sections_per_meter']
# get kite locations
kite_locations = []
kite_ref_locations = []
kite_rotations = []
for kite in kite_nodes:
traj = []
traj_ref = []
rot = []
parent = parent_map[kite]
for dim in range(3):
traj.append(
cas.vertcat(plot_dict['xd']['q' + str(kite) +
str(parent)][dim]) #,
)
if cosmetics['plot_ref']:
traj_ref.append(
cas.vertcat(plot_dict['ref']['xd']['q' + str(kite) +
str(parent)][dim]))
for dim in range(9):
rot.append(
plot_dict['outputs']['aerodynamics']['r' + str(kite)][dim])
kite_locations.append(traj)
kite_ref_locations.append(traj_ref)
kite_rotations.append(rot)
old_label = None
for kdx in range(len(kite_nodes)):
if init_colors == True:
local_color = 'k'
elif init_colors == False:
local_color = cosmetics['trajectory']['colors'][kdx]
else:
local_color = init_colors
vertically_stacked_kite_locations = cas.horzcat(
kite_locations[kdx][0], kite_locations[kdx][1],
kite_locations[kdx][2])
if (cosmetics['trajectory']['kite_bodies'] and plot_kites):
pdx = 0
q_local = []
for dim in range(3):
q_local = cas.vertcat(q_local, kite_locations[kdx][dim][pdx])
r_local = []
for dim in range(9):
r_local = cas.vertcat(r_local, kite_rotations[kdx][dim][pdx])
draw_kite(ax, q_local, r_local, model_options, local_color, side,
body_cross_sections_per_meter)
if old_label == label:
label = None
make_side_plot(ax,
vertically_stacked_kite_locations,
side,
local_color,
label=label)
if cosmetics['plot_ref']:
vertically_stacked_kite_ref_locations = cas.horzcat(
kite_ref_locations[kdx][0], kite_ref_locations[kdx][1],
kite_ref_locations[kdx][2])
make_side_plot(ax,
vertically_stacked_kite_ref_locations,
side,
local_color,
label=label,
linestyle='--')
old_label = label
def discretize_constraints(self, options, model, formulation, V, P):
"""Discretize dynamics and path constraints in a (possibly) parallelizable fashion
@param options nlp options
@param model awebox model
@param formulation awebox formulation
@param V decision variables
@param P nlp parameters
"""
# rearrange nlp variables
self.__ms_nlp_vars(options, model, V, P)
# implicit values ofalgebraic variables at interval nodes
ms_z0 = self.__ms_z0
# evaluate dynamics and constraint functions on all intervals
if options['parallelization']['include']:
# use function map for parallellization
parallellization = options['parallelization']['type']
F_map = self.__F.map('F_map', parallellization, self.__n_k, [], [])
path_constraints_fun = model.constraints_fun.map(
'constraints_map', parallellization, self.__n_k, [], [])
outputs_fun = model.outputs_fun.map('outputs_fun',
parallellization, self.__n_k,
[], [])
# integrate
ms_dynamics = F_map(x0=self.__ms_x, z0=self.__ms_z, p=self.__ms_p)
ms_xf = ms_dynamics['xf']
ms_qf = cas.horzcat(np.zeros(self.__dae.dae['quad'].size()),
ms_dynamics['qf'])
ms_constraints = path_constraints_fun(self.__ms_vars,
self.__ms_params)
ms_outputs = outputs_fun(self.__ms_vars, self.__ms_params)
# integrate quadrature outputs
for i in range(self.__n_k):
ms_qf[:, i + 1] = ms_qf[:, i + 1] + ms_qf[:, i]
# extract formulation information
# constraints_fun_ineq = formulation.constraints_fun['integral']['inequality'].map('integral_constraints_map_ineq', 'serial', N_coll, [], [])
# constraints_fun_eq = formulation.constraints_fun['integral']['equality'].map('integral_constraints_map_eq', 'serial', N_coll, [], [])
# integral_constraints = OrderedDict()
# integral_constraints['inequality'] = constraints_fun_ineq(coll_vars, coll_params)
# integral_constraints['equality'] = constraints_fun_eq(coll_vars, coll_params)
else:
# initialize function evaluations
ms_xf = []
ms_qf = np.zeros(self.__dae.dae['quad'].size())
ms_constraints = []
ms_outputs = []
# integral_constraints = OrderedDict()
# integral_constraints['inequality'] = []
# integral_constraints['equality'] = []
# evaluate functions in for loop
for i in range(self.__n_k):
ms_dynamics = self.__F(x0=self.__ms_x[:, i],
z0=self.__ms_z[:, i],
p=self.__ms_p[:, i])
ms_xf = cas.horzcat(ms_xf, ms_dynamics['xf'])
ms_qf = cas.horzcat(ms_qf, ms_qf[:, -1] + ms_dynamics['qf'])
ms_constraints = cas.horzcat(
ms_constraints,
model.constraints_fun(self.__ms_vars[:, i],
self.__ms_params[:, i]))
ms_outputs = cas.horzcat(
ms_outputs,
model.outputs_fun(self.__ms_vars[:, i],
self.__ms_params[:, i]))
# integral_constraints['inequality'] = cas.horzcat(integral_constraints['inequality'], formulation.constraints_fun['integral']['inequality'](coll_vars[:,i],coll_params[:,i]))
# integral_constraints['equality'] = cas.horzcat(integral_constraints['equality'], formulation.constraints_fun['integral']['equality'](coll_vars[:,i],coll_params[:,i]))
# integral outputs and constraints
Integral_outputs_list = self.__build_integral_outputs(
ms_qf, model.integral_outputs)
# Integral_constraints_list = []
# for kdx in range(self.__n_k):
# tf = struct_op.calculate_tf(options, V, kdx)
# Integral_constraints_list += [self.__integrate_integral_constraints(integral_constraints, kdx, tf)]
Integral_constraints_list = None
# construct state derivative struct
Xdot = struct_op.construct_Xdot_struct(options, model)
Xdot = self.__fill_in_Xdot(Xdot)
return ms_xf, ms_z0, Xdot, ms_constraints, ms_outputs, Integral_outputs_list, Integral_constraints_list
def __ms_nlp_vars(self, options, model, V, P):
"""Rearrange decision variables to dae-compatible form,
allowing for parallel function evaluations
@param model awebox model
@param V nlp decision variables
@param P nlp parameters
"""
# interval parameters
param_at_time = model.parameters(cas.vertcat(P['theta0'], V['phi']))
ms_params = cas.repmat(param_at_time, 1, self.__n_k)
if options['parallelization']['include']:
# use function map for rootfinder parallellization
G_map = self.__dae.rootfinder.map(
'G_map', options['parallelization']['type'], self.__n_k, [],
[])
x_root = []
z_root = []
p_root = []
else:
# compute implicit vars in for loop
z_implicit = []
# compute explicit values of implicit variables
ms_vars0 = []
for kdx in range(self.__n_k):
# get vars at time
var_at_time = struct_op.get_variables_at_time(
options, V, None, model, kdx)
ms_vars0 += [var_at_time]
# get dae vars at time
x, z, p = self.__dae.fill_in_dae_variables(var_at_time,
param_at_time)
if not options['parallelization']['include']:
# compute implicit vars in for loop
z_at_time = self.__dae.z(self.__dae.rootfinder(z, x, p))
z_implicit = cas.horzcat(z_implicit, z_at_time)
else:
# store vars for parallelization
x_root = cas.horzcat(x_root, x)
z_root = cas.horzcat(z_root, z)
p_root = cas.horzcat(p_root, p)
if options['parallelization']['include']:
# compute implicit vars in parallel fashion
z_implicit = G_map(z_root, x_root, p_root)
# construct list of all interval variables
ms_vars = []
ms_x = []
ms_z = []
ms_p = []
for kdx in range(self.__n_k):
# fill in non-lifted vars
var_at_time = self.__set_implicit_variables(
options, ms_vars0[kdx], param_at_time,
self.__dae.z(z_implicit[:, kdx]))
# update dae vars at time
x, z, p = self.__dae.fill_in_dae_variables(var_at_time,
param_at_time)
# store result
ms_vars = cas.horzcat(ms_vars, var_at_time)
ms_x = cas.horzcat(ms_x, x)
ms_z = cas.horzcat(ms_z, z)
ms_p = cas.horzcat(ms_p, p)
self.__ms_params = ms_params
self.__ms_vars = ms_vars
self.__ms_x = ms_x
self.__ms_z = ms_z
self.__ms_z0 = z_implicit
self.__ms_p = ms_p
return None
