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_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_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]])