def test_parsing_of_matrix_expressions():
expr = M*N
assert _parse_matrix_expression(expr) == CodegenArrayContraction(CodegenArrayTensorProduct(M, N), (1, 2))
expr = Transpose(M)
assert _parse_matrix_expression(expr) == CodegenArrayPermuteDims(M, [1, 0])
expr = M*Transpose(N)
assert _parse_matrix_expression(expr) == CodegenArrayContraction(CodegenArrayTensorProduct(M, CodegenArrayPermuteDims(N, [1, 0])), (1, 2))
def test_Adjoint():
from sympy.matrices import MatrixSymbol, Adjoint, Inverse, Transpose
X = MatrixSymbol('X', 2, 2)
Y = MatrixSymbol('Y', 2, 2)
assert latex(Adjoint(X)) == r'X^\dag'
assert latex(Adjoint(X + Y)) == r'\left(X + Y\right)^\dag'
assert latex(Adjoint(X) + Adjoint(Y)) == r'X^\dag + Y^\dag'
assert latex(Adjoint(X * Y)) == r'\left(X Y\right)^\dag'
assert latex(Adjoint(Y) * Adjoint(X)) == r'Y^\dag X^\dag'
assert latex(Adjoint(X**2)) == r'\left(X^{2}\right)^\dag'
assert latex(Adjoint(X)**2) == r'\left(X^\dag\right)^{2}'
assert latex(Adjoint(Inverse(X))) == r'\left(X^{-1}\right)^\dag'
assert latex(Inverse(Adjoint(X))) == r'\left(X^\dag\right)^{-1}'
assert latex(Adjoint(Transpose(X))) == r'\left(X^T\right)^\dag'
assert latex(Transpose(Adjoint(X))) == r'\left(X^\dag\right)^T'
def test_codegen_recognize_matrix_expression():
expr = CodegenArrayElementwiseAdd(M, CodegenArrayPermuteDims(M, [1, 0]))
assert recognize_matrix_expression(expr) == M + Transpose(M)
expr = M[i,j] + N[i,j]
p1, p2 = _codegen_array_parse(expr)
assert recognize_matrix_expression(p1) == M + N
expr = M[i,j] + N[j,i]
p1, p2 = _codegen_array_parse(expr)
assert recognize_matrix_expression(p1) == M + N.T
expr = M[i,j]*N[k,l] + N[i,j]*M[k,l]
p1, p2 = _codegen_array_parse(expr)
assert recognize_matrix_expression(p1) == CodegenArrayElementwiseAdd(
CodegenArrayTensorProduct(M, N),
CodegenArrayTensorProduct(N, M))
expr = (M*N*P)[i, j]
p1, p2 = _codegen_array_parse(expr)
assert recognize_matrix_expression(p1) == M*N*P
expr = Sum(M[i,j]*(N*P)[j,m], (j, 0, k-1))
p1, p2 = _codegen_array_parse(expr)
assert recognize_matrix_expression(p1) == M*N*P
expr = Sum((P[j, m] + P[m, j])*(M[i,j]*N[m,n] + N[i,j]*M[m,n]), (j, 0, k-1), (m, 0, k-1))
p1, p2 = _codegen_array_parse(expr)
assert recognize_matrix_expression(p1) == M*P*N + M*P.T*N + N*P*M + N*P.T*M
def test_arrayexpr_convert_array_to_matrix():
cg = ArrayContraction(ArrayTensorProduct(M), (0, 1))
assert convert_array_to_matrix(cg) == Trace(M)
cg = ArrayContraction(ArrayTensorProduct(M, N), (0, 1), (2, 3))
assert convert_array_to_matrix(cg) == Trace(M) * Trace(N)
cg = ArrayContraction(ArrayTensorProduct(M, N), (0, 3), (1, 2))
assert convert_array_to_matrix(cg) == Trace(M * N)
cg = ArrayContraction(ArrayTensorProduct(M, N), (0, 2), (1, 3))
assert convert_array_to_matrix(cg) == Trace(M * N.T)
cg = convert_matrix_to_array(M * N * P)
assert convert_array_to_matrix(cg) == M * N * P
cg = convert_matrix_to_array(M * N.T * P)
assert convert_array_to_matrix(cg) == M * N.T * P
cg = ArrayContraction(ArrayTensorProduct(M,N,P,Q), (1, 2), (5, 6))
assert convert_array_to_matrix(cg) == ArrayTensorProduct(M * N, P * Q)
cg = ArrayContraction(ArrayTensorProduct(-2, M, N), (1, 2))
assert convert_array_to_matrix(cg) == -2 * M * N
a = MatrixSymbol("a", k, 1)
b = MatrixSymbol("b", k, 1)
c = MatrixSymbol("c", k, 1)
cg = PermuteDims(
ArrayContraction(
ArrayTensorProduct(
a,
ArrayAdd(
ArrayTensorProduct(b, c),
ArrayTensorProduct(c, b),
)
), (2, 4)), [0, 1, 3, 2])
assert convert_array_to_matrix(cg) == a * (b.T * c + c.T * b)
za = ZeroArray(m, n)
assert convert_array_to_matrix(za) == ZeroMatrix(m, n)
cg = ArrayTensorProduct(3, M)
assert convert_array_to_matrix(cg) == 3 * M
# Partial conversion to matrix multiplication:
expr = ArrayContraction(ArrayTensorProduct(M, N, P, Q), (0, 2), (1, 4, 6))
assert convert_array_to_matrix(expr) == ArrayContraction(ArrayTensorProduct(M.T*N, P, Q), (0, 2, 4))
x = MatrixSymbol("x", k, 1)
cg = PermuteDims(
ArrayContraction(ArrayTensorProduct(OneArray(1), x, OneArray(1), DiagMatrix(Identity(1))),
(0, 5)), Permutation(1, 2, 3))
assert convert_array_to_matrix(cg) == x
expr = ArrayAdd(M, PermuteDims(M, [1, 0]))
assert convert_array_to_matrix(expr) == M + Transpose(M)
def test_invariants():
A = MatrixSymbol('A', n, m)
B = MatrixSymbol('B', m, l)
X = MatrixSymbol('X', n, n)
objs = [Identity(n), ZeroMatrix(m, n), A, MatMul(A, B), MatAdd(A, A),
Transpose(A), Adjoint(A), Inverse(X), MatPow(X, 2), MatPow(X, -1),
MatPow(X, 0)]
for obj in objs:
assert obj == obj.__class__(*obj.args)
def test_codegen_recognize_matrix_expression():
expr = CodegenArrayElementwiseAdd(M, CodegenArrayPermuteDims(M, [1, 0]))
rec = _recognize_matrix_expression(expr)
assert rec == _RecognizeMatOp(MatAdd, [M, _RecognizeMatOp(Transpose, [M])])
assert _unfold_recognized_expr(rec) == M + Transpose(M)
expr = M[i, j] + N[i, j]
p1, p2 = _codegen_array_parse(expr)
rec = _recognize_matrix_expression(p1)
assert rec == _RecognizeMatOp(MatAdd, [M, N])
assert _unfold_recognized_expr(rec) == M + N
expr = M[i, j] + N[j, i]
p1, p2 = _codegen_array_parse(expr)
rec = _recognize_matrix_expression(p1)
assert rec == _RecognizeMatOp(MatAdd, [M, _RecognizeMatOp(Transpose, [N])])
assert _unfold_recognized_expr(rec) == M + N.T
expr = M[i, j] * N[k, l] + N[i, j] * M[k, l]
p1, p2 = _codegen_array_parse(expr)
rec = _recognize_matrix_expression(p1)
assert rec == _RecognizeMatOp(
MatAdd, [_RecognizeMatMulLines([M, N]),
_RecognizeMatMulLines([N, M])])
#assert _unfold_recognized_expr(rec) == TensorProduct(M, N) + TensorProduct(N, M) maybe?
expr = (M * N * P)[i, j]
p1, p2 = _codegen_array_parse(expr)
rec = _recognize_matrix_expression(p1)
assert rec == _RecognizeMatMulLines([_RecognizeMatOp(MatMul, [M, N, P])])
assert _unfold_recognized_expr(rec) == M * N * P
expr = Sum(M[i, j] * (N * P)[j, m], (j, 0, k - 1))
p1, p2 = _codegen_array_parse(expr)
rec = _recognize_matrix_expression(p1)
assert rec == _RecognizeMatOp(MatMul, [M, N, P])
assert _unfold_recognized_expr(rec) == M * N * P
expr = Sum((P[j, m] + P[m, j]) * (M[i, j] * N[m, n] + N[i, j] * M[m, n]),
(j, 0, k - 1), (m, 0, k - 1))
p1, p2 = _codegen_array_parse(expr)
rec = _recognize_matrix_expression(p1)
assert rec == _RecognizeMatOp(MatAdd, [
_RecognizeMatOp(MatMul, [
M,
_RecognizeMatOp(MatAdd, [P, _RecognizeMatOp(Transpose, [P])]), N
]),
_RecognizeMatOp(MatMul, [
N,
_RecognizeMatOp(MatAdd, [P, _RecognizeMatOp(Transpose, [P])]), M
])
])
assert _unfold_recognized_expr(
rec) == M * (P + P.T) * N + N * (P + P.T) * M
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Define DH param symbols
# Joint angle symbols
# Modified DH params
# Define Modified DH Transformation matrix
# Create individual transformation matrices
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
# Calculate joint angles using Geometric IK method
# Get matrix form of position and orientation of end-effector
O = get_pos_rot_mat(px, py, pz, roll, pitch, yaw)
# Return from URDF to DH param coordinates (apply inverse correction)
O1 = O * Transpose(RG_corr)
theta1, theta2, theta3, theta4, theta5, theta6 = inverse_kinematics(
O1, s)
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def test_Identity():
n, m = symbols('n m', integer=True)
A = MatrixSymbol('A', n, m)
In = Identity(n)
Im = Identity(m)
assert A * Im == A
assert In * A == A
assert Transpose(In) == In
assert Inverse(In) == In
def test_ZeroMatrix():
n, m = symbols('n m', integer=True)
A = MatrixSymbol('A', n, m)
Z = ZeroMatrix(n, m)
assert A + Z == A
assert A * Z.T == ZeroMatrix(n, n)
assert Z * A.T == ZeroMatrix(n, n)
assert A - A == ZeroMatrix(*A.shape)
assert Transpose(Z) == ZeroMatrix(m, n)
def test_Identity():
A = MatrixSymbol('A', n, m)
In = Identity(n)
Im = Identity(m)
assert A * Im == A
assert In * A == A
assert Transpose(In) == In
assert Inverse(In) == In
assert In.conjugate() == In
def test_ZeroMatrix():
A = MatrixSymbol('A', n, m)
Z = ZeroMatrix(n, m)
assert A + Z == A
assert A * Z.T == ZeroMatrix(n, n)
assert Z * A.T == ZeroMatrix(n, n)
assert A - A == ZeroMatrix(*A.shape)
assert Transpose(Z) == ZeroMatrix(m, n)
assert Z.conjugate() == Z
def pdf(self, x):
M , U , V = self.location_matrix, self.scale_matrix_1, self.scale_matrix_2
n, p = M.shape
if isinstance(x, list):
x = ImmutableMatrix(x)
if not isinstance(x, (MatrixBase, MatrixSymbol)):
raise ValueError("%s should be an isinstance of Matrix "
"or MatrixSymbol" % str(x))
term1 = Inverse(V)*Transpose(x - M)*Inverse(U)*(x - M)
num = exp(-Trace(term1)/S(2))
den = (2*pi)**(S(n*p)/2) * Determinant(U)**S(p)/2 * Determinant(V)**S(n)/2
return num/den
def test_transpose():
Sq = MatrixSymbol('Sq', n, n)
assert Transpose(A).shape == (m, n)
assert Transpose(A * B).shape == (l, n)
assert Transpose(Transpose(A)) == A
assert Transpose(eye(3)) == eye(3)
assert Transpose(S(5)) == S(5)
assert Transpose(Matrix([[1, 2], [3, 4]])) == Matrix([[1, 3], [2, 4]])
assert Transpose(Trace(Sq)) == Trace(Sq)
def test_parsing_of_matrix_expressions():
expr = M * N
assert parse_matrix_expression(expr) == CodegenArrayContraction(
CodegenArrayTensorProduct(M, N), (1, 2))
expr = Transpose(M)
assert parse_matrix_expression(expr) == CodegenArrayPermuteDims(M, [1, 0])
expr = M * Transpose(N)
assert parse_matrix_expression(expr) == CodegenArrayContraction(
CodegenArrayTensorProduct(M, CodegenArrayPermuteDims(N, [1, 0])),
(1, 2))
expr = 3 * M * N
res = parse_matrix_expression(expr)
rexpr = recognize_matrix_expression(res)
assert expr == rexpr
expr = 3 * M + N * M.T * M + 4 * k * N
res = parse_matrix_expression(expr)
rexpr = recognize_matrix_expression(res)
assert expr == rexpr
expr = Inverse(M) * N
rexpr = recognize_matrix_expression(parse_matrix_expression(expr))
assert expr == rexpr
expr = M**2
rexpr = recognize_matrix_expression(parse_matrix_expression(expr))
assert expr == rexpr
expr = M * (2 * N + 3 * M)
res = parse_matrix_expression(expr)
rexpr = recognize_matrix_expression(res)
assert expr == rexpr
expr = Trace(M)
result = CodegenArrayContraction(M, (0, 1))
assert parse_matrix_expression(expr) == result
def pdf(self, x):
from sympy import eye
if isinstance(x, list):
x = ImmutableMatrix(x)
if not isinstance(x, (MatrixBase, MatrixSymbol)):
raise ValueError("%s should be an isinstance of Matrix "
"or MatrixSymbol" % str(x))
nu, M, Omega, Sigma = self.nu, self.location_matrix, self.scale_matrix_1, self.scale_matrix_2
n, p = M.shape
K = multigamma((nu + n + p - 1)/2, p) * Determinant(Omega)**(-n/2) * Determinant(Sigma)**(-p/2) \
/ ((pi)**(n*p/2) * multigamma((nu + p - 1)/2, p))
return K * (Determinant(eye(n) + Inverse(Sigma)*(x - M)*Inverse(Omega)*Transpose(x - M))) \
**(-(nu + n + p -1)/2)
def test_transpose():
n, m, l = symbols('n m l', integer=True)
A = MatrixSymbol('A', n, m)
B = MatrixSymbol('B', m, l)
Sq = MatrixSymbol('Sq', n, n)
assert Transpose(A).shape == (m, n)
assert Transpose(A * B).shape == (l, n)
assert Transpose(Transpose(A)) == A
assert Transpose(eye(3)) == eye(3)
assert Transpose(S(5)) == S(5)
assert Transpose(Matrix([[1, 2], [3, 4]])) == Matrix([[1, 3], [2, 4]])
assert Transpose(Trace(Sq)) == Trace(Sq)
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 test_arrayexpr_convert_array_to_matrix():
cg = ArrayContraction(ArrayTensorProduct(M), (0, 1))
assert convert_array_to_matrix(cg) == Trace(M)
cg = ArrayContraction(ArrayTensorProduct(M, N), (0, 1), (2, 3))
assert convert_array_to_matrix(cg) == Trace(M) * Trace(N)
cg = ArrayContraction(ArrayTensorProduct(M, N), (0, 3), (1, 2))
assert convert_array_to_matrix(cg) == Trace(M * N)
cg = ArrayContraction(ArrayTensorProduct(M, N), (0, 2), (1, 3))
assert convert_array_to_matrix(cg) == Trace(M * N.T)
cg = convert_matrix_to_array(M * N * P)
assert convert_array_to_matrix(cg) == M * N * P
cg = convert_matrix_to_array(M * N.T * P)
assert convert_array_to_matrix(cg) == M * N.T * P
cg = ArrayContraction(ArrayTensorProduct(M, N, P, Q), (1, 2), (5, 6))
assert convert_array_to_matrix(cg) == ArrayTensorProduct(M * N, P * Q)
cg = ArrayContraction(ArrayTensorProduct(-2, M, N), (1, 2))
assert convert_array_to_matrix(cg) == -2 * M * N
a = MatrixSymbol("a", k, 1)
b = MatrixSymbol("b", k, 1)
c = MatrixSymbol("c", k, 1)
cg = PermuteDims(
ArrayContraction(
ArrayTensorProduct(
a,
ArrayAdd(
ArrayTensorProduct(b, c),
ArrayTensorProduct(c, b),
)), (2, 4)), [0, 1, 3, 2])
assert convert_array_to_matrix(cg) == a * (b.T * c + c.T * b)
za = ZeroArray(m, n)
assert convert_array_to_matrix(za) == ZeroMatrix(m, n)
cg = ArrayTensorProduct(3, M)
assert convert_array_to_matrix(cg) == 3 * M
# TODO: not yet supported:
# cg = ArrayDiagonal(ArrayTensorProduct(M, N, P), (0, 2, 4), (1, 3, 5))
# assert recognize_matrix_expression(cg) == HadamardProduct(M, N, P)
# cg = ArrayDiagonal(ArrayTensorProduct(M, N, P), (0, 3, 4), (1, 2, 5))
# assert recognize_matrix_expression(cg) == HadamardProduct(M, N.T, P)
x = MatrixSymbol("x", k, 1)
cg = PermuteDims(
ArrayContraction(
ArrayTensorProduct(OneArray(1), x, OneArray(1),
DiagMatrix(Identity(1))), (0, 5)),
Permutation(1, 2, 3))
assert convert_array_to_matrix(cg) == x
expr = ArrayAdd(M, PermuteDims(M, [1, 0]))
assert convert_array_to_matrix(expr) == M + Transpose(M)
def SolveIK(ee_pos_vals, quaternion_vals, debug=False):
""" Solves inverse kinematics problem for Kuka arm. Generator, can return
multiple solutions."""
global R0_3
ee_pos = Matrix(ee_pos_vals)
T = _CreateTransform(quat2mat(quaternion_vals), ee_pos)
R0_EE = T[0:3, 0:3] * R_gripper
v_EE = T[0:3, 3]
# Decouple kinematics problem at WC origin.
wc_pos = v_EE - R0_EE * Matrix([0, 0, d_ee])
if debug:
print("wc_pos")
pprint(wc_pos)
# Theta 1-3. Geometric solution given wc_pos.
xc, yc, zc = wc_pos
theta1 = atan2(yc, xc)
# Alpha (alp) is the angle between 3 and 5th along y axis of the 3rd joint
# frame.
alp = atan2(-0.054, 1.5)
s = zc - d_z
r = sqrt(xc**2 + yc**2) - d_x
cosbet = (r**2 + s**2 - a[2]**2 - d_star**2) / (2 * a[2] * d_star)
for q3sign in [1, -1]:
# sinbet can have two solutions.
sinbet = q3sign * sqrt(1 - cosbet**2)
theta3 = -(pi / 2 - (atan2(sinbet, cosbet) + alp))
theta2 = pi / 2 - (atan2(s, r) +
atan2(d_star * sinbet, a[2] + d_star * cosbet))
# Theta 4-6. Solve R3_EE = (R0_3)^-1 * R0_EE.
R0_3_eval = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
# For a rotation matrix inverse equals transpose.
R3_EE = Transpose(R0_3_eval) * R0_EE
cosq5 = R3_EE[1, 2]
sinq5 = sqrt(R3_EE[0, 2]**2 + R3_EE[2, 2]**2)
for t5_flip in [1, 0]:
# Theta5 has an alternative solution at -Theta5
theta5 = (1 - 2 * t5_flip) * atan2(sinq5, cosq5)
sgn = sin(theta5)
sinq4 = R3_EE[2, 2] / sgn
cosq4 = -R3_EE[0, 2] / sgn
theta4 = atan2(sinq4, cosq4)
sinq6 = -R3_EE[1, 1] / sgn
cosq6 = R3_EE[1, 0] / sgn
theta6 = atan2(sinq6, cosq6)
thetas = [theta1, theta2, theta3, theta4, theta5, theta6]
if debug:
print("computed wc_pos")
pprint(SolveFKwc(thetas))
pprint("P0_EE")
pprint(T[0:3, 3])
print("Computed P0_EE")
pprint(SolveFK(thetas))
yield Matrix(thetas).evalf()
def test_recognize_diagonalized_vectors():
a = MatrixSymbol("a", k, 1)
b = MatrixSymbol("b", k, 1)
A = MatrixSymbol("A", k, k)
B = MatrixSymbol("B", k, k)
C = MatrixSymbol("C", k, k)
X = MatrixSymbol("X", k, k)
x = MatrixSymbol("x", k, 1)
I1 = Identity(1)
I = Identity(k)
# Check matrix recognition over trivial dimensions:
cg = CodegenArrayTensorProduct(a, b)
assert recognize_matrix_expression(cg) == a * b.T
cg = CodegenArrayTensorProduct(I1, a, b)
assert recognize_matrix_expression(cg) == a * I1 * b.T
# Recognize trace inside a tensor product:
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, B, C), (0, 3),
(1, 2))
assert recognize_matrix_expression(cg) == Trace(A * B) * C
# Transform diagonal operator to contraction:
cg = CodegenArrayDiagonal(CodegenArrayTensorProduct(A, a), (1, 2))
assert cg.transform_to_product() == CodegenArrayContraction(
CodegenArrayTensorProduct(A, DiagMatrix(a)), (1, 2))
assert recognize_matrix_expression(cg) == A * DiagMatrix(a)
cg = CodegenArrayDiagonal(CodegenArrayTensorProduct(a, b), (0, 2))
assert cg.transform_to_product() == CodegenArrayContraction(
CodegenArrayTensorProduct(DiagMatrix(a), b), (0, 2))
assert recognize_matrix_expression(cg).doit() == DiagMatrix(a) * b
cg = CodegenArrayDiagonal(CodegenArrayTensorProduct(A, a), (0, 2))
assert cg.transform_to_product() == CodegenArrayContraction(
CodegenArrayTensorProduct(A, DiagMatrix(a)), (0, 2))
assert recognize_matrix_expression(cg) == A.T * DiagMatrix(a)
cg = CodegenArrayDiagonal(CodegenArrayTensorProduct(I, x, I1), (0, 2),
(3, 5))
assert cg.transform_to_product() == CodegenArrayContraction(
CodegenArrayTensorProduct(I, DiagMatrix(x), I1), (0, 2))
cg = CodegenArrayDiagonal(CodegenArrayTensorProduct(I, x, A, B), (1, 2),
(5, 6))
assert cg.transform_to_product() == CodegenArrayDiagonal(
CodegenArrayContraction(
CodegenArrayTensorProduct(I, DiagMatrix(x), A, B), (1, 2)), (3, 4))
cg = CodegenArrayDiagonal(CodegenArrayTensorProduct(x, I1), (1, 2))
assert isinstance(cg, CodegenArrayDiagonal)
assert cg.diagonal_indices == ((1, 2), )
assert recognize_matrix_expression(cg) == x
cg = CodegenArrayDiagonal(CodegenArrayTensorProduct(x, I), (0, 2))
assert cg.transform_to_product() == CodegenArrayContraction(
CodegenArrayTensorProduct(DiagMatrix(x), I), (0, 2))
assert recognize_matrix_expression(cg).doit() == DiagMatrix(x)
cg = CodegenArrayDiagonal(x, (1, ))
assert cg == x
# Ignore identity matrices with contractions:
cg = CodegenArrayContraction(CodegenArrayTensorProduct(I, A, I, I), (0, 2),
(1, 3), (5, 7))
assert cg.split_multiple_contractions() == cg
assert recognize_matrix_expression(cg) == Trace(A) * I
cg = CodegenArrayContraction(CodegenArrayTensorProduct(Trace(A) * I, I, I),
(1, 5), (3, 4))
assert cg.split_multiple_contractions() == cg
assert recognize_matrix_expression(cg).doit() == Trace(A) * I
# Add DiagMatrix when required:
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a), (1, 2))
assert cg.split_multiple_contractions() == cg
assert recognize_matrix_expression(cg) == A * a
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a, B), (1, 2, 4))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(A, DiagMatrix(a), B), (1, 2), (3, 4))
assert recognize_matrix_expression(cg) == A * DiagMatrix(a) * B
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a, B), (0, 2, 4))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(A, DiagMatrix(a), B), (0, 2), (3, 4))
assert recognize_matrix_expression(cg) == A.T * DiagMatrix(a) * B
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a, b, a.T, B),
(0, 2, 4, 7, 9))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(A, DiagMatrix(a), DiagMatrix(b),
DiagMatrix(a), B), (0, 2), (3, 4), (5, 7),
(6, 9))
assert recognize_matrix_expression(
cg).doit() == A.T * DiagMatrix(a) * DiagMatrix(b) * DiagMatrix(a) * B.T
cg = CodegenArrayContraction(CodegenArrayTensorProduct(I1, I1, I1),
(1, 2, 4))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(I1, I1, I1), (1, 2), (3, 4))
assert recognize_matrix_expression(cg).doit() == Identity(1)
cg = CodegenArrayContraction(CodegenArrayTensorProduct(I, I, I, I, A),
(1, 2, 8), (5, 6, 9))
assert recognize_matrix_expression(
cg.split_multiple_contractions()).doit() == A
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a, C, a, B),
(1, 2, 4), (5, 6, 8))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(A, DiagMatrix(a), C, DiagMatrix(a), B),
(1, 2), (3, 4), (5, 6), (7, 8))
assert recognize_matrix_expression(
cg) == A * DiagMatrix(a) * C * DiagMatrix(a) * B
cg = CodegenArrayContraction(
CodegenArrayTensorProduct(a, I1, b, I1, (a.T * b).applyfunc(cos)),
(1, 2, 8), (5, 6, 9))
assert cg.split_multiple_contractions().dummy_eq(
CodegenArrayContraction(
CodegenArrayTensorProduct(a, I1, b, I1, (a.T * b).applyfunc(cos)),
(1, 2), (3, 8), (5, 6), (7, 9)))
assert recognize_matrix_expression(cg).dummy_eq(
MatMul(a, I1, (a.T * b).applyfunc(cos), Transpose(I1), b.T))
cg = CodegenArrayContraction(
CodegenArrayTensorProduct(A.T, a, b, b.T, (A * X * b).applyfunc(cos)),
(1, 2, 8), (5, 6, 9))
assert cg.split_multiple_contractions().dummy_eq(
CodegenArrayContraction(
CodegenArrayTensorProduct(A.T, DiagMatrix(a), b, b.T,
(A * X * b).applyfunc(cos)), (1, 2),
(3, 8), (5, 6, 9)))
# assert recognize_matrix_expression(cg)
# Check no overlap of lines:
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a, C, a, B),
(1, 2, 4), (5, 6, 8), (3, 7))
assert cg.split_multiple_contractions() == cg
cg = CodegenArrayContraction(CodegenArrayTensorProduct(a, b, A), (0, 2, 4),
(1, 3))
assert cg.split_multiple_contractions() == cg
print("theta0 end result is:", theta0 )
print("theta1 radiansend result is:", math.radians(math.degrees(theta1)))
print("theta2 end result is:" ,math.radians(math.degrees(theta2)))
#print(T0_3)
# Theta 4,5,6 calc
R0_3=T0_3.extract([0,1,2],[0,1,2])
R0_3 = R0_3.evalf(subs={q1: theta0, q2: theta1, q3: theta2-pi/2})
R3_6 = simplify(Transpose(R0_3))
R3_6 = R3_6 * RotM
r31 = R3_6[2,0]
r11 = R3_6[0,0]
r21 = R3_6[1,0]
r32 = R3_6[2,1]
r33 = R3_6[2,2]
beta = atan2(-r31, sqrt(r11 * r11 + r21 * r21))
gamma = atan2(r32, r33)-pi/2
alpha = atan2(r21, r11)-pi/2
print(" ")
print(" ")
print(" ")
print("alpha in degrees is = ",(math.degrees(alpha)))
print("alpha is = ",math.radians(math.degrees(alpha)))
def test_parsing_of_matrix_expressions():
expr = M*N
assert parse_matrix_expression(expr) == CodegenArrayContraction(CodegenArrayTensorProduct(M, N), (1, 2))
expr = Transpose(M)
assert parse_matrix_expression(expr) == CodegenArrayPermuteDims(M, [1, 0])
expr = M*Transpose(N)
assert parse_matrix_expression(expr) == CodegenArrayContraction(CodegenArrayTensorProduct(M, CodegenArrayPermuteDims(N, [1, 0])), (1, 2))
expr = 3*M*N
res = parse_matrix_expression(expr)
rexpr = recognize_matrix_expression(res)
assert expr == rexpr
expr = 3*M + N*M.T*M + 4*k*N
res = parse_matrix_expression(expr)
rexpr = recognize_matrix_expression(res)
assert expr == rexpr
expr = Inverse(M)*N
rexpr = recognize_matrix_expression(parse_matrix_expression(expr))
assert expr == rexpr
expr = M**2
rexpr = recognize_matrix_expression(parse_matrix_expression(expr))
assert expr == rexpr
expr = M*(2*N + 3*M)
res = parse_matrix_expression(expr)
rexpr = recognize_matrix_expression(res)
assert expr == rexpr
expr = Trace(M)
result = CodegenArrayContraction(M, (0, 1))
assert parse_matrix_expression(expr) == result
expr = 3*Trace(M)
result = CodegenArrayContraction(CodegenArrayTensorProduct(3, M), (0, 1))
assert parse_matrix_expression(expr) == result
expr = 3*Trace(Trace(M) * M)
result = CodegenArrayContraction(CodegenArrayTensorProduct(3, M, M), (0, 1), (2, 3))
assert parse_matrix_expression(expr) == result
expr = 3*Trace(M)**2
result = CodegenArrayContraction(CodegenArrayTensorProduct(3, M, M), (0, 1), (2, 3))
assert parse_matrix_expression(expr) == result
expr = HadamardProduct(M, N)
result = CodegenArrayDiagonal(CodegenArrayTensorProduct(M, N), (0, 2), (1, 3))
assert parse_matrix_expression(expr) == result
expr = HadamardPower(M, 2)
result = CodegenArrayDiagonal(CodegenArrayTensorProduct(M, M), (0, 2), (1, 3))
assert parse_matrix_expression(expr) == result
expr = M**2
assert isinstance(expr, MatPow)
assert parse_matrix_expression(expr) == CodegenArrayContraction(CodegenArrayTensorProduct(M, M), (1, 2))
def test_recognize_diagonalized_vectors():
a = MatrixSymbol("a", k, 1)
b = MatrixSymbol("b", k, 1)
A = MatrixSymbol("A", k, k)
B = MatrixSymbol("B", k, k)
C = MatrixSymbol("C", k, k)
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a), (1, 2))
assert cg.split_multiple_contractions() == cg
assert recognize_matrix_expression(cg) == A * a
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a, B), (1, 2, 4))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(A, DiagonalizeVector(a), B), (1, 2), (3, 4))
assert recognize_matrix_expression(cg) == A * DiagonalizeVector(a) * B
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a, B), (0, 2, 4))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(A, DiagonalizeVector(a), B), (0, 2), (3, 4))
assert recognize_matrix_expression(cg) == A.T * DiagonalizeVector(a) * B
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a, b, a.T, B),
(0, 2, 4, 7, 9))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(A,
DiagonalizeVector(a), DiagonalizeVector(b),
DiagonalizeVector(a), B), (0, 2), (3, 4),
(5, 7), (6, 9))
assert recognize_matrix_expression(cg).doit() == A.T * DiagonalizeVector(
a) * DiagonalizeVector(b) * DiagonalizeVector(a) * B.T
I1 = Identity(1)
cg = CodegenArrayContraction(CodegenArrayTensorProduct(I1, I1, I1),
(1, 2, 4))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(I1, I1, I1), (1, 2), (3, 4))
assert recognize_matrix_expression(cg).doit() == Identity(1)
I = Identity(k)
cg = CodegenArrayContraction(CodegenArrayTensorProduct(I, I, I, I, A),
(1, 2, 8), (5, 6, 9))
assert recognize_matrix_expression(
cg.split_multiple_contractions()).doit() == A
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a, C, a, B),
(1, 2, 4), (5, 6, 8))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(A, DiagonalizeVector(a), C,
DiagonalizeVector(a), B), (1, 2), (3, 4),
(5, 6), (7, 8))
assert recognize_matrix_expression(
cg) == A * DiagonalizeVector(a) * C * DiagonalizeVector(a) * B
cg = CodegenArrayContraction(
CodegenArrayTensorProduct(a, I1, b, I1, (a.T * b).applyfunc(cos)),
(1, 2, 8), (5, 6, 9))
assert cg.split_multiple_contractions() == CodegenArrayContraction(
CodegenArrayTensorProduct(a, I1, b, I1, (a.T * b).applyfunc(cos)),
(1, 2), (3, 8), (5, 6), (7, 9))
assert recognize_matrix_expression(cg) == MatMul(a, I1,
(a.T * b).applyfunc(cos),
Transpose(I1), b.T)
# Check no overlap of lines:
cg = CodegenArrayContraction(CodegenArrayTensorProduct(A, a, C, a, B),
(1, 2, 4), (5, 6, 8), (3, 7))
raises(ValueError, lambda: cg.split_multiple_contractions())
cg = CodegenArrayContraction(CodegenArrayTensorProduct(a, b, A), (0, 2, 4),
(1, 3))
raises(ValueError, lambda: cg.split_multiple_contractions())
print('2.4: ' + str(u.dot(v)))
# 51
print('2.5: ' + str(math.sqrt(u.dot(u))))
# 8.602325267042627
# Problem 3: Matrix Operations
try:
print('3.1: ' + str(A + C))
except:
print('3.1: Not Defined')
# 3.1: Not Defined
try:
print('3.2: ' + str(A - Transpose(C)))
except:
print('3.2: Not Defined')
# 3.2: Matrix([
# [-4, -7, -3],
# [ 3, 6, 4]])
try:
print('3.3: ' + str(Transpose(C) + 3 * D))
except:
print('3.3: Not Defined')
# 3.3: Matrix([
# [14, 3, 3],
# [ 2, 7, 9]])
def inverse_kinematics(O, s):
# Position of EE that we should reach
px = O[0, 3]
py = O[1, 3]
pz = O[2, 3]
# Get final rotation matrix
R = O[0:3, 0:3] # get_rot_mat(roll, pitch, yaw)
# print('R =')
# pprint(R)
# Distance from wrist center to end-effector
d = s[d7]
# Wrist Center
xc = px - d * R[0, 2]
yc = py - d * R[1, 2]
zc = pz - d * R[2, 2]
# print('xc =', xc)
# print('yc =', yc)
# print('zc =', zc)
# theta_1
theta_1 = atan2(yc, xc)
# print('theta_1 =', theta_1.evalf())
# joint_2 center
x0 = s[a1]
z0 = s[d1]
# print('x0 =', x0, ', z0 =', z0)
# Link
l1 = s[a2]
# print('s[a3] = ', s[a3], ', s[d4] =', s[d4])
l2 = sqrt(s[a3]**2 + s[d4]**2)
theta30 = atan2(Abs(s[a3]), s[d4])
# print('l1 =', l1, ', l2 =', l2)
# print('theta30 =', theta30)
# Dist from {0} to {wc}
dwc = sqrt((sqrt(xc**2 + yc**2) - x0)**2 + (zc - z0)**2)
# print('dwc =', dwc)
# Check validity
assert (l1 + l2 > dwc)
# Cosine law for cos(pi - beta):
D = (l1**2 + l2**2 - dwc**2) / (2 * l1 * l2)
# theta_3 = pi/2 - theta30 - atan2(sqrt(1 - D**2), D) # here is possible a second solution when elbow-down with "-" sign
# theta_3_alt = pi/2 - theta30 - atan2(-sqrt(1 - D**2), D) # alt solution
theta_3 = pi / 2 - theta30 - acos(
D) # here is possible a second solution when elbow-down with "-" sign
theta_3_alt = pi / 2 - theta30 + acos(D) # alt solution
# print('D =', D)
# print('theta_3 =', theta_3.evalf())
# print('theta_3_alt =', theta_3_alt.evalf())
# angle to l2 from l1
bet = pi / 2 + theta_3 + theta30
# print('bet =', bet)
# Find angle to {0} - {wc} line
gam = atan2(sqrt(xc**2 + yc**2) - x0, zc - z0)
# print('gam =', gam)
# Find angle between {0} - {wc} /_ l1
# cos (alph) = E
E = (l1**2 + dwc**2 - l2**2) / (2 * l1 * dwc)
alph = acos(E)
# print('E =', E)
# print('alph =', alph)
# if 0 < beta < pi then theta_2 = gam - alph else theta_2 = gam + alph
if 0 < bet and bet < pi:
theta_2 = gam - alph
else:
theta_2 = gam + alph
# print('theta_2 =', theta_2)
# pitch2 = theta_2 + theta_3
# print('pitch2 =', pitch2.evalf())
# Check by feeding angles to forward kinematics
t04 = T0_4.evalf(subs={q1: theta_1, q2: theta_2, q3: theta_3, q4: 0})
# print('wc_calc =')
# pprint(t04)
# Wrist center should be equal to what we've started from
# print('xc_diff =', xc - t04[0,3])
# print('yc_diff =', yc - t04[1,3])
# print('zc_diff =', zc - t04[2,3])
assert (abs(xc - t04[0, 3]) < 1e-6)
assert (abs(yc - t04[1, 3]) < 1e-6)
assert (abs(zc - t04[2, 3]) < 1e-6)
# Get R0_4 with theta_4 = 0
R0_4 = t04[0:3, 0:3]
# print('R0_4 =')
# pprint(R0_4.evalf(6))
# Calc R4_G - wrist rotation matrix
R4_G = Transpose(R0_4) * R[0:3, 0:3]
# print('R4_G =')
# pprint(R4_G.evalf(6))
# Calc symbolicaly wrist rotation matrix
R4_G_sym = rot_z(q4) * rot_x(pi / 2) * rot_z(q5) * rot_x(
-pi / 2) * rot_z(q6)
'''
# Show formula (for derivation)
prisnt('R4_G_sym =')
pprint(simplify(R4_G_sym)) # just for visualization
# Corner case q5 = 0
print('R4_G_sym (q5=0) =')
pprint(simplify(R4_G_sym.subs({q5: 0}))) # just for visualization
# Corner case q5 = pi
print('R4_G_sym (q5=pi) =')
pprint(simplify(R4_G_sym.subs({q5: pi}))) # just for visualization
'''
# Solve wrist rotation matrix
# inspired by https://www.geometrictools.com/Documentation/EulerAngles.pdf
if R4_G[2, 2] < 1:
if R4_G[2, 2] > -1:
# theta_5 between (0, pi)
theta_5 = acos(R4_G[2, 2])
theta_4 = atan2(-R4_G[1, 2], -R4_G[0, 2])
theta_6 = atan2(-R4_G[2, 1], R4_G[2, 0])
else:
# q5 = pi s.t. cos(q5) = -1 and sin(q5) = 0
theta_5 = pi
# Not a unique solution: q4 - q6 = atan2(-r10,-r00)
theta_4 = S(0)
theta_6 = -atan2(-R4_G[1, 0], -R4_G[0, 0])
else:
# q5 = 0 s.t. cos(q5) = 1 and sin(q5) = 0
theta_5 = S(0)
# Not a unique solution: q4 + q6 = atan2(r10, r00)
theta_4 = S(0)
theta_6 = atan2(R4_G[0, 1], R4_G[0, 0])
# print('theta_4 =', theta_4)
# print('theta_5 =', theta_5)
# print('theta_6 =', theta_6)
return theta_1, theta_2, theta_3, theta_4, theta_5, theta_6
