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