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