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 # -------------