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 __setPhysics__(self):
# Generalized Coordinates, q = [x_B, z_B, theta_H, theta_K]
q = ca.SX.sym('q', self.n_generalized, 1)
dq = ca.SX.sym('dq', self.n_generalized, 1)
# ddq = ca.SX.sym('ddq', self.n_generalized, 1)
# Inputs and external force, u = [u_H, u_K]
u = ca.SX.sym('u', self.dof, 1)
lam = ca.SX.sym('lambda', 3, self.n_contact)
"""Hopper Dynamics"""
"Joint positions from Base to end effector"
p = ca.SX.zeros(2, self.n_joints)
p[0, 0], p[1, 0] = q[0] + (self.length[1, 0]) * ca.sin(
q[2]), q[1] - (self.length[1, 0]) * ca.cos(q[2])
p[0, 1], p[1,
1] = p[0, 0] + self.length[2, 0] * ca.sin(q[2] + q[3]), p[
1, 0] - self.length[2, 0] * ca.cos(q[2] + q[3])
# COG
g = ca.SX.zeros(2, self.n_joints + 1)
g[0, 0], g[1, 0] = q[0], q[1]
g[0, 1], g[1, 1] = p[0, 0] / 2, p[1, 0] / 2
g[0, 2], g[
1, 2] = p[0, 0] + (self.length[2, 0] / 2) * ca.sin(q[2] + q[3]), p[
1, 0] - (self.length[2, 0] / 2) * ca.cos(q[2] + q[3])
J_ang = ca.DM([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1., 0],
[0, 0, 1., 1.]])
a = (J_ang @ q).T
J_ee = ca.jacobian(p[:, -1], q)
"For inertia matrix"
H = ca.SX.zeros(self.n_generalized, self.n_generalized)
for i in range(self.dof):
J_l = ca.jacobian(p[:, i], q)
J_a = ca.jacobian(a[:, i], q)
I = (1 / 12) * self.mass[i] * (self.length[i, 0]**2) # Rod
M = self.mass[i]
H += M * J_l.T @ J_l + I * J_a.T @ J_a
"For coriolis + centrifugal matrix"
C = ca.SX.zeros(self.n_generalized, self.n_generalized)
for i in range(self.n_generalized):
for j in range(self.n_generalized):
sum_ = 0
for k in range(self.n_generalized):
c_ijk = ca.jacobian(H[i, j], q[k]) + ca.jacobian(
H[i, j], q[j]) - ca.jacobian(H[k, j], q[i])
sum_ += c_ijk @ dq[k]
C[i, j] = (1 / 2) * sum_
# sum_ = 0
# for k in range(self.dof):
# c_ijk = ca.jacobian(H[i, j], q[k]) - (1/2)*ca.jacobian(H[j, k], q[i])
# sum_ += c_ijk @ dq[j] @ dq[k]
# C[i, j] = sum_
"For G matrix"
V = self.gravity * ca.sum1(self.mass * g[1, :].T)
G = ca.jacobian(V, q).T
"For B matrix"
B = ca.DM([[0., 0.], [0., 0.], [1., 0.], [0., 1.]])
"For external force"
pe_terrain = ca.SX.zeros(2, 1)
pe_terrain[0, 0] = p[0, -1]
pe_terrain[1, 0] = self.terrain.heightMap(p[0, -1])
phi = p[:, -1] - pe_terrain
B_J_C = ca.jacobian(phi, q)
theta_contact = ca.acos(
ca.dot(self.terrain.heightMapNormalVector(p[0, -1]), ca.DM([0,
1])))
# rotate lambda force represented in contact frame to world frame
B_Rot_C = ca.SX.zeros(2, 2)
B_Rot_C[0,
0], B_Rot_C[0,
1] = ca.cos(theta_contact), ca.sin(theta_contact)
B_Rot_C[1,
0], B_Rot_C[1,
1] = -ca.sin(theta_contact), ca.cos(theta_contact)
C_lam = ca.SX.zeros(2, 1)
C_lam[0, 0], C_lam[1, 0] = lam[0, 0] - lam[1, 0], lam[2, 0]
B_lam = B_Rot_C @ C_lam
dp = J_ee @ dq
# print(dp.shape)
psi = ca.dot(self.terrain.heightMapTangentVector(pe_terrain[0, 0]), dp)
# psi = dp[0, 0]
self.kinematics = ca.Function('Kinematics', [q], [p, g], ['q'],
['p', 'g'])
self.dynamics = ca.Function('Dynamics', [q, dq, u, lam], [
H, C, B, G, phi, J_ee, B_J_C, C_lam, B_lam, psi
], ['q', 'dq', 'u', 'lam'], [
'H', 'C', 'B', 'G', 'phi', 'J_ee', 'B_J_C', 'C_lam', 'B_lam', 'psi'
])
def _initialize_mav_objective(self):
""" Initialize the objective function to minimize the absolute value of
the parameter vector """
self.objective_sx += (self.pvar.alpha_sx[:] *
cs.sum1(cs.fabs(self.var.p_sx[:])))
def Max_velocity(pts, conf, show=False):
mu = conf.mu
m = conf.m
g = conf.g
l_f = conf.l_f
l_r = conf.l_r
safety_f = conf.force_f
f_max = mu * m * g * safety_f
f_long_max = l_f / (l_r + l_f) * f_max
max_v = conf.max_v
max_a = conf.max_a
s_i, th_i = convert_pts_s_th(pts)
th_i_1 = th_i[:-1]
s_i_1 = s_i[:-1]
N = len(s_i)
N1 = len(s_i) - 1
# setup possible casadi functions
d_x = ca.MX.sym('d_x', N - 1)
d_y = ca.MX.sym('d_y', N - 1)
vel = ca.Function('vel', [d_x, d_y],
[ca.sqrt(ca.power(d_x, 2) + ca.power(d_y, 2))])
dx = ca.MX.sym('dx', N)
dy = ca.MX.sym('dy', N)
dt = ca.MX.sym('t', N - 1)
f_long = ca.MX.sym('f_long', N - 1)
f_lat = ca.MX.sym('f_lat', N - 1)
nlp = {\
'x': ca.vertcat(dx, dy, dt, f_long, f_lat),
'f': ca.sum1(dt),
'g': ca.vertcat(
# dynamic constraints
dt - s_i_1 / ((vel(dx[:-1], dy[:-1]) + vel(dx[1:], dy[1:])) / 2 ),
# ca.arctan2(dy, dx) - th_i,
dx/dy - ca.tan(th_i),
dx[1:] - (dx[:-1] + (ca.sin(th_i_1) * f_long + ca.cos(th_i_1) * f_lat) * dt / m),
dy[1:] - (dy[:-1] + (ca.cos(th_i_1) * f_long - ca.sin(th_i_1) * f_lat) * dt / m),
# path constraints
ca.sqrt(ca.power(f_long, 2) + ca.power(f_lat, 2)),
# boundary constraints
# dx[0], dy[0]
) \
}
# S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':0}})
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 5}})
# make init sol
v0 = np.ones(N) * max_v / 2
dx0 = v0 * np.sin(th_i)
dy0 = v0 * np.cos(th_i)
dt0 = s_i_1 / ca.sqrt(ca.power(dx0[:-1], 2) + ca.power(dy0[:-1], 2))
f_long0 = np.zeros(N - 1)
ddx0 = dx0[1:] - dx0[:-1]
ddy0 = dy0[1:] - dy0[:-1]
a0 = (ddx0**2 + ddy0**2)**0.5
f_lat0 = a0 * m
x0 = ca.vertcat(dx0, dy0, dt0, f_long0, f_lat0)
# make lbx, ubx
# lbx = [-max_v] * N + [-max_v] * N + [0] * N1 + [-f_long_max] * N1 + [-f_max] * N1
# lbx = [-max_v] * N + [0] * N + [0] * N1 + [-f_long_max] * N1 + [-f_max] * N1
# ubx = [max_v] * N + [max_v] * N + [10] * N1 + [f_long_max] * N1 + [f_max] * N1
lbx = [-max_v] * N + [0] * N + [0] * N1 + [-ca.inf] * N1 + [-f_max] * N1
ubx = [max_v] * N + [max_v] * N + [10] * N1 + [ca.inf] * N1 + [f_max] * N1
#make lbg, ubg
lbg = [0] * N1 + [0] * N + [0] * 2 * N1 + [0] * N1 #+ [0] * 2
# ubg = [0] * N1 + [0] * N + [0] * 2 * N1 + [f_max] * N1 #+ [0] * 2
ubg = [0] * N1 + [0] * N + [0] * 2 * N1 + [ca.inf] * N1 #+ [0] * 2
r = S(x0=x0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
x_opt = r['x']
dx = np.array(x_opt[:N])
dy = np.array(x_opt[N:N * 2])
dt = np.array(x_opt[2 * N:N * 2 + N1])
f_long = np.array(x_opt[2 * N + N1:2 * N + N1 * 2])
f_lat = np.array(x_opt[-N1:])
f_t = (f_long**2 + f_lat**2)**0.5
# print(f"Dt: {dt.T}")
# print(f"DT0: {dt[0]}")
t = np.cumsum(dt)
t = np.insert(t, 0, 0)
# print(f"Dt: {dt.T}")
# print(f"Dx: {dx.T}")
# print(f"Dy: {dy.T}")
vs = (dx**2 + dy**2)**0.5
if show:
plt.figure(1)
plt.clf()
plt.title("Velocity vs dt")
plt.plot(t, vs)
plt.plot(t, th_i)
plt.legend(['vs', 'ths'])
# plt.plot(t, dx)
# plt.plot(t, dy)
# plt.legend(['v', 'dx', 'dy'])
plt.plot(t, np.ones_like(t) * max_v, '--')
plt.figure(3)
plt.clf()
plt.title("F_long, F_lat vs t")
plt.plot(t[:-1], f_long)
plt.plot(t[:-1], f_lat)
plt.plot(t[:-1], f_t, linewidth=3)
plt.plot(t, np.ones_like(t) * f_max, '--')
plt.plot(t, np.ones_like(t) * -f_max, '--')
plt.plot(t, np.ones_like(t) * f_long_max, '--')
plt.plot(t, np.ones_like(t) * -f_long_max, '--')
plt.ylim([-25, 25])
plt.legend(['Flong', "f_lat", "f_t"])
# plt.show()
plt.pause(0.001)
# plt.figure(9)
# plt.clf()
# plt.title("F_long, F_lat vs t")
# plt.plot(t[:-1], f_long)
# plt.plot(t[:-1], f_lat)
# plt.plot(t[:-1], f_t, linewidth=3)
# plt.plot(t, np.ones_like(t) * f_max, '--')
# plt.plot(t, np.ones_like(t) * -f_max, '--')
# plt.plot(t, np.ones_like(t) * f_long_max, '--')
# plt.plot(t, np.ones_like(t) * -f_long_max, '--')
# plt.legend(['Flong', "f_lat", "f_t"])
return vs
def compute_penalty_values(t, x, u, p, penalty, dt):
"""
Compute the penalty value for the given time, state, control, parameters, penalty and time step
Parameters
----------
t: int
Time index
x: ndarray
State vector with intermediate states
u: ndarray
Control vector with starting control (and sometimes final control)
p: ndarray
Parameters vector
penalty: Penalty
The penalty object containing details on how to compute it
dt: float
Time step for the whole interval
Returns
-------
Values computed for the given time, state, control, parameters, penalty and time step
"""
if len(x.shape) < 2:
x = x.reshape((-1, 1))
# if time is parameter of the ocp, we need to evaluate with current parameters
if isinstance(dt, Function):
dt = dt(p)
# The division is to account for the steps in the integration. The else is for Mayer term
dt = dt / (x.shape[1] - 1) if x.shape[1] > 1 else dt
if not isinstance(penalty.dt, (float, int)):
if dt.shape[0] > 1:
dt = dt[penalty.phase]
_target = (np.hstack([
p[..., penalty.node_idx.index(t)] for p in penalty.target
]) if penalty.target is not None and isinstance(t, int) else [])
out = []
if penalty.transition or penalty.multinode_constraint:
out.append(
penalty.weighted_function_non_threaded(
x.reshape((-1, 1)), u.reshape((-1, 1)), p,
penalty.weight, _target, dt))
elif penalty.derivative or penalty.explicit_derivative:
out.append(
penalty.weighted_function_non_threaded(
x[:, [0, -1]], u, p, penalty.weight, _target, dt))
elif (penalty.integration_rule == IntegralApproximation.TRAPEZOIDAL
or penalty.integration_rule
== IntegralApproximation.TRUE_TRAPEZOIDAL):
out = [
penalty.weighted_function_non_threaded(
x[:, [i, i + 1]], u[:, i], p, penalty.weight, _target,
dt) for i in range(x.shape[1] - 1)
]
else:
out.append(
penalty.weighted_function_non_threaded(
x, u, p, penalty.weight, _target, dt))
return sum1(horzcat(*out))
nominal_diameter = cas.exp(log_nominal_diameter)
thickness = 0.14e-3 * 5
opti.subject_to([
nominal_diameter > thickness,
])
def trapz(x):
out = (x[:-1] + x[1:]) / 2
out[0] += x[0] / 2
out[-1] += x[-1] / 2
return out
# Mass
volume = cas.sum1(cas.pi / 4 * trapz((nominal_diameter + thickness)**2 -
(nominal_diameter - thickness)**2) *
dx)
mass = volume * 1600
# Bending loads
I = cas.pi / 64 * ((nominal_diameter + thickness)**4 -
(nominal_diameter - thickness)**4)
EI = E * I
total_lift_force = 9.81 * (mass_total - mass) / 2 # 9.81 * 292 / 2
lift_distribution = "elliptical"
if lift_distribution == "rectangular":
force_per_unit_length = total_lift_force * cas.GenDM_ones(n) / L
elif lift_distribution == "elliptical":
force_per_unit_length = total_lift_force * cas.sqrt(1 - (x / L)**2) * (
4 / cas.pi) / L
def exitExpression(self, tree):
if isinstance(tree.operator, ast.ComponentRef):
op = tree.operator.name
else:
op = tree.operator
if op == '*':
op = 'mtimes' # .* differs from *
if op.startswith('.'):
op = op[1:]
logger.debug('exitExpression')
n_operands = len(tree.operands)
if op == 'der':
v = self.get_mx(tree.operands[0])
src = self.get_derivative(v)
elif op == '-' and n_operands == 1:
src = -self.get_mx(tree.operands[0])
elif op == 'not' and n_operands == 1:
src = ca.if_else(self.get_mx(tree.operands[0]), 0, 1, True)
elif op == 'mtimes':
assert n_operands >= 2
src = self.get_mx(tree.operands[0])
for i in tree.operands[1:]:
src = ca.mtimes(src, self.get_mx(i))
elif op == 'transpose' and n_operands == 1:
src = self.get_mx(tree.operands[0]).T
elif op == 'sum' and n_operands == 1:
v = self.get_mx(tree.operands[0])
src = ca.sum1(v)
elif op == 'linspace' and n_operands == 3:
a = self.get_mx(tree.operands[0])
b = self.get_mx(tree.operands[1])
n_steps = self.get_integer(tree.operands[2])
src = ca.linspace(a, b, n_steps)
elif op == 'fill' and n_operands == 2:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
src = val * ca.DM.ones(n_row)
elif op == 'fill' and n_operands == 3:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
n_col = self.get_integer(tree.operands[2])
src = val * ca.DM.ones(n_row, n_col)
elif op == 'zeros' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.zeros(n_row)
elif op == 'zeros' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.zeros(n_row, n_col)
elif op == 'ones' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.ones(n_row)
elif op == 'ones' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.ones(n_row, n_col)
elif op == 'identity' and n_operands == 1:
n = self.get_integer(tree.operands[0])
src = ca.DM.eye(n)
elif op == 'diagonal' and n_operands == 1:
diag = self.get_mx(tree.operands[0])
n = len(diag)
indices = list(range(n))
src = ca.DM.triplet(indices, indices, diag, n, n)
elif op == 'cat':
axis = self.get_integer(tree.operands[0])
assert axis == 1, "Currently only concatenation on first axis is supported"
entries = []
for sym in [self.get_mx(op) for op in tree.operands[1:]]:
if isinstance(sym, list):
for e in sym:
entries.append(e)
else:
entries.append(sym)
src = ca.vertcat(*entries)
elif op == 'delay' and n_operands == 2:
expr = self.get_mx(tree.operands[0])
duration = self.get_mx(tree.operands[1])
src = _new_mx('_pymoca_delay_{}'.format(self.delay_counter), *expr.size())
self.delay_counter += 1
for f in self.for_loops:
syms = set(ca.symvar(expr))
if syms.intersection(f.indexed_symbols):
f.register_indexed_symbol(src, lambda i: i, True, tree.operands[0], f.index_variable)
self.model.delay_states.append(src.name())
self.model.inputs.append(Variable(src))
delay_argument = DelayArgument(expr, duration)
self.model.delay_arguments.append(delay_argument)
elif op == '_pymoca_interp1d' and n_operands >= 3 and n_operands <= 4:
entered_class = self.entered_classes[-1]
if isinstance(tree.operands[0], ast.ComponentRef):
xp = self.get_mx(entered_class.symbols[tree.operands[0].name].value)
else:
xp = self.get_mx(tree.operands[0])
if isinstance(tree.operands[1], ast.ComponentRef):
yp = self.get_mx(entered_class.symbols[tree.operands[1].name].value)
else:
yp = self.get_mx(tree.operands[1])
arg = self.get_mx(tree.operands[2])
if n_operands == 4:
assert isinstance(tree.operands[3], ast.Primary)
mode = tree.operands[3].value
else:
mode = 'linear'
func = ca.interpolant('interpolant', mode, [xp], yp)
src = func(arg)
elif op == '_pymoca_interp2d' and n_operands >= 5 and n_operands <= 6:
entered_class = self.entered_classes[-1]
if isinstance(tree.operands[0], ast.ComponentRef):
xp = self.get_mx(entered_class.symbols[tree.operands[0].name].value)
else:
xp = self.get_mx(tree.operands[0])
if isinstance(tree.operands[1], ast.ComponentRef):
yp = self.get_mx(entered_class.symbols[tree.operands[1].name].value)
else:
yp = self.get_mx(tree.operands[1])
if isinstance(tree.operands[2], ast.ComponentRef):
zp = self.get_mx(entered_class.symbols[tree.operands[2].name].value)
else:
zp = self.get_mx(tree.operands[2])
arg_1 = self.get_mx(tree.operands[3])
arg_2 = self.get_mx(tree.operands[4])
if n_operands == 6:
assert isinstance(tree.operands[5], ast.Primary)
mode = tree.operands[5].value
else:
mode = 'linear'
func = ca.interpolant('interpolant', mode, [xp, yp], np.array(zp).ravel(order='F'))
src = func(ca.vertcat(arg_1, arg_2))
elif op in OP_MAP and n_operands == 2:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op(rhs)
elif op in OP_MAP and n_operands == 1:
lhs = ca.MX(self.get_mx(tree.operands[0]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op()
else:
src = ca.MX(self.get_mx(tree.operands[0]))
# Check for built-in operations, such as the
# elementary functions, first.
if hasattr(src, op) and n_operands <= 2:
if n_operands == 1:
src = ca.MX(self.get_mx(tree.operands[0]))
src = getattr(src, op)()
else:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, op)
src = lhs_op(rhs)
else:
func = self.get_function(op)
src = ca.vertcat(*func.call([self.get_mx(operand) for operand in tree.operands], *self.function_mode))
self.src[tree] = src
def calc_NLL(hyper, X, Y, squaredist, meanFunc='zero', prior=None):
""" Objective function
Calculate the negative log likelihood function using Casadi SX symbols.
# Arguments:
hyper: Array with hyperparameters [ell_1 .. ell_Nx sf sn], where Nx is the
number of inputs to the GP.
X: Training data matrix with inputs of size (N x Nx).
Y: Training data matrix with outpyts of size (N x Ny),
with Ny number of outputs.
# Returns:
NLL: The negative log likelihood function (scalar)
"""
N, Nx = ca.MX.size(X)
ell = hyper[:Nx]
sf2 = hyper[Nx]**2
sn2 = hyper[Nx + 1]**2
m = get_mean_function(hyper, X.T, func=meanFunc)
# Calculate covariance matrix
K_s = ca.SX.sym('K_s', N, N)
sqdist = ca.SX.sym('sqd', N, N)
elli = ca.SX.sym('elli')
ki = ca.Function('ki', [sqdist, elli, K_s], [sqdist / elli**2 + K_s])
K1 = ca.MX(N, N)
for i in range(Nx):
K1 = ki(squaredist[:, (i * N):(i + 1) * N], ell[i], K1)
sf2_s = ca.SX.sym('sf2')
exponent = ca.SX.sym('exp', N, N)
K_exp = ca.Function('K', [exponent, sf2_s],
[sf2_s * ca.SX.exp(-.5 * exponent)])
K2 = K_exp(K1, sf2)
K = K2 + sn2 * ca.MX.eye(N)
K = (K + K.T) * 0.5 # Make sure matrix is symmentric
A = ca.SX.sym('A', ca.MX.size(K))
cholesky = ca.Function('cholesky', [A], [ca.chol(A).T])
L = cholesky(K)
B = 2 * ca.sum1(ca.SX.log(ca.diag(A)))
log_determinant = ca.Function('log_det', [A], [B])
log_detK = log_determinant(L)
Y_s = ca.SX.sym('Y', ca.MX.size(Y))
L_s = ca.SX.sym('L', ca.Sparsity.lower(N))
sol = ca.Function('sol', [L_s, Y_s], [ca.solve(L_s, Y_s)])
invLy = sol(L, Y - m(X.T))
invLy_s = ca.SX.sym('invLy', ca.MX.size(invLy))
sol2 = ca.Function('sol2', [L_s, invLy_s], [ca.solve(L_s.T, invLy_s)])
alpha = sol2(L, invLy)
alph = ca.SX.sym('alph', ca.MX.size(alpha))
detK = ca.SX.sym('det')
# Calculate hyperpriors
theta = ca.SX.sym('theta')
mu = ca.SX.sym('mu')
s2 = ca.SX.sym('s2')
prior_gauss = ca.Function(
'hyp_prior', [theta, mu, s2],
[-(theta - mu)**2 / (2 * s2) - 0.5 * ca.log(2 * ca.pi * s2)])
log_prior = 0
if prior is not None:
for i in range(Nx):
log_prior += prior_gauss(ell[i], prior['ell_mean'],
prior['ell_std']**2)
log_prior += prior_gauss(sf2, prior['sf_mean'], prior['sf_std']**2)
log_prior += prior_gauss(sn2, prior['sn_mean'], prior['sn_std']**2)
NLL = ca.Function('NLL', [Y_s, alph, detK],
[0.5 * ca.mtimes(Y_s.T, alph) + 0.5 * detK])
return NLL(Y - m(X.T), alpha, log_detK) + log_prior
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 opt_mintime(reftrack: np.ndarray,
coeffs_x: np.ndarray,
coeffs_y: np.ndarray,
normvectors: np.ndarray,
pars: dict,
tpamap_path: str,
tpadata_path: str,
export_path: str,
print_debug: bool = False,
plot_debug: bool = False) -> tuple:
"""
Created by:
Fabian Christ
Documentation:
The minimum lap time problem is described as an optimal control problem, converted to a nonlinear program using
direct orthogonal Gauss-Legendre collocation and then solved by the interior-point method IPOPT. Reduced computing
times are achieved using a curvilinear abscissa approach for track description, algorithmic differentiation using
the software framework CasADi, and a smoothing of the track input data by approximate spline regression. The
vehicles behavior is approximated as a double track model with quasi-steady state tire load simplification and
nonlinear tire model.
Please refer to our paper for further information:
Christ, Wischnewski, Heilmeier, Lohmann
Time-Optimal Trajectory Planning for a Race Car Considering Variable Tire-Road Friction Coefficients
Inputs:
reftrack: track [x_m, y_m, w_tr_right_m, w_tr_left_m]
coeffs_x: coefficient matrix of the x splines with size (no_splines x 4)
coeffs_y: coefficient matrix of the y splines with size (no_splines x 4)
normvectors: array containing normalized normal vectors for every traj. point [x_component, y_component]
pars: parameters dictionary
tpamap_path: file path to tpa map (required for friction map loading)
tpadata_path: file path to tpa data (required for friction map loading)
export_path: path to output folder for warm start files and solution files
print_debug: determines if debug messages are printed
plot_debug: determines if debug plots are shown
Outputs:
alpha_opt: solution vector of the optimization problem containing the lateral shift in m for every point
v_opt: velocity profile for the raceline
"""
# ------------------------------------------------------------------------------------------------------------------
# PREPARE TRACK INFORMATION ----------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# spline lengths
spline_lengths_refline = tph.calc_spline_lengths.calc_spline_lengths(coeffs_x=coeffs_x,
coeffs_y=coeffs_y)
# calculate heading and curvature (numerically)
kappa_refline = tph.calc_head_curv_num. \
calc_head_curv_num(path=reftrack[:, :2],
el_lengths=spline_lengths_refline,
is_closed=True,
stepsize_curv_preview=pars["curv_calc_opts"]["d_preview_curv"],
stepsize_curv_review=pars["curv_calc_opts"]["d_review_curv"],
stepsize_psi_preview=pars["curv_calc_opts"]["d_preview_head"],
stepsize_psi_review=pars["curv_calc_opts"]["d_review_head"])[1]
# close track
kappa_refline_cl = np.append(kappa_refline, kappa_refline[0])
w_tr_left_cl = np.append(reftrack[:, 3], reftrack[0, 3])
w_tr_right_cl = np.append(reftrack[:, 2], reftrack[0, 2])
# step size along the reference line
h = pars["stepsize_opts"]["stepsize_reg"]
# optimization steps (0, 1, 2 ... end point/start point)
steps = [i for i in range(kappa_refline_cl.size)]
# number of control intervals
N = steps[-1]
# station along the reference line
s_opt = np.linspace(0.0, N * h, N + 1)
# interpolate curvature of reference line in terms of steps
kappa_interp = ca.interpolant('kappa_interp', 'linear', [steps], kappa_refline_cl)
# interpolate track width (left and right to reference line) in terms of steps
w_tr_left_interp = ca.interpolant('w_tr_left_interp', 'linear', [steps], w_tr_left_cl)
w_tr_right_interp = ca.interpolant('w_tr_right_interp', 'linear', [steps], w_tr_right_cl)
# describe friction coefficients from friction map with linear equations or gaussian basis functions
if pars["optim_opts"]["var_friction"] is not None:
w_mue_fl, w_mue_fr, w_mue_rl, w_mue_rr, center_dist = opt_mintime_traj.src. \
approx_friction_map.approx_friction_map(reftrack=reftrack,
normvectors=normvectors,
tpamap_path=tpamap_path,
tpadata_path=tpadata_path,
pars=pars,
dn=pars["optim_opts"]["dn"],
n_gauss=pars["optim_opts"]["n_gauss"],
print_debug=print_debug,
plot_debug=plot_debug)
# ------------------------------------------------------------------------------------------------------------------
# DIRECT GAUSS-LEGENDRE COLLOCATION --------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# degree of interpolating polynomial
d = 3
# legendre collocation points
tau = np.append(0, ca.collocation_points(d, 'legendre'))
# coefficient matrix for formulating the collocation equation
C = np.zeros((d + 1, d + 1))
# coefficient matrix for formulating the collocation equation
D = np.zeros(d + 1)
# coefficient matrix for formulating the collocation equation
B = np.zeros(d + 1)
# construct polynomial basis
for j in range(d + 1):
# construct Lagrange polynomials to get the polynomial basis at the collocation point
p = np.poly1d([1])
for r in range(d + 1):
if r != j:
p *= np.poly1d([1, -tau[r]]) / (tau[j] - tau[r])
# evaluate polynomial at the final time to get the coefficients of the continuity equation
D[j] = p(1.0)
# evaluate time derivative of polynomial at collocation points to get the coefficients of continuity equation
p_der = np.polyder(p)
for r in range(d + 1):
C[j, r] = p_der(tau[r])
# evaluate integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
B[j] = pint(1.0)
# ------------------------------------------------------------------------------------------------------------------
# STATE VARIABLES --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# number of state variables
nx = 5
# velocity [m/s]
v_n = ca.SX.sym('v_n')
v_s = 50
v = v_s * v_n
# side slip angle [rad]
beta_n = ca.SX.sym('beta_n')
beta_s = 0.5
beta = beta_s * beta_n
# yaw rate [rad/s]
omega_z_n = ca.SX.sym('omega_z_n')
omega_z_s = 1
omega_z = omega_z_s * omega_z_n
# lateral distance to reference line (positive = left) [m]
n_n = ca.SX.sym('n_n')
n_s = 5.0
n = n_s * n_n
# relative angle to tangent on reference line [rad]
xi_n = ca.SX.sym('xi_n')
xi_s = 1.0
xi = xi_s * xi_n
# scaling factors for state variables
x_s = np.array([v_s, beta_s, omega_z_s, n_s, xi_s])
# put all states together
x = ca.vertcat(v_n, beta_n, omega_z_n, n_n, xi_n)
# ------------------------------------------------------------------------------------------------------------------
# CONTROL VARIABLES ------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# number of control variables
nu = 4
# steer angle [rad]
delta_n = ca.SX.sym('delta_n')
delta_s = 0.5
delta = delta_s * delta_n
# positive longitudinal force (drive) [N]
f_drive_n = ca.SX.sym('f_drive_n')
f_drive_s = 7500.0
f_drive = f_drive_s * f_drive_n
# negative longitudinal force (brake) [N]
f_brake_n = ca.SX.sym('f_brake_n')
f_brake_s = 20000.0
f_brake = f_brake_s * f_brake_n
# lateral wheel load transfer [N]
gamma_y_n = ca.SX.sym('gamma_y_n')
gamma_y_s = 5000.0
gamma_y = gamma_y_s * gamma_y_n
# scaling factors for control variables
u_s = np.array([delta_s, f_drive_s, f_brake_s, gamma_y_s])
# put all controls together
u = ca.vertcat(delta_n, f_drive_n, f_brake_n, gamma_y_n)
# ------------------------------------------------------------------------------------------------------------------
# MODEL EQUATIONS --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# extract vehicle and tire parameters
veh = pars["vehicle_params_mintime"]
tire = pars["tire_params_mintime"]
# general constants
g = pars["veh_params"]["g"]
mass = pars["veh_params"]["mass"]
# curvature of reference line [rad/m]
kappa = ca.SX.sym('kappa')
# drag force [N]
f_xdrag = pars["veh_params"]["dragcoeff"] * v ** 2
# rolling resistance forces [N]
f_xroll_fl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_xroll_fr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_xroll_rl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
f_xroll_rr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
f_xroll = tire["c_roll"] * mass * g
# static normal tire forces [N]
f_zstat_fl = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_zstat_fr = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_zstat_rl = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
f_zstat_rr = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
# dynamic normal tire forces (aerodynamic downforces) [N]
f_zlift_fl = 0.5 * veh["liftcoeff_front"] * v ** 2
f_zlift_fr = 0.5 * veh["liftcoeff_front"] * v ** 2
f_zlift_rl = 0.5 * veh["liftcoeff_rear"] * v ** 2
f_zlift_rr = 0.5 * veh["liftcoeff_rear"] * v ** 2
# dynamic normal tire forces (load transfers) [N]
f_zdyn_fl = (-0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
- veh["k_roll"] * gamma_y)
f_zdyn_fr = (-0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
+ veh["k_roll"] * gamma_y)
f_zdyn_rl = (0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
- (1.0 - veh["k_roll"]) * gamma_y)
f_zdyn_rr = (0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
+ (1.0 - veh["k_roll"]) * gamma_y)
# sum of all normal tire forces [N]
f_z_fl = f_zstat_fl + f_zlift_fl + f_zdyn_fl
f_z_fr = f_zstat_fr + f_zlift_fr + f_zdyn_fr
f_z_rl = f_zstat_rl + f_zlift_rl + f_zdyn_rl
f_z_rr = f_zstat_rr + f_zlift_rr + f_zdyn_rr
# slip angles [rad]
alpha_fl = delta - ca.atan((v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) /
(v * ca.cos(beta) - 0.5 * veh["track_width_front"] * omega_z))
alpha_fr = delta - ca.atan((v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) /
(v * ca.cos(beta) + 0.5 * veh["track_width_front"] * omega_z))
alpha_rl = ca.atan((-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) /
(v * ca.cos(beta) - 0.5 * veh["track_width_rear"] * omega_z))
alpha_rr = ca.atan((-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) /
(v * ca.cos(beta) + 0.5 * veh["track_width_rear"] * omega_z))
# lateral tire forces [N]
f_y_fl = (pars["optim_opts"]["mue"] * f_z_fl * (1 + tire["eps_front"] * f_z_fl / tire["f_z0"])
* ca.sin(tire["C_front"] * ca.atan(tire["B_front"] * alpha_fl - tire["E_front"]
* (tire["B_front"] * alpha_fl - ca.atan(tire["B_front"] * alpha_fl)))))
f_y_fr = (pars["optim_opts"]["mue"] * f_z_fr * (1 + tire["eps_front"] * f_z_fr / tire["f_z0"])
* ca.sin(tire["C_front"] * ca.atan(tire["B_front"] * alpha_fr - tire["E_front"]
* (tire["B_front"] * alpha_fr - ca.atan(tire["B_front"] * alpha_fr)))))
f_y_rl = (pars["optim_opts"]["mue"] * f_z_rl * (1 + tire["eps_rear"] * f_z_rl / tire["f_z0"])
* ca.sin(tire["C_rear"] * ca.atan(tire["B_rear"] * alpha_rl - tire["E_rear"]
* (tire["B_rear"] * alpha_rl - ca.atan(tire["B_rear"] * alpha_rl)))))
f_y_rr = (pars["optim_opts"]["mue"] * f_z_rr * (1 + tire["eps_rear"] * f_z_rr / tire["f_z0"])
* ca.sin(tire["C_rear"] * ca.atan(tire["B_rear"] * alpha_rr - tire["E_rear"]
* (tire["B_rear"] * alpha_rr - ca.atan(tire["B_rear"] * alpha_rr)))))
# longitudinal tire forces [N]
f_x_fl = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh["k_brake_front"] - f_xroll_fl
f_x_fr = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh["k_brake_front"] - f_xroll_fr
f_x_rl = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (1 - veh["k_brake_front"]) - f_xroll_rl
f_x_rr = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (1 - veh["k_brake_front"]) - f_xroll_rr
# longitudinal acceleration [m/s²]
ax = (f_x_rl + f_x_rr + (f_x_fl + f_x_fr) * ca.cos(delta) - (f_y_fl + f_y_fr) * ca.sin(delta)
- pars["veh_params"]["dragcoeff"] * v ** 2) / mass
# lateral acceleration [m/s²]
ay = ((f_x_fl + f_x_fr) * ca.sin(delta) + f_y_rl + f_y_rr + (f_y_fl + f_y_fr) * ca.cos(delta)) / mass
# time-distance scaling factor (dt/ds)
sf = (1.0 - n * kappa) / (v * (ca.cos(xi + beta)))
# model equations for two track model (ordinary differential equations)
dv = (sf / mass) * ((f_x_rl + f_x_rr) * ca.cos(beta) + (f_x_fl + f_x_fr) * ca.cos(delta - beta)
+ (f_y_rl + f_y_rr) * ca.sin(beta) - (f_y_fl + f_y_fr) * ca.sin(delta - beta)
- f_xdrag * ca.cos(beta))
dbeta = sf * (-omega_z + (-(f_x_rl + f_x_rr) * ca.sin(beta) + (f_x_fl + f_x_fr) * ca.sin(delta - beta)
+ (f_y_rl + f_y_rr) * ca.cos(beta) + (f_y_fl + f_y_fr) * ca.cos(delta - beta)
+ f_xdrag * ca.sin(beta)) / (mass * v))
domega_z = (sf / veh["I_z"]) * ((f_x_rr - f_x_rl) * veh["track_width_rear"] / 2
- (f_y_rl + f_y_rr) * veh["wheelbase_rear"]
+ ((f_x_fr - f_x_fl) * ca.cos(delta)
+ (f_y_fl - f_y_fr) * ca.sin(delta)) * veh["track_width_front"] / 2
+ ((f_y_fl + f_y_fr) * ca.cos(delta)
+ (f_x_fl + f_x_fr) * ca.sin(delta)) * veh["track_width_front"])
dn = sf * v * ca.sin(xi + beta)
dxi = sf * omega_z - kappa
# put all ODEs together
dx = ca.vertcat(dv, dbeta, domega_z, dn, dxi) / x_s
# ------------------------------------------------------------------------------------------------------------------
# CONTROL BOUNDARIES -----------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
delta_min = -veh["delta_max"] / delta_s # min. steer angle [rad]
delta_max = veh["delta_max"] / delta_s # max. steer angle [rad]
f_drive_min = 0.0 # min. longitudinal drive force [N]
f_drive_max = veh["f_drive_max"] / f_drive_s # max. longitudinal drive force [N]
f_brake_min = -veh["f_brake_max"] / f_brake_s # min. longitudinal brake force [N]
f_brake_max = 0.0 # max. longitudinal brake force [N]
gamma_y_min = -np.inf # min. lateral wheel load transfer [N]
gamma_y_max = np.inf # max. lateral wheel load transfer [N]
# ------------------------------------------------------------------------------------------------------------------
# STATE BOUNDARIES -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
v_min = 1.0 / v_s # min. velocity [m/s]
v_max = pars["veh_params"]["v_max"] / v_s # max. velocity [m/s]
beta_min = -0.5 * np.pi / beta_s # min. side slip angle [rad]
beta_max = 0.5 * np.pi / beta_s # max. side slip angle [rad]
omega_z_min = - 0.5 * np.pi / omega_z_s # min. yaw rate [rad/s]
omega_z_max = 0.5 * np.pi / omega_z_s # max. yaw rate [rad/s]
xi_min = - 0.5 * np.pi / xi_s # min. relative angle to tangent on reference line [rad]
xi_max = 0.5 * np.pi / xi_s # max. relative angle to tangent on reference line [rad]
# ------------------------------------------------------------------------------------------------------------------
# INITIAL GUESS FOR DECISION VARIABLES -----------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
v_guess = 20.0 / v_s
# ------------------------------------------------------------------------------------------------------------------
# HELPER FUNCTIONS -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# continuous time dynamics
f_dyn = ca.Function('f_dyn', [x, u, kappa], [dx, sf], ['x', 'u', 'kappa'], ['dx', 'sf'])
# longitudinal tire forces [N]
f_fx = ca.Function('f_fx', [x, u], [f_x_fl, f_x_fr, f_x_rl, f_x_rr],
['x', 'u'], ['f_x_fl', 'f_x_fr', 'f_x_rl', 'f_x_rr'])
# lateral tire forces [N]
f_fy = ca.Function('f_fy', [x, u], [f_y_fl, f_y_fr, f_y_rl, f_y_rr],
['x', 'u'], ['f_y_fl', 'f_y_fr', 'f_y_rl', 'f_y_rr'])
# vertical tire forces [N]
f_fz = ca.Function('f_fz', [x, u], [f_z_fl, f_z_fr, f_z_rl, f_z_rr],
['x', 'u'], ['f_z_fl', 'f_z_fr', 'f_z_rl', 'f_z_rr'])
# longitudinal and lateral acceleration [m/s²]
f_a = ca.Function('f_a', [x, u], [ax, ay], ['x', 'u'], ['ax', 'ay'])
# ------------------------------------------------------------------------------------------------------------------
# FORMULATE NONLINEAR PROGRAM --------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# initialize NLP vectors
w = []
w0 = []
lbw = []
ubw = []
J = 0
g = []
lbg = []
ubg = []
# initialize ouput vectors
x_opt = []
u_opt = []
dt_opt = []
tf_opt = []
ax_opt = []
ay_opt = []
ec_opt = []
# initialize control vectors (for regularization)
delta_p = []
F_p = []
# boundary constraint: lift initial conditions
Xk = ca.MX.sym('X0', nx)
w.append(Xk)
n_min = (-w_tr_right_interp(0) + pars["optim_opts"]["width_opt"] / 2) / n_s
n_max = (w_tr_left_interp(0) - pars["optim_opts"]["width_opt"] / 2) / n_s
lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min])
ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max])
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
x_opt.append(Xk * x_s)
# loop along the racetrack and formulate path constraints & system dynamic
for k in range(N):
# add decision variables for the control
Uk = ca.MX.sym('U_' + str(k), nu)
w.append(Uk)
lbw.append([delta_min, f_drive_min, f_brake_min, gamma_y_min])
ubw.append([delta_max, f_drive_max, f_brake_max, gamma_y_max])
w0.append([0.0] * nu)
# add decision variables for the state at collocation points
Xc = []
for j in range(d):
Xkj = ca.MX.sym('X_' + str(k) + '_' + str(j), nx)
Xc.append(Xkj)
w.append(Xkj)
lbw.append([-np.inf] * nx)
ubw.append([np.inf] * nx)
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
# loop over all collocation points
Xk_end = D[0] * Xk
sf_opt = []
for j in range(1, d + 1):
# calculate the state derivative at the collocation point
xp = C[0, j] * Xk
for r in range(d):
xp = xp + C[r + 1, j] * Xc[r]
# interpolate kappa at the collocation point
kappa_col = kappa_interp(k + tau[j])
# append collocation equations (system dynamic)
fj, qj = f_dyn(Xc[j - 1], Uk, kappa_col)
g.append(h * fj - xp)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# add contribution to the end state
Xk_end = Xk_end + D[j] * Xc[j - 1]
# add contribution to quadrature function
J = J + B[j] * qj * h
# add contribution to scaling factor (for calculating lap time)
sf_opt.append(B[j] * qj * h)
# calculate used energy
dt_opt.append(sf_opt[0] + sf_opt[1] + sf_opt[2])
ec_opt.append(Xk[0] * v_s * Uk[1] * f_drive_s * dt_opt[-1])
# add new decision variables for state at end of the collocation interval
Xk = ca.MX.sym('X_' + str(k + 1), nx)
w.append(Xk)
n_min = (-w_tr_right_interp(k + 1) + pars["optim_opts"]["width_opt"] / 2.0) / n_s
n_max = (w_tr_left_interp(k + 1) - pars["optim_opts"]["width_opt"] / 2.0) / n_s
lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min])
ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max])
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
# add equality constraint
g.append(Xk_end - Xk)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# get tire forces
f_x_flk, f_x_frk, f_x_rlk, f_x_rrk = f_fx(Xk, Uk)
f_y_flk, f_y_frk, f_y_rlk, f_y_rrk = f_fy(Xk, Uk)
f_z_flk, f_z_frk, f_z_rlk, f_z_rrk = f_fz(Xk, Uk)
# get accelerations (longitudinal + lateral)
axk, ayk = f_a(Xk, Uk)
# path constraint: limitied engine power
g.append(Xk[0] * Uk[1])
lbg.append([-np.inf])
ubg.append([veh["power_max"] / (f_drive_s * v_s)])
# get constant friction coefficient
if pars["optim_opts"]["var_friction"] is None:
mue_fl = pars["optim_opts"]["mue"]
mue_fr = pars["optim_opts"]["mue"]
mue_rl = pars["optim_opts"]["mue"]
mue_rr = pars["optim_opts"]["mue"]
# calculate variable friction coefficients along the reference line (regression with linear equations)
elif pars["optim_opts"]["var_friction"] == "linear":
# friction coefficient for each tire
mue_fl = w_mue_fl[k + 1, 0] * Xk[3] * n_s + w_mue_fl[k + 1, 1]
mue_fr = w_mue_fr[k + 1, 0] * Xk[3] * n_s + w_mue_fr[k + 1, 1]
mue_rl = w_mue_rl[k + 1, 0] * Xk[3] * n_s + w_mue_rl[k + 1, 1]
mue_rr = w_mue_rr[k + 1, 0] * Xk[3] * n_s + w_mue_rr[k + 1, 1]
# calculate variable friction coefficients along the reference line (regression with gaussian basis functions)
elif pars["optim_opts"]["var_friction"] == "gauss":
# gaussian basis functions
sigma = 2.0 * center_dist[k + 1, 0]
n_gauss = pars["optim_opts"]["n_gauss"]
n_q = np.linspace(-n_gauss, n_gauss, 2 * n_gauss + 1) * center_dist[k + 1, 0]
gauss_basis = []
for i in range(2 * n_gauss + 1):
gauss_basis.append(ca.exp(-(Xk[3] * n_s - n_q[i]) ** 2 / (2 * (sigma ** 2))))
gauss_basis = ca.vertcat(*gauss_basis)
mue_fl = ca.dot(w_mue_fl[k + 1, :-1], gauss_basis) + w_mue_fl[k + 1, -1]
mue_fr = ca.dot(w_mue_fr[k + 1, :-1], gauss_basis) + w_mue_fr[k + 1, -1]
mue_rl = ca.dot(w_mue_rl[k + 1, :-1], gauss_basis) + w_mue_rl[k + 1, -1]
mue_rr = ca.dot(w_mue_rr[k + 1, :-1], gauss_basis) + w_mue_rr[k + 1, -1]
else:
raise ValueError("No friction coefficients are available!")
# path constraint: Kamm's Circle for each wheel
g.append(((f_x_flk / (mue_fl * f_z_flk)) ** 2 + (f_y_flk / (mue_fl * f_z_flk)) ** 2))
g.append(((f_x_frk / (mue_fr * f_z_frk)) ** 2 + (f_y_frk / (mue_fr * f_z_frk)) ** 2))
g.append(((f_x_rlk / (mue_rl * f_z_rlk)) ** 2 + (f_y_rlk / (mue_rl * f_z_rlk)) ** 2))
g.append(((f_x_rrk / (mue_rr * f_z_rrk)) ** 2 + (f_y_rrk / (mue_rr * f_z_rrk)) ** 2))
lbg.append([0.0] * 4)
ubg.append([1.0] * 4)
# path constraint: lateral wheel load transfer
g.append(((f_y_flk + f_y_frk) * ca.cos(Uk[0] * delta_s) + f_y_rlk + f_y_rrk
+ (f_x_flk + f_x_frk) * ca.sin(Uk[0] * delta_s))
* veh["cog_z"] / ((veh["track_width_front"] + veh["track_width_rear"]) / 2) - Uk[3] * gamma_y_s)
lbg.append([0.0])
ubg.append([0.0])
# path constraint: f_drive * f_brake == 0 (no simultaneous operation of brake and accelerator pedal)
g.append(Uk[1] * Uk[2])
lbg.append([-20000.0 / (f_drive_s * f_brake_s)])
ubg.append([0.0])
# path constraint: actor dynamic
if k > 0:
sigma = (1 - kappa_interp(k) * Xk[3] * n_s) / (Xk[0] * v_s)
g.append((Uk - w[1 + (k - 1) * nx]) / (pars["stepsize_opts"]["stepsize_reg"] * sigma))
lbg.append([delta_min / (veh["t_delta"]), -np.inf, f_brake_min / (veh["t_brake"]), -np.inf])
ubg.append([delta_max / (veh["t_delta"]), f_drive_max / (veh["t_drive"]), np.inf, np.inf])
# path constraint: safe trajectories with acceleration ellipse
if pars["optim_opts"]["safe_traj"]:
g.append((ca.fmax(axk, 0) / pars["optim_opts"]["ax_pos_safe"]) ** 2
+ (ayk / pars["optim_opts"]["ay_safe"]) ** 2)
g.append((ca.fmin(axk, 0) / pars["optim_opts"]["ax_neg_safe"]) ** 2
+ (ayk / pars["optim_opts"]["ay_safe"]) ** 2)
lbg.append([0.0] * 2)
ubg.append([1.0] * 2)
# append controls (for regularization)
delta_p.append(Uk[0] * delta_s)
F_p.append(Uk[1] * f_drive_s / 10000.0 + Uk[2] * f_brake_s / 10000.0)
# append outputs
x_opt.append(Xk * x_s)
u_opt.append(Uk * u_s)
tf_opt.extend([f_x_flk, f_y_flk, f_z_flk, f_x_frk, f_y_frk, f_z_frk])
tf_opt.extend([f_x_rlk, f_y_rlk, f_z_rlk, f_x_rrk, f_y_rrk, f_z_rrk])
ax_opt.append(axk)
ay_opt.append(ayk)
# boundary constraint: start states = final states
g.append(w[0] - Xk)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# path constraint: limited energy consumption
if pars["optim_opts"]["limit_energy"]:
g.append(ca.sum1(ca.vertcat(*ec_opt)) / 3600000.0)
lbg.append([0])
ubg.append([pars["optim_opts"]["energy_limit"]])
# formulate differentiation matrix (for regularization)
diff_matrix = np.eye(N)
for i in range(N - 1):
diff_matrix[i, i + 1] = -1.0
diff_matrix[N - 1, 0] = -1.0
# regularization (delta)
delta_p = ca.vertcat(*delta_p)
Jp_delta = ca.mtimes(ca.MX(diff_matrix), delta_p)
Jp_delta = ca.dot(Jp_delta, Jp_delta)
# regularization (f_drive + f_brake)
F_p = ca.vertcat(*F_p)
Jp_f = ca.mtimes(ca.MX(diff_matrix), F_p)
Jp_f = ca.dot(Jp_f, Jp_f)
# formulate objective
J = J + pars["optim_opts"]["penalty_F"] * Jp_f + pars["optim_opts"]["penalty_delta"] * Jp_delta
# concatenate NLP vectors
w = ca.vertcat(*w)
g = ca.vertcat(*g)
w0 = np.concatenate(w0)
lbw = np.concatenate(lbw)
ubw = np.concatenate(ubw)
lbg = np.concatenate(lbg)
ubg = np.concatenate(ubg)
# concatenate output vectors
x_opt = ca.vertcat(*x_opt)
u_opt = ca.vertcat(*u_opt)
tf_opt = ca.vertcat(*tf_opt)
dt_opt = ca.vertcat(*dt_opt)
ax_opt = ca.vertcat(*ax_opt)
ay_opt = ca.vertcat(*ay_opt)
ec_opt = ca.vertcat(*ec_opt)
# ------------------------------------------------------------------------------------------------------------------
# CREATE NLP SOLVER ------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# fill nlp structure
nlp = {'f': J, 'x': w, 'g': g}
# solver options
opts = {"expand": True,
"verbose": print_debug,
"ipopt.max_iter": 2000,
"ipopt.tol": 1e-7}
# solver options for warm start
if pars["optim_opts"]["warm_start"]:
opts_warm_start = {"ipopt.warm_start_init_point": "yes",
"ipopt.warm_start_bound_push": 1e-9,
"ipopt.warm_start_mult_bound_push": 1e-9,
"ipopt.warm_start_slack_bound_push": 1e-9,
"ipopt.mu_init": 1e-9}
opts.update(opts_warm_start)
# load warm start files
if pars["optim_opts"]["warm_start"]:
try:
w0 = np.loadtxt(os.path.join(export_path, 'w0.csv'))
lam_x0 = np.loadtxt(os.path.join(export_path, 'lam_x0.csv'))
lam_g0 = np.loadtxt(os.path.join(export_path, 'lam_g0.csv'))
except IOError:
print('\033[91m' + 'WARNING: Failed to load warm start files!' + '\033[0m')
sys.exit(1)
# check warm start files
if pars["optim_opts"]["warm_start"] and not len(w0) == len(lbw):
print('\033[91m' + 'WARNING: Warm start files do not fit to the dimension of the NLP!' + '\033[0m')
sys.exit(1)
# create solver instance
solver = ca.nlpsol("solver", "ipopt", nlp, opts)
# ------------------------------------------------------------------------------------------------------------------
# SOLVE NLP --------------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# start time measure
t0 = time.perf_counter()
# solve NLP
if pars["optim_opts"]["warm_start"]:
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg, lam_x0=lam_x0, lam_g0=lam_g0)
else:
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
# end time measure
tend = time.perf_counter()
# ------------------------------------------------------------------------------------------------------------------
# EXTRACT SOLUTION -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# helper function to extract solution for state variables, control variables, tire forces, time
f_sol = ca.Function('f_sol', [w], [x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt],
['w'], ['x_opt', 'u_opt', 'tf_opt', 'dt_opt', 'ax_opt', 'ay_opt', 'ec_opt'])
# extract solution
x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt = f_sol(sol['x'])
# solution for state variables
x_opt = np.reshape(x_opt, (N + 1, nx))
# solution for control variables
u_opt = np.reshape(u_opt, (N, nu))
# solution for tire forces
tf_opt = np.append(tf_opt[-12:], tf_opt[:])
tf_opt = np.reshape(tf_opt, (N + 1, 12))
# solution for time
t_opt = np.hstack((0.0, np.cumsum(dt_opt)))
# solution for acceleration
ax_opt = np.append(ax_opt[-1], ax_opt)
ay_opt = np.append(ay_opt[-1], ay_opt)
atot_opt = np.sqrt(np.power(ax_opt, 2) + np.power(ay_opt, 2))
# solution for energy consumption
ec_opt_cum = np.hstack((0.0, np.cumsum(ec_opt))) / 3600.0
# ------------------------------------------------------------------------------------------------------------------
# EXPORT SOLUTION --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# export data to CSVs
opt_mintime_traj.src.export_mintime_solution.export_mintime_solution(file_path=export_path,
s=s_opt,
t=t_opt,
x=x_opt,
u=u_opt,
tf=tf_opt,
ax=ax_opt,
ay=ay_opt,
atot=atot_opt,
w0=sol["x"],
lam_x0=sol["lam_x"],
lam_g0=sol["lam_g"])
# ------------------------------------------------------------------------------------------------------------------
# PLOT & PRINT RESULTS ---------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
if plot_debug:
opt_mintime_traj.src.result_plots_mintime.result_plots_mintime(pars=pars,
reftrack=reftrack,
s=s_opt,
t=t_opt,
x=x_opt,
u=u_opt,
ax=ax_opt,
ay=ay_opt,
atot=atot_opt,
tf=tf_opt,
ec=ec_opt_cum)
if print_debug:
print("INFO: Laptime: %.3fs" % t_opt[-1])
print("INFO: NLP solving time: %.3fs" % (tend - t0))
print("INFO: Maximum abs(ay): %.2fm/s2" % np.amax(ay_opt))
print("INFO: Maximum ax: %.2fm/s2" % np.amax(ax_opt))
print("INFO: Minimum ax: %.2fm/s2" % np.amin(ax_opt))
print("INFO: Maximum total acc: %.2fm/s2" % np.amax(atot_opt))
print('INFO: Energy consumption: %.3fWh' % ec_opt_cum[-1])
return -x_opt[:-1, 3], x_opt[:-1, 0]
def __init__(self, model, options=dict()):
# declare object fields
self.model = model
self.input_vals = None
self.options = {}
self.model_info = {}
self.stats = {}
self.problem_functions = None
self.status = "new"
self.clib_id = -1
# default options
self.options['printLevel'] = 0
self.options['hbbSettings'] = {'numProcesses': 1}
self.options['integration_opts'] = None
self.options['max_iteration_time'] = cs.inf
self.options['relaxed_tail_length'] = 0
self.options['prediction_horizon_length'] = 1
self.options['variable_parameters_exp'] = {}
# load user options
if not options is None:
for k in options.keys():
if k in self.options:
self.options[k] = options[k]
else:
warn('Option not recognized: ' + k)
# check the model
syms_ = [v.sym for v in model.x] + [v.sym for v in model.y] + [
v.sym for v in model.a
] + [v.sym for v in model.u + model.v]
if len(model.p) > 0:
raise NameError(
'MPCcontroller is not suited to optimize parameters, please fix them or give them a state dependent expression using the \'varing_parameters_exp\' option'
)
if len(model.icns) > 0:
raise NameError('MPCcontroller: initial constraints not supported')
obj_ = 0.0
if not model.lag is None: obj_ += model.lag
if not model.ipn is None: obj_ += model.ipn
if not model.may is None: obj_ += model.may
if oc.get_order(obj_, syms_) > 2:
raise NameError(
"Objectives with order higher than 2 are not currently supported. Please, perform an epigraph reformulation of the objective."
)
# prepare collection of performace statistics
self.stats['times'] = {
'total': 0.,
'preprocessing': 0.,
'iterations': {
'total': 0.,
'pre/post-processing': 0.,
'minlp_solver': {
'total': 0.,
'nlp': 0.,
'mip': 0.,
'linearizations_generation': 0.
}
}
}
self.stats['num_solves'] = {'nlp': 0, 'qp': 0}
##############################################################################################
####################################### Preprocessing #######################################
##############################################################################################
# start timing
start_time = time()
# collect info
PHL = self.options['prediction_horizon_length']
RTL = self.options['relaxed_tail_length']
self.model_info['num_states'] = num_states = len(model.x) + len(
model.y)
self.model_info['num_algebraic'] = num_algebraic = len(model.a)
self.model_info['num_controls'] = num_controls = len(model.u + model.v)
self.model_info['num_path_constraints'] = len(model.pcns)
num_vars_per_step = num_states + num_algebraic + num_controls
############## construct the parametric mpc model ###############
mpc_time_vector = cs.MX.sym('t', PHL + RTL + 1, 1)
mpc_model = deepcopy(model)
# impose the initial state with a set of initial constraints
mpc_model.icns.extend([
oc.eq(v.sym, 0, '<initial_state>')
for v in mpc_model.x + mpc_model.y
])
# we do not support non-linear terminal costs in the shift yet (use a slack to represent the terminal cost as a terminal constraint)
if not model.may is None and oc.get_order(model.may, syms_) > 1:
mpc_model.a.append(
oc.variable('terminal_cost_slack', -cs.DM.inf(), cs.DM.inf(),
0))
mpc_model.pcns = [
oc.eq(mpc_model.a[-1].sym, 0.0,
'<terminal_cost_reformulation>')
] + mpc_model.pcns
mpc_model.fcns.append(
oc.leq(mpc_model.may, mpc_model.a[-1].sym,
'<terminal_cost_reformulation>'))
mpc_model.may = mpc_model.a[-1].sym
self.model_info['num_algebraic'] += 1
# assign variables to the inputs for later definition
for k in range(len(model.i)):
mpc_model.i[k].val = cs.MX.sym(model.i[k].nme, PHL + RTL + 1, 1)
param_symbols = [mpc_time_vector] + [i.val for i in mpc_model.i]
# construct a parametric optimization problem describing the mpc short time horizon
self.mpc_problem = oc.Problem()
oc.multiple_shooting(
mpc_model, self.mpc_problem, mpc_time_vector,
{'integration_opts': self.options['integration_opts']}, True)
# relax the tail (if required) #TODO check what happens to the sos1 groups
for k in range(PHL * num_vars_per_step + num_states + num_algebraic,
len(self.mpc_problem.var['dsc'])):
self.mpc_problem.var['dsc'][k] = False
# store the parametric version of the constraint set and variable bounds
self.problem_functions = {
'cns':
oc.CvxFuncForJulia('cns',
[self.mpc_problem.var['sym']] + param_symbols,
self.mpc_problem.cns['exp']),
'obj':
oc.LinQuadForJulia('obj',
[self.mpc_problem.var['sym']] + param_symbols,
oc.sum1(self.mpc_problem.obj['exp']))
}
# compile the parametric function created
self.problem_functions['cns'].compile(oc.home_dir + '/temp_jit_files')
# store the parametric version of the terminal part of the constraint set and the objective (for shift)
num_constraints_per_step = num_states + len(mpc_model.pcns)
num_steps = PHL + RTL
self.problem_terminals = {
'cns':
oc.CvxFuncForJulia(
'cns', [self.mpc_problem.var['sym']] + param_symbols,
self.mpc_problem.cns['exp'][num_constraints_per_step *
(num_steps - 1):]),
'obj':
oc.LinQuadForJulia(
'obj', [self.mpc_problem.var['sym']] + param_symbols,
cs.sum1(self.mpc_problem.obj['exp'][num_steps - 1:]))
}
# load the OpenBB interface
self.openbb = oc.openbb.OpenBBinterface()
# load the python interface of the mpc addon
self.mpc_addon = oc.mpc_addon.MPC_addon(self.openbb)
# load some additional helper functions in OpenBB
self.openbb.eval_string('include(\"' + oc.home_dir +
'/src/openbb_interface/helper_functions.jl\")')
# check for multiprocessing
if 'numProcesses' in self.options['hbbSettings'] and self.options[
'hbbSettings']['numProcesses'] > 1:
# prepare for multi-processing
current_num_procs = self.openbb.eval_string('nprocs()')
if current_num_procs < self.options['hbbSettings']['numProcesses']:
self.openbb.eval_string(
'addprocs(' +
str(self.options['hbbSettings']['numProcesses'] -
current_num_procs) + ')')
self.openbb.eval_string(
'@sync for k in workers() @async remotecall_fetch(Main.eval,k,:(using OpenBB)) end'
)
# collect timing statistics
self.stats['times']['preprocessing'] = time() - start_time
self.stats['times']['total'] += self.stats['times']['preprocessing']
def solve(self, solver="ipopt", show_online_optim=False, options_ipopt={}):
"""
Gives to CasADi states, controls, constraints, sum of all objective functions and theirs bounds.
Gives others parameters to control how solver works.
"""
all_J = MX()
for j_nodes in self.J:
for j in j_nodes:
all_J = vertcat(all_J, j)
for nlp in self.nlp:
for obj_nodes in nlp["J"]:
for obj in obj_nodes:
all_J = vertcat(all_J, obj)
all_g = MX()
all_g_bounds = Bounds(interpolation_type=InterpolationType.CONSTANT)
for i in range(len(self.g)):
for j in range(len(self.g[i])):
all_g = vertcat(all_g, self.g[i][j])
all_g_bounds.concatenate(self.g_bounds[i][j])
for nlp in self.nlp:
for i in range(len(nlp["g"])):
for j in range(len(nlp["g"][i])):
all_g = vertcat(all_g, nlp["g"][i][j])
all_g_bounds.concatenate(nlp["g_bounds"][i][j])
nlp = {"x": self.V, "f": sum1(all_J), "g": all_g}
options_common = {}
if show_online_optim:
options_common["iteration_callback"] = OnlineCallback(self)
if solver == "ipopt":
options = {
"ipopt.tol": 1e-6,
"ipopt.max_iter": 1000,
"ipopt.hessian_approximation":
"exact", # "exact", "limited-memory"
"ipopt.limited_memory_max_history": 50,
"ipopt.linear_solver": "mumps", # "ma57", "ma86", "mumps"
}
for key in options_ipopt:
ipopt_key = key
if key[:6] != "ipopt.":
ipopt_key = "ipopt." + key
options[ipopt_key] = options_ipopt[key]
opts = {**options, **options_common}
else:
raise RuntimeError("Available solvers are: 'ipopt'")
solver = casadi.nlpsol("nlpsol", solver, nlp, opts)
# Bounds and initial guess
arg = {
"lbx": self.V_bounds.min,
"ubx": self.V_bounds.max,
"lbg": all_g_bounds.min,
"ubg": all_g_bounds.max,
"x0": self.V_init.init,
}
# Solve the problem
return solver.call(arg)
def run():
alignment = SplineLeicaAlignment()
alignment.deserialize()
ground_truth = min_bags.readGroundTruthFromImuGtoBag()
t_L_Ps, p_L_Ps = leica.parseTextFile(
os.path.join(flags.rawDataPath(), 'leica.txt'))
while True:
aligned_gt = GroundTruthInLeicaFrame(ground_truth, alignment)
t_L_PGTs = aligned_gt.times()
t_G0_PGTs = [(i - t_L_PGTs[0]).to_sec() for i in t_L_PGTs]
t_G0_Ps = [(i - t_L_PGTs[0]).to_sec() for i in t_L_Ps]
# Calculate errors
T_L_I_of_t_G0 = aligned_gt.getLinearInterpolation()
num_errors = numericalErrors(T_L_I_of_t_G0, t_G0_Ps, p_L_Ps,
aligned_gt.alignment.p_I_P)
print('Mean square error is %f' % np.mean(np.square(num_errors)))
# Empirically, optimization only made stuff worse after this (?)
# break
opti = casadi.Opti()
t_corr_G0 = opti.variable()
T_corr_L_twist = opti.variable(6)
p_I_P = opti.variable(3)
opti.set_initial(p_I_P, aligned_gt.alignment.p_I_P)
errvec = symbolicalErrors(T_L_I_of_t_G0, t_G0_Ps, p_L_Ps, p_I_P,
t_corr_G0, T_corr_L_twist)
opti.minimize(casadi.sum1(errvec))
opti.solver('ipopt')
sol = opti.solve()
upd_coeffs = np.hstack(
(sol.value(t_corr_G0), sol.value(T_corr_L_twist),
sol.value(p_I_P) - alignment.p_I_P.ravel()))
if np.max(np.abs(upd_coeffs)) < 1e-5:
break
alignment.t_G_L = alignment.t_G_L + sol.value(t_corr_G0)
alignment.T_L_G = pose.fromTwist(
sol.value(T_corr_L_twist)) * alignment.T_L_G
alignment.p_I_P = sol.value(p_I_P).reshape((3, 1))
alignment.serialize('gt_leica_alignment')
out_files = flags.SplineLeicaAlignmentFiles('gt_leica_alignment')
np.savetxt(out_files.errors, np.array(num_errors))
if FLAGS.gui:
plt.figure(0)
plots.xyPlot(aligned_gt.imuPositions(), aligned_gt.prismPositions(),
p_L_Ps)
plt.title('x,y trajectory of %s' % flags.sequenceSensorString())
plt.figure(1)
plots.xyztPlot(t_G0_PGTs, aligned_gt.imuPositions(),
aligned_gt.prismPositions(), t_G0_Ps, p_L_Ps)
plt.title('x,y,z trajectory of %s' % flags.sequenceSensorString())
plt.figure(2)
plt.semilogx(sorted(num_errors),
np.arange(len(num_errors), dtype=float) / len(num_errors))
plt.grid()
plt.xlabel('Leica error [m]')
plt.ylabel('Fraction of measurements with smaller error')
plt.title('Cumulative distribution of Leica error for %s' %
flags.sequenceSensorString())
plt.show()
def BaseOptiX():
pathpoints = 30
ref_path = {}
ref_path['x'] = 5 * np.sin(np.linspace(0, 2 * np.pi, pathpoints + 1))
ref_path['y'] = np.linspace(1, 2, pathpoints + 1)**2 * 10
wp = ca.horzcat(ref_path['x'], ref_path['y']).T
N = 5
N1 = N - 1
wpts = np.array(wp[:, 0:N])
x = ca.MX.sym('x', N)
x_dot = ca.MX.sym('xd', N - 1)
dt = ca.MX.sym('dt', N - 1)
nlp = {\
'x': ca.vertcat(x, x_dot, dt),
'f': ca.sumsqr(x - wpts[0, :]) *100 + ca.sum1(dt),
'g': ca.vertcat(\
x[1:] - (x[:-1] + x_dot * dt),
x[0] - wpts[0, 0],
),
}
S = ca.nlpsol('S', 'ipopt', nlp)
# x0
x00 = wpts[0, :]
T = 5
dt00 = [T / N1] * N1
xd00 = (x00[1:] - x00[:-1]) / dt00
x0 = ca.vertcat(x00, xd00, dt00)
max_speed = 1
lbx = [0] * N + [-max_speed] * N1 + [0] * N1
ubx = [ca.inf] * N + [max_speed] * N1 + [10] * N1
lbg = [0] * N
ubg = [0] * N
# solve
r = S(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
x_opt = r['x']
x = np.array(x_opt[:N])
xds = np.array(x_opt[N:N + N1])
times = np.array(x_opt[N + N1:])
total_time = np.sum(times)
print(f"Times: {times}")
print(f"Total Time: {total_time}")
print(f"X dots: {xds.T}")
print(f"xs: {x.T}")
print(f"----------------")
print(f"{xds * times}")
plt.figure(1)
plt.plot(wpts[0, :], np.ones_like(wpts[0, :]), 'o', markersize=12)
plt.plot(x, np.ones_like(x), '+', markersize=20)
plt.show()
def setup(self, bending_BC_type="cantilevered"):
"""
Sets up the problem. Run this last.
:return: None (in-place)
"""
### Discretize and assign loads
# Discretize
point_load_locations = [load["location"] for load in self.point_loads]
point_load_locations.insert(0, 0)
point_load_locations.append(self.length)
self.x = cas.vertcat(*[
cas.linspace(point_load_locations[i], point_load_locations[i + 1],
self.points_per_point_load)
for i in range(len(point_load_locations) - 1)
])
# Post-process the discretization
self.n = self.x.shape[0]
dx = cas.diff(self.x)
# Add point forces
self.point_forces = cas.GenMX_zeros(self.n - 1)
for i in range(len(self.point_loads)):
load = self.point_loads[i]
self.point_forces[self.points_per_point_load * (i + 1) -
1] = load["force"]
# Add distributed loads
self.force_per_unit_length = cas.GenMX_zeros(self.n)
self.moment_per_unit_length = cas.GenMX_zeros(self.n)
for load in self.distributed_loads:
if load["type"] == "uniform":
self.force_per_unit_length += load["force"] / self.length
elif load["type"] == "elliptical":
load_to_add = load["force"] / self.length * (
4 / cas.pi * cas.sqrt(1 - (self.x / self.length)**2))
self.force_per_unit_length += load_to_add
else:
raise ValueError(
"Bad value of \"type\" for a load within beam.distributed_loads!"
)
# Initialize optimization variables
log_nominal_diameter = self.opti.variable(self.n)
self.opti.set_initial(log_nominal_diameter,
cas.log(self.diameter_guess))
self.nominal_diameter = cas.exp(log_nominal_diameter)
self.opti.subject_to([log_nominal_diameter > cas.log(self.thickness)])
def trapz(x):
out = (x[:-1] + x[1:]) / 2
# out[0] += x[0] / 2
# out[-1] += x[-1] / 2
return out
# Mass
self.volume = cas.sum1(
cas.pi / 4 * trapz((self.nominal_diameter + self.thickness)**2 -
(self.nominal_diameter - self.thickness)**2) *
dx)
self.mass = self.volume * self.density
# Mass proxy
self.volume_proxy = cas.sum1(cas.pi * trapz(self.nominal_diameter) *
dx * self.thickness)
self.mass_proxy = self.volume_proxy * self.density
# Find moments of inertia
self.I = cas.pi / 64 * ( # bending
(self.nominal_diameter + self.thickness)**4 -
(self.nominal_diameter - self.thickness)**4)
self.J = cas.pi / 32 * ( # torsion
(self.nominal_diameter + self.thickness)**4 -
(self.nominal_diameter - self.thickness)**4)
if self.bending:
# Set up derivatives
self.u = 1 * self.opti.variable(self.n)
self.du = 0.1 * self.opti.variable(self.n)
self.ddu = 0.01 * self.opti.variable(self.n)
self.dEIddu = 1 * self.opti.variable(self.n)
self.opti.set_initial(self.u, 0)
self.opti.set_initial(self.du, 0)
self.opti.set_initial(self.ddu, 0)
self.opti.set_initial(self.dEIddu, 0)
# Define derivatives
self.opti.subject_to([
cas.diff(self.u) == trapz(self.du) * dx,
cas.diff(self.du) == trapz(self.ddu) * dx,
cas.diff(self.E * self.I *
self.ddu) == trapz(self.dEIddu) * dx,
cas.diff(
self.dEIddu) == trapz(self.force_per_unit_length) * dx +
self.point_forces,
])
# Add BCs
if bending_BC_type == "cantilevered":
self.opti.subject_to([
self.u[0] == 0,
self.du[0] == 0,
self.ddu[-1] == 0, # No tip moment
self.dEIddu[-1] == 0, # No tip higher order stuff
])
else:
raise ValueError("Bad value of bending_BC_type!")
# Stress
self.stress_axial = (self.nominal_diameter +
self.thickness) / 2 * self.E * self.ddu
if self.torsion:
# Set up derivatives
phi = 0.1 * self.opti.variable(self.n)
dphi = 0.01 * self.opti.variable(self.n)
# Add forcing term
ddphi = -self.moment_per_unit_length / (self.G * self.J)
self.stress = self.stress_axial
self.opti.subject_to([
self.stress / self.max_allowable_stress < 1,
self.stress / self.max_allowable_stress > -1,
])
def exitExpression(self, tree):
if isinstance(tree.operator, ast.ComponentRef):
op = tree.operator.name
else:
op = tree.operator
if op == '*':
op = 'mtimes' # .* differs from *
if op.startswith('.'):
op = op[1:]
n_operands = len(tree.operands)
if op == 'der':
orig = self.get_mx(tree.operands[0])
if orig in self.derivative:
src = self.derivative[orig]
else:
s = ca.MX.sym("der({})".format(orig.name()), orig.sparsity())
self.derivative[orig] = s
self.nodes[s] = s
src = s
elif op == '-' and n_operands == 1:
src = -self.get_mx(tree.operands[0])
elif op == 'mtimes':
src = self.get_mx(tree.operands[0])
for i in tree.operands[1:]:
src = ca.mtimes(src, self.get_mx(i))
elif op == 'transpose' and n_operands == 1:
src = self.get_mx(tree.operands[0]).T
elif op == 'sum' and n_operands == 1:
v = self.get_mx(tree.operands[0])
src = ca.sum1(v)
elif op == 'linspace' and n_operands == 3:
a = self.get_mx(tree.operands[0])
b = self.get_mx(tree.operands[1])
n_steps = self.get_integer(tree.operands[2])
src = ca.linspace(a, b, n_steps)
elif op == 'fill' and n_operands == 2:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
src = val * ca.DM.ones(n_row)
elif op == 'fill' and n_operands == 3:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
n_col = self.get_integer(tree.operands[2])
src = val * ca.DM.ones(n_row, n_col)
elif op == 'zeros' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.zeros(n_row)
elif op == 'zeros' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.zeros(n_row, n_col)
elif op == 'ones' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.ones(n_row)
elif op == 'ones' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.ones(n_row, n_col)
elif op == 'identity' and n_operands == 1:
n = self.get_integer(tree.operands[0])
src = ca.DM.eye(n)
elif op == 'diagonal' and n_operands == 1:
diag = self.get_mx(tree.operands[0])
n = len(diag)
indices = list(range(n))
src = ca.DM.triplet(indices, indices, diag, n, n)
elif op == 'delay' and n_operands == 2:
expr = self.get_mx(tree.operands[0])
delay_time = self.get_mx(tree.operands[1])
assert isinstance(expr, MX)
src = ca.MX.sym('{}_delayed_{}'.format(expr.name, delay_time),
expr.size1(), expr.size2())
elif op in OP_MAP and n_operands == 2:
lhs = self.get_mx(tree.operands[0])
rhs = self.get_mx(tree.operands[1])
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op(rhs)
elif op in OP_MAP and n_operands == 1:
lhs = self.get_mx(tree.operands[0])
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op()
elif n_operands == 1:
src = self.get_mx(tree.operands[0])
src = getattr(src, tree.operator.name)()
elif n_operands == 2:
lhs = self.get_mx(tree.operands[0])
rhs = self.get_mx(tree.operands[1])
lhs_op = getattr(lhs, tree.operator.name)
src = lhs_op(rhs)
else:
raise Exception("Unknown operator {}({})".format(
op, ','.join(n_operands * ['.'])))
self.src[tree] = src
def update_controls(self, target_belief, x_obs, x):
ts = time.time()
self.x = x
nx, ny = target_belief.shape
xmin, xmax, ymin, ymax = self.bounds
L1 = xmax - xmin
L2 = ymax - ymin
if self.mode == 'mi':
mu = convolve2d(target_belief, self.footprint_mask, mode='same')
pos = self.tp_rate * mu + self.fp_rate * (1 - mu)
mi = (-self.tp_rate * mu * np.log(pos / self.tp_rate) -
(1 - self.tp_rate) * mu * np.log(
(1 - pos) / (1 - self.tp_rate)) - self.fp_rate *
(1 - mu) * np.log(pos / self.fp_rate) - (1 - self.fp_rate) *
(1 - mu) * np.log((1 - pos) / (1 - self.fp_rate)))
info = mi / np.sum(mi)
elif self.mode == 'p':
mu = convolve2d(target_belief, self.footprint_mask, mode='same')
info = mu / np.sum(mu)
elif self.mode == 'b':
info = target_belief / np.sum(target_belief)
elif self.mode == 'alpha':
mu = convolve2d(target_belief, self.footprint_mask, mode='same')
pos = self.tp_rate * mu + self.fp_rate * (1 - mu)
alpha = .5
ami = (1 /
(1 - alpha)) * (pos * np.log(mu *
(self.tp_rate / pos)**alpha +
(1 - mu) *
(self.fp_rate / pos)**alpha) +
(1 - pos) * np.log(mu *
((1 - self.tp_rate) /
(1 - pos))**alpha +
(1 - mu) *
((1 - self.fp_rate) /
(1 - pos))**alpha))
info = ami / np.sum(ami)
muk = dctn(info * np.sqrt(nx) * np.sqrt(ny), type=2, norm='ortho')
muk = muk[0:self.nfourier, 0:self.nfourier].flatten()
x1_comp = np.cos(np.outer((x_obs[:, 0] - xmin) / L1, self.k1))
x2_comp = np.cos(np.outer((x_obs[:, 1] - ymin) / L2, self.k2))
ck_prev = (1 / self.hk) * np.sum(x1_comp * x2_comp, axis=0)
if len(self.ck_history_cum) == 0:
self.ck_history_cum.append(ck_prev)
ck_init = np.zeros(self.k1.shape[0])
t_total = self.t_horizon
else:
prev_cum = self.ck_history_cum[-1]
self.ck_history_cum.append(prev_cum + ck_prev)
if len(self.ck_history_cum) > self.t_history:
ck_init = self.ck_history_cum[-1] - self.ck_history_cum[
-self.t_history - 1]
t_total = self.t_history + self.t_horizon
else:
ck_init = self.ck_history_cum[-1]
t_total = len(self.ck_history_cum) + self.t_horizon
opti = casadi.Opti()
v_u = [opti.variable(2, self.t_horizon) for i in range(self.nagents)]
v_x = [casadi.cumsum(v_u[i], 2) + x[i, :] for i in range(self.nagents)]
v_ck = (ck_init + (1 / self.hk) * sum([
casadi.sum2(
casadi.cos(self.k1 @ (v_x[i][0, :] - xmin) / L1) *
casadi.cos(self.k2 @ (v_x[i][1, :] - ymin) / L2))
for i in range(self.nagents)
])) / (self.nagents * t_total)
erg_metric = casadi.sum1(self.Lambdak * (v_ck - muk)**2)
change_penalties = []
for i in range(self.nagents):
opti.subject_to(casadi.sum2(v_u[i]**2) < self.u_max**2)
opti.subject_to(v_x[i][0, :] > xmin)
opti.subject_to(v_x[i][0, :] < xmax)
opti.subject_to(v_x[i][1, :] > ymin)
opti.subject_to(v_x[i][1, :] < ymax)
for j in range(i + 1, self.nagents):
opti.subject_to(
casadi.sum1((v_x[i] - v_x[j])**2) > self.min_dist**2)
u_init = np.zeros((2, self.t_horizon))
u_init[:, 0:-1] = self.u[i][:, 1:]
opti.set_initial(v_u[i], u_init)
change_penalties.append(
self.change_paramt0 *
casadi.sum1(casadi.sum2((v_u[i][:, 0] - self.u[i][:, 0])**2)))
change_penalties.append(
self.change_param *
casadi.sum1(casadi.sum2((v_u[i][:, 1:] - v_u[i][:, 0:-1])**2)))
change_cost = sum(change_penalties)
opti.minimize(erg_metric + change_cost)
p_opts = {}
# s_opts = {'max_cpu_time': .09, 'print_level': 0}
s_opts = {'print_level': 0}
opti.solver('ipopt', p_opts, s_opts)
sol = opti.solve()
self.u = [sol.value(v_u[i]) for i in range(self.nagents)]
xs = [sol.value(v_x[i]) for i in range(self.nagents)]
# try:
# sol = opti.solve()
# self.u = [sol.value(v_u[i]) for i in range(self.nagents)]
# xs = [sol.value(v_x[i]) for i in range(self.nagents)]
# except:
# self.u = [opti.debug.value(v_u[i]) for i in range(self.nagents)]
# xs = [opti.debug.value(v_x[i]) for i in range(self.nagents)]
action = np.zeros((self.nagents, 2))
for i in range(self.nagents):
action[i, :] = xs[i][:, 0]
t_comp = time.time() - ts
self.x = x
if self.gui:
xs_plot = [
np.insert(xs[i], 0, self.x[i, :], axis=1)
for i in range(self.nagents)
]
# display information map, planned trajectories, and
if self.image1 is None:
fig, (ax1, ax2) = plt.subplots(2, 1, num='erg', figsize=(2, 4))
self.image1 = ax1.imshow(info.T / np.max(info),
extent=self.bounds,
origin='lower',
cmap='gray')
self.image2 = ax2.imshow(target_belief.T /
np.max(target_belief),
extent=self.bounds,
origin='lower',
cmap='gray')
ax1.set_title('Information Map')
ax2.set_title('Target Belief')
ax1.axis('off')
ax2.axis('off')
self.lines = []
self.inits = []
for i in range(self.nagents):
line, = ax1.plot(xs[i][0, :], xs[i][1, :],
'r.-') #, linestyle=':')
self.lines.append(line)
init, = ax1.plot(self.x[i, 0], self.x[i, 1], 'g.')
self.inits.append(init)
else:
plt.figure('erg')
self.image1.set_data(info.T / np.max(info))
self.image2.set_data(target_belief.T / np.max(target_belief))
for i in range(self.nagents):
self.lines[i].set_xdata(xs[i][0, :])
self.lines[i].set_ydata(xs[i][1, :])
self.inits[i].set_xdata(self.x[i, 0])
self.inits[i].set_ydata(self.x[i, 1])
plt.draw()
plt.pause(.001)
return action, t_comp
opti.subject_to( casadi.vec(x) >= 0 ) # elements nonnegative
opti.subject_to( casadi.vec(x) <= 1 ) # elements bounded by 1
opti.subject_to( x@o == 1 ) # rows sum to 1
# opti.subject_to( casadi.eig_symbolic(A) <= np.sqrt(k) ) # minimize eigenvalue - eigenvalue vs sparsity
# https://math.stackexchange.com/questions/199268/eigenvalues-vs-matrix-sparsity
# opti.subject_to( casadi.sum1(casadi.sum2(x)) <= 0.3*9 )
# opti.subject_to( (x.nnz() / x.numel()) <= 0.5 ) # calculating sparsity directly
# some reason it defaults to dense matrix
opti.set_initial(x, np.diag(np.ones(len(c))))
rowsum = x@o
csum = casadi.sum1(c)*o
avmetric = casadi.sum1(((x@c - csum)**2)) # |Xc - average(c)| - this shows for one c, but there will be more c_k
# sparsemetric = x.nnz() / x.numel() # some reason it defaults to dense matrix
sparsemetric = casadi.sum1(casadi.sum2(elt_metric(x)))/x.numel() # sparsity metric - average penalization over elements
opti.minimize(avmetric + sparsemetric)
p_opts = {}
s_opts = {'print_level': 0}
opti.solver('ipopt', p_opts, s_opts)
sol = opti.solve()
print("Solution \n", sol.value(x))
def exitExpression(self, tree):
if isinstance(tree.operator, ast.ComponentRef):
op = tree.operator.name
else:
op = tree.operator
if op == '*':
op = 'mtimes' # .* differs from *
if op.startswith('.'):
op = op[1:]
logger.debug('exitExpression')
n_operands = len(tree.operands)
if op == 'der':
v = self.get_mx(tree.operands[0])
src = self.get_derivative(v)
elif op == '-' and n_operands == 1:
src = -self.get_mx(tree.operands[0])
elif op == 'not' and n_operands == 1:
src = ca.if_else(self.get_mx(tree.operands[0]), 0, 1, True)
elif op == 'mtimes':
assert n_operands >= 2
src = self.get_mx(tree.operands[0])
for i in tree.operands[1:]:
src = ca.mtimes(src, self.get_mx(i))
elif op == 'transpose' and n_operands == 1:
src = self.get_mx(tree.operands[0]).T
elif op == 'sum' and n_operands == 1:
v = self.get_mx(tree.operands[0])
src = ca.sum1(v)
elif op == 'linspace' and n_operands == 3:
a = self.get_mx(tree.operands[0])
b = self.get_mx(tree.operands[1])
n_steps = self.get_integer(tree.operands[2])
src = ca.linspace(a, b, n_steps)
elif op == 'fill' and n_operands == 2:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
src = val * ca.DM.ones(n_row)
elif op == 'fill' and n_operands == 3:
val = self.get_mx(tree.operands[0])
n_row = self.get_integer(tree.operands[1])
n_col = self.get_integer(tree.operands[2])
src = val * ca.DM.ones(n_row, n_col)
elif op == 'zeros' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.zeros(n_row)
elif op == 'zeros' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.zeros(n_row, n_col)
elif op == 'ones' and n_operands == 1:
n_row = self.get_integer(tree.operands[0])
src = ca.DM.ones(n_row)
elif op == 'ones' and n_operands == 2:
n_row = self.get_integer(tree.operands[0])
n_col = self.get_integer(tree.operands[1])
src = ca.DM.ones(n_row, n_col)
elif op == 'identity' and n_operands == 1:
n = self.get_integer(tree.operands[0])
src = ca.DM.eye(n)
elif op == 'diagonal' and n_operands == 1:
diag = self.get_mx(tree.operands[0])
n = len(diag)
indices = list(range(n))
src = ca.DM.triplet(indices, indices, diag, n, n)
elif op == 'cat':
axis = self.get_integer(tree.operands[0])
assert axis == 1, "Currently only concatenation on first axis is supported"
entries = []
for sym in [self.get_mx(op) for op in tree.operands[1:]]:
if isinstance(sym, list):
for e in sym:
entries.append(e)
else:
entries.append(sym)
src = ca.vertcat(*entries)
elif op == 'delay' and n_operands == 2:
expr = self.get_mx(tree.operands[0])
duration = self.get_mx(tree.operands[1])
src = _new_mx('_pymoca_delay_{}'.format(self.delay_counter), *expr.size())
self.delay_counter += 1
for f in self.for_loops:
syms = set(ca.symvar(expr))
if syms.intersection(f.indexed_symbols):
f.register_indexed_symbol(src, lambda i: i, True, tree.operands[0], f.index_variable)
self.model.delay_states.append(src.name())
self.model.inputs.append(Variable(src))
delay_argument = DelayArgument(expr, duration)
self.model.delay_arguments.append(delay_argument)
elif op in OP_MAP and n_operands == 2:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op(rhs)
elif op in OP_MAP and n_operands == 1:
lhs = ca.MX(self.get_mx(tree.operands[0]))
lhs_op = getattr(lhs, OP_MAP[op])
src = lhs_op()
else:
src = ca.MX(self.get_mx(tree.operands[0]))
# Check for built-in operations, such as the
# elementary functions, first.
if hasattr(src, op) and n_operands <= 2:
if n_operands == 1:
src = ca.MX(self.get_mx(tree.operands[0]))
src = getattr(src, op)()
else:
lhs = ca.MX(self.get_mx(tree.operands[0]))
rhs = ca.MX(self.get_mx(tree.operands[1]))
lhs_op = getattr(lhs, op)
src = lhs_op(rhs)
else:
func = self.get_function(op)
src = ca.vertcat(*func.call([self.get_mx(operand) for operand in tree.operands], *self.function_mode))
self.src[tree] = src
def optimize_weights2(agents,
agentidx,
server,
N,
cur_iter,
num_steps,
elt_metric=(lambda x: oneminusoneover_metric(x, 100))):
# only optimizes agent's controls so that final c_k ergodicity is low
""" Casadi Optimization -- Optimal Adjacency Matrix """
opti = casadi.Opti()
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 == agentidx) for j in range(N)]))
ck_new = {}
for k in np.ndindex(*[K] * n):
ck = np.array([server[j][k] for j in range(N)])
ck_new[k] = A_opt @ ck
""" Unrolling of planning horizon """
u_opt_plan = opti.variable(num_steps, agents[agentidx].m)
init = np.array([[
agents[agentidx].umax * int(agents[agentidx].m - 1 == x)
for x in range(agents[agentidx].m)
]])
opti.set_initial(u_opt_plan, np.repeat(init, [num_steps], axis=0))
curr_x_plan = [agents[agentidx].x_log[-1]]
curr_v_plan = [np.zeros(agents[agentidx].m)]
curr_c_k_plan = [ck_new]
curr_agent_c_k_plan = [agents[agentidx].agent_c_k]
for plan_i in range(0, num_steps):
i = cur_iter + plan_i
t = i * delta_t # [time, time+time_step]
u = u_opt_plan[plan_i, :].T
opti.subject_to(
casadi.sum1(u_opt_plan[plan_i, :]**2) <= agents[agentidx].umax**2)
x, v = agents[agentidx].move(u,
t,
delta_t,
is_pred=True,
prev_x=curr_x_plan[-1],
prev_v=curr_v_plan[-1])
curr_x_plan.append(x)
curr_v_plan.append(v)
opti.subject_to(curr_x_plan[-1][:] >= 0)
opti.subject_to(
curr_x_plan[-1][:] <= np.array(agents[agentidx].U_shape))
curr_c_k, curr_agent_c_k = agents[agentidx].recalculate_c_k(
t,
delta_t,
prev_x=curr_x_plan[-2],
curr_x=curr_x_plan[-1],
old_c_k=curr_c_k_plan[-1],
old_agent_c_k=curr_agent_c_k_plan[-1])
curr_c_k_plan.append(curr_c_k)
curr_agent_c_k_plan.append(curr_agent_c_k)
# 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):
ave_c_k = np.array([server[j][k] for j in range(N)])
ave_c_k[agentidx] = curr_c_k_plan[-1][k]
ave_c_k = sum(ave_c_k) / N
e_plan += lambd[k] * (ave_c_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()
r = 2
A = sol.value(A_opt).round(r)
u_plan = sol.value(u_opt_plan)
x_plan = [sol.value(curr_x_plan[plan_i]) for plan_i in range(num_steps)]
print("e_plan: ", sol.value(e_plan))
if np.count_nonzero(A) == 1 and A[agentidx] != 0:
# only communicating with itself
A = np.zeros(N)
return A, x_plan, u_plan
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()')
curr_v_plan = [[np.zeros(agents[j].m)] for j in range(N)]
curr_c_k_plan = [[agents[j].c_k] for j in range(N)]
curr_agent_c_k_plan = [[agents[j].agent_c_k] for j in range(N)]
u_opt_plan = []
for j in range(N):
u_j = opti.variable(num_plan, agents[j].m)
init = np.array([[
agents[j].umax * int(agents[j].m - 1 == x) for x in range(agents[j].m)
]])
opti.set_initial(u_j, np.repeat(init, [num_plan], axis=0))
u_opt_plan.append(u_j)
for j in range(N):
for plan_i in range(num_plan):
opti.subject_to(
casadi.sum1(u_opt_plan[j][plan_i, :]**2) <= agents[j].umax**2)
for plan_i in range(0, num_plan):
# i = big_i*num_plan + plan_i
i = plan_i
t = i * delta_t # [time, time+time_step]
for j in range(N):
x, v = agents[j].move(u_opt_plan[j][plan_i, :].T,
t,
delta_t,
is_pred=True,
prev_x=curr_x_plan[j][-1],
prev_v=curr_v_plan[j][-1])
curr_x_plan[j].append(x)
curr_v_plan[j].append(v)
