def preprocess(self):
self.VarList = tuple(self.Vars)
self.NumVars = len(self.VarList)
self.VarVector = BlockMatrix((tuple(self.Vars[var] for var in self.VarList),))
self.NumDims = self.VarVector.shape[1]
self.Mean = BlockMatrix((tuple(self.Param[('Mean', var)] for var in self.VarList),))
self.DemeanedVarVector = self.VarVector - self.Mean
cov = [self.NumVars * [None] for _ in range(self.NumVars)] # careful not to create same mutable object
for i in range(self.NumVars):
for j in range(i):
if ('Cov', self.VarList[i], self.VarList[j]) in self.Param:
cov[i][j] = self.Param[('Cov', self.VarList[i], self.VarList[j])]
cov[j][i] = cov[i][j].T
else:
cov[j][i] = self.Param[('Cov', self.VarList[j], self.VarList[i])]
cov[i][j] = cov[j][i].T
cov[i][i] = self.Param[('Cov', self.VarList[i])]
self.Cov = BlockMatrix(cov)
try:
cov = CompyledFunc(var_names_and_syms={}, dict_or_expr=self.Cov)()
sign, self.LogDetCov = slogdet(cov)
self.LogDetCov *= sign
self.InvCov = inv(cov)
except:
pass
self.PreProcessed = True
def test_squareBlockMatrix():
n,m,l,k = symbols('n m l k', integer=True)
A = MatrixSymbol('A', n, n)
B = MatrixSymbol('B', n, m)
C = MatrixSymbol('C', m, n)
D = MatrixSymbol('D', m, m)
X = BlockMatrix([[A,B],[C,D]])
Y = BlockMatrix([[A]])
assert X.is_square
assert block_collapse(X+Identity(m+n)) == BlockMatrix(
[[A+Identity(n), B], [C, D+Identity(m)]])
Q = X+Identity(m+n)
assert block_collapse(Inverse(Q)) == Inverse(block_collapse(Q))
assert (X + MatrixSymbol('Q', n+m, n+m)).is_Add
assert (X * MatrixSymbol('Q', n+m, n+m)).is_Mul
assert Y.I[0,0] == A.I
assert Inverse(X, expand=True) == BlockMatrix([
[(-B*D.I*C + A).I, -A.I*B*(D+-C*A.I*B).I],
[-(D-C*A.I*B).I*C*A.I, (D-C*A.I*B).I]])
assert Inverse(X, expand=False).is_Inverse
assert X.inverse().is_Inverse
assert not X.is_Identity
Z = BlockMatrix([[Identity(n),B],[C,D]])
assert not Z.is_Identity
def test_BlockMatrix():
n,m,l,k,p = symbols('n m l k p', integer=True)
A = MatrixSymbol('A', n, m)
B = MatrixSymbol('B', n, k)
C = MatrixSymbol('C', l, m)
D = MatrixSymbol('D', l, k)
M = MatrixSymbol('M', m+k, p)
N = MatrixSymbol('N', l+n, k+m)
X = BlockMatrix(Matrix([[A,B],[C,D]]))
# block_collapse does nothing on normal inputs
E = MatrixSymbol('E', n, m)
assert block_collapse(A+2*E) == A+2*E
F = MatrixSymbol('F', m, m)
assert block_collapse(E.T*A*F) == E.T*A*F
assert X.shape == (l+n, k+m)
assert (block_collapse(Transpose(X)) ==
BlockMatrix(Matrix([[A.T, C.T], [B.T, D.T]])))
assert Transpose(X).shape == X.shape[::-1]
assert X.blockshape == (2,2)
# Test that BlockMatrices and MatrixSymbols can still mix
assert (X*M).is_Mul
assert X._blockmul(M).is_Mul
assert (X*M).shape == (n+l, p)
assert (X+N).is_Add
assert X._blockadd(N).is_Add
assert (X+N).shape == X.shape
E = MatrixSymbol('E', m, 1)
F = MatrixSymbol('F', k, 1)
Y = BlockMatrix(Matrix([[E], [F]]))
assert (X*Y).shape == (l+n, 1)
assert block_collapse(X*Y)[0,0] == A*E + B*F
assert block_collapse(X*Y)[1,0] == C*E + D*F
assert (block_collapse(Transpose(block_collapse(Transpose(X*Y)))) ==
block_collapse(X*Y))
# block_collapse passes down into container objects, transposes, and inverse
assert block_collapse((X*Y, 2*X)) == (block_collapse(X*Y), block_collapse(2*X))
assert block_collapse(Tuple(X*Y, 2*X)) == (
block_collapse(X*Y), block_collapse(2*X))
assert (block_collapse(Transpose(X*Y)) ==
block_collapse(Transpose(block_collapse(X*Y))))
Ab = BlockMatrix([[A]])
Z = MatrixSymbol('Z', *A.shape)
# Make sure that MatrixSymbols will enter 1x1 BlockMatrix if it simplifies
assert block_collapse(Ab+Z) == BlockMatrix([[A+Z]])
def exp_deriv_generators(self, params):
'''
constructs the generators for computing the first derivative of the
time ordered exponential of self, with respect to params
Note: I'll need to either add in stuff that keeps track of where the
blocks go, or just have a convention that is maintained everywhere.
Convention is easier, but it might be nice to also return some data
structure with this info
'''
D, C = self.parameter_decomposition(params)
new_mats = []
for sys_idx in range(len(self.mat_list)):
G = self.mat_list[sys_idx]
m0 = zeros(self.shape[sys_idx])
for d_idx in range(len(params)):
A = C[d_idx].mat_list[sys_idx]
new_mats.append(Matrix(BlockMatrix([[G, A], [m0, G]])))
return Sym_UTB_Matrix(new_mats)
def convert_to_dynamic_controller(self):
"""
The Euler-Lagrange formalism is transformed to the state and output equation notation of the DynamicController class.
Returns:
--------
result : tuple
The tuple contains the transformed matrices that are compatible with the function define_controller of DynamicController.
"""
dim_states = len(self.minimal_states)
D0_inv = self.inertia_matrix**(-1)
In = eye(dim_states)
Z = zeros(dim_states)
K0_D0 = -D0_inv * self.stiffness_matrix
C0_D0 = -D0_inv * self.damping_matrix
A = Matrix(BlockMatrix([[Z, In], [K0_D0, C0_D0]]))
C1_D0 = D0_inv * self.nonlinear_coefficient_matrix
Z = zeros(dim_states, len(self.nonlinear_stiffness_fcts))
B = Matrix(BlockMatrix([[Z], [C1_D0]]))
f = self.nonlinear_stiffness_fcts
Z = zeros(dim_states, len(f))
if self.nl_stiffness:
C = Matrix(BlockMatrix([[self.nonlinear_coefficient_matrix], [Z]]))
else:
C = Matrix(BlockMatrix([[Z], [self.nonlinear_coefficient_matrix]]))
NA_D0 = -D0_inv * self.gain_inputs
NB_D0 = -D0_inv * self.gain_dinputs
Z = zeros(dim_states, 1)
eta = Matrix(
BlockMatrix([[Z],
[
NA_D0 * Matrix(self.minimal_inputs) +
NB_D0 * Matrix(self.dinputs)
]]))
phi = self.gain_inputs.T * D0_inv * self.stiffness_matrix**(
-1) * self.inertia_matrix * self.stiffness_matrix * Matrix(
self.minimal_states
) - self.gain_dinputs.T * D0_inv * self.stiffness_matrix**(
-1) * self.inertia_matrix * self.stiffness_matrix * Matrix(
self.minimal_dstates)
return A, B, C, f, eta, phi
def test_squareBlockMatrix():
n, m, l, k = symbols('n m l k', integer=True)
A = MatrixSymbol('A', n, n)
B = MatrixSymbol('B', n, m)
C = MatrixSymbol('C', m, n)
D = MatrixSymbol('D', m, m)
X = BlockMatrix([[A, B], [C, D]])
Y = BlockMatrix([[A]])
assert X.is_square
assert block_collapse(X + Identity(m + n)) == BlockMatrix(
[[A + Identity(n), B], [C, D + Identity(m)]])
Q = X + Identity(m + n)
assert block_collapse(Inverse(Q)) == Inverse(block_collapse(Q))
assert (X + MatrixSymbol('Q', n + m, n + m)).is_Add
assert (X * MatrixSymbol('Q', n + m, n + m)).is_Mul
assert Y.I.blocks[0, 0] == A.I
assert Inverse(X, expand=True) == BlockMatrix([[
(-B * D.I * C + A).I, -A.I * B * (D + -C * A.I * B).I
], [-(D - C * A.I * B).I * C * A.I, (D - C * A.I * B).I]])
assert Inverse(X, expand=False).is_Inverse
assert X.inverse().is_Inverse
assert not X.is_Identity
Z = BlockMatrix([[Identity(n), B], [C, D]])
assert not Z.is_Identity
def test_BlockMatrix():
n, m, l, k, p = symbols('n m l k p', integer=True)
A = MatrixSymbol('A', n, m)
B = MatrixSymbol('B', n, k)
C = MatrixSymbol('C', l, m)
D = MatrixSymbol('D', l, k)
M = MatrixSymbol('M', m + k, p)
N = MatrixSymbol('N', l + n, k + m)
X = BlockMatrix(Matrix([[A, B], [C, D]]))
# block_collapse does nothing on normal inputs
E = MatrixSymbol('E', n, m)
assert block_collapse(A + 2 * E) == A + 2 * E
F = MatrixSymbol('F', m, m)
assert block_collapse(E.T * A * F) == E.T * A * F
assert X.shape == (l + n, k + m)
assert (block_collapse(Transpose(X)) == BlockMatrix(
Matrix([[A.T, C.T], [B.T, D.T]])))
assert Transpose(X).shape == X.shape[::-1]
assert X.blockshape == (2, 2)
# Test that BlockMatrices and MatrixSymbols can still mix
assert (X * M).is_Mul
assert X._blockmul(M).is_Mul
assert (X * M).shape == (n + l, p)
assert (X + N).is_Add
assert X._blockadd(N).is_Add
assert (X + N).shape == X.shape
E = MatrixSymbol('E', m, 1)
F = MatrixSymbol('F', k, 1)
Y = BlockMatrix(Matrix([[E], [F]]))
assert (X * Y).shape == (l + n, 1)
assert block_collapse(X * Y).blocks[0, 0] == A * E + B * F
assert block_collapse(X * Y).blocks[1, 0] == C * E + D * F
assert (block_collapse(Transpose(block_collapse(Transpose(
X * Y)))) == block_collapse(X * Y))
# block_collapse passes down into container objects, transposes, and inverse
assert block_collapse(
(X * Y, 2 * X)) == (block_collapse(X * Y), block_collapse(2 * X))
assert block_collapse(Tuple(X * Y, 2 * X)) == (block_collapse(X * Y),
block_collapse(2 * X))
assert (block_collapse(Transpose(X * Y)) == block_collapse(
Transpose(block_collapse(X * Y))))
Ab = BlockMatrix([[A]])
Z = MatrixSymbol('Z', *A.shape)
# Make sure that MatrixSymbols will enter 1x1 BlockMatrix if it simplifies
assert block_collapse(Ab + Z) == BlockMatrix([[A + Z]])
def nonlinear_stiffness_fcts(self, matrix: Matrix):
if len(matrix) == len(self.minimal_states):
Z = zeros(len(self.minimal_states), len(matrix))
if self.nl_stiffness:
C = Matrix(
BlockMatrix([[self.nonlinear_coefficient_matrix], [Z]]))
else:
C = Matrix(
BlockMatrix([[Z], [self.nonlinear_coefficient_matrix]]))
argument = Array(C.T * Matrix(self.states))
completed_f = []
for idx, fct in enumerate(matrix):
if callable(fct[0]):
completed_f.append([fct[0](argument[idx])])
elif fct[0]:
completed_f.append(0)
else:
error_text = '[EulerLagrangeController] f should be a callable function or identical 0.'
raise AssertionError(error_text)
self._nl = Matrix(completed_f)
else:
error_text = '[EulerLagrangeController.nonlinear_stiffness_fcts (setter)] The stiffness matrix should have the same row dimension as the number of states.'
raise ValueError(error_text)
def test_BlockMatrix_Trace():
A, B, C, D = map(lambda s: MatrixSymbol(s, 3, 3), 'ABCD')
X = BlockMatrix([[A, B], [C, D]])
assert Trace(X) == Trace(A) + Trace(D)
73.687667, 73.806480, 74.882408, 81.997367, 89.673179, 96.831947
]
x = Matrix(N, 1, xinit)
print x
A0 = ones(N, 1)
A1 = x
A2 = x.multiply_elementwise(x)
#print(A0)
#print(A1)
#print(A2)
A = BlockMatrix([A0, A1, A2])
A = A.as_explicit()
yErr = [0.0005205, 0.0006515, 0.0004069, 0.0047973, 0.0055708, 0.0055551, \
0.0054941, 0.0043147, 0.3925145, 0.0041837, 0.4294512, 0.0042747, \
0.4657444, 0.0038275, 0.0038912, 0.0037950, 0.2330908, 0.0038282, \
0.2650930, 0.0029776, 0.2709266, 0.0030374, 0.3159595, 0.0023857]
MatErr = [
4.753 * 10**-6, 4.060 * 10**-6, 4.060 * 10**-6, 1.872 * 10**-5,
2.800 * 10**-5, 2.800 * 10**-5, 1.061 * 10**-5, 1.061 * 10**-5,
5.467 * 10**-6, 5.467 * 10**-6, 8.882 * 10**-6, 8.882 * 10**-6,
1.315 * 10**-5, 9.930 * 10**-6, 4.358 * 10**-6, 4.358 * 10**-6,
4.691 * 10**-6, 4.691 * 10**-6, 4.880 * 10**-6, 4.880 * 10**-6,
9.760 * 10**-6, 8.704 * 10**-6, 8.388 * 10**-6, 8.370 * 10**-6
]
def gauss_cond(pdf, cond={}, **kw_cond):
cond = combine_dict_and_kwargs(cond, kw_cond)
new_cond = pdf.Cond.copy()
new_cond.update(cond)
scope = pdf.Scope.copy()
for var in cond:
del scope[var]
point_cond = {}
for var, value in list(cond.items()):
if value is not None:
point_cond[pdf.Vars[var]] = value
cond_vars = tuple(cond)
num_cond_vars = len(cond_vars)
scope_vars = tuple(set(pdf.VarsList) - set(cond))
num_scope_vars = len(scope_vars)
x_c = BlockMatrix((tuple(pdf.Vars[cond_var] for cond_var in cond_vars),))
m_c = BlockMatrix((tuple(pdf.Param[('Mean', cond_var)] for cond_var in cond_vars),))
m_s = BlockMatrix((tuple(pdf.Param[('Mean', scope_var)] for scope_var in scope_vars),))
S_c = [num_cond_vars * [None] for _ in range(num_cond_vars)] # careful not to create same mutable object
for i in range(num_cond_vars):
for j in range(i):
if ('Cov', cond_vars[i], cond_vars[j]) in pdf.Param:
S_c[i][j] = pdf.Param[('Cov', cond_vars[i], cond_vars[j])]
S_c[j][i] = S_c[i][j].T
else:
S_c[j][i] = pdf.Param[('Cov', cond_vars[j], cond_vars[i])]
S_c[i][j] = S_c[j][i].T
S_c[i][i] = pdf.Param[('Cov', cond_vars[i])]
S_c = BlockMatrix(S_c)
S_s = [num_scope_vars * [None] for _ in range(num_scope_vars)] # careful not to create same mutable object
for i in range(num_scope_vars):
for j in range(i):
if ('Cov', scope_vars[i], scope_vars[j]) in pdf.Param:
S_s[i][j] = pdf.Param[('Cov', scope_vars[i], scope_vars[j])]
S_s[j][i] = S_s[i][j].T
else:
S_s[j][i] = pdf.Param[('Cov', scope_vars[j], scope_vars[i])]
S_s[i][j] = S_s[j][i].T
S_s[i][i] = pdf.Param[('Cov', scope_vars[i])]
S_s = BlockMatrix(S_s)
S_cs = [num_scope_vars * [None] for _ in range(num_cond_vars)] # careful not to create same mutable object
for i, j in product(list(range(num_cond_vars)), list(range(num_scope_vars))):
if ('Cov', cond_vars[i], scope_vars[j]) in pdf.Param:
S_cs[i][j] = pdf.Param[('Cov', cond_vars[i], scope_vars[j])]
else:
S_cs[i][j] = pdf.Param[('Cov', scope_vars[j], cond_vars[i])].T
S_cs = BlockMatrix(S_cs)
S_sc = S_cs.T
m = (m_s + (x_c - m_c) * S_c.inverse() * S_cs).xreplace(point_cond)
S = S_s - S_sc * S_c.inverse() * S_cs
param = {}
index_ranges_from = []
index_ranges_to = []
k = 0
for i in range(num_scope_vars):
l = k + pdf.Vars[scope_vars[i]].shape[1]
index_ranges_from += [k]
index_ranges_to += [l]
param[('Mean', scope_vars[i])] = m[0, index_ranges_from[i]:index_ranges_to[i]]
for j in range(i):
param[('Cov', scope_vars[j], scope_vars[i])] =\
S[index_ranges_from[j]:index_ranges_to[j], index_ranges_from[i]:index_ranges_to[i]]
param[('Cov', scope_vars[i])] =\
S[index_ranges_from[i]:index_ranges_to[i], index_ranges_from[i]:index_ranges_to[i]]
k = l
return GaussPDF(var_names_and_syms=pdf.Vars.copy(), param=param, cond=new_cond, scope=scope)
def gauss_cond(pdf, cond={}, **kw_cond):
cond = combine_dict_and_kwargs(cond, kw_cond)
new_cond = pdf.Cond.copy()
new_cond.update(cond)
scope = pdf.Scope.copy()
for var in cond:
del scope[var]
point_cond = {}
for var, value in cond.items():
if value is not None:
point_cond[pdf.Vars[var]] = value
cond_vars = tuple(cond)
num_cond_vars = len(cond_vars)
scope_vars = tuple(set(pdf.VarsList) - set(cond))
num_scope_vars = len(scope_vars)
x_c = BlockMatrix((tuple(pdf.Vars[cond_var] for cond_var in cond_vars),))
m_c = BlockMatrix((tuple(pdf.Param[('Mean', cond_var)] for cond_var in cond_vars),))
m_s = BlockMatrix((tuple(pdf.Param[('Mean', scope_var)] for scope_var in scope_vars),))
S_c = [num_cond_vars * [None] for _ in range(num_cond_vars)] # careful not to create same mutable object
for i in range(num_cond_vars):
for j in range(i):
if ('Cov', cond_vars[i], cond_vars[j]) in pdf.Param:
S_c[i][j] = pdf.Param[('Cov', cond_vars[i], cond_vars[j])]
S_c[j][i] = S_c[i][j].T
else:
S_c[j][i] = pdf.Param[('Cov', cond_vars[j], cond_vars[i])]
S_c[i][j] = S_c[j][i].T
S_c[i][i] = pdf.Param[('Cov', cond_vars[i])]
S_c = BlockMatrix(S_c)
S_s = [num_scope_vars * [None] for _ in range(num_scope_vars)] # careful not to create same mutable object
for i in range(num_scope_vars):
for j in range(i):
if ('Cov', scope_vars[i], scope_vars[j]) in pdf.Param:
S_s[i][j] = pdf.Param[('Cov', scope_vars[i], scope_vars[j])]
S_s[j][i] = S_s[i][j].T
else:
S_s[j][i] = pdf.Param[('Cov', scope_vars[j], scope_vars[i])]
S_s[i][j] = S_s[j][i].T
S_s[i][i] = pdf.Param[('Cov', scope_vars[i])]
S_s = BlockMatrix(S_s)
S_cs = [num_scope_vars * [None] for _ in range(num_cond_vars)] # careful not to create same mutable object
for i, j in product(range(num_cond_vars), range(num_scope_vars)):
if ('Cov', cond_vars[i], scope_vars[j]) in pdf.Param:
S_cs[i][j] = pdf.Param[('Cov', cond_vars[i], scope_vars[j])]
else:
S_cs[i][j] = pdf.Param[('Cov', scope_vars[j], cond_vars[i])].T
S_cs = BlockMatrix(S_cs)
S_sc = S_cs.T
m = (m_s + (x_c - m_c) * S_c.inverse() * S_cs).xreplace(point_cond)
S = S_s - S_sc * S_c.inverse() * S_cs
param = {}
index_ranges_from = []
index_ranges_to = []
k = 0
for i in range(num_scope_vars):
l = k + pdf.Vars[scope_vars[i]].shape[1]
index_ranges_from += [k]
index_ranges_to += [l]
param[('Mean', scope_vars[i])] = m[0, index_ranges_from[i]:index_ranges_to[i]]
for j in range(i):
param[('Cov', scope_vars[j], scope_vars[i])] =\
S[index_ranges_from[j]:index_ranges_to[j], index_ranges_from[i]:index_ranges_to[i]]
param[('Cov', scope_vars[i])] =\
S[index_ranges_from[i]:index_ranges_to[i], index_ranges_from[i]:index_ranges_to[i]]
k = l
return GaussPDF(var_names_and_syms=pdf.Vars.copy(), param=param, cond=new_cond, scope=scope)
