def test_pin_joint_chaos_pendulum():
mA, mB, lA, lB, h = symbols('mA, mB, lA, lB, h')
theta, phi, omega, alpha = dynamicsymbols('theta phi omega alpha')
N = ReferenceFrame('N')
A = ReferenceFrame('A')
B = ReferenceFrame('B')
lA = (lB - h / 2) / 2
lC = (lB / 2 + h / 4)
rod = Body('rod', frame=A, mass=mA)
plate = Body('plate', mass=mB, frame=B)
C = Body('C', frame=N)
J1 = PinJoint('J1',
C,
rod,
coordinates=theta,
speeds=omega,
child_joint_pos=lA * A.z,
parent_axis=N.y,
child_axis=A.y)
J2 = PinJoint('J2',
rod,
plate,
coordinates=phi,
speeds=alpha,
parent_joint_pos=lC * A.z,
parent_axis=A.z,
child_axis=B.z)
# Check orientation
assert A.dcm(N) == Matrix([[cos(theta), 0, -sin(theta)], [0, 1, 0],
[sin(theta), 0, cos(theta)]])
assert A.dcm(B) == Matrix([[cos(phi), -sin(phi), 0],
[sin(phi), cos(phi), 0], [0, 0, 1]])
assert B.dcm(N) == Matrix(
[[cos(phi) * cos(theta),
sin(phi), -sin(theta) * cos(phi)],
[-sin(phi) * cos(theta),
cos(phi), sin(phi) * sin(theta)], [sin(theta), 0,
cos(theta)]])
# Check Angular Velocity
assert A.ang_vel_in(N) == omega * N.y
assert A.ang_vel_in(B) == -alpha * A.z
assert N.ang_vel_in(B) == -omega * N.y - alpha * A.z
# Check kde
assert J1.kdes == [omega - theta.diff(t)]
assert J2.kdes == [alpha - phi.diff(t)]
# Check pos of masscenters
assert C.masscenter.pos_from(rod.masscenter) == lA * A.z
assert rod.masscenter.pos_from(plate.masscenter) == -lC * A.z
# Check Linear Velocities
assert rod.masscenter.vel(N) == (h / 4 - lB / 2) * omega * A.x
assert plate.masscenter.vel(N) == ((h / 4 - lB / 2) * omega +
(h / 4 + lB / 2) * omega) * A.x
def test_orient_axis():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
A.orient_axis(B, -B.x, 1)
A1 = A.dcm(B)
A.orient_axis(B, B.x, -1)
A2 = A.dcm(B)
A.orient_axis(B, 1, -B.x)
A3 = A.dcm(B)
assert A1 == A2
assert A2 == A3
raises(TypeError, lambda: A.orient_axis(B, 1, 1))
def test_pin_joint_double_pendulum():
q1, q2 = dynamicsymbols('q1 q2')
u1, u2 = dynamicsymbols('u1 u2')
m, l = symbols('m l')
N = ReferenceFrame('N')
A = ReferenceFrame('A')
B = ReferenceFrame('B')
C = Body('C', frame=N) # ceiling
PartP = Body('P', frame=A, mass=m)
PartR = Body('R', frame=B, mass=m)
J1 = PinJoint('J1',
C,
PartP,
speeds=u1,
coordinates=q1,
child_joint_pos=-l * A.x,
parent_axis=C.frame.z,
child_axis=PartP.frame.z)
J2 = PinJoint('J2',
PartP,
PartR,
speeds=u2,
coordinates=q2,
child_joint_pos=-l * B.x,
parent_axis=PartP.frame.z,
child_axis=PartR.frame.z)
# Check orientation
assert N.dcm(A) == Matrix([[cos(q1), -sin(q1), 0], [sin(q1),
cos(q1), 0], [0, 0,
1]])
assert A.dcm(B) == Matrix([[cos(q2), -sin(q2), 0], [sin(q2),
cos(q2), 0], [0, 0,
1]])
assert _simplify_matrix(N.dcm(B)) == Matrix(
[[cos(q1 + q2), -sin(q1 + q2), 0], [sin(q1 + q2),
cos(q1 + q2), 0], [0, 0, 1]])
# Check Angular Velocity
assert A.ang_vel_in(N) == u1 * N.z
assert B.ang_vel_in(A) == u2 * A.z
assert B.ang_vel_in(N) == u1 * N.z + u2 * A.z
# Check kde
assert J1.kdes == [u1 - q1.diff(t)]
assert J2.kdes == [u2 - q2.diff(t)]
# Check Linear Velocity
assert PartP.masscenter.vel(N) == l * u1 * A.y
assert PartR.masscenter.vel(A) == l * u2 * B.y
assert PartR.masscenter.vel(N) == l * u1 * A.y + l * (u1 + u2) * B.y
def test_dcm():
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = A.orientnew('B', 'Axis', [q2, A.x])
C = B.orientnew('C', 'Axis', [q3, B.y])
D = N.orientnew('D', 'Axis', [q4, N.y])
E = N.orientnew('E', 'Space', [q1, q2, q3], '123')
assert N.dcm(C) == Matrix([
[- sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), - sin(q1) *
cos(q2), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1)], [sin(q1) *
cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) *
sin(q3) - sin(q2) * cos(q1) * cos(q3)], [- sin(q3) * cos(q2), sin(q2),
cos(q2) * cos(q3)]])
# This is a little touchy. Is it ok to use simplify in assert?
test_mat = D.dcm(C) - Matrix(
[[cos(q1) * cos(q3) * cos(q4) - sin(q3) * (- sin(q4) * cos(q2) +
sin(q1) * sin(q2) * cos(q4)), - sin(q2) * sin(q4) - sin(q1) *
cos(q2) * cos(q4), sin(q3) * cos(q1) * cos(q4) + cos(q3) * (- sin(q4) *
cos(q2) + sin(q1) * sin(q2) * cos(q4))], [sin(q1) * cos(q3) +
sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) * sin(q3) -
sin(q2) * cos(q1) * cos(q3)], [sin(q4) * cos(q1) * cos(q3) -
sin(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)), sin(q2) *
cos(q4) - sin(q1) * sin(q4) * cos(q2), sin(q3) * sin(q4) * cos(q1) +
cos(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4))]])
assert test_mat.expand() == zeros(3, 3)
assert E.dcm(N) == Matrix(
[[cos(q2)*cos(q3), sin(q3)*cos(q2), -sin(q2)],
[sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) +
cos(q1)*cos(q3), sin(q1)*cos(q2)], [sin(q1)*sin(q3) +
sin(q2)*cos(q1)*cos(q3), - sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1),
cos(q1)*cos(q2)]])
def test_dcm():
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = A.orientnew('B', 'Axis', [q2, A.x])
C = B.orientnew('C', 'Axis', [q3, B.y])
D = N.orientnew('D', 'Axis', [q4, N.y])
E = N.orientnew('E', 'Space', [q1, q2, q3], '123')
assert N.dcm(C) == Matrix([
[- sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), - sin(q1) *
cos(q2), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1)], [sin(q1) *
cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) *
sin(q3) - sin(q2) * cos(q1) * cos(q3)], [- sin(q3) * cos(q2), sin(q2),
cos(q2) * cos(q3)]])
# This is a little touchy. Is it ok to use simplify in assert?
test_mat = D.dcm(C) - Matrix(
[[cos(q1) * cos(q3) * cos(q4) - sin(q3) * (- sin(q4) * cos(q2) +
sin(q1) * sin(q2) * cos(q4)), - sin(q2) * sin(q4) - sin(q1) *
cos(q2) * cos(q4), sin(q3) * cos(q1) * cos(q4) + cos(q3) * (- sin(q4) *
cos(q2) + sin(q1) * sin(q2) * cos(q4))], [sin(q1) * cos(q3) +
sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) * sin(q3) -
sin(q2) * cos(q1) * cos(q3)], [sin(q4) * cos(q1) * cos(q3) -
sin(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)), sin(q2) *
cos(q4) - sin(q1) * sin(q4) * cos(q2), sin(q3) * sin(q4) * cos(q1) +
cos(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4))]])
assert test_mat.expand() == zeros(3, 3)
assert E.dcm(N) == Matrix(
[[cos(q2)*cos(q3), sin(q3)*cos(q2), -sin(q2)],
[sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) +
cos(q1)*cos(q3), sin(q1)*cos(q2)], [sin(q1)*sin(q3) +
sin(q2)*cos(q1)*cos(q3), - sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1),
cos(q1)*cos(q2)]])
def test_orient_body():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
B.orient_body_fixed(A, (1, 1, 0), 'XYX')
assert B.dcm(A) == Matrix([[cos(1), sin(1)**2, -sin(1) * cos(1)],
[0, cos(1), sin(1)],
[sin(1), -sin(1) * cos(1),
cos(1)**2]])
def test_dcm():
q1, q2, q3, q4 = dynamicsymbols("q1 q2 q3 q4")
N = ReferenceFrame("N")
A = N.orientnew("A", "Axis", [q1, N.z])
B = A.orientnew("B", "Axis", [q2, A.x])
C = B.orientnew("C", "Axis", [q3, B.y])
D = N.orientnew("D", "Axis", [q4, N.y])
E = N.orientnew("E", "Space", [q1, q2, q3], "123")
assert N.dcm(C) == Matrix([
[
-sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
-sin(q1) * cos(q2),
sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1),
],
[
sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
cos(q1) * cos(q2),
sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3),
],
[-sin(q3) * cos(q2), sin(q2),
cos(q2) * cos(q3)],
])
# This is a little touchy. Is it ok to use simplify in assert?
test_mat = D.dcm(C) - Matrix([
[
cos(q1) * cos(q3) * cos(q4) - sin(q3) *
(-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)),
-sin(q2) * sin(q4) - sin(q1) * cos(q2) * cos(q4),
sin(q3) * cos(q1) * cos(q4) + cos(q3) *
(-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)),
],
[
sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
cos(q1) * cos(q2),
sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3),
],
[
sin(q4) * cos(q1) * cos(q3) - sin(q3) *
(cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)),
sin(q2) * cos(q4) - sin(q1) * sin(q4) * cos(q2),
sin(q3) * sin(q4) * cos(q1) + cos(q3) *
(cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)),
],
])
assert test_mat.expand() == zeros(3, 3)
assert E.dcm(N) == Matrix([
[cos(q2) * cos(q3), sin(q3) * cos(q2), -sin(q2)],
[
sin(q1) * sin(q2) * cos(q3) - sin(q3) * cos(q1),
sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
sin(q1) * cos(q2),
],
[
sin(q1) * sin(q3) + sin(q2) * cos(q1) * cos(q3),
-sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
cos(q1) * cos(q2),
],
])
def test_dcm_cache_dict():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
C = ReferenceFrame('C')
D = ReferenceFrame('D')
a, b, c = symbols('a b c')
B.orient_axis(A, A.x, a)
C.orient_axis(B, B.x, b)
D.orient_axis(C, C.x, c)
assert D._dcm_dict == {
C: Matrix([[1, 0, 0], [0, cos(c), sin(c)], [0, -sin(c),
cos(c)]])
}
assert C._dcm_dict == {B: Matrix([[1, 0, 0],[0, cos(b), sin(b)],[0, -sin(b), cos(b)]]), \
D: Matrix([[1, 0, 0],[0, cos(c), -sin(c)],[0, sin(c), cos(c)]])}
assert B._dcm_dict == {A: Matrix([[1, 0, 0],[0, cos(a), sin(a)],[0, -sin(a), cos(a)]]), \
C: Matrix([[1, 0, 0],[0, cos(b), -sin(b)],[0, sin(b), cos(b)]])}
assert A._dcm_dict == {
B: Matrix([[1, 0, 0], [0, cos(a), -sin(a)], [0, sin(a),
cos(a)]])
}
assert D._dcm_dict == D._dcm_cache
D.dcm(
A
) # Check calculated dcm relation is stored in _dcm_cache and not in _dcm_dict
assert list(A._dcm_cache.keys()) == [A, B, D]
assert list(D._dcm_cache.keys()) == [C, A]
assert list(A._dcm_dict.keys()) == [B]
assert list(D._dcm_dict.keys()) == [C]
assert A._dcm_dict != A._dcm_cache
A.orient_axis(
B, B.x, b) # _dcm_cache of A is wiped out and new relation is stored.
assert A._dcm_dict == {
B: Matrix([[1, 0, 0], [0, cos(b), sin(b)], [0, -sin(b),
cos(b)]])
}
assert A._dcm_dict == A._dcm_cache
assert B._dcm_dict == {C: Matrix([[1, 0, 0],[0, cos(b), -sin(b)],[0, sin(b), cos(b)]]), \
A: Matrix([[1, 0, 0],[0, cos(b), -sin(b)],[0, sin(b), cos(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_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_orient_quaternion():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
B.orient_quaternion(A, (0, 0, 0, 0))
assert B.dcm(A) == Matrix([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
def test_orient_space():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
B.orient_space_fixed(A, (0, 0, 0), '123')
assert B.dcm(A) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
def test_orient_explicit():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
A.orient_explicit(B, eye(3))
assert A.dcm(B) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
