def collocation_points(degree, cp='radau', with_zero=False):
if with_zero:
return [0] + collocation_points(degree,
cp) # All collocation time points
else:
return collocation_points(degree,
cp) # All collocation time points
def construct_polynomial_basis(d, root):
# Get collocation points
tau_root = np.append(0, cad.collocation_points(d, root))
# Coefficients of the collocation equation
C = np.zeros((d + 1, d + 1))
# Coefficients of the continuity equation
D = np.zeros(d + 1)
# Coefficients of the quadrature function
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_root[r]]) / (tau_root[j] - tau_root[r])
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
D[j] = p(1.0)
# Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the collocation
# equation
pder = np.polyder(p)
for r in range(d + 1):
C[j, r] = pder(tau_root[r])
# Evaluate the integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
B[j] = pint(1.0)
return C, D, B
def __init__(self, *args, degree=4, scheme='radau', **kwargs):
SamplingMethod.__init__(self, *args, **kwargs)
self.degree = degree
self.tau = collocation_points(degree, scheme)
[self.C, self.D, self.B] = collocation_coeff(self.tau)
self.Zc = [] # List that will hold algebraic decision variables
self.Xc = [] # List that will hold helper collocation states
def __init__(self,d):
self.d = d
self.B = np.zeros(self.d+1)
self.C = np.zeros([self.d+1,self.d+1])
self.D = np.zeros(self.d+1)
self.tau = np.append(0, collocation_points(self.d, 'legendre'))
# Construct polynomial basis
for j in range(self.d+1):
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
p = np.poly1d([1])
for r in range(self.d+1):
if r != j:
p *= np.poly1d([1, -self.tau[r]]) / (self.tau[j]-self.tau[r])
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
self.D[j] = p(1.0)
# Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
pder = np.polyder(p)
for r in range(self.d+1):
self.C[j,r] = pder(self.tau[r])
# Evaluate the integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
self.B[j] = pint(1.0)
def get_collocation_points(d, include0=True, include1=False):
tau_root = casadi.collocation_points(d, 'legendre')
if include0:
tau_root = [0] + tau_root
if include1:
tau_root = tau_root + [1]
return tau_root
def collocation_points(order, scheme):
# "collocationPoints -> collocation_points. Also the returned list is
# now one element less than before. To get the old behaviour, prepend
# the result with zero.", see
# https://github.com/casadi/casadi/wiki/Changes-v2.4.0-to-v3.0.0
return [0.0] + ca.collocation_points(order, scheme)
def collocationPoints(order, scheme):
'''Obtain collocation points of specific order and scheme.
'''
if scheme == 'chebyshev':
return np.array([0]) if order == 0 else (
1 - np.cos(np.pi * np.arange(order + 1) / order)) / 2
else:
return np.append(0, cs.collocation_points(order, scheme))
def define_interpolation_polynomial(d, poly_type='legendre'):
"""TODO: Docstring for define_interpolation_polynomial.
:d: Degree of interpolating polynomial
:poly_type: Type of polynomial
:returns: TODO
"""
# Degree of interpolating polynomial
d = 3
# Get collocation points
tau_root = np.append(0, cs.collocation_points(d, 'legendre'))
# Coefficients of the collocation equation
C = np.zeros((d + 1, d + 1))
# Coefficients of the continuity equation
D = np.zeros(d + 1)
# Coefficients of the quadrature function
B = np.zeros(d + 1)
F = dict()
# 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_root[r]]) / (tau_root[j] - tau_root[r])
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
D[j] = p(1.0)
# Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
pder = np.polyder(p)
for r in range(d + 1):
C[j, r] = pder(tau_root[r])
# Evaluate the integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
B[j] = pint(1.0)
F[j] = p
# Plot the polynomials
if False:
t = np.arange(0, 1, 0.01)
y = dict()
for j in range(d + 1):
y[j] = F[j](t)
plt.plot(t, y[j])
plt.show()
return {'B': B, 'C': C, 'D': D}
def __init__(self, params):
self.model = KinematicBicycleCar(N=params["N"], step=params["step"])
self.sys = self.model.getDae()
self.cost = 0
self.Uk_prev = None
self.indices_to_stop = None
self.indices_to_start = None
# hack
simparams = {
'x': self.sys.x[0],
'p': self.sys.u[0],
'ode': self.sys.ode[0]
}
self.sim = ca.integrator('F', 'idas', simparams, {
't0': 0,
'tf': self.model.step
})
# Set up collocation (from direct_collocation example)
# Degree of interpolating polynomial
self._d = 3
# Get collocation points
tau_root = np.append(0, ca.collocation_points(self._d, 'legendre'))
# Coefficients of the collocation equation
self._C = np.zeros((self._d + 1, self._d + 1))
# Coefficients of the continuity equation
self._D = np.zeros(self._d + 1)
# Coefficients of the quadrature function
self._B = np.zeros(self._d + 1)
# Construct polynomial basis
for j in range(self._d + 1):
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
p = np.poly1d([1])
for r in range(self._d + 1):
if r != j:
p *= np.poly1d([1, -tau_root[r]
]) / (tau_root[j] - tau_root[r])
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
self._D[j] = p(1.0)
# Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
pder = np.polyder(p)
for r in range(self._d + 1):
self._C[j, r] = pder(tau_root[r])
# Evaluate the integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
self._B[j] = pint(1.0)
def butcherTableuForCollocationMethod(order: int, method: str):
"""Butcher table for collocation method with a given number of collocation points
@param order number of collocation points
@param method defines collocation method ('radau' or 'legendre')
"""
return butcherTableuForCollocationPoints(
cs.collocation_points(order, method))
def __init__(self, ode: dict, ode_opt: dict):
"""
Parameters
----------
ode: dict
The ode description
ode_opt: dict
The ode options
"""
super(COLLOCATION, self).__init__(ode, ode_opt)
self.method = ode_opt["method"]
self.degree = ode_opt["irk_polynomial_interpolation_degree"]
# Coefficients of the collocation equation
self._c = self.cx.zeros((self.degree + 1, self.degree + 1))
# Coefficients of the continuity equation
self._d = self.cx.zeros(self.degree + 1)
# Choose collocation points
self.step_time = [0] + collocation_points(self.degree, self.method)
# Dimensionless time inside one control interval
time_control_interval = self.cx.sym("time_control_interval")
# For all collocation points
for j in range(self.degree + 1):
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
_l = 1
for r in range(self.degree + 1):
if r != j:
_l *= (time_control_interval - self.step_time[r]) / (
self.step_time[j] - self.step_time[r])
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
lfcn = Function("lfcn", [time_control_interval], [_l])
self._d[j] = lfcn(1.0)
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
_l = 1
for r in range(self.degree + 1):
if r != j:
_l *= (time_control_interval - self.step_time[r]) / (
self.step_time[j] - self.step_time[r])
# Evaluate the time derivative of the polynomial at all collocation points to get
# the coefficients of the continuity equation
tfcn = Function("tfcn", [time_control_interval],
[tangent(_l, time_control_interval)])
for r in range(self.degree + 1):
self._c[j, r] = tfcn(self.step_time[r])
self._finish_init()
def _initialize_polynomial_coefs(self):
""" Setup radau polynomials and initialize the weight factor matricies
"""
self.col_vars['tau_root'] = [0] + cs.collocation_points(
self.d, "radau")
# Dimensionless time inside one control interval
tau = cs.SX.sym("tau")
# For all collocation points
L = [[]] * (self.d + 1)
for j in range(self.d + 1):
# Construct Lagrange polynomials to get the polynomial basis at the
# collocation point
L[j] = 1
for r in range(self.d + 1):
if r != j:
L[j] *= ((tau - self.col_vars['tau_root'][r]) /
(self.col_vars['tau_root'][j] -
self.col_vars['tau_root'][r]))
self.col_vars['lfcn'] = lfcn = cs.Function('lfcn', [tau],
[cs.vertcat(*L)])
# Evaluate the polynomial at the final time to get the coefficients of
# the continuity equation
# Coefficients of the continuity equation
self.col_vars['D'] = np.asarray(lfcn(1.0)).squeeze()
# Evaluate the time derivative of the polynomial at all collocation
# points to get the coefficients of the continuity equation
tfcn = lfcn.tangent()
# Coefficients of the collocation equation
self.col_vars['C'] = np.zeros((self.d + 1, self.d + 1))
for r in range(self.d + 1):
self.col_vars['C'][:, r] = np.asarray(
tfcn(self.col_vars['tau_root'][r])[0]).squeeze()
# Find weights for gaussian quadrature: approximate int_0^1 f(x) by
# Sum(
xtau = cs.SX.sym("xtau")
Phi = [[]] * (self.d + 1)
for j in range(self.d + 1):
dae = dict(t=tau, x=xtau, ode=L[j])
tau_integrator = cs.integrator("integrator", "cvodes", dae, {
't0': 0.,
'tf': 1
})
Phi[j] = np.asarray(tau_integrator(x0=0)['xf'])
self.col_vars['Phi'] = np.array(Phi)
def _initialize_polynomial_coefs(self):
""" Setup radau polynomials and initialize the weight factor matricies
"""
self.col_vars['tau_root'] = [0] + cs.collocation_points(self.d, "radau")
# Dimensionless time inside one control interval
tau = cs.SX.sym("tau")
# For all collocation points
L = [[]]*(self.d+1)
for j in range(self.d+1):
# Construct Lagrange polynomials to get the polynomial basis at the
# collocation point
L[j] = 1
for r in range(self.d+1):
if r != j:
L[j] *= (
(tau - self.col_vars['tau_root'][r]) /
(self.col_vars['tau_root'][j] -
self.col_vars['tau_root'][r]))
self.col_vars['lfcn'] = lfcn = cs.Function(
'lfcn', [tau], [cs.vertcat(*L)])
# Evaluate the polynomial at the final time to get the coefficients of
# the continuity equation
# Coefficients of the continuity equation
self.col_vars['D'] = np.asarray(lfcn(1.0)).squeeze()
# Evaluate the time derivative of the polynomial at all collocation
# points to get the coefficients of the continuity equation
tfcn = lfcn.tangent()
# Coefficients of the collocation equation
self.col_vars['C'] = np.zeros((self.d+1, self.d+1))
for r in range(self.d+1):
self.col_vars['C'][:,r] = np.asarray(tfcn(self.col_vars['tau_root'][r])[0]).squeeze()
# Find weights for gaussian quadrature: approximate int_0^1 f(x) by
# Sum(
xtau = cs.SX.sym("xtau")
Phi = [[]] * (self.d+1)
for j in range(self.d+1):
dae = dict(t=tau, x=xtau, ode=L[j])
tau_integrator = cs.integrator(
"integrator", "cvodes", dae, {'t0':0., 'tf':1})
Phi[j] = np.asarray(tau_integrator(x0=0)['xf'])
self.col_vars['Phi'] = np.array(Phi)
def makePolynomial(self):
# Degree of interpolating polynomial
self.d = 3
# Get collocation points
self.tau_root = np.append(0, ca.collocation_points(self.d, 'legendre'))
# Coefficients of the collocation equation
self.C = np.zeros((self.d + 1, self.d + 1))
# Coefficients of the continuity equation
self.D = np.zeros(self.d + 1)
# Coefficients of the quadrature function
self.B = np.zeros(self.d + 1)
def test_butcherTableuForCollocationPoints(self):
'''
Test correctness of Butcher tableus for collocation methods defned by collocation nodes.
'''
for d in range(1, 5):
tableau = butcherTableuForCollocationPoints(
cs.collocation_points(d, 'legendre'))
# Test polynomial and its integral
p = np.poly1d(np.random.rand(d))
P = np.polyint(p)
nptest.assert_allclose(np.dot(tableau.A, p(tableau.c)),
P(tableau.c))
nptest.assert_allclose(np.dot(tableau.b, p(tableau.c)), P(1.0))
def _create_lagrangian_polynomial_basis(tau, degree, point_at_zero=False):
tau_root = collocation_points(
degree, 'radau') # type: list # All collocation time points
if point_at_zero:
tau_root.insert(0, 0.0)
# For all collocation points: eq 10.4 or 10.17 in Biegler's book
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
l_list = []
for j in range(len(tau_root)):
ell = 1
for k in range(len(tau_root)):
if k != j:
ell *= (tau - tau_root[k]) / (tau_root[j] - tau_root[k])
l_list.append(ell)
return l_list
def _setup_collocation_options(self):
self.d = 3
self.tau_root = [0] + ca.collocation_points(self.d, 'radau')
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 dxdt(self, h: float, states: Union[MX, SX], controls: Union[MX, SX], params: Union[MX, SX]) -> tuple:
"""
The dynamics of the system
Parameters
----------
h: float
The time step
states: Union[MX, SX]
The states of the system
controls: Union[MX, SX]
The controls of the system
params: Union[MX, SX]
The parameters of the system
Returns
-------
The derivative of the states
"""
nu = controls.shape[0]
nx = states.shape[0]
# Choose collocation points
time_points = [0] + collocation_points(self.degree, "legendre")
# Coefficients of the collocation equation
C = self.CX.zeros((self.degree + 1, self.degree + 1))
# Coefficients of the continuity equation
D = self.CX.zeros(self.degree + 1)
# Dimensionless time inside one control interval
time_control_interval = self.CX.sym("time_control_interval")
# For all collocation points
for j in range(self.degree + 1):
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
L = 1
for r in range(self.degree + 1):
if r != j:
L *= (time_control_interval - time_points[r]) / (time_points[j] - time_points[r])
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
lfcn = Function("lfcn", [time_control_interval], [L])
D[j] = lfcn(1.0)
# Evaluate the time derivative of the polynomial at all collocation points to get
# the coefficients of the continuity equation
tfcn = Function("tfcn", [time_control_interval], [tangent(L, time_control_interval)])
for r in range(self.degree + 1):
C[j, r] = tfcn(time_points[r])
# Total number of variables for one finite element
x0 = states
u = controls
x_irk_points = [self.CX.sym(f"X_irk_{j}", nx, 1) for j in range(1, self.degree + 1)]
x = [x0] + x_irk_points
x_irk_points_eq = []
for j in range(1, self.degree + 1):
t_norm_init = (j - 1) / self.degree # normalized time
# Expression for the state derivative at the collocation point
xp_j = 0
for r in range(self.degree + 1):
xp_j += C[r, j] * x[r]
# Append collocation equations
f_j = self.fun(x[j], self.get_u(u, t_norm_init), params)[:, self.idx]
x_irk_points_eq.append(h * f_j - xp_j)
# Concatenate constraints
x_irk_points = vertcat(*x_irk_points)
x_irk_points_eq = vertcat(*x_irk_points_eq)
# Root-finding function, implicitly defines x_irk_points as a function of x0 and p
vfcn = Function("vfcn", [x_irk_points, x0, u, params], [x_irk_points_eq]).expand()
# Create a implicit function instance to solve the system of equations
ifcn = rootfinder("ifcn", "newton", vfcn)
x_irk_points = ifcn(self.CX(), x0, u, params)
x = [x0 if r == 0 else x_irk_points[(r - 1) * nx : r * nx] for r in range(self.degree + 1)]
# Get an expression for the state at the end of the finite element
xf = self.CX.zeros(nx, self.degree + 1) # 0 #
for r in range(self.degree + 1):
xf[:, r] = xf[:, r - 1] + D[r] * x[r]
return xf[:, -1], horzcat(x0, xf[:, -1])
def compute_mprim(state_i, lattice, L, v, u_max, print_level):
N = 75
nx = 3
Nc = 3
mprim = []
for state_f in lattice:
x = ca.MX.sym('x', nx)
u = ca.MX.sym('u')
F = ca.Function('f', [x, u],
[v*ca.cos(x[2]), v*ca.sin(x[2]), v*ca.tan(u)/L])
opti = ca.Opti()
X = opti.variable(nx, N+1)
pos_x = X[0, :]
pos_y = X[1, :]
ang_th = X[2, :]
U = opti.variable(N, 1)
T = opti.variable(1)
dt = T/N
opti.set_initial(T, 0.1)
opti.set_initial(U, 0.0*np.ones(N))
opti.set_initial(pos_x, np.linspace(state_i[0], state_f[0], N+1))
opti.set_initial(pos_y, np.linspace(state_i[1], state_f[1], N+1))
tau = ca.collocation_points(Nc, 'radau')
C, _ = ca.collocation_interpolators(tau)
Xc_vec = []
for k in range(N):
Xc = opti.variable(nx, Nc)
Xc_vec.append(Xc)
X_kc = ca.horzcat(X[:, k], Xc)
for j in range(Nc):
fo = F(Xc[:, j], U[k])
opti.subject_to(X_kc@C[j+1] == dt*ca.vertcat(fo[0], fo[1], fo[2]))
opti.subject_to(X_kc[:, Nc] == X[:, k+1])
for k in range(N):
opti.subject_to(U[k] <= u_max)
opti.subject_to(-u_max <= U[k])
opti.subject_to(T >= 0.001)
opti.subject_to(X[:, 0] == state_i)
opti.subject_to(X[:, -1] == state_f)
alpha = 1e-2
opti.minimize(T + alpha*ca.sumsqr(U))
opti.solver('ipopt', {'expand': True},
{'tol': 10**-8, 'print_level': print_level})
sol = opti.solve()
pos_x_opt = sol.value(pos_x)
pos_y_opt = sol.value(pos_y)
ang_th_opt = sol.value(ang_th)
u_opt = sol.value(U)
T_opt = sol.value(T)
mprim.append({'x': pos_x_opt, 'y': pos_y_opt, 'th': ang_th_opt,
'u': u_opt, 'T': T_opt, 'ds': T_opt*v})
return np.array(mprim)
def collocate(xd, xa, u, p, puni, l, nk, d, dynamics, out_fun, out):
# -----------------------------------------------------------------------------
# Collocation setup
# -----------------------------------------------------------------------------
# Choose collocation points
tau_root = ca.collocation_points(d, 'radau')
tau_root = ca.veccat(0, tau_root)
# Size of the finite elements
h = 1. / nk
# Coefficients of the collocation equation
C = np.zeros((d + 1, d + 1))
# Coefficients of the continuity equation
D = np.zeros(d + 1)
# Dimensionless time inside one control interval
tau = ca.SX.sym('tau')
# All collocation time points
T = np.zeros((nk, d + 1))
for k in range(nk):
for j in range(d + 1):
T[k, j] = (k + tau_root[j])
# For all collocation points
for j in range(d + 1):
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
L = 1
for r in range(d + 1):
if r != j:
L *= (tau - tau_root[r]) / (tau_root[j] - tau_root[r])
lfcn = ca.Function('lfcn', [tau], [L])
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
D[j] = lfcn(1.0)
# Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
tfcn = lfcn.jacobian()
for r in range(d + 1):
C[j][r] = tfcn(tau_root[r], 0)
# --------------------------------------
# NLP Variables
# --------------------------------------
# Structure holding NLP variables and parameters
V = ca.struct_symMX([(
ca.entry('Xd', repeat=[nk, d + 1], struct=xd),
ca.entry('XA', repeat=[nk, d], struct=xa),
ca.entry('U', repeat=[nk], struct=u),
ca.entry('tf'),
ca.entry('vlift'),
ca.entry(
'l', struct=l
) # tether length #NOTE: COMMENT FOR MAIN.PY MODEL VALIDATION # TODO: adjust model validation code ...
)])
P = ca.struct_symMX([
ca.entry('p', repeat=[nk, d + 1], struct=p),
ca.entry(
'puni', struct=puni
), #NOTE: COMMENT FOR MAIN.PY MODEL VALIDATION # TODO: adjust model validation code ...
ca.entry('toggle_to_energy'),
# ca.entry('tf_previous'),
# ca.entry('tf_LM_regularization')
ca.entry('tf')
])
# --------------------------------------
# CONSTRAINTS
# --------------------------------------
Output_list = {}
for name in out.keys():
Output_list[name] = []
# Constraint function for the NLP
coll_cstr = [] # Endpoint should match start point
continuity_cstr = [] # At each collocation point dynamics should meet
# For all finite elements
for k in range(nk):
# For all collocation points
for j in range(1, d + 1):
# Get an expression for the state derivative at the collocation point
xp_jk = 0
for r in range(d + 1):
xp_jk += C[r, j] * V['Xd', k, r]
# Add collocation equations to the NLP
# fk = dynamics(V['Xd',k,j],xp_jk/h/V['tf'], V['XA',k,j-1], V['U',k], P['p',k,j]) #NOTE: NEEDED FOR MAIN.PY MODEL VALIDATION # TODO: adjust model validation code ...
fk = dynamics(V['Xd', k, j], xp_jk / h / V['tf'], V['XA', k,
j - 1],
V['U', k], P['p', k, j], P['puni'], V['l'])
coll_cstr.append(fk)
# Get an expression for the state at the end of the finite element
xf_k = 0
for r in range(d + 1):
xf_k += D[r] * V['Xd', k, r]
# Add continuity equation to NLP
if k < nk - 1:
continuity_cstr.append(V['Xd', k + 1, 0] - xf_k)
# Create Outputs
# NOTE: !!! In principle the range should be range(1,d+1) due to the algebraic variable. But only one output: F_tether is dependent on the alg. var.
# For plotting F_tether the point on :,0 is wrong and should not be printed. All outputs dependent on algebraic variables are discontinous. !!!
for j in range(1, d + 1):
# outk = out_fun(V['Xd',k,j],V['XA',k,j-1], V['U',k], P['p',k,j]) #NOTE: NEEDED FOR MAIN.PY MODEL VALIDATION # TODO: adjust model validation code ...
outk = out_fun(V['Xd', k, j], V['XA', k, j - 1], P['p', k, j],
P['puni'], V['l'])
for name in out.keys():
Output_list[name].append(out(outk)[name])
Output = ca.struct_MX([
ca.entry(name, expr=Output_list[name])
for name in Output_list.keys()
])
# Final time
xtf = 0
for r in range(d + 1):
xtf += D[r] * V['Xd', -1, r]
return V, P, coll_cstr, continuity_cstr, Output
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"]
rho = pars["veh_params"]["rho_air"]
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 * 0.5 * veh["clA_front"] * rho * v**2
f_zlift_fr = 0.5 * 0.5 * veh["clA_front"] * rho * v**2
f_zlift_rl = 0.5 * 0.5 * veh["clA_rear"] * rho * v**2
f_zlift_rr = 0.5 * 0.5 * veh["clA_rear"] * rho * 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["optim_opts"]["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]
# Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public
# License along with CasADi; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
#
#
import casadi as ca
import numpy as np
import matplotlib.pyplot as plt
# Degree of interpolating polynomial
d = 3
# Get collocation points
tau_root = np.append(0, ca.collocation_points(d, 'legendre'))
# Coefficients of the collocation equation
C = np.zeros((d+1,d+1))
# Coefficients of the continuity equation
D = np.zeros(d+1)
# Coefficients of the quadrature function
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):
def forecast(self, initial_state, time_to_go, cont_int_2go):
# Degree of interpolating polynomial
d = self.args['order']
# Get collocation points
tau_root = np.append(0, ca.collocation_points(d, self.args['points']))
# Coefficients of the collocation equation
C = np.zeros((d + 1, d + 1))
# Coefficients of the continuity equation
D = np.zeros(d + 1)
# Coefficients of the quadrature function
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_root[r]
]) / (tau_root[j] - tau_root[r])
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
D[j] = p(1.0)
# Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
pder = np.polyder(p)
for r in range(d + 1):
C[j, r] = pder(tau_root[r])
# Evaluate the integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
B[j] = pint(1.0)
# Time horizon
T = time_to_go
########################################
# ----- Defining Dynamic System ----- #
########################################
# Define vectors with names of states
states = ['Cx', 'Cn', 'Cl']
nd = len(states)
xd = ca.SX.sym('xd', nd)
for i in range(nd):
globals()[states[i]] = xd[i]
# Define vectors with names of algebraic variables
algebraics = []
na = len(algebraics)
xa = ca.SX.sym('xa', na)
for i in range(na):
globals()[algebraics[i]] = xa[i]
# Define vectors with names of control variables
inputs = ['Fnin', 'I0']
nu = len(inputs)
u = ca.SX.sym("u", nu)
for i in range(nu):
globals()[inputs[i]] = u[i]
# Define model parameter names and values
modpar = [
'u_m', 'K_N', 'u_d', 'Y_nx', 'k_m', 'Kd', 'K_NL', 'Ks', 'Ki',
'Ksl', 'Kil', 'tau', 'Ka', 'L'
]
modparval = [
0.152, 30, 5.95e-3, 305, 0.35, 3.71e-3, 10, 142.8, 214.2, 320.6,
480.9, 0.120 * 1000, 0.0, 0.084
]
nmp = len(modpar)
uncertainty = ca.SX.sym('uncp', nmp)
for i in range(nmp):
globals()[modpar[i]] = ca.SX(modparval[i])
# algebraic equations
disc_int = 11
Izlist = []
def lb_law(tau, X, Ka, z, L, I0):
Iz = I0 * (ca.exp(-(tau * X + Ka) * z) + ca.exp(-(tau * X + Ka) *
(L - z)))
return Iz
Iz = ca.SX.sym("Iz", disc_int)
for i in range(disc_int):
#print(i, i*L/(disc_int-1), L )
Iz[i] = ca.SX(lb_law(tau, Cx, Ka, i * L / (disc_int - 1), L, I0))
um_trap = (Iz[0] / ((Iz[0] + Ks + (Iz[0]**2) / Ki)) + Iz[-1] /
((Iz[-1] + Ks + (Iz[-1]**2) / Ki)))
km_trap = (Iz[0] / ((Iz[0] + Ksl + (Iz[0]**2) / Kil)) + Iz[-1] /
((Iz[-1] + Ksl + (Iz[-1]**2) / Kil)))
for i in range(1, disc_int - 1):
um_trap += 2 * Iz[i] / (Iz[i] + Ks + (Iz[i]**2) / Ki)
km_trap += 2 * Iz[i] / (Iz[i] + Ksl + (Iz[i]**2) / Kil)
u_0 = u_m / 20 * um_trap
k_0 = k_m / 20 * km_trap
# variable rate equations - model construction
dev_Cx = u_0 * Cx * Cn / (Cn + K_N) - u_d * Cx
dev_Cn = -Y_nx * u_0 * Cx * Cn / (Cn + K_N) + Fnin
dev_Cl = k_0 * Cn / (Cn + K_NL) * Cx - Kd * Cl * Cx
ODEeq = ca.vertcat(dev_Cx, dev_Cn, dev_Cl)
# self.b = np.array([2.6, -0.15, 0])
# self.A = np.array([[1, 0, 0], [0, -1, 0], [-1.67/1000, 0, 1]])
# Constraint formulation
g1 = Cx - 2.6
g2 = -Cn - 150
g3 = -1.67 * Cx + Cl
g = ca.vertcat(g1, g2, g3)
# Continuous time dynamics
f = ca.Function('f', [xd, u], [ODEeq, g], ['x', 'u'], ['ODEeq', 'LT'])
# Control discretization
N = cont_int_2go # number of control intervals
h = T / N
# Generating initial guesses
#init_sobol = sobol_seq.i4_sobol_generate(nu + nd,N) # shape (steps_, 2)
#ctrl_sobol = (lb + (ub-lb)*init_sobol[:,:]).T
# Start with an empty NLP
w = []
w0 = []
lbw = []
ubw = []
J = 0
g = []
lbg = []
ubg = []
# For plotting x and u given w
x_plot = []
u_plot = []
# "Lift" initial conditions
Xk = ca.MX.sym('X0', nd) # initialising state symbolically
w.append(Xk) # appending initial state to sequence
lbw.append([float(initial_state[i]) for i in range(nd)
]) # setting lower bound of initial state
ubw.append(
[float(initial_state[i]) for i in range(nd)]
) # setting upper bound of initial state (note that upper and lower bound are the same enforcing the constraint)
w0.append([float(initial_state[i])
for i in range(nd)]) # setting initial guess for state
x_plot.append(Xk) # appending symbolic variable to the plotting list
#print(f'forecasting for {N} discrete timesteps')
# Formulate the NLP
for k in range(N):
# New NLP variable for the control
Uk = ca.MX.sym('U_' + str(k), nu) # defining symbolic variable u_k
w.append(Uk) # appending to sequence to be optimised
lbw.append([0.1, 100]) # setting lower bounds for control
ubw.append([100., 1000]) # setting upper bounds for the control
w0.append([30, 1000]) # providing an initial guess for the control
u_plot.append(Uk) # appending symbolic variable for plotting
# add operational constraint
_, qk = f(Xk, Uk) # enforcing operational constraint
g.append(qk)
lbg.append([-np.inf, -np.inf, -np.inf]) # enforcing lower bounds
ubg.append([0, 0, 0]) # enforcing upper bounds
# State at collocation points
Xc = [] # initialising list
for j in range(d):
Xkj = ca.MX.sym(
'X_' + str(k) + '_' + str(j), nd
) # defining a symbolic variable for the collocation point
Xc.append(
Xkj
) # appending state collocation variable to list for path constraint (next code block)
w.append(
Xkj
) # appending state collocation variable to sequence to be optimised
lbw.append([
0., 0., 0.
]) # setting lower bound on the state collocation variable
ubw.append([
100, 1e5, 100
]) # setting upper bound on the state collocation variable
w0.append(
[1.2, 800,
2]) # initialising a guess for the collocation variable
#g.append()
# Loop over collocation points
Xk_end = D[
0] * Xk # declaring final state of element for continuity
for j in range(1, d + 1):
# Expression for 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] # representing time derivative of state via polynomial basis i.e. collocation equations
# Append collocation equations
fj, qj = f(
Xc[j - 1], Uk
) # formulating expression of true time derivative with state and control as input and derivative and objective as return
g.append(h * fj -
xp) # formulating constraint on collocation equations
lbg.append([0, 0, 0]) # enforcing constraint via LB=UB
ubg.append([0, 0, 0]) # enforcing constraint via LB=UB
# append operational constraints
g.append(qj) # appending constraint from function
lbg.append([-np.inf, -np.inf,
-np.inf]) # enforcing lower bounds
ubg.append([0, 0, 0]) # enforcing upper bounds
# Add contribution to the end state
Xk_end = Xk_end + D[j] * Xc[j - 1]
# calculating state for end of finite element, for subsequent continuity constraint (Lagrange)
# Add contribution to quadrature function (not required for my objective function)
#J = J + B[j]*qj*h # calculating state for end of finite element, for subsequent continuity constraint (RK) # forecasting contribution of state trajectory to objective function across a finite element using RK
# New NLP variable for state at end of interval
Xk = ca.MX.sym('X_' + str(k + 1),
nd) # defining new symbolic state
w.append(Xk) # appending state to optimisation sequence
lbw.append([0., 0., 0.]) # appending lower bounds
ubw.append([100, 1e5, 100]) # appending upper bounds
w0.append([1.2, 800, 2]) # appending initialisation of variable
x_plot.append(Xk) # appending state to plot sequence
# Add equality constraint
g.append(Xk_end - Xk) # enforcing path continuity constraint
lbg.append([0, 0, 0]) # enforcing constraint via LB=UB
ubg.append([0, 0, 0]) # enforcing constraint via LB=UB
if k == N - 1:
# add operational constraint
_, qk = f(Xk, Uk) # enforcing operational constraint
g.append(qk)
lbg.append([-np.inf, -np.inf,
-np.inf]) # enforcing lower bounds
ubg.append([0, 0, 0]) # enforcing upper bounds
# Objective function (written as in the RL context, hence we minimise J in the problem)
# Objective function (written as in the RL context, hence fprintwe minimise J in the problem)
if k == N - 1:
J = J + 4 * Xk[-1] - 1e-3 * Xk[1] - (
(Uk[0] - self.Uk_prev[0]) * 4 / 10)**2 - (
(Uk[1] - self.Uk_prev[1]) * 9 / 1000)**2
else:
J = J - ((Uk[0] - self.Uk_prev[0]) * 4 / 10)**2 - (
(Uk[1] - self.Uk_prev[1]) * 9 / 1000)**2
Uk_prev = Uk
# Concatenate vectors
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)
# Create an NLP solver
prob = {'f': -J, 'x': w, 'g': g}
solver = ca.nlpsol('solver', 'ipopt', prob,
{'ipopt': {
'max_iter': 5e3
}})
#solver.print_options()
# Function to get x and u trajectories from w
trajectories = ca.Function('trajectories', [w], [x_plot, u_plot],
['w'], ['x', 'u'])
# Solve the NLP
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() # to numpy array
# Plot the result
"""
tgrid = np.linspace(0, T, N+1)
plt.figure(1)
ax = plt.subplot(3,2,1)
plt.plot(tgrid, x_opt[0], '--')
plt.ylabel(r'Biomass Conc ($\mathregular{g L^{-1}}$)')
plt.xlabel('time (hours)')
ax = plt.subplot(3,2,3)
plt.plot(tgrid, x_opt[1], '--')
plt.ylabel(r'Nitrate Conc ($\mathregular{mg L^{-1}}$)')
plt.xlabel('time (hours)')
ax = plt.subplot(3,2,5)
plt.plot(tgrid, x_opt[2], '--')
plt.ylabel(r'Lutein Conc ($\mathregular{mg L^{-1}}$)')
plt.xlabel('time (hours)')
ax = plt.subplot(3,2,2)
plt.step(tgrid, np.append(np.nan, u_opt[0]), '-.')
plt.ylabel('Nitrate Inflow ($\mathregular{mg h^{-1}}$)')
plt.xlabel('time (hours)')
ax = plt.subplot(3,2,4)
plt.step(tgrid, np.append(np.nan, u_opt[1]), '-.')
plt.ylabel('Incident Light Intensity ($\mathregular{\mu E}$)')
plt.xlabel('time (hours)')
plt.grid()
plt.show()
"""
self.Uk_prev = u_opt[:, 0]
return self.Uk_prev
import size_based_ecosystem as sbe
import casadi as ca
import numpy as np
import matplotlib.pyplot as plt
# Degree of interpolating polynomial
d = 3
# Get collocation points
tau_root = np.append(0, ca.collocation_points(d, 'legendre'))
# Coefficients of the collocation equation
C = np.zeros((d + 1, d + 1))
# Coefficients of the continuity equation
D = np.zeros(d + 1)
# Coefficients of the quadrature function
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_root[r]]) / (tau_root[j] - tau_root[r])
# Evaluate the polynomial at the final time to get the coefficients of the continuity equation
D[j] = p(1.0)
lbg['h', 'max_power'] = -params['Pnom'] * 1e6 # [W]
ubg['h', 'max_power'] = ca.inf
# --------------------------------
# OBJECTOVE FUNCTION
# ------------------------------
Cost = Kite.CostFunction(out, V, P, params)
print '##################################'
print '### Initializing... ###'
print '##################################'
# --------------
# INITIALIZE STATES
# -------------
vars_init = V()
tau_roots = ca.collocation_points(d, 'radau')
tau_roots = ca.veccat(0, tau_roots)
for k in range(nk):
for j in range(d + 1):
t = (k + tau_roots[j]) * tf_init / float(nk)
guess = Kite.initial_guess(t, tf_init, params)
vars_init['Xd', k, j, 'q'] = guess['q']
vars_init['Xd', k, j, 'dq'] = guess['dq']
vars_init['Xd', k, j, 'w'] = guess['w']
vars_init['Xd', k, j, 'R'] = guess['R']
# --------------
# BOUNDS
# -------------
