def _initialize_polynomial_coefs(self): """ Setup radau polynomials and initialize the weight factor matricies """ self.col_vars['tau_root'] = cs.collocationPoints(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.SXFunction( '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'] = lfcn([1.0])[0].toArray().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] = tfcn([self.col_vars['tau_root'][r]] )[0].toArray().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): tau_f_integrator = cs.SXFunction('ode', cs.daeIn(t=tau, x=xtau), cs.daeOut(ode=L[j])) tau_integrator = cs.Integrator( "integrator", "cvodes", tau_f_integrator, {'t0':0., 'tf':1}) Phi[j] = np.asarray(tau_integrator({'x0' : 0})['xf'])[0][0] self.col_vars['Phi'] = np.array(Phi) self.col_vars['alpha'] = cs.SX.sym('alpha')
def _initialize_polynomial_coefs(self): """ Setup radau polynomials and initialize the weight factor matricies """ self.col_vars['tau_root'] = cs.collocationPoints(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.SXFunction( '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'] = lfcn([1.0])[0].toArray().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] = tfcn([self.col_vars['tau_root'][r]] )[0].toArray().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): tau_f_integrator = cs.SXFunction('ode', cs.daeIn(t=tau, x=xtau), cs.daeOut(ode=L[j])) tau_integrator = cs.Integrator( "integrator", "cvodes", tau_f_integrator, {'t0':0., 'tf':1}) Phi[j] = np.asarray(tau_integrator({'x0' : 0})['xf'])[0][0] self.col_vars['Phi'] = np.array(Phi)
def _initialize_tgrid(self): # Choose collocation points tau_root = cs.collocationPoints(self.opts["degree"], self.opts["polynomial"]) # Degree of interpolating polynomial d = self.opts["degree"] # Size of the finite elements self.opts["h"] = self.opts["tf"] / self.opts["nk"] # Coefficients of the collocation equation self.opts["C"] = np.zeros((d + 1, d + 1)) # Dimensionless time inside one control interval tau = cs.SX.sym("tau") T = np.zeros((self.opts["nk"], d + 1)) for k in range(self.opts["nk"]): for j in range(d + 1): T[k, j] = self.opts["h"] * (k + tau_root[j]) self.opts["T"] = T # Construct Lagrange polynomials to get the polynomial basis at the # collocation point L = [[]] * (d + 1) # For all collocation points for j in range(d + 1): L[j] = 1 for r in range(d + 1): if r != j: L[j] *= (tau - tau_root[r]) / (tau_root[j] - tau_root[r]) self._lfcn = cs.SXFunction("lfcn", [tau], [cs.vertcat(L)]) # Evaluate the polynomial at the final time to get the coefficients of # the continuity equation self.opts["D"] = np.asarray(self._lfcn([1.0])[0]).squeeze() # Evaluate the time derivative of the polynomial at all collocation # points to get the coefficients of the continuity equation tfcn = self._lfcn.tangent() for r in range(d + 1): self.opts["C"][:, r] = tfcn([tau_root[r]])[0].toArray().squeeze() self._tgrid = np.array( [ point + self.opts["h"] * np.array(tau_root) for point in np.linspace(0, self.opts["tf"], self.opts["nk"], endpoint=False) ] ).flatten()
def _initialize_tgrid(self): # Choose collocation points tau_root = cs.collocationPoints(self.opts['degree'], self.opts['polynomial']) # Degree of interpolating polynomial d = self.opts['degree'] # Size of the finite elements self.opts['h'] = self.opts['tf'] / self.opts['nk'] # Coefficients of the collocation equation self.opts['C'] = np.zeros((d + 1, d + 1)) # Dimensionless time inside one control interval tau = cs.SX.sym("tau") T = np.zeros((self.opts['nk'], d + 1)) for k in range(self.opts['nk']): for j in range(d + 1): T[k, j] = self.opts['h'] * (k + tau_root[j]) self.opts['T'] = T # Construct Lagrange polynomials to get the polynomial basis at the # collocation point L = [[]] * (d + 1) # For all collocation points for j in range(d + 1): L[j] = 1 for r in range(d + 1): if r != j: L[j] *= (tau - tau_root[r]) / (tau_root[j] - tau_root[r]) self._lfcn = cs.SXFunction('lfcn', [tau], [cs.vertcat(L)]) # Evaluate the polynomial at the final time to get the coefficients of # the continuity equation self.opts['D'] = np.asarray(self._lfcn([1.0])[0]).squeeze() # Evaluate the time derivative of the polynomial at all collocation # points to get the coefficients of the continuity equation tfcn = self._lfcn.tangent() for r in range(d + 1): self.opts['C'][:, r] = tfcn([tau_root[r]])[0].toArray().squeeze() self._tgrid = np.array([ point + self.opts['h'] * np.array(tau_root) for point in np.linspace(0, self.opts['tf'], self.opts['nk'], endpoint=False) ]).flatten()
def collocation_points(order, scheme): return ca.collocationPoints(order, scheme)
def __init__(self, system = None, \ tu = None, uN = None, \ ty = None, yN = None, pinit = None, \ xinit = None, \ scheme = "radau", \ order = 3): self.tstart_setup = time.time() SetupsBaseClass.__init__(self) if not type(system) is systems.ExplODE: raise TypeError("Setup-method " + self.__class__.__name__ + \ " not allowed for system of type " + str(type(system)) + ".") self.system = system # Dimensions self.nx = system.x.shape[0] self.nu = system.u.shape[0] self.np = system.p.shape[0] self.neps_e = system.eps_e.shape[0] self.neps_u = system.eps_u.shape[0] self.nphi = system.phi.shape[0] if np.atleast_2d(tu).shape[0] == 1: self.tu = np.asarray(tu) elif np.atleast_2d(tu).shape[1] == 1: self.tu = np.squeeze(np.atleast_2d(tu).T) else: raise ValueError("Invalid dimension for argument tu.") if ty == None: self.ty = self.tu elif np.atleast_2d(ty).shape[0] == 1: self.ty = np.asarray(ty) elif np.atleast_2d(ty).shape[1] == 1: self.ty = np.squeeze(np.atleast_2d(ty).T) else: raise ValueError("Invalid dimension for argument ty.") self.nsteps = self.tu.shape[0] - 1 self.scheme = scheme self.order = order self.tauroot = ca.collocationPoints(order, scheme) # Degree of interpolating polynomial self.ntauroot = len(self.tauroot) - 1 # Define the optimization variables self.P = ca.MX.sym("P", self.np) self.X = ca.MX.sym("X", (self.nx * (self.ntauroot+1)), self.nsteps) self.XF = ca.MX.sym("XF", self.nx) self.V = ca.MX.sym("V", self.nphi, self.nsteps+1) if self.neps_e != 0: self.EPS_E = ca.MX.sym("EPS_E", \ (self.neps_e * self.ntauroot), self.nsteps) else: self.EPS_E = ca.DMatrix(0, self.nsteps) if self.neps_u != 0: self.EPS_U = ca.MX.sym("EPS_U", \ (self.neps_u * self.ntauroot), self.nsteps) else: self.EPS_U = ca.DMatrix(0, self.nsteps) # Define bounds and initial values self.check_and_set_initials( \ uN = uN, \ pinit = pinit, \ xinit = xinit) # Set tp the collocation coefficients # Coefficients of the collocation equation self.C = np.zeros((self.ntauroot + 1, self.ntauroot + 1)) # Coefficients of the continuity equation self.D = np.zeros(self.ntauroot + 1) # Dimensionless time inside one control interval tau = ca.SX.sym("tau") # Construct the matrix T that contains all collocation time points self.T = np.zeros((self.nsteps, self.ntauroot + 1)) for k in range(self.nsteps): for j in range(self.ntauroot + 1): self.T[k,j] = self.tu[k] + \ (self.tu[k+1] - self.tu[k]) * self.tauroot[j] self.T = self.T.T # For all collocation points self.lfcns = [] for j in range(self.ntauroot + 1): # Construct Lagrange polynomials to get the polynomial basis # at the collocation point L = 1 for r in range(self.ntauroot + 1): if r != j: L *= (tau - self.tauroot[r]) / \ (self.tauroot[j] - self.tauroot[r]) lfcn = ca.SXFunction("lfcn", [tau],[L]) # Evaluate the polynomial at the final time to get the # coefficients of the continuity equation [self.D[j]] = lfcn([1]) # Evaluate the time derivative of the polynomial at all # collocation points to get the coefficients of the # collocation equation tfcn = lfcn.tangent() for r in range(self.ntauroot + 1): self.C[j,r] = tfcn([self.tauroot[r]])[0] self.lfcns.append(lfcn) # Initialize phiN self.phiN = [] # Initialize measurement function phifcn = ca.MXFunction("phifcn", \ [system.t, system.u, system.x, system.eps_u, system.p], \ [system.phi]) # Initialzie setup of g self.g = [] # Initialize ODE right-hand-side ffcn = ca.MXFunction("ffcn", \ [system.t, system.u, system.x, system.eps_e, system.eps_u, \ system.p], [system.f]) # Collect information for measurement function # Structs to hold variables for later mapped evaluation Tphi = [] Uphi = [] Xphi = [] EPS_Uphi = [] for k in range(self.nsteps): hk = self.tu[k + 1] - self.tu[k] t_meas = self.ty[np.where(np.logical_and( \ self.ty >= self.tu[k], self.ty < self.tu[k + 1]))] for t_meas_j in t_meas: Uphi.append(self.uN[:, k]) EPS_Uphi.append(self.EPS_U[:self.neps_u, k]) if t_meas_j == self.tu[k]: Tphi.append(self.tu[k]) Xphi.append(self.X[:self.nx, k]) else: tau = (t_meas_j - self.tu[k]) / hk x_temp = 0 for r in range(self.ntauroot + 1): x_temp += self.lfcns[r]([tau])[0] * \ self.X[r*self.nx : (r+1) * self.nx, k] Tphi.append(t_meas_j) Xphi.append(x_temp) if self.tu[-1] in self.ty: Tphi.append(self.tu[-1]) Uphi.append(self.uN[:,-1]) Xphi.append(self.XF) EPS_Uphi.append(self.EPS_U[:self.neps_u,-1]) # Mapped calculation of the collocation equations # Collocation nodes hc = ca.MX.sym("hc", 1) tc = ca.MX.sym("tc", self.ntauroot) xc = ca.MX.sym("xc", self.nx * (self.ntauroot+1)) eps_ec = ca.MX.sym("eps_ec", self.neps_e * self.ntauroot) eps_uc = ca.MX.sym("eps_uc", self.neps_u * self.ntauroot) coleqn = ca.vertcat([ \ hc * ffcn([tc[j-1], \ system.u, \ xc[j*self.nx : (j+1)*self.nx], \ eps_ec[(j-1)*self.neps_e : j*self.neps_e], \ eps_uc[(j-1)*self.neps_u : j*self.neps_u], \ system.p])[0] - \ sum([self.C[r,j] * xc[r*self.nx : (r+1)*self.nx] \ for r in range(self.ntauroot + 1)]) \ for j in range(1, self.ntauroot + 1)]) coleqnfcn = ca.MXFunction("coleqnfcn", \ [hc, tc, system.u, xc, eps_ec, eps_uc, system.p], [coleqn]) coleqnfcn = coleqnfcn.expand() [gcol] = coleqnfcn.map([ \ np.atleast_2d((self.tu[1:] - self.tu[:-1])), self.T[1:,:], \ self.uN, self.X, self.EPS_E, self.EPS_U, self.P]) # Continuity nodes xnext = ca.MX.sym("xnext", self.nx) conteqn = xnext - sum([self.D[r] * xc[r*self.nx : (r+1)*self.nx] \ for r in range(self.ntauroot + 1)]) conteqnfcn = ca.MXFunction("conteqnfcn", [xnext, xc], [conteqn]) conteqnfcn = conteqnfcn.expand() [gcont] = conteqnfcn.map([ \ ca.horzcat([self.X[:self.nx, 1:], self.XF]), self.X]) # Stack equality constraints together self.g = ca.veccat([gcol, gcont]) # Evaluation of the measurement function [self.phiN] = phifcn.map( \ [ca.horzcat(k) for k in Tphi, Uphi, Xphi, EPS_Uphi] + \ [self.P]) # self.phiNfcn = ca.MXFunction("phiNfcn", [self.Vars], [self.phiN]) self.tend_setup = time.time() self.duration_setup = self.tend_setup - self.tstart_setup print('Initialization of ExplODE system sucessful.')