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)
