def test_coordinate_vars():
"""Tests the coordinate variables functionality"""
A = ReferenceFrame('A')
assert CoordinateSym('Ax', A, 0) == A[0]
assert CoordinateSym('Ax', A, 1) == A[1]
assert CoordinateSym('Ax', A, 2) == A[2]
raises(ValueError, lambda: CoordinateSym('Ax', A, 3))
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
assert isinstance(A[0], CoordinateSym) and \
isinstance(A[0], CoordinateSym) and \
isinstance(A[0], CoordinateSym)
assert A.variable_map(A) == {A[0]: A[0], A[1]: A[1], A[2]: A[2]}
assert A[0].frame == A
B = A.orientnew('B', 'Axis', [q, A.z])
assert B.variable_map(A) == {
B[2]: A[2],
B[1]: -A[0] * sin(q) + A[1] * cos(q),
B[0]: A[0] * cos(q) + A[1] * sin(q)
}
assert A.variable_map(B) == {
A[0]: B[0] * cos(q) - B[1] * sin(q),
A[1]: B[0] * sin(q) + B[1] * cos(q),
A[2]: B[2]
}
assert time_derivative(B[0], A) == -A[0] * sin(q) * qd + A[1] * cos(q) * qd
assert time_derivative(B[1], A) == -A[0] * cos(q) * qd - A[1] * sin(q) * qd
assert time_derivative(B[2], A) == 0
assert express(B[0], A, variables=True) == A[0] * cos(q) + A[1] * sin(q)
assert express(B[1], A, variables=True) == -A[0] * sin(q) + A[1] * cos(q)
assert express(B[2], A, variables=True) == A[2]
assert time_derivative(A[0] * A.x + A[1] * A.y + A[2] * A.z,
B) == A[1] * qd * A.x - A[0] * qd * A.y
assert time_derivative(B[0] * B.x + B[1] * B.y + B[2] * B.z,
A) == -B[1] * qd * B.x + B[0] * qd * B.y
assert express(B[0]*B[1]*B[2], A, variables=True) == \
A[2]*(-A[0]*sin(q) + A[1]*cos(q))*(A[0]*cos(q) + A[1]*sin(q))
assert (time_derivative(B[0] * B[1] * B[2], A) -
(A[2] * (-A[0]**2 * cos(2 * q) - 2 * A[0] * A[1] * sin(2 * q) +
A[1]**2 * cos(2 * q)) * qd)).trigsimp() == 0
assert express(B[0]*B.x + B[1]*B.y + B[2]*B.z, A) == \
(B[0]*cos(q) - B[1]*sin(q))*A.x + (B[0]*sin(q) + \
B[1]*cos(q))*A.y + B[2]*A.z
assert express(B[0]*B.x + B[1]*B.y + B[2]*B.z, A, variables=True) == \
A[0]*A.x + A[1]*A.y + A[2]*A.z
assert express(A[0]*A.x + A[1]*A.y + A[2]*A.z, B) == \
(A[0]*cos(q) + A[1]*sin(q))*B.x + \
(-A[0]*sin(q) + A[1]*cos(q))*B.y + A[2]*B.z
assert express(A[0]*A.x + A[1]*A.y + A[2]*A.z, B, variables=True) == \
B[0]*B.x + B[1]*B.y + B[2]*B.z
N = B.orientnew('N', 'Axis', [-q, B.z])
assert N.variable_map(A) == {N[0]: A[0], N[2]: A[2], N[1]: A[1]}
C = A.orientnew('C', 'Axis', [q, A.x + A.y + A.z])
mapping = A.variable_map(C)
assert mapping[A[0]] == 2*C[0]*cos(q)/3 + C[0]/3 - 2*C[1]*sin(q + pi/6)/3 +\
C[1]/3 - 2*C[2]*cos(q + pi/3)/3 + C[2]/3
assert mapping[A[1]] == -2*C[0]*cos(q + pi/3)/3 + \
C[0]/3 + 2*C[1]*cos(q)/3 + C[1]/3 - 2*C[2]*sin(q + pi/6)/3 + C[2]/3
assert mapping[A[2]] == -2*C[0]*sin(q + pi/6)/3 + C[0]/3 - \
2*C[1]*cos(q + pi/3)/3 + C[1]/3 + 2*C[2]*cos(q)/3 + C[2]/3
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)
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_coordinate_vars():
"""Tests the coordinate variables functionality"""
A = ReferenceFrame("A")
assert CoordinateSym("Ax", A, 0) == A[0]
assert CoordinateSym("Ax", A, 1) == A[1]
assert CoordinateSym("Ax", A, 2) == A[2]
raises(ValueError, lambda: CoordinateSym("Ax", A, 3))
q = dynamicsymbols("q")
qd = dynamicsymbols("q", 1)
assert (isinstance(A[0], CoordinateSym)
and isinstance(A[0], CoordinateSym)
and isinstance(A[0], CoordinateSym))
assert A.variable_map(A) == {A[0]: A[0], A[1]: A[1], A[2]: A[2]}
assert A[0].frame == A
B = A.orientnew("B", "Axis", [q, A.z])
assert B.variable_map(A) == {
B[2]: A[2],
B[1]: -A[0] * sin(q) + A[1] * cos(q),
B[0]: A[0] * cos(q) + A[1] * sin(q),
}
assert A.variable_map(B) == {
A[0]: B[0] * cos(q) - B[1] * sin(q),
A[1]: B[0] * sin(q) + B[1] * cos(q),
A[2]: B[2],
}
assert time_derivative(B[0], A) == -A[0] * sin(q) * qd + A[1] * cos(q) * qd
assert time_derivative(B[1], A) == -A[0] * cos(q) * qd - A[1] * sin(q) * qd
assert time_derivative(B[2], A) == 0
assert express(B[0], A, variables=True) == A[0] * cos(q) + A[1] * sin(q)
assert express(B[1], A, variables=True) == -A[0] * sin(q) + A[1] * cos(q)
assert express(B[2], A, variables=True) == A[2]
assert (time_derivative(A[0] * A.x + A[1] * A.y + A[2] * A.z,
B) == A[1] * qd * A.x - A[0] * qd * A.y)
assert (time_derivative(B[0] * B.x + B[1] * B.y + B[2] * B.z,
A) == -B[1] * qd * B.x + B[0] * qd * B.y)
assert express(B[0] * B[1] * B[2], A, variables=True) == A[2] * (
-A[0] * sin(q) + A[1] * cos(q)) * (A[0] * cos(q) + A[1] * sin(q))
assert (time_derivative(B[0] * B[1] * B[2], A) -
(A[2] * (-A[0]**2 * cos(2 * q) - 2 * A[0] * A[1] * sin(2 * q) +
A[1]**2 * cos(2 * q)) * qd)).trigsimp() == 0
assert (express(B[0] * B.x + B[1] * B.y + B[2] * B.z,
A) == (B[0] * cos(q) - B[1] * sin(q)) * A.x +
(B[0] * sin(q) + B[1] * cos(q)) * A.y + B[2] * A.z)
assert (express(B[0] * B.x + B[1] * B.y + B[2] * B.z, A,
variables=True) == A[0] * A.x + A[1] * A.y + A[2] * A.z)
assert (express(A[0] * A.x + A[1] * A.y + A[2] * A.z,
B) == (A[0] * cos(q) + A[1] * sin(q)) * B.x +
(-A[0] * sin(q) + A[1] * cos(q)) * B.y + A[2] * B.z)
assert (express(A[0] * A.x + A[1] * A.y + A[2] * A.z, B,
variables=True) == B[0] * B.x + B[1] * B.y + B[2] * B.z)
N = B.orientnew("N", "Axis", [-q, B.z])
assert N.variable_map(A) == {N[0]: A[0], N[2]: A[2], N[1]: A[1]}
C = A.orientnew("C", "Axis", [q, A.x + A.y + A.z])
mapping = A.variable_map(C)
assert (mapping[A[0]] == 2 * C[0] * cos(q) / 3 + C[0] / 3 -
2 * C[1] * sin(q + pi / 6) / 3 + C[1] / 3 -
2 * C[2] * cos(q + pi / 3) / 3 + C[2] / 3)
assert (mapping[A[1]] == -2 * C[0] * cos(q + pi / 3) / 3 + C[0] / 3 +
2 * C[1] * cos(q) / 3 + C[1] / 3 - 2 * C[2] * sin(q + pi / 6) / 3 +
C[2] / 3)
assert (mapping[A[2]] == -2 * C[0] * sin(q + pi / 6) / 3 + C[0] / 3 -
2 * C[1] * cos(q + pi / 3) / 3 + C[1] / 3 + 2 * C[2] * cos(q) / 3 +
C[2] / 3)