def test_dcm_diff_16824(): # NOTE : This is a regression test for the bug introduced in PR 14758, # identified in 16824, and solved by PR 16828. # This is the solution to Problem 2.2 on page 264 in Kane & Lenvinson's # 1985 book. q1, q2, q3 = dynamicsymbols('q1:4') s1 = sin(q1) c1 = cos(q1) s2 = sin(q2) c2 = cos(q2) s3 = sin(q3) c3 = cos(q3) dcm = Matrix([[c2 * c3, s1 * s2 * c3 - s3 * c1, c1 * s2 * c3 + s3 * s1], [c2 * s3, s1 * s2 * s3 + c3 * c1, c1 * s2 * s3 - c3 * s1], [-s2, s1 * c2, c1 * c2]]) A = ReferenceFrame('A') B = ReferenceFrame('B') B.orient(A, 'DCM', dcm) AwB = B.ang_vel_in(A) alpha2 = s3 * c2 * q1.diff() + c3 * q2.diff() beta2 = s1 * c2 * q3.diff() + c1 * q2.diff() assert simplify(AwB.dot(A.y) - alpha2) == 0 assert simplify(AwB.dot(B.y) - beta2) == 0
def test_w_diff_dcm1(): # Ref: # Dynamics Theory and Applications, Kane 1985 # Sec. 2.1 ANGULAR VELOCITY A = ReferenceFrame('A') B = ReferenceFrame('B') c11, c12, c13 = dynamicsymbols('C11 C12 C13') c21, c22, c23 = dynamicsymbols('C21 C22 C23') c31, c32, c33 = dynamicsymbols('C31 C32 C33') c11d, c12d, c13d = dynamicsymbols('C11 C12 C13', level=1) c21d, c22d, c23d = dynamicsymbols('C21 C22 C23', level=1) c31d, c32d, c33d = dynamicsymbols('C31 C32 C33', level=1) DCM = Matrix([[c11, c12, c13], [c21, c22, c23], [c31, c32, c33]]) B.orient(A, 'DCM', DCM) b1a = (B.x).express(A) b2a = (B.y).express(A) b3a = (B.z).express(A) # Equation (2.1.1) B.set_ang_vel( A, B.x * (dot((b3a).dt(A), B.y)) + B.y * (dot( (b1a).dt(A), B.z)) + B.z * (dot((b2a).dt(A), B.x))) # Equation (2.1.21) expr = ((c12 * c13d + c22 * c23d + c32 * c33d) * B.x + (c13 * c11d + c23 * c21d + c33 * c31d) * B.y + (c11 * c12d + c21 * c22d + c31 * c32d) * B.z) assert B.ang_vel_in(A) - expr == 0
def test_auto_point_vel_connected_frames(): t = dynamicsymbols._t q, q1, q2, u = dynamicsymbols('q q1 q2 u') N = ReferenceFrame('N') B = ReferenceFrame('B') O = Point('O') O.set_vel(N, u * N.x) P = Point('P') P.set_pos(O, q1 * N.x + q2 * B.y) raises(ValueError, lambda: P.vel(N)) N.orient(B, 'Axis', (q, B.x)) assert P.vel( N) == (u + q1.diff(t)) * N.x + q2.diff(t) * B.y - q2 * q.diff(t) * B.z
def test_w_diff_dcm(): a = ReferenceFrame('a') b = ReferenceFrame('b') c11, c12, c13, c21, c22, c23, c31, c32, c33 = dynamicsymbols('c11 c12 c13 c21 c22 c23 c31 c32 c33') c11d, c12d, c13d, c21d, c22d, c23d, c31d, c32d, c33d = dynamicsymbols('c11 c12 c13 c21 c22 c23 c31 c32 c33', 1) b.orient(a, 'DCM', Matrix([c11,c12,c13,c21,c22,c23,c31,c32,c33]).reshape(3, 3)) b1a=(b.x).express(a) b2a=(b.y).express(a) b3a=(b.z).express(a) b.set_ang_vel(a, b.x*(dot((b3a).dt(a), b.y)) + b.y*(dot((b1a).dt(a), b.z)) + b.z*(dot((b2a).dt(a), b.x))) expr = ((c12*c13d + c22*c23d + c32*c33d)*b.x + (c13*c11d + c23*c21d + c33*c31d)*b.y + (c11*c12d + c21*c22d + c31*c32d)*b.z) assert b.ang_vel_in(a) - expr == 0
def test_issue_11498(): A = ReferenceFrame('A') B = ReferenceFrame('B') # Identity transformation A.orient(B, 'DCM', eye(3)) assert A.dcm(B) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) assert B.dcm(A) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # x -> y # y -> -z # z -> -x A.orient(B, 'DCM', Matrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])) assert B.dcm(A) == Matrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]) assert A.dcm(B) == Matrix([[0, 0, -1], [1, 0, 0], [0, -1, 0]]) assert B.dcm(A).T == A.dcm(B)
def test_issue_11498(): A = ReferenceFrame('A') B = ReferenceFrame('B') # Identity transformation A.orient(B, 'DCM', eye(3)) assert A.dcm(B) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) assert B.dcm(A) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # x -> y # y -> -z # z -> -x A.orient(B, 'DCM', Matrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])) assert B.dcm(A) == Matrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]) assert A.dcm(B) == Matrix([[0, 0, -1], [1, 0, 0], [0, -1, 0]]) assert B.dcm(A).T == A.dcm(B)
def test_issue_11503(): A = ReferenceFrame("A") B = A.orientnew("B", "Axis", [35, A.y]) C = ReferenceFrame("C") A.orient(C, "Axis", [70, C.z])
# Funções das Bibliotecas Utilizadas from sympy import symbols, trigsimp, pprint from sympy.physics.mechanics import dynamicsymbols from sympy.physics.vector import ReferenceFrame, Vector from sympy.physics.vector import time_derivative # Variáveis Simbólicas THETA_1, THETA_2 = dynamicsymbols('theta_1 theta_2') L_1, L_2 = symbols('l_1 l_2', positive=True) # Referenciais # Referencial Parado B0 = ReferenceFrame('B0') B1 = ReferenceFrame('B1') # Referencial móvel: theta_1 em relação a B0.z B1.orient(B0, 'Axis', [THETA_1, B0.z]) B2 = ReferenceFrame('B2') # Referencial móvel: theta_2 em relação a B1.z B2.orient(B1, 'Axis', [THETA_2, B1.z]) # Vetores Posição entre os Pontos # Vetor Nulo B0_R_OA = Vector(0) # Vetor que liga os pontos A e B expresso no referencial móvel B1 B1_R_AB = L_1 * B1.x # Vetor que liga os pontos B e C expresso no referencial móvel B2 B2_R_BC = L_2 * B2.x # Cinemática do ponto A em relação ao referencial B0 R_A = B0_R_OA V_A = time_derivative(R_A, B0)
def test_reference_frame(): raises(TypeError, lambda: ReferenceFrame(0)) raises(TypeError, lambda: ReferenceFrame('N', 0)) raises(ValueError, lambda: ReferenceFrame('N', [0, 1])) raises(TypeError, lambda: ReferenceFrame('N', [0, 1, 2])) raises(TypeError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], 0)) raises(ValueError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], [0, 1])) raises(TypeError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], [0, 1, 2])) raises(TypeError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], ['a', 'b', 'c'], 0)) raises( ValueError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], ['a', 'b', 'c'], [0, 1])) raises( TypeError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], ['a', 'b', 'c'], [0, 1, 2])) N = ReferenceFrame('N') assert N[0] == CoordinateSym('N_x', N, 0) assert N[1] == CoordinateSym('N_y', N, 1) assert N[2] == CoordinateSym('N_z', N, 2) raises(ValueError, lambda: N[3]) N = ReferenceFrame('N', ['a', 'b', 'c']) assert N['a'] == N.x assert N['b'] == N.y assert N['c'] == N.z raises(ValueError, lambda: N['d']) assert str(N) == 'N' A = ReferenceFrame('A') B = ReferenceFrame('B') q0, q1, q2, q3 = symbols('q0 q1 q2 q3') raises(TypeError, lambda: A.orient(B, 'DCM', 0)) raises(TypeError, lambda: B.orient(N, 'Space', [q1, q2, q3], '222')) raises(TypeError, lambda: B.orient(N, 'Axis', [q1, N.x + 2 * N.y], '222')) raises(TypeError, lambda: B.orient(N, 'Axis', q1)) raises(IndexError, lambda: B.orient(N, 'Axis', [q1])) raises(TypeError, lambda: B.orient(N, 'Quaternion', [q0, q1, q2, q3], '222')) raises(TypeError, lambda: B.orient(N, 'Quaternion', q0)) raises(TypeError, lambda: B.orient(N, 'Quaternion', [q0, q1, q2])) raises(NotImplementedError, lambda: B.orient(N, 'Foo', [q0, q1, q2])) raises(TypeError, lambda: B.orient(N, 'Body', [q1, q2], '232')) raises(TypeError, lambda: B.orient(N, 'Space', [q1, q2], '232')) N.set_ang_acc(B, 0) assert N.ang_acc_in(B) == Vector(0) N.set_ang_vel(B, 0) assert N.ang_vel_in(B) == Vector(0)
def test_issue_11503(): A = ReferenceFrame("A") A.orientnew("B", "Axis", [35, A.y]) C = ReferenceFrame("C") A.orient(C, "Axis", [70, C.z])
def test_reference_frame(): raises(TypeError, lambda: ReferenceFrame(0)) raises(TypeError, lambda: ReferenceFrame('N', 0)) raises(ValueError, lambda: ReferenceFrame('N', [0, 1])) raises(TypeError, lambda: ReferenceFrame('N', [0, 1, 2])) raises(TypeError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], 0)) raises(ValueError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], [0, 1])) raises(TypeError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], [0, 1, 2])) raises(TypeError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], ['a', 'b', 'c'], 0)) raises(ValueError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], ['a', 'b', 'c'], [0, 1])) raises(TypeError, lambda: ReferenceFrame('N', ['a', 'b', 'c'], ['a', 'b', 'c'], [0, 1, 2])) N = ReferenceFrame('N') assert N[0] == CoordinateSym('N_x', N, 0) assert N[1] == CoordinateSym('N_y', N, 1) assert N[2] == CoordinateSym('N_z', N, 2) raises(ValueError, lambda: N[3]) N = ReferenceFrame('N', ['a', 'b', 'c']) assert N['a'] == N.x assert N['b'] == N.y assert N['c'] == N.z raises(ValueError, lambda: N['d']) assert str(N) == 'N' A = ReferenceFrame('A') B = ReferenceFrame('B') q0, q1, q2, q3 = symbols('q0 q1 q2 q3') raises(TypeError, lambda: A.orient(B, 'DCM', 0)) raises(TypeError, lambda: B.orient(N, 'Space', [q1, q2, q3], '222')) raises(TypeError, lambda: B.orient(N, 'Axis', [q1, N.x + 2 * N.y], '222')) raises(TypeError, lambda: B.orient(N, 'Axis', q1)) raises(TypeError, lambda: B.orient(N, 'Axis', [q1])) raises(TypeError, lambda: B.orient(N, 'Quaternion', [q0, q1, q2, q3], '222')) raises(TypeError, lambda: B.orient(N, 'Quaternion', q0)) raises(TypeError, lambda: B.orient(N, 'Quaternion', [q0, q1, q2])) raises(NotImplementedError, lambda: B.orient(N, 'Foo', [q0, q1, q2])) raises(TypeError, lambda: B.orient(N, 'Body', [q1, q2], '232')) raises(TypeError, lambda: B.orient(N, 'Space', [q1, q2], '232')) N.set_ang_acc(B, 0) assert N.ang_acc_in(B) == Vector(0) N.set_ang_vel(B, 0) assert N.ang_vel_in(B) == Vector(0)
# Funções das Bibliotecas Utilizadas from sympy import symbols, trigsimp, pprint from sympy.physics.mechanics import dynamicsymbols from sympy.physics.vector import ReferenceFrame, Vector from sympy.physics.vector import time_derivative # Variáveis Simbólicas THETA_1, THETA_2, THETA_3 = dynamicsymbols('THETA_1 THETA_2 THETA_3') L_1, L_2 = symbols('L_1 L_2', positive=True) # Referenciais # Referencial Parado B0 = ReferenceFrame('B0') # Referencial móvel: THETA_1 em relação a B0.y B1 = ReferenceFrame('B1') B1.orient(B0, 'Axis', [THETA_1, B0.y]) # Referencial móvel: THETA_2 em relação a B1.z B2 = ReferenceFrame('B2') B2.orient(B1, 'Axis', [THETA_2, B1.z]) # Referencial móvel: THETA_3 em relação a B2.z B3 = ReferenceFrame('B3') B3.orient(B2, 'Axis', [THETA_3, B2.z]) # Vetores Posição entre os Pontos # Vetor Nulo B0_R_OA = Vector(0) # Vetor que liga os pontos A e B expresso no referencial móvel B2 B2_R_AB = L_1 * B2.x # Vetor que liga os pontos B e C expresso no referencial óel B3 B3_R_BC = L_2 * B3.x
# Funções das Bibliotecas Utilizadas from sympy import symbols, trigsimp, latex, pprint from sympy.physics.mechanics import dynamicsymbols from sympy.physics.vector import ReferenceFrame, Vector from sympy.physics.vector import time_derivative # Variáveis Simbólicas theta_1, theta_2, theta_3 = dynamicsymbols('theta_1 theta_2 theta_3') l_1, l_2 = symbols('l_1 l_2', positive=True) # Referenciais B0 = ReferenceFrame('B0') # Referencial Parado B1 = ReferenceFrame('B1') B1.orient(B0, 'Axis', [theta_1, B0.y]) # Referencial móvel: theta_1 em relação a B0.y B2 = ReferenceFrame('B2') B2.orient(B1, 'Axis', [theta_2, B1.z]) # Referencial móvel: theta_2 em relação a B1.z B3 = ReferenceFrame('B3') B3.orient(B2, 'Axis', [theta_3, B2.z]) # Referencial móvel: theta_3 em relação a B2.z # Vetores Posição entre os Pontos B0_r_OA = Vector(0) # Vetor Nulo B2_r_AB = l_1 * B2.x # Vetor que liga os pontos A e B expresso no referencial móvel B2 B3_r_BC = l_2 * B3.x # Vetor que liga os pontos B e C expresso no referencial móvel B3 # Cinemática do ponto A em relação ao referencial B0 r_A = B0_r_OA v_A = time_derivative(r_A, B0) a_A = time_derivative(v_A, B0)
def create_body_frame(inertial_frame, p0, pT): body_frame = ReferenceFrame("A") dcm = align_x_to_vector_dcm(inertial_frame, pT - p0) body_frame.orient(inertial_frame, "DCM", dcm) return body_frame
# Funções das Bibliotecas Utilizadas from sympy import symbols, trigsimp, latex, pprint from sympy.physics.mechanics import dynamicsymbols from sympy.physics.vector import ReferenceFrame, Vector from sympy.physics.vector import time_derivative # Variáveis Simbólicas theta_1, theta_2 = dynamicsymbols('theta_1 theta_2') l_1, l_2, r_1, r_2 = symbols('l_1 l_2 r_1 r_2', positive = True) # Referenciais B0 = ReferenceFrame('B0') # Referencial Parado B1 = ReferenceFrame('B1') B1.orient(B0, 'Axis', [theta_1, B0.z]) # Referencial móvel: theta_1 em relação a B0.z B2 = ReferenceFrame('B2') B2.orient(B1, 'Axis', [theta_2, B1.z]) # Referencial móvel: theta_2 em relação a B1.z # Vetores Posição entre os Pontos B0_r_OA = Vector(0) # Vetor Nulo B1_r_AB = l_1 * B1.x # Vetor que liga os pontos A e B expresso no referencial móvel B1 B2_r_BC = l_2 * B2.x # Vetor que liga os pontos B e C expresso no referencial móvel B2 # Cinemática do ponto A em relação ao referencial B0 r_A = B0_r_OA v_A = time_derivative(r_A, B0) a_A = time_derivative(v_A, B0) # Cinemática do ponto B em relação ao referencial B0 r_B = r_A + B1_r_AB.express(B0) v_B = time_derivative(r_B, B0)
def test_reference_frame(): raises(TypeError, lambda: ReferenceFrame(0)) raises(TypeError, lambda: ReferenceFrame("N", 0)) raises(ValueError, lambda: ReferenceFrame("N", [0, 1])) raises(TypeError, lambda: ReferenceFrame("N", [0, 1, 2])) raises(TypeError, lambda: ReferenceFrame("N", ["a", "b", "c"], 0)) raises(ValueError, lambda: ReferenceFrame("N", ["a", "b", "c"], [0, 1])) raises(TypeError, lambda: ReferenceFrame("N", ["a", "b", "c"], [0, 1, 2])) raises(TypeError, lambda: ReferenceFrame("N", ["a", "b", "c"], ["a", "b", "c"], 0)) raises( ValueError, lambda: ReferenceFrame("N", ["a", "b", "c"], ["a", "b", "c"], [0, 1]), ) raises( TypeError, lambda: ReferenceFrame("N", ["a", "b", "c"], ["a", "b", "c"], [0, 1, 2]), ) N = ReferenceFrame("N") assert N[0] == CoordinateSym("N_x", N, 0) assert N[1] == CoordinateSym("N_y", N, 1) assert N[2] == CoordinateSym("N_z", N, 2) raises(ValueError, lambda: N[3]) N = ReferenceFrame("N", ["a", "b", "c"]) assert N["a"] == N.x assert N["b"] == N.y assert N["c"] == N.z raises(ValueError, lambda: N["d"]) assert str(N) == "N" A = ReferenceFrame("A") B = ReferenceFrame("B") q0, q1, q2, q3 = symbols("q0 q1 q2 q3") raises(TypeError, lambda: A.orient(B, "DCM", 0)) raises(TypeError, lambda: B.orient(N, "Space", [q1, q2, q3], "222")) raises(TypeError, lambda: B.orient(N, "Axis", [q1, N.x + 2 * N.y], "222")) raises(TypeError, lambda: B.orient(N, "Axis", q1)) raises(TypeError, lambda: B.orient(N, "Axis", [q1])) raises(TypeError, lambda: B.orient(N, "Quaternion", [q0, q1, q2, q3], "222")) raises(TypeError, lambda: B.orient(N, "Quaternion", q0)) raises(TypeError, lambda: B.orient(N, "Quaternion", [q0, q1, q2])) raises(NotImplementedError, lambda: B.orient(N, "Foo", [q0, q1, q2])) raises(TypeError, lambda: B.orient(N, "Body", [q1, q2], "232")) raises(TypeError, lambda: B.orient(N, "Space", [q1, q2], "232")) N.set_ang_acc(B, 0) assert N.ang_acc_in(B) == Vector(0) N.set_ang_vel(B, 0) assert N.ang_vel_in(B) == Vector(0)