def lagrangiana_3D(x, y, z, n):
Lx = Matrix(lagrangiana_1D(x, n))
Ly = Transpose(Matrix(lagrangiana_1D(y, n)))
Lz = Transpose(Matrix(lagrangiana_1D(z, n)))
Lxy = expandir_matriz((Lx * Ly))
filas = Lxy.shape[0]
Lxyz = []
for i in range(filas):
resultado = expandir_matriz((Lxy[:, i] * Lz))
Lxyz = np.append(Lxyz, resultado)
return Lxyz.reshape(filas, filas, filas)
def _insert_candidate_into_editor(editor: _EditArrayContraction,
arg_with_ind: _ArgE, candidate: _ArgE,
transpose1: bool, transpose2: bool):
other = candidate.element
other_index: int
if transpose2:
other = Transpose(other)
other_index = candidate.indices[0]
else:
other_index = candidate.indices[1]
new_element = (Transpose(arg_with_ind.element)
if transpose1 else arg_with_ind.element) * other
editor.args_with_ind.remove(candidate)
new_arge = _ArgE(new_element)
return new_arge, other_index
def new_ind_miller(old_ind_miller, old_VPP_RD, new_VPP_RD, dim):
"""
Parameters
----------
old_ind_miller : [h,k,l] ind de miller de la base vieja
old_VPP_RD : coordenadas de los viejos vectores primitivos de la red directa
(ai,aj,ak)
new_VPP_RD : coordenadas de los nuevos vectores primitivos de la red directa
(a1,a2,a3), quiero los indices de miller en esta base
dim : dimension de los VPP de la RD
Returns
-------
new_ind_miller : [h,k,l] ind de miller de los new_VPP_RD
"""
coordi, coordj, coordk = old_VPP_RD
# ai = Vector(coordi,dim)
# aj = Vector(coordj,dim)
# ak = Vector(coordk,dim)
# print('VP viejos de la RR:', bi,bj,bk)
bi, bj, bk = VP_RR(coordi, coordj, coordk)
coord1, coord2, coord3 = new_VPP_RD
# a1 = Vector(coord1,dim)
# a2 = Vector(coord2,dim)
# a3 = Vector(coord3,dim)
# print('VP nuevos de la RR:', b1,b2,b3)
b1, b2, b3 = VP_RR(coord1, coord2, coord3)
B_ijk = Matrix_coord([bi, bj, bk]) #base vieja
B_xyz = Matrix_coord([b1, b2, b3]) #base nueva
# print('Cambio de base')
BT_xyz = Transpose(B_xyz) #transposer la matriz de coordenadas
BT_ijk = Transpose(B_ijk)
C_base = BT_xyz.inv() * BT_ijk
C_base = np.matrix(C_base)
ind_nuevo = np.array(C_base.dot(old_ind_miller))[0]
return ind_nuevo
def Matrix_to_dictionary(mat_rep,basis):
# Convert matrix representation of linear transformation to dictionary
mat_rep = Transpose(mat_rep)
dict_rep = {}
n = len(basis)
if mat_rep.rows != n or mat_rep.cols != n:
raise ValueError('Matrix and Basis dimensions not equal for Matrix = ' + str(mat))
n_range = list(range(n))
for row in n_range:
dict_rep[basis[row]] = S(0)
for col in n_range:
dict_rep[basis[row]] += mat_rep[row,col]*basis[col]
return dict_rep
def get_inverse(position, orientation):
"""
Given the desired position and orientation of the end effector, calculate the required joint angles
:param position:
:param orientation:
:return:
"""
wc = get_wrist_center(position, orientation)
theta1, theta2, theta3 = get_first_three_joints(wc)
r0_3 = (T0_1 * T1_2 * T2_3).evalf(subs={q1: theta1, q2: theta2, q3: theta3})[:3, :3]
r0_6 = get_corrected_total_rotation(orientation)
r3_6 = Transpose(r0_3) * r0_6
theta4, theta5, theta6 = wrist_angles_from_transform(r3_6)
return theta1, theta2, theta3, theta4, theta5, theta6
def Dictionary_to_Matrix(dict_rep, ga):
""" Convert dictionary representation of linear transformation to matrix """
lst_mat = [] # list representation of sympy matrix
for e_row in ga.basis:
lst_mat_row = len(ga.basis) * [S.Zero]
element = dict_rep.get(e_row, S.Zero)
if isinstance(element, mv.Mv):
element = element.obj
for coef, base in metric.linear_expand_terms(element):
index = ga.basis.index(base)
lst_mat_row[index] = coef
lst_mat.append(lst_mat_row)
# expand the transpose
return Transpose(Matrix(lst_mat)).doit()
def _normalize(self):
# Normalization of trace of matrix products. Use transposition and
# cyclic properties of traces to make sure the arguments of the matrix
# product are sorted and the first argument is not a trasposition.
from sympy import MatMul, Transpose, default_sort_key
trace_arg = self.arg
if isinstance(trace_arg, MatMul):
indmin = min(range(len(trace_arg.args)),
key=lambda x: default_sort_key(trace_arg.args[x]))
if isinstance(trace_arg.args[indmin], Transpose):
trace_arg = Transpose(trace_arg).doit()
indmin = min(range(len(trace_arg.args)),
key=lambda x: default_sort_key(trace_arg.args[x]))
trace_arg = MatMul.fromiter(trace_arg.args[indmin:] +
trace_arg.args[:indmin])
return Trace(trace_arg)
return self
def Dictionary_to_Matrix(dict_rep, ga):
# Convert dictionary representation of linear transformation to matrix
basis = list(dict_rep.keys())
n = len(basis)
n_range = list(range(n))
lst_mat = [] # list representation of sympy matrix
for row in n_range:
e_row = ga.basis[row]
lst_mat_row = n * [S(0)]
if e_row in basis: # If not in basis row all zeros
(coefs,bases) = metric.linear_expand(dict_rep[e_row])
for (coef,base) in zip(coefs,bases):
index = basis.index(base)
lst_mat_row[index] = coef
lst_mat.append(lst_mat_row)
return Transpose(Matrix(lst_mat))
def Dictionary_to_Matrix(dict_rep, ga):
""" Convert dictionary representation of linear transformation to matrix """
basis = list(dict_rep.keys())
n = len(basis)
n_range = list(range(n))
lst_mat = [] # list representation of sympy matrix
for row in n_range:
e_row = ga.basis[row]
lst_mat_row = n * [S.Zero]
if e_row in basis: # If not in basis row all zeros
element = dict_rep[e_row]
if isinstance(element, mv.Mv):
element = element.obj
coefs, bases = metric.linear_expand(element)
for coef, base in zip(coefs, bases):
index = ga.basis.index(base)
lst_mat_row[index] = coef
lst_mat.append(lst_mat_row)
return Transpose(Matrix(lst_mat))
])
Ry = Matrix([
[cos(theta),0,sin(theta)],
[0, 1, 0],
[-sin(theta),0,cos(theta)]
])
Rz = Matrix([
[cos(psi),-sin(psi),0],
[sin(psi),cos(psi),0],
[0, 0, 1]
])
R = Rz*Rx*Ry
R_T = Transpose(R)
R_dot = diff(R,t)
w_b = R_dot*R_T
print(simplify(w_b))
# print(simplify(R_T*R))
# # phi_d = Matrix([[phi_dot],[0],[0]])
# # t_d = Matrix([[0],[theta_dot],[0]])
# # psi_d = Matrix([[0],[0],[psi_dot]])
#
# phi_d = Matrix([[1],[0],[0]])
# t_d = Matrix([[0],[1],[0]])
# psi_d = Matrix([[0],[0],[1]])
def lagrangiana_2D(x, y, n):
Lx = Matrix(lagrangiana_1D(x, n))
Ly = Transpose(Matrix(lagrangiana_1D(y, n)))
Lxy = expandir_matriz((Lx * Ly))
return Lxy
Symbol: lambda e, s: d(e) if (e in s) else 0,
MatrixSymbol: lambda e, s: d(e) if (e in s) else ZeroMatrix(*e.shape),
SymmetricMatrixSymbol: lambda e, s: d(e) if (e in s) else ZeroMatrix(*e.shape),
Add: lambda e, s: Add(*[_matDiff_apply(arg, s) for arg in e.args]),
Mul: lambda e, s: _matDiff_apply(e.args[0], s) if len(e.args)==1 else Mul(_matDiff_apply(e.args[0],s),Mul(*e.args[1:])) + Mul(e.args[0], _matDiff_apply(Mul(*e.args[1:]),s)),
MatAdd: lambda e, s: MatAdd(*[_matDiff_apply(arg, s) for arg in e.args]),
MatMul: lambda e, s: _matDiff_apply(e.args[0], s) if len(e.args)==1 else MatMul(_matDiff_apply(e.args[0],s),MatMul(*e.args[1:])) + MatMul(e.args[0], _matDiff_apply(MatMul(*e.args[1:]),s)),
Kron: lambda e, s: _matDiff_apply(e.args[0],s) if len(e.args)==1 else Kron(_matDiff_apply(e.args[0],s),Kron(*e.args[1:]))
+ Kron(e.args[0],_matDiff_apply(Kron(*e.args[1:]),s)),
Determinant: lambda e, s: MatMul(Determinant(e.args[0]), Trace(e.args[0].I*_matDiff_apply(e.args[0], s))),
# inverse always has 1 arg, so we index
Inverse: lambda e, s: -Inverse(e.args[0]) * _matDiff_apply(e.args[0], s) * Inverse(e.args[0]),
# trace always has 1 arg
Trace: lambda e, s: Trace(_matDiff_apply(e.args[0], s)),
# transpose also always has 1 arg, index
Transpose: lambda e, s: Transpose(_matDiff_apply(e.args[0], s))
}
def _matDiff_apply(expr, syms):
if expr.__class__ in list(MATRIX_DIFF_RULES.keys()):
return MATRIX_DIFF_RULES[expr.__class__](expr, syms)
elif expr.is_constant():
return 0
else:
raise TypeError("Don't know how to differentiate class %s", expr.__class__)
def _diff_to_grad(expr, s):
# if expr is a trace, sum of traces, or scalar times a trace, we can do it
# scalar times a trace
def _eval_transpose(self):
from sympy import Transpose
return self.func(self.function, Transpose(self.expr).doit())
def identify_hadamard_products(expr: Union[ArrayContraction, ArrayDiagonal]):
mapping = _get_mapping_from_subranks(expr.subranks)
editor: _EditArrayContraction
if isinstance(expr, ArrayContraction):
editor = _EditArrayContraction(expr)
elif isinstance(expr, ArrayDiagonal):
if isinstance(expr.expr, ArrayContraction):
editor = _EditArrayContraction(expr.expr)
diagonalized = ArrayContraction._push_indices_down(
expr.expr.contraction_indices, expr.diagonal_indices)
elif isinstance(expr.expr, ArrayTensorProduct):
editor = _EditArrayContraction(None)
editor.args_with_ind = [
_ArgE(arg) for i, arg in enumerate(expr.expr.args)
]
diagonalized = expr.diagonal_indices
else:
raise NotImplementedError("not implemented")
# Trick: add diagonalized indices as negative indices into the editor object:
for i, e in enumerate(diagonalized):
for j in e:
arg_pos, rel_pos = mapping[j]
editor.args_with_ind[arg_pos].indices[rel_pos] = -1 - i
map_contr_to_args: Dict[FrozenSet, List[_ArgE]] = defaultdict(list)
map_ind_to_inds = defaultdict(int)
for arg_with_ind in editor.args_with_ind:
for ind in arg_with_ind.indices:
map_ind_to_inds[ind] += 1
if None in arg_with_ind.indices:
continue
map_contr_to_args[frozenset(arg_with_ind.indices)].append(arg_with_ind)
k: FrozenSet[int]
v: List[_ArgE]
for k, v in map_contr_to_args.items():
if len(k) != 2:
# Hadamard product only defined for matrices:
continue
if len(v) == 1:
# Hadamard product with a single argument makes no sense:
continue
for ind in k:
if map_ind_to_inds[ind] <= 2:
# There is no other contraction, skip:
continue
# Check if expression is a trace:
if all([map_ind_to_inds[j] == len(v) and j >= 0 for j in k]):
# This is a trace
continue
# This is a Hadamard product:
def check_transpose(x):
x = [i if i >= 0 else -1 - i for i in x]
return x == sorted(x)
hp = hadamard_product(*[
i.element if check_transpose(i.indices) else Transpose(i.element)
for i in v
])
hp_indices = v[0].indices
if not check_transpose(v[0].indices):
hp_indices = list(reversed(hp_indices))
editor.insert_after(v[0], _ArgE(hp, hp_indices))
for i in v:
editor.args_with_ind.remove(i)
# Count the ranks of the arguments:
counter = 0
# Create a collector for the new diagonal indices:
diag_indices = defaultdict(list)
count_index_freq = Counter()
for arg_with_ind in editor.args_with_ind:
count_index_freq.update(Counter(arg_with_ind.indices))
free_index_count = count_index_freq[None]
# Construct the inverse permutation:
inv_perm1 = []
inv_perm2 = []
# Keep track of which diagonal indices have already been processed:
done = set([])
# Counter for the diagonal indices:
counter4 = 0
for arg_with_ind in editor.args_with_ind:
# If some diagonalization axes have been removed, they should be
# permuted in order to keep the permutation.
# Add permutation here
counter2 = 0 # counter for the indices
for i in arg_with_ind.indices:
if i is None:
inv_perm1.append(counter4)
counter2 += 1
counter4 += 1
continue
if i >= 0:
continue
# Reconstruct the diagonal indices:
diag_indices[-1 - i].append(counter + counter2)
if count_index_freq[i] == 1 and i not in done:
inv_perm1.append(free_index_count - 1 - i)
done.add(i)
elif i not in done:
inv_perm2.append(free_index_count - 1 - i)
done.add(i)
counter2 += 1
# Remove negative indices to restore a proper editor object:
arg_with_ind.indices = [
i if i is not None and i >= 0 else None
for i in arg_with_ind.indices
]
counter += len([i for i in arg_with_ind.indices if i is None or i < 0])
inverse_permutation = inv_perm1 + inv_perm2
permutation = _af_invert(inverse_permutation)
if isinstance(expr, ArrayContraction):
return editor.to_array_contraction()
else:
# Get the diagonal indices after the detection of HadamardProduct in the expression:
diag_indices_filtered = [
tuple(v) for v in diag_indices.values() if len(v) > 1
]
expr1 = editor.to_array_contraction()
expr2 = ArrayDiagonal(expr1, *diag_indices_filtered)
expr3 = PermuteDims(expr2, permutation)
return expr3
def _a2m_transpose(arg):
if isinstance(arg, _CodegenArrayAbstract):
return PermuteDims(arg, [1, 0])
else:
from sympy import Transpose
return Transpose(arg).doit()
def test_arrayexpr_convert_matrix_to_array():
expr = M * N
result = ArrayContraction(ArrayTensorProduct(M, N), (1, 2))
assert convert_matrix_to_array(expr) == result
expr = M * N * M
result = ArrayContraction(ArrayTensorProduct(M, N, M), (1, 2), (3, 4))
assert convert_matrix_to_array(expr) == result
expr = Transpose(M)
assert convert_matrix_to_array(expr) == PermuteDims(M, [1, 0])
expr = M * Transpose(N)
assert convert_matrix_to_array(expr) == ArrayContraction(
ArrayTensorProduct(M, PermuteDims(N, [1, 0])), (1, 2))
expr = 3 * M * N
res = convert_matrix_to_array(expr)
rexpr = convert_array_to_matrix(res)
assert expr == rexpr
expr = 3 * M + N * M.T * M + 4 * k * N
res = convert_matrix_to_array(expr)
rexpr = convert_array_to_matrix(res)
assert expr == rexpr
expr = Inverse(M) * N
rexpr = convert_array_to_matrix(convert_matrix_to_array(expr))
assert expr == rexpr
expr = M**2
rexpr = convert_array_to_matrix(convert_matrix_to_array(expr))
assert expr == rexpr
expr = M * (2 * N + 3 * M)
res = convert_matrix_to_array(expr)
rexpr = convert_array_to_matrix(res)
assert expr == rexpr
expr = Trace(M)
result = ArrayContraction(M, (0, 1))
assert convert_matrix_to_array(expr) == result
expr = 3 * Trace(M)
result = ArrayContraction(ArrayTensorProduct(3, M), (0, 1))
assert convert_matrix_to_array(expr) == result
expr = 3 * Trace(Trace(M) * M)
result = ArrayContraction(ArrayTensorProduct(3, M, M), (0, 1), (2, 3))
assert convert_matrix_to_array(expr) == result
expr = 3 * Trace(M)**2
result = ArrayContraction(ArrayTensorProduct(3, M, M), (0, 1), (2, 3))
assert convert_matrix_to_array(expr) == result
expr = HadamardProduct(M, N)
result = ArrayDiagonal(ArrayTensorProduct(M, N), (0, 2), (1, 3))
assert convert_matrix_to_array(expr) == result
expr = HadamardPower(M, 2)
result = ArrayDiagonal(ArrayTensorProduct(M, M), (0, 2), (1, 3))
assert convert_matrix_to_array(expr) == result
expr = M**2
assert isinstance(expr, MatPow)
assert convert_matrix_to_array(expr) == ArrayContraction(
ArrayTensorProduct(M, M), (1, 2))
expr = a.T * b
cg = convert_matrix_to_array(expr)
assert cg == ArrayContraction(ArrayTensorProduct(a, b), (0, 2))
# + {"slideshow": {"slide_type": "fragment"}}
x, y, z = sym.symbols('x y z')
Matrix([sin(x) + y, cos(y) + x, z]).jacobian([x, y, z])
# + [markdown] {"slideshow": {"slide_type": "slide"}}
# ## Matrix symbols
#
# SymPy can also operate on matrices of symbolic dimension ($n \times m$). `MatrixSymbol("M", n, m)` creates a matrix $M$ of shape $n \times m$.
# + {"slideshow": {"slide_type": "fragment"}}
from sympy import MatrixSymbol, Transpose
n, m = sym.symbols('n m', integer=True)
M = MatrixSymbol("M", n, m)
b = MatrixSymbol("b", m, 1)
Transpose(M*b)
# + {"slideshow": {"slide_type": "fragment"}}
Transpose(M*b).doit()
# + [markdown] {"slideshow": {"slide_type": "slide"}}
# ## Solving systems of equations
#
# `solve` solves equations symbolically (not numerically). The return value is a list of solutions. It automatically assumes that it is equal to 0.
# + {"slideshow": {"slide_type": "fragment"}}
from sympy import Eq, solve
solve(Eq(x**2, 4), x)
# + {"slideshow": {"slide_type": "fragment"}}
solve(x**2 + 3*x - 3, x)
def _support_function_tp1_recognize(contraction_indices, args):
subranks = [get_rank(i) for i in args]
coeff = reduce(lambda x, y: x * y,
[arg for arg, srank in zip(args, subranks) if srank == 0],
S.One)
mapping = _get_mapping_from_subranks(subranks)
new_contraction_indices = list(contraction_indices)
newargs = args[:] # make a copy of the list
removed = [None for i in newargs]
cumul = list(accumulate([0] + [get_rank(arg) for arg in args]))
new_perms = [
list(range(cumul[i], cumul[i + 1])) for i, arg in enumerate(args)
]
for pi, contraction_pair in enumerate(contraction_indices):
if len(contraction_pair) != 2:
continue
i1, i2 = contraction_pair
a1, e1 = mapping[i1]
a2, e2 = mapping[i2]
while removed[a1] is not None:
a1, e1 = removed[a1]
while removed[a2] is not None:
a2, e2 = removed[a2]
if a1 == a2:
trace_arg = newargs[a1]
newargs[a1] = Trace(trace_arg)._normalize()
new_contraction_indices[pi] = None
continue
if not isinstance(newargs[a1], MatrixExpr) or not isinstance(
newargs[a2], MatrixExpr):
continue
arg1 = newargs[a1]
arg2 = newargs[a2]
if (e1 == 1 and e2 == 1) or (e1 == 0 and e2 == 0):
arg2 = Transpose(arg2)
if e1 == 1:
argnew = arg1 * arg2
else:
argnew = arg2 * arg1
removed[a2] = a1, e1
new_perms[a1][e1] = new_perms[a2][1 - e2]
new_perms[a2] = None
newargs[a1] = argnew
newargs[a2] = None
new_contraction_indices[pi] = None
new_contraction_indices = [
i for i in new_contraction_indices if i is not None
]
newargs2 = [arg for arg in newargs if arg is not None]
if len(newargs2) == 0:
return coeff
tp = _a2m_tensor_product(*newargs2)
tc = ArrayContraction(tp, *new_contraction_indices)
new_perms2 = ArrayContraction._push_indices_up(
contraction_indices, [i for i in new_perms if i is not None])
permutation = _af_invert(
[j for i in new_perms2 for j in i if j is not None])
if permutation == [1, 0] and len(newargs2) == 1:
return Transpose(newargs2[0]).doit()
tperm = PermuteDims(tc, permutation)
return tperm