pl.ones(3), \
pl.zeros(N), \
ydata_noise[0,:].T
)
sol = nlpsolver(x0=V0)
p_est_single_shooting = sol["x"][:3]
tstart_Sigma_p = time()
J_s = ca.jacobian(r, V)
F_s = ca.mtimes(J_s.T, J_s)
beta = (ca.mtimes(r.T, r) / (r.numel() - V.numel()))
Sigma_p_s = beta * ca.solve(F_s, ca.MX.eye(F_s.shape[0]), "csparse")
beta_fcn = ca.Function("beta_fcn", [V], [beta])
print beta_fcn.call([sol["x"]])[0]
Sigma_p_s_fcn = ca.Function("Sigma_p_s_fcn", \
[V] , [Sigma_p_s])
Cov_p = Sigma_p_s_fcn.call([sol["x"]])[0][:3, :3]
tend_Sigma_p = time()
time_covariance_matrix_evlaution.append(tend_Sigma_p - tstart_Sigma_p)
def rocket_equations(jit=True):
x = ca.SX.sym('x', 14)
u = ca.SX.sym('u', 4)
p = ca.SX.sym('p', 16)
t = ca.SX.sym('t')
dt = ca.SX.sym('dt')
# State: x
# body frame: Forward, Right, Down
omega_b = x[0:3] # inertial angular velocity expressed in body frame
r_nb = x[3:7] # modified rodrigues parameters
v_b = x[7:10] # inertial velocity expressed in body components
p_n = x[10:13] # positon in nav frame
m_fuel = x[13] # mass
# Input: u
m_dot = ca.if_else(m_fuel > 0, u[0], 0)
fin = u_to_fin(u)
# Parameters: p
g = p[0] # gravity
Jx = p[1] # moment of inertia
Jy = p[2]
Jz = p[3]
Jxz = p[4]
ve = p[5]
l_fin = p[6]
w_fin = p[7]
CL_alpha = p[8]
CL0 = p[9]
CD0 = p[10]
K = p[11]
s_fin = p[12]
rho = p[13]
m_empty = p[14]
l_motor = p[15]
# Calculations
m = m_empty + m_fuel
J_b = ca.SX.zeros(3, 3)
J_b[0, 0] = Jx + m_fuel * l_motor**2
J_b[1, 1] = Jy + m_fuel * l_motor**2
J_b[2, 2] = Jz
J_b[0, 2] = J_b[2, 0] = Jxz
C_nb = so3.Dcm.from_mrp(r_nb)
g_n = ca.vertcat(0, 0, g)
v_n = ca.mtimes(C_nb, v_b)
# aerodynamics
VT = ca.norm_2(v_b)
q = 0.5 * rho * ca.dot(v_b, v_b)
fins = {
'top': {
'fwd': [1, 0, 0],
'up': [0, 1, 0],
'angle': fin[0]
},
'left': {
'fwd': [1, 0, 0],
'up': [0, 0, -1],
'angle': fin[1]
},
'down': {
'fwd': [1, 0, 0],
'up': [0, -1, 0],
'angle': fin[2]
},
'right': {
'fwd': [1, 0, 0],
'up': [0, 0, 1],
'angle': fin[3]
},
}
rel_wind_dir = v_b / VT
# build fin lift/drag forces
vel_tol = 1e-3
FA_b = ca.vertcat(0, 0, 0)
MA_b = ca.vertcat(0, 0, 0)
for key, data in fins.items():
fwd = data['fwd']
up = data['up']
angle = data['angle']
U = ca.dot(fwd, v_b)
W = ca.dot(up, v_b)
side = ca.cross(fwd, up)
alpha = ca.if_else(ca.fabs(U) > vel_tol, -ca.atan(W / U), 0)
perp_wind_dir = ca.cross(side, rel_wind_dir)
norm_perp = ca.norm_2(perp_wind_dir)
perp_wind_dir = ca.if_else(
ca.fabs(norm_perp) > vel_tol, perp_wind_dir / norm_perp, up)
CL = CL0 + CL_alpha * (alpha + angle)
CD = CD0 + K * (CL - CL0)**2
# model stall as no lift if above 23 deg.
L = ca.if_else(ca.fabs(alpha) < 0.4, CL * q * s_fin, 0)
D = CD * q * s_fin
FAi_b = L * perp_wind_dir - D * rel_wind_dir
FA_b += FAi_b
MA_b += ca.cross(-l_fin * fwd - w_fin * side, FAi_b)
FA_b = ca.if_else(ca.fabs(VT) > vel_tol, FA_b, ca.SX.zeros(3))
MA_b = ca.if_else(ca.fabs(VT) > vel_tol, MA_b, ca.SX.zeros(3))
# propulsion
FP_b = ca.vertcat(m_dot * ve, 0, 0)
# force and momental total
F_b = FA_b + FP_b + ca.mtimes(C_nb.T, m * g_n)
M_b = MA_b
force_moment = ca.Function('force_moment', [x, u, p], [F_b, M_b],
['x', 'u', 'p'], ['F_b', 'M_b'])
# right hand side
rhs = ca.Function('rhs', [x, u, p], [
ca.vertcat(
ca.mtimes(ca.inv(J_b),
M_b - ca.cross(omega_b, ca.mtimes(J_b, omega_b))),
so3.Mrp.kinematics(r_nb, omega_b),
F_b / m - ca.cross(omega_b, v_b), ca.mtimes(C_nb, v_b), -m_dot)
], ['x', 'u', 'p'], ['rhs'], {'jit': jit})
# prediction
t0 = ca.SX.sym('t0')
h = ca.SX.sym('h')
x0 = ca.SX.sym('x', 14)
x1 = rk4(lambda t, x: rhs(x, u, p), t0, x0, h)
x1[3:7] = so3.Mrp.shadow_if_necessary(x1[3:7])
predict = ca.Function('predict', [x0, u, p, t0, h], [x1], {'jit': jit})
def schedule(t, start, ty_pairs):
val = start
for ti, yi in ty_pairs:
val = ca.if_else(t > ti, yi, val)
return val
# reference trajectory
pitch_d = 1.0
euler = so3.Euler.from_mrp(r_nb) # roll, pitch, yaw
pitch = euler[1]
# control
u_control = ca.SX.zeros(4)
# these controls are just test controls to make sure the fins are working
u_control[0] = 0.1 # mass flow rate
u_control[1] = 0
u_control[2] = (pitch - 1)
u_control[3] = 0
control = ca.Function('control', [x, p, t, dt], [u_control],
['x', 'p', 't', 'dt'], ['u'])
# initialize
pitch_deg = ca.SX.sym('pitch_deg')
omega0_b = ca.vertcat(0, 0, 0)
r0_nb = so3.Mrp.from_euler(ca.vertcat(0, pitch_deg * ca.pi / 180, 0))
v0_b = ca.vertcat(0, 0, 0)
p0_n = ca.vertcat(0, 0, 0)
m0_fuel = 0.8
# x: omega_b, r_nb, v_b, p_n, m_fuel
x0 = ca.vertcat(omega0_b, r0_nb, v0_b, p0_n, m0_fuel)
# g, Jx, Jy, Jz, Jxz, ve, l_fin, w_fin, CL_alpha, CL0, CD0, K, s, rho, m_emptpy, l_motor
p0 = [
9.8, 0.05, 1.0, 1.0, 0.0, 350, 1.0, 0.05, 2 * np.pi, 0, 0.01, 0.01,
0.05, 1.225, 0.2, 1.0
]
initialize = ca.Function('initialize', [pitch_deg], [x0, p0])
return {
'rhs': rhs,
'predict': predict,
'control': control,
'initialize': initialize,
'force_moment': force_moment,
'x': x,
'u': u,
'p': p
}
return rhs, x, u, p
def u2c2np(asd):
return cs.Function("temp",[],[asd])()["o0"].toarray()
def euler_to_quaternion(angle):
quaternion = biorbd.Quaternion.fromMatrix(
biorbd.Rotation.fromEulerAngles(angle, "xyz")).to_mx()
quaternion /= cas.norm_fro(quaternion)
return cas.Function("euler_to_quaternion", [angle], [quaternion])
def createTimeContFun(ode, NX, NU):
x = ca.SX.sym('x', NX)
u = ca.SX.sym('u', NU)
return ca.Function('timeContFun', [x, u], [ode(x, u)])
def MinCurvatureTrajectory(pts, nvecs, ws):
"""
This function uses optimisation to minimise the curvature of the path
"""
w_min = -ws[:, 0] * 0.9
w_max = ws[:, 1] * 0.9
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
N = len(pts)
n_f_a = ca.MX.sym('n_f', N)
n_f = ca.MX.sym('n_f', N - 1)
th_f = ca.MX.sym('n_f', N - 1)
x0_f = ca.MX.sym('x0_f', N - 1)
x1_f = ca.MX.sym('x1_f', N - 1)
y0_f = ca.MX.sym('y0_f', N - 1)
y1_f = ca.MX.sym('y1_f', N - 1)
th1_f = ca.MX.sym('y1_f', N - 1)
th2_f = ca.MX.sym('y1_f', N - 1)
th1_f1 = ca.MX.sym('y1_f', N - 2)
th2_f1 = ca.MX.sym('y1_f', N - 2)
o_x_s = ca.Function('o_x', [n_f], [pts[:-1, 0] + nvecs[:-1, 0] * n_f])
o_y_s = ca.Function('o_y', [n_f], [pts[:-1, 1] + nvecs[:-1, 1] * n_f])
o_x_e = ca.Function('o_x', [n_f], [pts[1:, 0] + nvecs[1:, 0] * n_f])
o_y_e = ca.Function('o_y', [n_f], [pts[1:, 1] + nvecs[1:, 1] * n_f])
dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
[ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])
track_length = ca.Function('length', [n_f_a], [
dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
o_y_e(n_f_a[1:]))
])
real = ca.Function(
'real', [th1_f, th2_f],
[ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
im = ca.Function(
'im', [th1_f, th2_f],
[-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
[ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))])
get_th_n = ca.Function(
'gth', [th_f],
[sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))])
d_n = ca.Function('d_n', [n_f_a, th_f],
[track_length(n_f_a) / ca.tan(get_th_n(th_f))])
# objective
real1 = ca.Function(
'real1', [th1_f1, th2_f1],
[ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)])
im1 = ca.Function(
'im1', [th1_f1, th2_f1],
[-ca.cos(th1_f1) * ca.sin(th2_f1) + ca.sin(th1_f1) * ca.cos(th2_f1)])
sub_cmplx1 = ca.Function(
'a_cpx1', [th1_f1, th2_f1],
[ca.atan2(im1(th1_f1, th2_f1), real1(th1_f1, th2_f1))])
# define symbols
n = ca.MX.sym('n', N)
th = ca.MX.sym('th', N - 1)
nlp = {\
'x': ca.vertcat(n, th),
'f': ca.sumsqr(sub_cmplx1(th[1:], th[:-1])),
# 'f': ca.sumsqr(track_length(n)),
'g': ca.vertcat(
# dynamic constraints
n[1:] - (n[:-1] + d_n(n, th)),
# boundary constraints
n[0], #th[0],
n[-1], #th[-1],
) \
}
# S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':5}})
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 0}})
ones = np.ones(N)
n0 = ones * 0
th0 = []
for i in range(N - 1):
th_00 = lib.get_bearing(pts[i, 0:2], pts[i + 1, 0:2])
th0.append(th_00)
th0 = np.array(th0)
x0 = ca.vertcat(n0, th0)
lbx = list(w_min) + [-np.pi] * (N - 1)
ubx = list(w_max) + [np.pi] * (N - 1)
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
x_opt = r['x']
n_set = np.array(x_opt[:N])
# thetas = np.array(x_opt[1*N:2*(N-1)])
return n_set
def _OptimProbl(self):
# the symbolic optimization problem
w = [] # Variáveis de otimização
lbw = [] # Lower bound de w
ubw = [] # Upper bound de w
g = [] # Restrições não lineares
lbg = [] # Lower bound de g
ubg = [] # Upper bound de g
N = self.N
Ysp = self.Ysp # set-point
syN = self.syN # Variáveis de folga na saída
siN = self.siN # Variável de folga terminal
X = self.X
U = self.U
X_pred = self.X_pred
Y_pred = self.Y_pred
U_pred = self.U_pred
dU_pred = self.dU_pred
J = self.J
Pesos = self.Pesos
ViN_ant = self.ViN_ant
w += dU_pred
for _ in range(0, N):
# Adiciona duk nas variáveis de decisão
lbw += [self.dulb] # bound inferior na variável
ubw += [self.duub] # bound superior na variável
# variáveis de folga
w += [syN]
lbw += [self.sylb] # bound inferior na variável
ubw += [self.syub] # bound superior na variável
w += [siN]
lbw += [self.silb] # bound inferior na variável
ubw += [self.siub] # bound superior na variável
g += X_pred
for _ in range(0, N):
# Bounds em x
lbg += [self.xlb]
ubg += [self.xub]
g += U_pred
for _ in range(0,N):
# Bounds em u
lbg += [self.ulb]
ubg += [self.uub]
# Bounds em y (nenhuma)
# estado terminal
XN = self.X_pred[-1] # terminal state
# Restrições terminais do ihmpc
XiN = XN[self.nxs+self.nxd:self.nxs+self.nxd+self.nxi]
res1 = XiN - siN
XsN = XN[0:self.nxs]
res2 = XsN - Ysp - syN
# YN = Y_pred[-1]
# res2 = YN - Ysp - syN
# Restrição 1
g += [res1]
lbg += [self.rilb]
ubg += [self.riub]
# Restrição 2
g += [res2]
lbg += [self.rslb]
ubg += [self.rsub]
# restrições nos sub-objetivos
l = len(self.V)
for i in range(l):
g += [self.V[i].V]
lbg += [self.V[i].min] # in the case of ViN, min = ViN_ant
ubg += [self.V[i].max]
g = csd.vertcat(*g)
w = csd.vertcat(*w)
p = csd.vertcat(X, Ysp, U, *Pesos, *ViN_ant)
lbw = csd.vertcat(*lbw)
ubw = csd.vertcat(*ubw)
lbg = csd.vertcat(*lbg)
ubg = csd.vertcat(*ubg)
X_pred = csd.vertcat(*X_pred)
Y_pred = csd.vertcat(*Y_pred)
U_pred = csd.vertcat(*U_pred)
pred = csd.Function('pred', [X, U, csd.vertcat(*dU_pred)],
[X_pred, Y_pred, U_pred],
['x0', 'u0', 'du_opt'],
['x_pred', 'y_pred', 'u_pred'])
prob = {'f': J, 'x': w, 'g': g, 'p': p}
bounds = {'lbw': lbw, 'ubw': ubw, 'lbg': lbg, 'ubg': ubg}
return prob, bounds, pred
def _prepare_function_for_casadi(self):
Qsym = casadi.MX.sym("Q", self.m.nbQ(), 1)
self.CoM = casadi.Function("CoM", [Qsym],
[self.m.CoM(Qsym).to_mx()])
def formulateMPC(self):
X = ca.MX.sym('X')
Y = ca.MX.sym('Y')
th = ca.MX.sym('th')
self.e_c = ca.Function('e_c', [X, Y, th], [
ca.sin(self.Phi(th)) * (X - self.cs_x(th)) - ca.cos(self.Phi(th)) *
(Y - self.cs_y(th))
])
self.e_l = ca.Function('e_l', [X, Y, th], [
-ca.cos(self.Phi(th)) *
(X - self.cs_x(th)) - ca.sin(self.Phi(th)) * (Y - self.cs_y(th))
])
gp_in = ca.SX.sym('gp_in', 4, 1)
mean = self.gp_dx.predict(gp_in)[0]
f_mean_dx = ca.Function('f_mean_dx', [gp_in], [mean])
mean = self.gp_dy.predict(gp_in)[0]
f_mean_dy = ca.Function('f_mean_dy', [gp_in], [mean])
gp_in2 = ca.SX.sym('gp_in2', 2, 1)
mean = self.gp_dth.predict(gp_in2)[0]
f_mean_dth = ca.Function('f_mean_dth', [gp_in2], [mean])
x = ca.SX.sym('x')
y = ca.SX.sym('y')
th = ca.SX.sym('th')
v = ca.SX.sym('v')
delta = ca.SX.sym('delta')
state = np.array([x, y, th])
control = np.array([v, delta])
rhs = np.array([
f_mean_dx(np.array([np.cos(th), np.sin(th), v, delta])),
f_mean_dy(np.array([np.cos(th), np.sin(th), v, delta])),
f_mean_dth(np.array([v, delta]))
]) + state
self.f_dyn = ca.Function('f_dyn', [state, control], [rhs])
self.mpc_opti = ca.casadi.Opti()
self.U = self.mpc_opti.variable(2, self.H)
self.X = self.mpc_opti.variable(3, self.H + 1)
self.TH = self.mpc_opti.variable(self.H + 1)
self.NU = self.mpc_opti.variable(self.H)
self.P_1 = self.mpc_opti.parameter(3)
self.P_2 = self.mpc_opti.parameter(2)
J = 0
for k in range(self.H + 1):
J += self.qc * self.e_c(self.X[0, k], self.X[
1, k], self.TH[k])**2 + self.ql * self.e_l(
self.X[0, k], self.X[1, k], self.TH[k])**2
for k in range(self.H - 1):
J += self.Ru[0] * (self.U[0, k + 1] - self.U[0, k])**2 + self.Ru[
1] * (self.U[1, k + 1] - self.U[1, k])**2 + self.Rv * (
self.NU[k + 1] - self.NU[k])**2
J += -self.gamma * self.TH[-1]
self.mpc_opti.minimize(J)
for k in range(self.H):
self.mpc_opti.subject_to(
self.X[:, k + 1] == self.f_dyn(self.X[:, k], self.U[:, k]))
self.mpc_opti.subject_to(self.TH[k + 1] == self.TH[k] +
self.T * self.NU[k])
self.mpc_opti.subject_to(0 <= self.TH)
self.mpc_opti.subject_to(self.TH <= self.L)
self.mpc_opti.subject_to(self.nu_min <= self.NU)
self.mpc_opti.subject_to(self.NU <= self.nu_max)
self.mpc_opti.subject_to(self.v_min <= self.U[0, :])
self.mpc_opti.subject_to(self.U[0, :] <= self.v_max)
self.mpc_opti.subject_to(self.delta_min <= self.U[1, :])
self.mpc_opti.subject_to(self.U[1, :] <= self.delta_max)
self.mpc_opti.subject_to(self.ramp_v_min <= self.U[0, 0] - self.P_2[0])
self.mpc_opti.subject_to(self.U[0, 0] - self.P_2[0] <= self.ramp_v_max)
self.mpc_opti.subject_to(
self.ramp_delta_min <= self.U[1, 0] - self.P_2[1])
self.mpc_opti.subject_to(
self.U[1, 0] - self.P_2[1] <= self.ramp_delta_max)
self.mpc_opti.subject_to(
self.ramp_v_min <= self.U[0, 1:] - self.U[0, 0:-1])
self.mpc_opti.subject_to(
self.U[0, 1:] - self.U[0, 0:-1] <= self.ramp_v_max)
self.mpc_opti.subject_to(
self.ramp_delta_min <= self.U[1, 1:] - self.U[1, 0:-1])
self.mpc_opti.subject_to(
self.U[1, 1:] - self.U[1, 0:-1] <= self.ramp_delta_max)
self.mpc_opti.subject_to(self.X[:, 0] == self.P_1[0:3])
p_opts = {'verbose_init': False}
s_opts = {'tol': 0.01, 'print_level': 0, 'max_iter': 100}
self.mpc_opti.solver('ipopt', p_opts, s_opts)
# Warm up
self.mpc_opti.set_value(self.P_1, self.state)
self.mpc_opti.set_value(self.P_2, self.input)
sol = self.mpc_opti.solve()
self.mpc_opti.set_initial(self.U, sol.value(self.U))
self.mpc_opti.set_initial(self.X, sol.value(self.X))
self.mpc_opti.set_initial(self.NU, sol.value(self.NU))
self.mpc_opti.set_initial(self.TH, sol.value(self.TH))
def _prepare_function_for_casadi(self):
Qsym = casadi.MX.sym("Q", self.m.nbQ(), 1)
self.CoMs = casadi.Function(
"CoMbySegment", [Qsym],
[self.m.CoMbySegmentInMatrix(Qsym).to_mx()])
def _prepare_function_for_casadi(self):
q_sym = casadi.MX.sym("Q", self.m.nbQ(), 1)
self.markers = casadi.Function("Markers", [q_sym],
[self.m.markers(q_sym)])
def direct_collocation(opt):
# 对控制量和主变量都做离散化，不再需要对离散时间动态的构造
# 用了多项式插值的方法参数化整个状态轨迹,从而实现匹配
d = 3 # degree of interpolating polynomial
tau_root = np.append(0,ca.collocation_points(d,'legendre'))
#匹配方程的参数矩阵
C = np.zeros((d+1,d+1))
#连续方程的参数矩阵
D = np.zeros(d+1)
#二次方程的参数矩阵
B = np.zeros(d+1)
for j in range(d+1):
p = np.poly1d([1])
for r in range(d+1):
if r != j:
p*= np.poly1d([1,-tau_root[r]]) / (tau_root[j] - tau_root[r])
D[j] = p(1.0)
pder = np.polyder(p)
for r in range(d+1):
C[j,r] = pder(tau_root[r])
pint = np.polyint(p)
B[j] = pint(1.0)
T = 10
x1 = ca.MX.sym('x1')
x2 = ca.MX.sym('x2')
x = ca.vertcat(x1,x2)
u = ca.MX.sym('u')
xdot = ca.vertcat((1 - x2 ** 2) * x1 - x2 + u, x1)
L = x1 ** 2 + x2 ** 2 + u ** 2
# 连续动态的建模
f = ca.Function('f',[x,u],[xdot,L],['x','u'],['xdot','L'])
# 离散化控制量
N = 20
h = T/N
w = []
w0 = []
lbw = []
ubw = []
J = 0
g = []
lbg = []
ubg = []
x_plot = []
u_plot = []
#这一步是与multi方法一致的
Xk = ca.MX.sym('X0',2)
w.append(Xk)
# 醉了，前面用append不好吗？明明优雅很多
lbw.append([0,1])
ubw.append([0,1])
w0.append([0,1])
x_plot.append(Xk)
for k in range(N):
Uk = ca.MX.sym('U_'+str(k))
w.append(Uk)
lbw.append([-1])
ubw.append([1])
w0.append([0])
u_plot.append(Uk)
Xc = []
for j in range(d):
Xkj = ca.MX.sym('X_'+str(k)+'_'+str(j),2)
Xc.append(Xkj)
w.append(Xkj)
lbw.append([-0.25,-np.inf])
ubw.append([np.inf,np.inf])
w0.append([0,0])
Xk_end = D[0]*Xk
for j in range(1,d+1):
xp = C[0,j]*Xk
for r in range(d): xp = xp+C[r+1,j]*Xc[r]
fj,qj = f(Xc[j-1],Uk)
g.append(h*fj-xp)
lbg.append([0,0])
ubg.append([0,0])
Xk_end = Xk_end + D[j]*Xc[j-1]
J = J + B[j]*qj*h
Xk = ca.MX.sym('X_'+str(k+1),2)
w.append(Xk)
lbw.append([-0.25,-np.inf])
ubw.append([np.inf,np.inf])
w0.append([0,0])
x_plot.append(Xk)
g.append(Xk_end-Xk)
lbg.append([0,0])
ubg.append([0,0])
w = ca.vertcat(*w)
g = ca.vertcat(*g)
x_plot = ca.horzcat(*x_plot)
u_plot = ca.horzcat(*u_plot)
w0 = np.concatenate(w0)
lbw = np.concatenate(lbw)
ubw = np.concatenate(ubw)
lbg = np.concatenate(lbg)
ubg = np.concatenate(ubg)
prob = {'f':J,'x':w,'g':g}
solver = ca.nlpsol('solver','ipopt',prob)
trajectories = ca.Function('trajectories',[w],[x_plot,u_plot],['w'],['x','u'])
sol = solver(x0 = w0, lbx = lbw, ubx = ubw, lbg = lbg, ubg = ubg)
x_opt,u_opt = trajectories(sol['x'])
x_opt = x_opt.full() # to numpy array
u_opt = u_opt.full()
tgrid = [T / N * k for k in range(N + 1)]
import matplotlib.pyplot as plt
plt.figure(1)
plt.clf()
plt.plot(tgrid, x_opt[0], '--')
plt.plot(tgrid, x_opt[1], '-')
plt.step(tgrid, np.append(np.nan,u_opt[0]), '-.')
plt.xlabel('t')
plt.legend(['x1', 'x2', 'u'])
plt.grid()
plt.show()
def direct_single_shooting(opt):
T = 10
N = 20
x1 = ca.MX.sym('x1')
x2 = ca.MX.sym('x2')
x = ca.vertcat(x1,x2)
u = ca.MX.sym('u')
xdot = ca.vertcat((1-x2**2)*x1-x2+u,x1)
L = x1**2+x2**2+u**2
if opt==False:
# using CVodes instead of RK4
dae = {'x':x,'p':u,'ode':xdot,'quad':L}
opts = {'tf':T/N} #这里是做了离散化，N是控制区间
F = ca.integrator('F','cvodes',dae,opts)
else:
#using fixed step Runge-kutta 4 integrator
# ref: https: // blog.csdn.net / xiaokun19870825 / article / details / 78763739
M = 4
DT = T/M/N
f = ca.Function('f',[x,u],[xdot,L])
X0 = ca.MX.sym('X0',2)
U = ca.MX.sym('U')
X = X0
Q = 0
for j in range(M):
k1,k1_q = f(X,U)
k2,k2_q = f(X+DT/2*k1,U)
k3,k3_q = f(X+DT/2*k2,U)
k4,k4_q = f(X+DT*k3,U)
X = X+DT/6*(k1 + 2*k2+ 2*k3 + k4)
Q = Q + DT / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q)
F = ca.Function('F', [X0, U], [X, Q], ['x0', 'p'], ['xf', 'qf'])
# Evaluate at a test point
# Fk = F(x0=[0.2,0.3],p=0.4)
# print(Fk['xf'])
# print(Fk['qf'])
# Start with an empty NLP
w=[]
w0 = []
lbw = []
ubw = []
J = 0
g=[]
lbg = []
ubg = []
# Formulate the NLP
# 这时u是离散控制量
Xk = ca.MX([0, 1])
for k in range(N):
# New NLP variable for the control
Uk = ca.MX.sym('U_' + str(k))
w += [Uk] # w是局部变量，这里就是u
# 至于下列的约束为什么都是List类型： 因为是离散控制量，目标函数是L在区间上的积分，所以对每个分割内进行计算并求和
lbw += [-1]
ubw += [1]
w0 += [0]
# Integrate till the end of the interval
Fk = F(x0=Xk, p=Uk)
Xk = Fk['xf']
J=J+Fk['qf'] # 通过累加操作来逼近积分，又因为用了RK4，所以积分的精度得到了保证。
# Add inequality constraint
g += [Xk[0]]
lbg += [-.25]
ubg += [ca.inf]
# Create an NLP solver
# 这里x指向的是控制量！！！
# 因为目标函数所求的是最小的u,x
# 而x是由u给出的，得到了u就可以根据动态系统得到x
prob = {'f': J, 'x': ca.vertcat(*w), 'g': ca.vertcat(*g)}
solver = ca.nlpsol('solver', 'ipopt', prob)
# Solve the NLP
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
w_opt = sol['x']
# Plot the solution
u_opt = w_opt #这里的操作验证了上述注释的内容
x_opt = [[0, 1]]
for k in range(N):
Fk = F(x0=x_opt[-1], p=u_opt[k])
x_opt += [Fk['xf'].full()] #.full()转化为np.array
x1_opt = [r[0] for r in x_opt]
x2_opt = [r[1] for r in x_opt]
tgrid = [T/N*k for k in range(N+1)]
import matplotlib.pyplot as plt
plt.figure(1)
plt.clf()
plt.plot(tgrid, x1_opt, '--')
plt.plot(tgrid, x2_opt, '-')
plt.step(tgrid, ca.vertcat(ca.DM.nan(1), u_opt), '-.')
plt.xlabel('t')
plt.legend(['x1','x2','u'])
plt.grid()
plt.show()
def direct_multi_shooting(opt):
# control and state nodes start value in NLP
# 与single_shooting相似，但是把状态点放在了特定的shooting nodes上，并作为NLP的决策变量：
# 注意到在single_shooting方法中，NLP的决策变量唯一且为控制量。
# multi方法往往是优于single方法的，因为将问题的维度提高了，从而提高了问题的收敛性
T = 10
N = 20
x1 = ca.MX.sym("x1")
x2 = ca.MX.sym("x2")
x = ca.vertcat(x1,x2)
u = ca.MX.sym('u')
xdot = ca.vertcat((1-x2**2)*x1-x2+u,x1)
L = x1**2+x2**2+u**2
if opt == False:
dea = {'x':x,'p':u,'ode':xdot,'quad':L}
opts = {'tf': T/N}
F = ca.integrator('F','cvodes',dea,opts)
else:
M = 4
DT = T/N/M
f = ca.Function('f',[x,u],[xdot,L])
X0 = ca.MX.sym('x0',2) # 这里的X0是initial
U = ca.MX.sym('U')
X = X0
Q = 0 # Q 是 L 的逼近斜率
for i in range(M):
k1,k1_q = f(X,U)
k2,k2_q = f(X+DT/2*k1,U)
k3,k3_q = f(X+DT/2*k2,U)
k4,k4_q = f(X+DT*k3,U)
X = X+DT/6*(k1+2*k2+2*k3+k4)
Q = Q+DT/6*(k1_q+2*k2_q+2*k3_q+k4_q)
F = ca.Function('F',[X0,U],[X,Q],['x0','p'],['xf','qf'])
Fk = F(x0 = [0.2,0.3],p=0.4)
print(Fk['xf'])
print(Fk['qf'])
w = []
w0= []
lbw=[]
ubw=[]
J = 0
g = []
lbg = []
ubg = []
# 初始变量是[0,1]
Xk = ca.MX.sym('X0',2)
w += [Xk] #这里开始和single有区别：（single中为 w += [Uk])
lbw += [0,1]
ubw += [0,1]
w0 += [0,1]
for k in range(N):
Uk = ca.MX.sym('U_' + str(k))
w += [Uk]
lbw += [-1]
ubw += [1]
w0 += [0]
Fk = F(x0=Xk,p=Uk)
Xk_end = Fk['xf']
J = J + Fk['qf']
Xk = ca.MX.sym('X_' + str(k+1),2)
w += [Xk]
lbw += [-0.25,-ca.inf]
ubw += [ca.inf,ca.inf]
w0 += [0,0]
#这里添加了等式约束，在single中添加的是等式约束
g += [Xk_end-Xk]
lbg += [0,0]
ubg += [0,0]
prob = {'f':J,'x':ca.vertcat(*w),'g':ca.vertcat(*g)}
solver= ca.nlpsol('solver','ipopt',prob)
sol = solver(x0 = w0,lbx = lbw, ubx = ubw, lbg = lbg, ubg= ubg)
w_opt = sol['x'].full().flatten()
x1_opt = w_opt[0::3]
x2_opt = w_opt[1::3]
u_opt = w_opt[2::3]
tgrid = [T/N*k for k in range(N+1)]
import matplotlib.pyplot as plt
plt.figure(1)
plt.clf()
plt.plot(tgrid, x1_opt, '--')
plt.plot(tgrid, x2_opt, '-')
plt.step(tgrid, ca.vertcat(ca.DM.nan(1), u_opt), '-.')
plt.xlabel('t')
plt.legend(['x1','x2','u'])
plt.grid()
plt.show()
def set_up(self, model, inputs=None):
"""Unpack model, perform checks, simplify and calculate jacobian.
Parameters
----------
model : :class:`pybamm.BaseModel`
The model whose solution to calculate. Must have attributes rhs and
initial_conditions
inputs : dict, optional
Any input parameters to pass to the model when solving
"""
# Check model.algebraic for ode solvers
if self.ode_solver is True and len(model.algebraic) > 0:
raise pybamm.SolverError(
"Cannot use ODE solver '{}' to solve DAE model".format(
self.name))
# Check model.rhs for algebraic solvers
if self.algebraic_solver is True and len(model.rhs) > 0:
raise pybamm.SolverError(
"""Cannot use algebraic solver to solve model with time derivatives"""
)
inputs = inputs or {}
y0 = model.concatenated_initial_conditions.evaluate(0,
None,
inputs=inputs)
# Set model timescale
model.timescale_eval = model.timescale.evaluate(inputs=inputs)
if self.ode_solver is True:
self.root_method = None
if (isinstance(self,
(pybamm.CasadiSolver, pybamm.CasadiAlgebraicSolver))
) and model.convert_to_format != "casadi":
pybamm.logger.warning(
"Converting {} to CasADi for solving with CasADi solver".
format(model.name))
model.convert_to_format = "casadi"
if self.root_method == "casadi" and model.convert_to_format != "casadi":
pybamm.logger.warning(
"Converting {} to CasADi for calculating ICs with CasADi".
format(model.name))
model.convert_to_format = "casadi"
if model.convert_to_format != "casadi":
simp = pybamm.Simplification()
# Create Jacobian from concatenated rhs and algebraic
y = pybamm.StateVector(slice(0, np.size(y0)))
# set up Jacobian object, for re-use of dict
jacobian = pybamm.Jacobian()
else:
# Convert model attributes to casadi
t_casadi = casadi.MX.sym("t")
y_diff = casadi.MX.sym(
"y_diff",
len(model.concatenated_rhs.evaluate(0, y0, inputs=inputs)))
y_alg = casadi.MX.sym(
"y_alg",
len(model.concatenated_algebraic.evaluate(0, y0,
inputs=inputs)),
)
y_casadi = casadi.vertcat(y_diff, y_alg)
p_casadi = {}
for name, value in inputs.items():
if isinstance(value, numbers.Number):
p_casadi[name] = casadi.MX.sym(name)
else:
p_casadi[name] = casadi.MX.sym(name, value.shape[0])
p_casadi_stacked = casadi.vertcat(*[p for p in p_casadi.values()])
def process(func, name, use_jacobian=None):
def report(string):
# don't log event conversion
if "event" not in string:
pybamm.logger.info(string)
if use_jacobian is None:
use_jacobian = model.use_jacobian
if model.convert_to_format != "casadi":
# Process with pybamm functions
if model.use_simplify:
report(f"Simplifying {name}")
func = simp.simplify(func)
if use_jacobian:
report(f"Calculating jacobian for {name}")
jac = jacobian.jac(func, y)
if model.use_simplify:
report(f"Simplifying jacobian for {name}")
jac = simp.simplify(jac)
if model.convert_to_format == "python":
report(f"Converting jacobian for {name} to python")
jac = pybamm.EvaluatorPython(jac)
jac = jac.evaluate
else:
jac = None
if model.convert_to_format == "python":
report(f"Converting {name} to python")
func = pybamm.EvaluatorPython(func)
func = func.evaluate
else:
# Process with CasADi
report(f"Converting {name} to CasADi")
func = func.to_casadi(t_casadi, y_casadi, inputs=p_casadi)
if use_jacobian:
report(f"Calculating jacobian for {name} using CasADi")
jac_casadi = casadi.jacobian(func, y_casadi)
jac = casadi.Function(
name, [t_casadi, y_casadi, p_casadi_stacked],
[jac_casadi])
else:
jac = None
func = casadi.Function(name,
[t_casadi, y_casadi, p_casadi_stacked],
[func])
if name == "residuals":
func_call = Residuals(func, name, model)
else:
func_call = SolverCallable(func, name, model)
if jac is not None:
jac_call = SolverCallable(jac, name + "_jac", model)
else:
jac_call = None
return func, func_call, jac_call
# Check for heaviside functions in rhs and algebraic and add discontinuity
# events if these exist.
# Note: only checks for the case of t < X, t <= X, X < t, or X <= t, but also
# accounts for the fact that t might be dimensional
# Only do this for DAE models as ODE models can deal with discontinuities fine
if len(model.algebraic) > 0:
for symbol in itertools.chain(
model.concatenated_rhs.pre_order(),
model.concatenated_algebraic.pre_order(),
):
if isinstance(symbol, pybamm.Heaviside):
found_t = False
# Dimensionless
if symbol.right.id == pybamm.t.id:
expr = symbol.left
found_t = True
elif symbol.left.id == pybamm.t.id:
expr = symbol.right
found_t = True
# Dimensional
elif symbol.right.id == (pybamm.t * model.timescale).id:
expr = symbol.left.new_copy(
) / symbol.right.right.new_copy()
found_t = True
elif symbol.left.id == (pybamm.t * model.timescale).id:
expr = symbol.right.new_copy(
) / symbol.left.right.new_copy()
found_t = True
# Update the events if the heaviside function depended on t
if found_t:
model.events.append(
pybamm.Event(
str(symbol),
expr.new_copy(),
pybamm.EventType.DISCONTINUITY,
))
# Process rhs, algebraic and event expressions
rhs, rhs_eval, jac_rhs = process(model.concatenated_rhs, "RHS")
algebraic, algebraic_eval, jac_algebraic = process(
model.concatenated_algebraic, "algebraic")
terminate_events_eval = [
process(event.expression, "event", use_jacobian=False)[1]
for event in model.events
if event.event_type == pybamm.EventType.TERMINATION
]
# discontinuity events are evaluated before the solver is called, so don't need
# to process them
discontinuity_events_eval = [
event for event in model.events
if event.event_type == pybamm.EventType.DISCONTINUITY
]
# Add the solver attributes
model.rhs_eval = rhs_eval
model.algebraic_eval = algebraic_eval
model.jac_algebraic_eval = jac_algebraic
model.terminate_events_eval = terminate_events_eval
model.discontinuity_events_eval = discontinuity_events_eval
# Save CasADi functions for the CasADi solver
# Note: when we pass to casadi the ode part of the problem must be in explicit
# form so we pre-multiply by the inverse of the mass matrix
if self.root_method == "casadi" or isinstance(self,
pybamm.CasadiSolver):
# can use DAE solver to solve model with algebraic equations only
if len(model.rhs) > 0:
mass_matrix_inv = casadi.MX(model.mass_matrix_inv.entries)
explicit_rhs = mass_matrix_inv @ rhs(t_casadi, y_casadi,
p_casadi_stacked)
model.casadi_rhs = casadi.Function(
"rhs", [t_casadi, y_casadi, p_casadi_stacked],
[explicit_rhs])
model.casadi_algebraic = algebraic
if self.algebraic_solver is True:
# we don't calculate consistent initial conditions
# for an algebraic solver as this will be the job of the algebraic solver
model.residuals_eval = Residuals(algebraic, "residuals", model)
model.jacobian_eval = jac_algebraic
model.y0 = y0.flatten()
elif len(model.algebraic) == 0:
# can use DAE solver to solve ODE model
# - no initial condition initialization needed
model.residuals_eval = Residuals(rhs, "residuals", model)
model.jacobian_eval = jac_rhs
model.y0 = y0.flatten()
# Calculate consistent initial conditions for the algebraic equations
else:
if len(model.rhs) > 0:
all_states = pybamm.NumpyConcatenation(
model.concatenated_rhs, model.concatenated_algebraic)
# Process again, uses caching so should be quick
residuals_eval, jacobian_eval = process(
all_states, "residuals")[1:]
model.residuals_eval = residuals_eval
model.jacobian_eval = jacobian_eval
else:
model.residuals_eval = Residuals(algebraic, "residuals", model)
model.jacobian_eval = jac_algebraic
y0_guess = y0.flatten()
model.y0 = self.calculate_consistent_state(model, 0, y0_guess,
inputs)
pybamm.logger.info("Finish solver set-up")
## Listing 1 ##############################################
import nloed as nl, casadi as cs
import numpy as np, pandas as pd
#CasADi input and parameter symbols
x = cs.SX.sym('x', 1)
theta = cs.SX.sym('theta', 4)
#Transform; ensures parameter positivity
q = cs.exp(theta)
#Expressions for the sampling statistics
mean = (q[0] + q[1] * x[0]**q[2] / (q[3]**q[2] + x[0]**q[2]))
s = 0.059
var = (s * mean)**2
#Create a sampling statistics vector
stats = cs.vertcat(mean, var)
#Create a CasADi function for f(.)
func = cs.Function('GFP', [x, theta], [stats])
## Listing 2 ##############################################
#Input and parameter name lists
xname = ['Light']
tname = ['ln_A0', 'ln_A', 'ln_N', 'ln_K']
#Assign a normal dist. to the output
obs_var = [(func, 'Normal')]
#Call the NLoed Model constructor
model = nl.Model(obs_var, xname, tname)
## Listing 3 ##############################################
#Initial green light levels (%)
levels0 = [0.01, 0.01, 0.01, 1.5, 1.5, 1.5, 6, 6, 6, 25, 25, 25, 100, 100, 100]
#Initial GFP observations (a.u.)
obs0 = [
def Max_velocity(pts, config, show=False):
mu = config['car']['mu']
m = config['car']['m']
g = config['car']['g']
l_f = config['car']['l_f']
l_r = config['car']['l_r']
safety_f = config['pp']['force_f']
f_max = mu * m * g * safety_f
f_long_max = l_f / (l_r + l_f) * f_max
max_v = config['lims'][
'max_v'] # parameter to be adapted so that optimiser isnt too fast
max_a = config['lims']['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
#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
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.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(2)
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"])
# plt.figure(3)
# plt.title("Theta vs t")
# plt.plot(t, th_i)
# plt.plot(t, np.abs(th_i))
# plt.figure(5)
# plt.title(f"t vs dt")
# plt.plot(t[1:], dt)
# plt.plot(t[1:], dt, '+')
# 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 wrapperRWSC(IC, args, optimal):
# Converting the Optimal Control Problem into a Non-Linear Programming Problem
numStates = 6
numInputs = 2
nodes = 30 # Keep this Number Small to Reduce Runtime
dt = SX.sym("dt")
states = SX.sym("state", nodes, numStates)
controls = SX.sym("controls", nodes, numInputs)
variables_list = [dt, states, controls]
variables_name = ["dt", "states", "controls"]
variables_flat = casadi.vertcat(*[casadi.reshape(e, -1, 1) for e in variables_list])
pack_variables_fn = casadi.Function(
"pack_variables_fn", variables_list, [variables_flat], variables_name, ["flat"]
)
unpack_variables_fn = casadi.Function(
"unpack_variables_fn",
[variables_flat],
variables_list,
["flat"],
variables_name,
)
# Bounds
bds = [
[np.sqrt(np.finfo(float).eps), np.inf],
[-100, 300],
[0, np.inf],
[-np.inf, np.inf],
[-np.inf, np.inf],
[np.sqrt(np.finfo(float).eps), np.inf],
[-1, 1],
[np.sqrt(np.finfo(float).eps), 1],
]
lower_bounds = unpack_variables_fn(flat=-float("inf"))
lower_bounds["dt"][:, :] = bds[0][0]
lower_bounds["states"][:, 0] = bds[1][0]
lower_bounds["states"][:, 1] = bds[2][0]
lower_bounds["states"][:, 4] = bds[5][0]
lower_bounds["controls"][:, 0] = bds[6][0]
lower_bounds["controls"][:, 1] = bds[7][0]
upper_bounds = unpack_variables_fn(flat=float("inf"))
upper_bounds["dt"][:, :] = bds[0][1]
upper_bounds["states"][:, 0] = bds[1][1]
upper_bounds["controls"][:, 0] = bds[6][1]
upper_bounds["controls"][:, 1] = bds[7][1]
# Set Initial Conditions
# Casadi does not accept equality constraints, so boundary constraints are
# set as box constraints with 0 area.
lower_bounds["states"][0, 0] = IC[0]
lower_bounds["states"][0, 1] = IC[1]
lower_bounds["states"][0, 2] = IC[2]
lower_bounds["states"][0, 3] = IC[3]
lower_bounds["states"][0, 4] = IC[4]
lower_bounds["states"][0, 5] = IC[5]
upper_bounds["states"][0, 0] = IC[0]
upper_bounds["states"][0, 1] = IC[1]
upper_bounds["states"][0, 2] = IC[2]
upper_bounds["states"][0, 3] = IC[3]
upper_bounds["states"][0, 4] = IC[4]
upper_bounds["states"][0, 5] = IC[5]
# Set Final Conditions
# Currently set for a soft touchdown at the origin
lower_bounds["states"][-1, 0] = 0
lower_bounds["states"][-1, 1] = 0
lower_bounds["states"][-1, 2] = 0
lower_bounds["states"][-1, 3] = 0
lower_bounds["states"][-1, 5] = 0
upper_bounds["states"][-1, 0] = 0
upper_bounds["states"][-1, 1] = 0
upper_bounds["states"][-1, 2] = 0
upper_bounds["states"][-1, 3] = 0
upper_bounds["states"][-1, 5] = 0
# Initial Guess Generation
# Generate the initial guess as a line between initial and final conditions
xIG = np.array(
[
np.linspace(IC[0], 0, nodes),
np.linspace(IC[1], 0, nodes),
np.linspace(IC[2], 0, nodes),
np.linspace(IC[3], 0, nodes),
np.linspace(IC[4], IC[4] * 0.5, nodes),
np.linspace(IC[5], 0, nodes),
]
).T
uIG = np.array([np.linspace(0, 1, nodes), np.linspace(1, 1, nodes)]).T
ig_list = [60 / nodes, xIG, uIG]
ig_flat = casadi.vertcat(*[casadi.reshape(e, -1, 1) for e in ig_list])
# Generating Defect Vector
xLow = states[0 : (nodes - 1), :]
xHigh = states[1:nodes, :]
contLow = controls[0 : (nodes - 1), :]
contHi = controls[1:nodes, :]
contMid = (contLow + contHi) / 2
# Use a RK4 Method for Generating the Defects
k1 = RWSC(xLow, contLow, args)
k2 = RWSC(xLow + (0.5 * dt * k1), contMid, args)
k3 = RWSC(xLow + (0.5 * dt * k2), contMid, args)
k4 = RWSC(xLow + k3, contHi, args)
xNew = xLow + ((dt / 6) * (k1 + (2 * k2) + (2 * k3) + k4))
defect = casadi.reshape(xNew - xHigh, -1, 1)
# Choose the Cost Function
if optimal == 1:
J = -states[-1, 4] # Mass Optimal Cost Function
elif optimal == 2:
J = dt[0] # Time Optimal Cost Function
# Put the OCP into a form that Casadi can solve
nlp = {"x": variables_flat, "f": J, "g": defect}
solver = casadi.nlpsol(
"solver", "bonmin", nlp
) # Use bonmin instead of ipopt due to speed
result = solver(
x0=ig_flat,
lbg=0.0,
ubg=0.0,
lbx=pack_variables_fn(**lower_bounds)["flat"],
ubx=pack_variables_fn(**upper_bounds)["flat"],
)
results = unpack_variables_fn(flat=result["x"])
return results
n_controls = 2 # len([controls])
rhs = ca.vertcat(v * ca.cos(theta), v * ca.sin(theta), omega)
# Obstacle states in each predictions
MOx = ca.SX.sym('MOx')
MOy = ca.SX.sym('MOy')
MOth = ca.SX.sym('MOth')
MOv = ca.SX.sym('MOv')
MOr = ca.SX.sym('MOr')
Ost = [MOx, MOy, MOth, MOv, MOr]
n_MOst = len(Ost)
# System setup for casadi
mapping_func = ca.Function('f', [states, controls], [rhs])
# Declare empty system matrices
U = ca.SX.sym('U', n_controls, N)
# Parameters:initial state(x0), reference state (xref), obstacles (O)
P = ca.SX.sym('P', n_states + n_states + n_MO * (N + 1) * n_MOst + n_SO*3)
X = ca.SX.sym('X', n_states, (N + 1)) # Prediction matrix
# Objective Function and Constraints
# weighing matrices (states)
Q = np.zeros((3, 3))
Q[0, 0] = 1 # x
Q[1, 1] = 5 # y
(
ca_tools.entry('v'),
ca_tools.entry('omega')
)
])
v, omega = controls[...]
n_controls = controls.size
## rhs
rhs = ca_tools.struct_SX(states)
rhs['x'] = v*ca.cos(theta)
rhs['y'] = v*ca.sin(theta)
rhs['theta'] = omega
## function
f = ca.Function('f', [states, controls], [rhs], ['input_state', 'control_input'], ['rhs'])
## for MPC
optimizing_target = ca_tools.struct_symSX([
(
ca_tools.entry('U', repeat=N, struct=controls),
ca_tools.entry('X', repeat=N+1, struct=states)
)
])
U, X, = optimizing_target[...] # data are stored in list [], notice that ',' cannot be missed
# X = ca.SX.sym('X', n_states, N+1)
current_parameters = ca_tools.struct_symSX([
(
ca_tools.entry('P', shape=n_states+n_states),
def _convert(self, symbol, t, y, y_dot, inputs):
""" See :meth:`CasadiConverter.convert()`. """
if isinstance(
symbol,
(
pybamm.Scalar,
pybamm.Array,
pybamm.Time,
pybamm.InputParameter,
pybamm.ExternalVariable,
),
):
return casadi.MX(symbol.evaluate(t, y, y_dot, inputs))
elif isinstance(symbol, pybamm.StateVector):
if y is None:
raise ValueError(
"Must provide a 'y' for converting state vectors")
return casadi.vertcat(*[y[y_slice] for y_slice in symbol.y_slices])
elif isinstance(symbol, pybamm.StateVectorDot):
if y_dot is None:
raise ValueError(
"Must provide a 'y_dot' for converting state vectors")
return casadi.vertcat(
*[y_dot[y_slice] for y_slice in symbol.y_slices])
elif isinstance(symbol, pybamm.BinaryOperator):
left, right = symbol.children
# process children
converted_left = self.convert(left, t, y, y_dot, inputs)
converted_right = self.convert(right, t, y, y_dot, inputs)
if isinstance(symbol, pybamm.Modulo):
return casadi.fmod(converted_left, converted_right)
if isinstance(symbol, pybamm.Minimum):
return casadi.fmin(converted_left, converted_right)
if isinstance(symbol, pybamm.Maximum):
return casadi.fmax(converted_left, converted_right)
# _binary_evaluate defined in derived classes for specific rules
return symbol._binary_evaluate(converted_left, converted_right)
elif isinstance(symbol, pybamm.UnaryOperator):
converted_child = self.convert(symbol.child, t, y, y_dot, inputs)
if isinstance(symbol, pybamm.AbsoluteValue):
return casadi.fabs(converted_child)
if isinstance(symbol, pybamm.Floor):
return casadi.floor(converted_child)
if isinstance(symbol, pybamm.Ceiling):
return casadi.ceil(converted_child)
return symbol._unary_evaluate(converted_child)
elif isinstance(symbol, pybamm.Function):
converted_children = [
self.convert(child, t, y, y_dot, inputs)
for child in symbol.children
]
# Special functions
if symbol.function == np.min:
return casadi.mmin(*converted_children)
elif symbol.function == np.max:
return casadi.mmax(*converted_children)
elif symbol.function == np.abs:
return casadi.fabs(*converted_children)
elif symbol.function == np.sqrt:
return casadi.sqrt(*converted_children)
elif symbol.function == np.sin:
return casadi.sin(*converted_children)
elif symbol.function == np.arcsinh:
return casadi.arcsinh(*converted_children)
elif symbol.function == np.arccosh:
return casadi.arccosh(*converted_children)
elif symbol.function == np.tanh:
return casadi.tanh(*converted_children)
elif symbol.function == np.cosh:
return casadi.cosh(*converted_children)
elif symbol.function == np.sinh:
return casadi.sinh(*converted_children)
elif symbol.function == np.cos:
return casadi.cos(*converted_children)
elif symbol.function == np.exp:
return casadi.exp(*converted_children)
elif symbol.function == np.log:
return casadi.log(*converted_children)
elif symbol.function == np.sign:
return casadi.sign(*converted_children)
elif symbol.function == special.erf:
return casadi.erf(*converted_children)
elif isinstance(symbol, pybamm.Interpolant):
return casadi.interpolant(
"LUT", "bspline", symbol.x,
symbol.y.flatten())(*converted_children)
elif symbol.function.__name__.startswith("elementwise_grad_of_"):
differentiating_child_idx = int(symbol.function.__name__[-1])
# Create dummy symbolic variables in order to differentiate using CasADi
dummy_vars = [
casadi.MX.sym("y_" + str(i))
for i in range(len(converted_children))
]
func_diff = casadi.gradient(
symbol.differentiated_function(*dummy_vars),
dummy_vars[differentiating_child_idx],
)
# Create function and evaluate it using the children
casadi_func_diff = casadi.Function("func_diff", dummy_vars,
[func_diff])
return casadi_func_diff(*converted_children)
# Other functions
else:
return symbol._function_evaluate(converted_children)
elif isinstance(symbol, pybamm.Concatenation):
converted_children = [
self.convert(child, t, y, y_dot, inputs)
for child in symbol.children
]
if isinstance(symbol,
(pybamm.NumpyConcatenation, pybamm.SparseStack)):
return casadi.vertcat(*converted_children)
# DomainConcatenation specifies a particular ordering for the concatenation,
# which we must follow
elif isinstance(symbol, pybamm.DomainConcatenation):
slice_starts = []
all_child_vectors = []
for i in range(symbol.secondary_dimensions_npts):
child_vectors = []
for child_var, slices in zip(converted_children,
symbol._children_slices):
for child_dom, child_slice in slices.items():
slice_starts.append(
symbol._slices[child_dom][i].start)
child_vectors.append(
child_var[child_slice[i].start:child_slice[i].
stop])
all_child_vectors.extend([
v for _, v in sorted(zip(slice_starts, child_vectors))
])
return casadi.vertcat(*all_child_vectors)
else:
raise TypeError("""
Cannot convert symbol of type '{}' to CasADi. Symbols must all be
'linear algebra' at this stage.
""".format(type(symbol)))
def MinCurvature():
track = run_map_gen()
txs = track[:, 0]
tys = track[:, 1]
nvecs = track[:, 2:4]
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
n_max = 3
N = len(track)
n_f_a = ca.MX.sym('n_f', N)
n_f = ca.MX.sym('n_f', N - 1)
th_f = ca.MX.sym('n_f', N - 1)
x0_f = ca.MX.sym('x0_f', N - 1)
x1_f = ca.MX.sym('x1_f', N - 1)
y0_f = ca.MX.sym('y0_f', N - 1)
y1_f = ca.MX.sym('y1_f', N - 1)
th1_f = ca.MX.sym('y1_f', N - 1)
th2_f = ca.MX.sym('y1_f', N - 1)
th1_f1 = ca.MX.sym('y1_f', N - 2)
th2_f1 = ca.MX.sym('y1_f', N - 2)
o_x_s = ca.Function('o_x', [n_f], [track[:-1, 0] + nvecs[:-1, 0] * n_f])
o_y_s = ca.Function('o_y', [n_f], [track[:-1, 1] + nvecs[:-1, 1] * n_f])
o_x_e = ca.Function('o_x', [n_f], [track[1:, 0] + nvecs[1:, 0] * n_f])
o_y_e = ca.Function('o_y', [n_f], [track[1:, 1] + nvecs[1:, 1] * n_f])
dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
[ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])
track_length = ca.Function('length', [n_f_a], [
dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
o_y_e(n_f_a[1:]))
])
real = ca.Function(
'real', [th1_f, th2_f],
[ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
im = ca.Function(
'im', [th1_f, th2_f],
[-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
[ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))])
get_th_n = ca.Function(
'gth', [th_f],
[sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))])
d_n = ca.Function('d_n', [n_f_a, th_f],
[track_length(n_f_a) / ca.tan(get_th_n(th_f))])
# define symbols
n = ca.MX.sym('n', N)
th = ca.MX.sym('th', N)
nlp = {\
'x': ca.vertcat(n, th),
'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])),
'g': ca.vertcat(
# dynamic constraints
n[1:] - (n[:-1] + d_n(n, th[:-1])),
# boundary constraints
n[0], th[0],
n[-1], #th[-1],
) \
}
S = ca.nlpsol('S', 'ipopt', nlp)
ones = np.ones(N)
n0 = ones * 0
th0 = []
for i in range(N - 1):
th_00 = lib.get_bearing(track[i, 0:2], track[i + 1, 0:2])
th0.append(th_00)
th0.append(0)
th0 = np.array(th0)
x0 = ca.vertcat(n0, th0)
lbx = [-n_max] * N + [-np.pi] * N
ubx = [n_max] * N + [np.pi] * N
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
x_opt = r['x']
n_set = np.array(x_opt[:N])
thetas = np.array(x_opt[1 * N:2 * N])
plot_race_line(np.array(track), n_set, wait=True)
return n_set
nx = sol['sys']['vars']['x'].shape[0]
nu = sol['sys']['vars']['u'].shape[0]
ns = sol['sys']['vars']['us'].shape[0]
# set-up open-loop scenario
Nmpc = 20
alpha_steps = 20
# tether length
l_t = np.sqrt(sol['wsol']['x', 0][0]**2 + sol['wsol']['x', 0][1]**2 +
sol['wsol']['x', 0][2]**2)
opts = {}
# add projection operator for terminal constraint
opts['p_operator'] = ca.Function(
'p_operator', [sol['sys']['vars']['x']],
[ct.vertcat(sol['sys']['vars']['x'][1:3], sol['sys']['vars']['x'][4:])])
# add MPC slacks to active constraints
mpc_sys = preprocessing.add_mpc_slacks(sol['sys'],
sol['lam_g'],
sol['indeces_As'],
slack_flag='active')
# create controllers
ctrls = {}
# # economic MPC
ctrls['EMPC'] = pmpc.Pmpc(N=Nmpc,
sys=mpc_sys,
cost=user_input['l'],
wref=sol['wsol'],
def pre(self):
# Call parent class first for default behaviour.
super().pre()
# Get curve fitting options from curvefit_options.ini file
ini_path = os.path.join(
self.__lookup_table_folder, 'curvefit_options.ini')
try:
ini_config = configparser.RawConfigParser()
ini_config.readfp(open(ini_path))
no_curvefit_options = False
except IOError:
logger.info(
"CSVLookupTableMixin: No curvefit_options.ini file found. Using default values.")
no_curvefit_options = True
def get_curvefit_options(curve_name, no_curvefit_options=no_curvefit_options):
if no_curvefit_options:
return 0, 0, 0
curvefit_options = []
def get_property(prop_name):
try:
prop = int(ini_config.get(curve_name, prop_name))
except configparser.NoSectionError:
prop = 0
except configparser.NoOptionError:
prop = 0
except ValueError:
raise Exception(
'CSVLookupTableMixin: Invalid {0} constraint for {1}. {0} should '
'be either -1, 0, or 1.'.format(prop_name, curve_name))
return prop
for prop_name in ['monotonicity', 'monotonicity2', 'curvature']:
curvefit_options.append(get_property(prop_name))
logger.debug("CSVLookupTableMixin: Curve fit option for {}:({},{},{})".format(
curve_name, *curvefit_options))
return tuple(curvefit_options)
# Read CSV files
logger.info(
"CSVLookupTableMixin: Generating Splines from lookup table data.")
self.__lookup_tables = {}
for filename in glob.glob(os.path.join(self.__lookup_table_folder, "*.csv")):
logger.debug(
"CSVLookupTableMixin: Reading lookup table from {}".format(filename))
csvinput = csv.load(filename, delimiter=self.csv_delimiter)
output = csvinput.dtype.names[0]
inputs = csvinput.dtype.names[1:]
# Get monotonicity and curvature from ini file
mono, mono2, curv = get_curvefit_options(output)
logger.debug(
"CSVLookupTableMixin: Output is {}, inputs are {}.".format(output, inputs))
tck = None
# If tck file is newer than the csv file, first try to load the cached values from the tck file
tck_filename = filename.replace('.csv', '.tck')
valid_cache = False
if os.path.exists(tck_filename):
if no_curvefit_options:
valid_cache = os.path.getmtime(filename) < os.path.getmtime(tck_filename)
else:
valid_cache = (os.path.getmtime(filename) < os.path.getmtime(tck_filename)) and \
(os.path.getmtime(ini_path) < os.path.getmtime(tck_filename))
if valid_cache:
logger.debug(
'CSVLookupTableMixin: Attempting to use cached tck values for {}'.format(output))
with open(tck_filename, 'rb') as f:
try:
tck = pickle.load(f)
except Exception:
valid_cache = False
if not valid_cache:
logger.info(
'CSVLookupTableMixin: Recalculating tck values for {}'.format(output))
if len(csvinput.dtype.names) == 2:
if not valid_cache:
k = 3 # default value
# 1D spline fitting needs k+1 data points
if len(csvinput[output]) >= k + 1:
tck = BSpline1D.fit(csvinput[inputs[0]], csvinput[
output], k=k, monotonicity=mono, curvature=curv)
else:
raise Exception(
'CSVLookupTableMixin: Too few data points in {} to do spline fitting. '
'Need at least {} points.'.format(filename, k + 1))
if self.csv_lookup_table_debug:
import pylab
i = np.linspace(csvinput[inputs[0]][0], csvinput[
inputs[0]][-1], self.csv_lookup_table_debug_points)
o = splev(i, tck)
pylab.clf()
# TODO: Figure out why cross-section B0607 in NZV does not
# conform to constraints!
pylab.plot(i, o)
pylab.plot(csvinput[inputs[0]], csvinput[
output], linestyle='', marker='x', markersize=10)
figure_filename = filename.replace('.csv', '.png')
pylab.savefig(figure_filename)
symbols = [ca.SX.sym(inputs[0])]
function = ca.Function('f', symbols, [BSpline1D(*tck)(symbols[0])])
self.__lookup_tables[output] = LookupTable(symbols, function, tck)
elif len(csvinput.dtype.names) == 3:
if tck is None:
kx = 3 # default value
ky = 3 # default value
# 2D spline fitting needs (kx+1)*(ky+1) data points
if len(csvinput[output]) >= (kx + 1) * (ky + 1):
# TODO: add curvature parameters from curvefit_options.ini
# once 2d fit is implemented
tck = bisplrep(csvinput[inputs[0]], csvinput[
inputs[1]], csvinput[output], kx=kx, ky=ky)
else:
raise Exception(
'CSVLookupTableMixin: Too few data points in {} to do spline fitting. '
'Need at least {} points.'.format(filename, (kx + 1) * (ky + 1)))
if self.csv_lookup_table_debug:
import pylab
i1 = np.linspace(csvinput[inputs[0]][0], csvinput[
inputs[0]][-1], self.csv_lookup_table_debug_points)
i2 = np.linspace(csvinput[inputs[1]][0], csvinput[
inputs[1]][-1], self.csv_lookup_table_debug_points)
i1, i2 = np.meshgrid(i1, i2)
i1 = i1.flatten()
i2 = i2.flatten()
o = bisplev(i1, i2, tck)
pylab.clf()
pylab.plot_surface(i1, i2, o)
figure_filename = filename.replace('.csv', '.png')
pylab.savefig(figure_filename)
symbols = [ca.SX.sym(inputs[0]), ca.SX.sym(inputs[1])]
function = ca.Function('f', symbols, [BSpline2D(*tck)(symbols[0], symbols[1])])
self.__lookup_tables[output] = LookupTable(symbols, function, tck)
else:
raise Exception(
'CSVLookupTableMixin: {}-dimensional lookup tables not implemented yet.'.format(
len(csvinput.dtype.names)))
if not valid_cache:
pickle.dump(tck, open(filename.replace('.csv', '.tck'), 'wb'), protocol=pickle.HIGHEST_PROTOCOL)
def MinCurvatureTrajectory(track, obs_map=None):
w_min = -track[:, 4] * 0.9
w_max = track[:, 5] * 0.9
nvecs = track[:, 2:4]
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
xgrid = np.arange(0, obs_map.shape[1])
ygrid = np.arange(0, obs_map.shape[0])
data_flat = np.array(obs_map).ravel(order='F')
lut = ca.interpolant('lut', 'bspline', [xgrid, ygrid], data_flat)
print(lut([10, 20]))
n_max = 3
N = len(track)
n_f_a = ca.MX.sym('n_f', N)
n_f = ca.MX.sym('n_f', N - 1)
th_f = ca.MX.sym('n_f', N - 1)
x0_f = ca.MX.sym('x0_f', N - 1)
x1_f = ca.MX.sym('x1_f', N - 1)
y0_f = ca.MX.sym('y0_f', N - 1)
y1_f = ca.MX.sym('y1_f', N - 1)
th1_f = ca.MX.sym('y1_f', N - 1)
th2_f = ca.MX.sym('y1_f', N - 1)
th1_f1 = ca.MX.sym('y1_f', N - 2)
th2_f1 = ca.MX.sym('y1_f', N - 2)
o_x_s = ca.Function('o_x', [n_f], [track[:-1, 0] + nvecs[:-1, 0] * n_f])
o_y_s = ca.Function('o_y', [n_f], [track[:-1, 1] + nvecs[:-1, 1] * n_f])
o_x_e = ca.Function('o_x', [n_f], [track[1:, 0] + nvecs[1:, 0] * n_f])
o_y_e = ca.Function('o_y', [n_f], [track[1:, 1] + nvecs[1:, 1] * n_f])
dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
[ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])
track_length = ca.Function('length', [n_f_a], [
dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
o_y_e(n_f_a[1:]))
])
real = ca.Function(
'real', [th1_f, th2_f],
[ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
im = ca.Function(
'im', [th1_f, th2_f],
[-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
[ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))])
get_th_n = ca.Function(
'gth', [th_f],
[sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))])
d_n = ca.Function('d_n', [n_f_a, th_f],
[track_length(n_f_a) / ca.tan(get_th_n(th_f))])
# objective
real1 = ca.Function(
'real1', [th1_f1, th2_f1],
[ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)])
im1 = ca.Function(
'im1', [th1_f1, th2_f1],
[-ca.cos(th1_f1) * ca.sin(th2_f1) + ca.sin(th1_f1) * ca.cos(th2_f1)])
sub_cmplx1 = ca.Function(
'a_cpx1', [th1_f1, th2_f1],
[ca.atan2(im1(th1_f1, th2_f1), real1(th1_f1, th2_f1))])
# define symbols
n = ca.MX.sym('n', N)
th = ca.MX.sym('th', N - 1)
nlp = {\
'x': ca.vertcat(n, th),
'f': ca.sumsqr(sub_cmplx1(th[1:], th[:-1])),
# 'f': ca.sumsqr(track_length(n)),
'g': ca.vertcat(
# dynamic constraints
n[1:] - (n[:-1] + d_n(n, th)),
# lut(ca.horzcat(o_x_s(n[:-1]), o_y_s(n[:-1])).T).T,
# boundary constraints
n[0], #th[0],
n[-1], #th[-1],
) \
}
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 5}})
# S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':0}})
ones = np.ones(N)
n0 = ones * 0
th0 = []
for i in range(N - 1):
th_00 = lib.get_bearing(track[i, 0:2], track[i + 1, 0:2])
th0.append(th_00)
th0 = np.array(th0)
x0 = ca.vertcat(n0, th0)
# lbx = [-n_max] * N + [-np.pi]*(N-1)
# ubx = [n_max] * N + [np.pi]*(N-1)
lbx = list(w_min) + [-np.pi] * (N - 1)
ubx = list(w_max) + [np.pi] * (N - 1)
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
x_opt = r['x']
n_set = np.array(x_opt[:N])
thetas = np.array(x_opt[1 * N:2 * (N - 1)])
# lib.plot_race_line(np.array(track), n_set, wait=True)
return n_set
# Define CasADi symbolic variables.
x = casadi.SX.sym("x", Nx)
u = casadi.SX.sym("u", Nu)
# Make integrator object.
ode_integrator = dict(x=x, p=u, ode=ode(x, u))
intoptions = {
"abstol": 1e-8,
"reltol": 1e-8,
"tf": Delta,
}
vdp = casadi.integrator("int_ode", "cvodes", ode_integrator, intoptions)
# Then get nonlinear casadi functions
# and RK4 discretization.
ode_casadi = casadi.Function("ode", [x, u], [ode(x, u)])
k1 = ode_casadi(x, u)
k2 = ode_casadi(x + Delta / 2 * k1, u)
k3 = ode_casadi(x + Delta / 2 * k2, u)
k4 = ode_casadi(x + Delta * k3, u)
xrk4 = x + Delta / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
ode_rk4_casadi = casadi.Function("ode_rk4", [x, u], [xrk4])
# Define stage cost and terminal weight.
lfunc = (casadi.mtimes(x.T, x) + casadi.mtimes(u.T, u))
l = casadi.Function("l", [x, u], [lfunc])
Pffunc = casadi.mtimes(x.T, x)
Pf = casadi.Function("Pf", [x], [Pffunc])
# x[4] ,\
# x[5] ,\
# -1/p[0]*(u[0] + u[1])*casadi.sin(x[2]) ,\
# 1/p[0]*(u[0] + u[1])*casadi.cos(x[2]) - p[3],\
# p[2]/p[1]*(u[0] - u[1])
# )
ode = casadi.vertcat(
x[3] ,\
x[4] ,\
x[5] ,\
-1/m*(u[0] + u[1])*casadi.sin(x[2]) ,\
1/m*(u[0] + u[1])*casadi.cos(x[2]) - g,\
l/I*(u[0] - u[1])
)
f = casadi.Function('f', [x,u], [ode], ['x','u'], ['ode'])
# === create integrator expression ===
intg_options = dict(tf = dt) # define integration step
dae = dict(x = x, p = u, ode = f(x,u)) # define the dae to integrate (ZOH)
# define the integrator
intg = casadi.integrator('intg', 'rk', dae, intg_options)
res = intg(x0 = x, p = u)
# define symbolic expression of integrator
x_next = res['xf']
# define discretized ode
F = casadi.Function('F', [x, u], [x_next], ['x', 'u'], ['x_next'])
# === define OCP ===
'''
def _get_diff_eq(self, cost_func):
'''Function that returns the RhS of the differential equations. See the papers for additional info'''
RHS = []
# RHS that's in common for all cases
rhs1 = self.q_dot
rhs2 = self.M_inv @ (-self.Cq - self.G - self.Fd @ self.q_dot - self.Ff @ cs.sign(self.q_dot))
# Adjusting RHS for SEA with known inertia. Check the paper for more info.
if self.sea and self.SEAdynamics:
rhs2 -= self.M_inv @ self.tau_sea
rhs3 = self.theta_dot
rhs4 = cs.pinv(self.B) @ (-self.FDsea @ self.theta_dot + self.u + self.tau_sea - self.FMusea @ cs.sign(self.theta_dot))
RHS = [rhs1, rhs2, rhs3, rhs4]
# Adjusting the lenght of the variables
self.x = cs.vertcat(self.q, self.q_dot, self.theta, self.theta_dot)
self.num_state_var = self.num_joints * 2
self.lower_q = self.lower_q + self.lower_theta
self.lower_qd = self.lower_qd + self.lower_thetad
self.upper_q = self.upper_q + self.upper_theta
self.upper_qd = self.upper_qd + self.upper_thetad
# Adjusting RHS for SEA modeling, with motor inertia unknown
elif self.sea and not self.SEAdynamics:
rhs2 += -self.M_inv @ self.K @ (self.q - self.u)
# self.upper_u, self.lower_u = self.upper_q, self.lower_q
RHS = [rhs1, rhs2]
# State variable
self.x = cs.vertcat(self.q, self.q_dot)
self.num_state_var = self.num_joints
# Adjusting RHS for classic robot modeling, without any SEA
else:
rhs2 += self.M_inv @ self.u
RHS = [rhs1, rhs2]
# State variable
self.x = cs.vertcat(self.q, self.q_dot)
self.num_state_var = self.num_joints
# Evaluating q_ddot, in order to handle it when given as an input of cost function or constraints
self.q_ddot_val = self._get_joints_accelerations(rhs2)
# The differentiation of J will be the cost function given by the user, with our symbolic
# variables as inputs
if self.sea and self.SEAdynamics:
q_ddot_J = self.q_ddot_val(self.q, self.q_dot, self.theta, self.theta_dot, self.u)
else:
q_ddot_J = self.q_ddot_val(self.q,self.q_dot, self.u)
if self.sea and not self.SEAdynamics:
u_J = self.u - self.q # the instantaneous spent energy is prop to |u-q|
else:
u_J = self.u
J_dot = cost_func( self.q - self.traj,
self.q_dot - self.traj_dot,
q_ddot_J,
self.ee_pos(self.q),
u_J,
self.t
)
# Setting the relationship
self.x_dot = cs.vertcat(*RHS)
# Defining the differential equation
f = cs.Function('f', [self.x, self.u, self.t], # inputs
[self.x_dot, J_dot]) # outputs
return f
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]
K_dr = cs.exp(parameters[0])
K_T = cs.exp(parameters[1])
B_o_prime = cs.exp(parameters[2])
B_prime = cs.exp(parameters[3])
K_m = cs.exp(parameters[4])
ccar = 1 / (cs.sqrt(K_dr * (K_T / inputs + 1)**2 + 1) + cs.sqrt(K_dr) *
(K_T / inputs + 1))**2
gfp_mean = (B_o_prime + B_prime * ccar / (K_m + ccar))
#assume some hetroskedasticity, std_dev 5% of mean expression level
gfp_var = (0.05**2) * gfp_mean**2
#link the deterministic model to the sampling statistics (here normal mean and variance)
gfp_stats = cs.vertcat(gfp_mean, gfp_var)
#create a casadi function mapping input and parameters to sampling statistics (mean and var)
gfp_model = cs.Function('GFP', [inputs, parameters], [gfp_stats])
# K_g_val = 68.61725640178
# Sigma_m_val = 0.01180105629
# Sigma_g_val = 0.01381497437
# K_dr_val = 0.00166160831
# K_T_val = 11979.25516474195
# K_m_val = 0.28169444728
# B_o_val = 0.00137146821
# B_val = 0.03525605965
K_g_val = 1.6260543
Sigma_m_val = 0.00490364
Sigma_g_val = 0.046474403
K_dr_val = 4.04461938
K_T_val = 11306.84788
