def test_directCollocationSimple(self): """Test direct collocation on a very simple model """ # Double integrator model x = ce.struct_symMX([ce.entry('q'), ce.entry('v')]) u = cs.MX.sym('u') ode = ce.struct_MX(x) ode['q'] = x['v'] ode['v'] = u quad = x['v']**2 NT = 2 # number of control intervals N = 3 # number of collocation points per interval ts = 1 # time step # DAE model dae = dae_model.SemiExplicitDae(x=x.cat, ode=ode.cat, u=u, quad=quad) # Create direct collocation scheme scheme = cl.CollocationScheme(dae=dae, t=np.arange(NT + 1) * ts, order=N) # Optimization variable w = scheme.combine(['x', 'K', 'Z', 'u']) # Objective f = scheme.q[:, -1] # Constraints g = ce.struct_MX([ ce.entry('eq', expr=scheme.eq), ce.entry('initial', expr=scheme.x[:, 0]), # q0 = 0, v0 = 0 ce.entry('final', expr=scheme.x[:, -1] - np.array([1, 0])) # qf = 1, vf = 0 ]) # Make NLP nlp = {'x': w, 'g': g, 'f': f} # Init NLP solver opts = {'ipopt.linear_solver': 'ma86'} solver = cs.nlpsol('solver', 'ipopt', nlp, opts) # Run NLP solver sol = solver(lbg=0, ubg=0) if np.isnan(float(sol['f'])): raise RuntimeError('Nlp Solver failed') sol_w = w(sol['x']) # Check agains the known solution nptest.assert_allclose(sol_w['u'], [[1, -1]]) nptest.assert_allclose(sol['f'], 2. / 3.)
def __init__(self, dae, t, poly_order=5, tdp_fun=None): '''Constructor ''' pdq = Pdq(t, poly_order) N = len(pdq.collocationPoints) scheme = CollocationScheme(dae, pdq, tdp_fun=tdp_fun) x0 = cs.MX.sym('x0', dae.nx) X = scheme.x Z = scheme.z z0 = dae.z # Solve the collocation equations w.r.t. (X,Z) var = scheme.combine(['x', 'z']) eq = cs.Function('eq', [var, x0, scheme.u, scheme.p], [cs.vertcat(scheme.eq, scheme.x[:, 0] - x0)]) rf = cs.rootfinder('rf', 'newton', eq) # Initial point for the rootfinder w0 = ce.struct_MX(var) w0['x'] = cs.repmat(x0, 1, N) w0['z'] = cs.repmat(z0, 1, N - 1) sol = var(rf(w0, x0, scheme.u, scheme.p)) sol_X = sol['x'] sol_Z = sol['z'] [sol_Q] = cs.substitute([scheme.q], [X, Z], [sol_X, sol_Z]) self._simulate = cs.Function('CollocationSimulator', [x0, z0, scheme.u, scheme.p], [sol_X[:, -1], sol_Z[:, -1], sol_Q[:, -1], sol_X, sol_Z, sol_Q], ['x0', 'z0', 'u', 'p'], ['xf', 'zf', 'qf', 'X', 'Z', 'Q']) self._pdq = pdq self._dae = dae
def combine(self, what): """Return a struct_MX combining the specified parts of the collocation scheme. @param what is a list of strings with possible values 'x0', 'x', 'z', 'u', 'p', 'eq', 'q'. """ what_set = ['K', 'x', 'Z', 'U', 'u', 'p', 'eq', 'q'] assert all([w in what_set for w in what]) return ce.struct_MX([ce.entry(w, expr=getattr(self, w)) for w in what])
def parallel(models): '''Connect multiple DAE models in parallel ''' d = {} for attr in ['x', 'z', 'u', 'p', 'ode', 'alg', 'quad']: # TODO: what do we do with t? d[attr] = ce.struct_MX([ ce.entry('model_{0}'.format(i), expr=getattr(m, attr)) for i, m in enumerate(models) ]) return Dae(**d)
def __init__(self, name, dae, t, order, method='legendre', tdp_fun=None): """Make an integrator based on collocation method """ N = order scheme = CollocationScheme(dae, t=t, order=order, method=method, tdp_fun=tdp_fun) x0 = cs.MX.sym('x0', dae.nx) z0 = dae.z # Solve the collocation equations w.r.t. (x,K,Z) var = scheme.combine(['x', 'K', 'Z']) eq = cs.Function('eq', [var, x0, scheme.u, scheme.p], [cs.vertcat(scheme.eq, scheme.x[:, 0] - x0)]) rf = cs.rootfinder('rf', 'newton', eq) # Initial point for the rootfinder w0 = ce.struct_MX(var) w0['x'] = cs.repmat(x0, 1, scheme.x.shape[1]) w0['K'] = cs.MX.zeros(scheme.K.shape) w0['Z'] = cs.repmat(z0, 1, scheme.Z.shape[1]) sol = var(rf(w0, x0, scheme.u, dae.p)) sol_x = sol['x'] sol_K = sol['K'] sol_Z = sol['Z'] [sol_q, sol_Q, sol_X] = cs.substitute([scheme.q, scheme.Q, scheme.X], [scheme.x, scheme.K, scheme.Z], [sol_x, sol_K, sol_Z]) # TODO: return correct value for zf! # TODO: return only x instead of x and xf? super().__init__(name, [x0, z0, scheme.u, dae.p], [sol_x[:, 1 :], np.repeat(np.nan, dae.nz), sol_q[:, -1], sol_X, sol_Z, sol_Q, sol_x, sol_K, scheme.tc], ['x0', 'z0', 'u', 'p'], ['xf', 'zf', 'qf', 'X', 'Z', 'Q', 'x', 'K', 'tc']) self._scheme = scheme
def test_directCollocationReach(self): """Test direct collocation on a toy problem The problem: bring the double integrator from state [0, 0] to state [1, 0] while minimizing L2 norm of the control input. """ # Double integrator model x = ce.struct_symMX([ce.entry('q'), ce.entry('v')]) u = cs.MX.sym('u') ode = ce.struct_MX(x) ode['q'] = x['v'] ode['v'] = u quad = u**2 NT = 5 # number of control intervals N = 3 # number of collocation points per interval ts = 1 # time step # DAE model dae = dae_model.SemiExplicitDae(x=x.cat, ode=ode.cat, u=u, quad=quad) # Create direct collocation scheme scheme = cl.CollocationScheme(dae=dae, t=np.arange(NT + 1) * ts, order=N) # Optimization variable w = scheme.combine(['x', 'K', 'u']) # Objective f = scheme.q[:, -1] # Constraints g = ce.struct_MX([ ce.entry('eq', expr=scheme.eq), ce.entry('initial', expr=scheme.x[:, 0]), # q0 = 0, v0 = 0 ce.entry('final', expr=scheme.x[:, -1] - np.array([1, 0])) # qf = 1, vf = 0 ]) # Make NLP nlp = {'x': w, 'g': g, 'f': f} # Init NLP solver opts = {'ipopt.linear_solver': 'ma86'} solver = cs.nlpsol('solver', 'ipopt', nlp, opts) # Run NLP solver sol = solver(lbg=0, ubg=0) if np.isnan(float(sol['f'])): raise RuntimeError('Nlp Solver failed') sol_w = w(sol['x']) # Check against the known solution nptest.assert_allclose(sol_w['u'], [[0.2, 0.1, 0, -0.1, -0.2]], atol=1e-16) plt.plot(scheme.t, sol_w['x'].T, 'o') plt.plot(scheme.tc, scheme.evalX(sol_w['x'], sol_w['K']).T, 'x') fi = scheme.piecewisePolyX(sol_w['x'], sol_w['K']) t, val = fi.discretize(ts / 10.) plt.plot(t, val.T) plt.grid(True) plt.show()
def __init__(self, dae, t, order, method='legendre', parallelization='serial', tdp_fun=None, expand=True, repeat_param=False, options={}): """Constructor @param t time vector of length N+1 defining N collocation intervals @param order number of collocation points per interval @param method collocation method ('legendre', 'radau') @param dae DAE model @param parallelization parallelization of the outer map. Possible set of values is the same as for casadi.Function.map(). @return Returns a dictionary with the following keys: 'X' -- state at collocation points 'Z' -- alg. state at collocation points 'x0' -- initial state 'eq' -- the expression eq == 0 defines the collocation equation. eq depends on X, Z, x0, p. 'Q' -- quadrature values at collocation points depending on x0, X, Z, p. """ # Convert whatever DAE to implicit DAE dae = dae.makeImplicit() M = order N = len(t) - 1 # # Define variables and functions corresponfing to all control intervals # K = cs.MX.sym('K', dae.nx, N * M) # State derivatives at collocation points Z = cs.MX.sym('Z', dae.nz, N * M) # Alg state at collocation points x = cs.MX.sym('x', dae.nx, N + 1) # State at the ends of collocation intervals (t) u = cs.MX.sym('u', dae.nu, N) # Input on collocation intervals U = cs.horzcat(*[cs.repmat(u[:, n], 1, M) for n in range(N)]) # Input at collocation points # Butcher tableau for the selected method butcher = butcherTableuForCollocationMethod(order, method) # Interval lengths h = np.diff(t) # Integrated state at collocation points Mx = cs.kron(cs.DM.eye(N), cs.DM.ones(1, M)) MK = cs.kron(cs.diagcat(*h), butcher.A.T) # integration matrix X = cs.mtimes(x[:, : -1], Mx) + cs.mtimes(K, MK) # Integrated state at the ends of collocation intervals xf = x[:, : -1] + cs.mtimes(K, cs.kron(cs.diagcat(*h), butcher.b)) # Points in time at which the collocation equations are calculated # TODO: this possibly can be sped up a little bit. tc = np.hstack([t[n] + h[n] * butcher.c for n in range(N)]) # Values of the time-dependent parameter if tdp_fun is not None: tdp_val = cs.horzcat(*[tdp_fun(t) for t in tc]) else: assert dae.ntdp == 0 tdp_val = np.zeros((0, tc.size)) # DAE function dae_fun = dae.createFunction('dae', ['xdot', 'x', 'z', 'u', 'p', 't', 'tdp'], ['dae', 'quad']) if expand: dae_fun = dae_fun.expand() # expand() for speed if repeat_param: reduce_in = [] p = cs.MX.sym('P', dae.np, N * M) else: reduce_in = [4] p = cs.MX.sym('P', dae.np) dae_map = dae_fun.map('dae_map', parallelization, N * M, reduce_in, [], options) dae_out = dae_map(xdot=K, x=X, z=Z, u=U, p=p, t=tc, tdp=tdp_val) eqc = ce.struct_MX([ ce.entry('collocation', expr=dae_out['dae']), ce.entry('continuity', expr=xf - x[:, 1 :]), ce.entry('param', expr=cs.diff(p, 1, 1)) ]) # Integrate the quadrature state quad = dae_out['quad'] #t0 = time.time() q = [cs.MX.zeros(dae.nq)] # Integrated quadrature at interval ends # TODO: speed up the calculation of q. for n in range(N): q.append(q[-1] + h[n] * cs.mtimes(quad[:, n * M : (n + 1) * M], butcher.b)) q = cs.horzcat(*q) Q = cs.mtimes(q[:, : -1], Mx) + cs.mtimes(quad, MK) # Integrated quadrature at collocation points #print('Creating Q took {0:.3f} s.'.format(time.time() - t0)) self._N = N self._M = M self._eq = eqc self._x = x self._X = X self._K = K self._Z = Z self._U = U self._u = u self._quad = quad self._Q = Q self._q = q self._p = p self._tc = tc self._butcher = butcher self._tdp = tdp_val self._t = t