def __init__(self, frame, q_ind, u_ind, kd_eqs=None, q_dependent=None, configuration_constraints=None, u_dependent=None, velocity_constraints=None, acceleration_constraints=None, u_auxiliary=None): """Please read the online documentation. """ if not q_ind: q_ind = [dynamicsymbols('dummy_q')] kd_eqs = [dynamicsymbols('dummy_kd')] if not isinstance(frame, ReferenceFrame): raise TypeError('An intertial ReferenceFrame must be supplied') self._inertial = frame self._fr = None self._frstar = None self._forcelist = None self._bodylist = None self._initialize_vectors(q_ind, q_dependent, u_ind, u_dependent, u_auxiliary) self._initialize_kindiffeq_matrices(kd_eqs) self._initialize_constraint_matrices(configuration_constraints, velocity_constraints, acceleration_constraints)
def test_point_funcs(): q, q2 = dynamicsymbols('q q2') qd, q2d = dynamicsymbols('q q2', 1) qdd, q2dd = dynamicsymbols('q q2', 2) N = ReferenceFrame('N') B = ReferenceFrame('B') B.set_ang_vel(N, 5 * B.y) O = Point('O') P = O.locatenew('P', q * B.x) assert P.pos_from(O) == q * B.x P.set_vel(B, qd * B.x + q2d * B.y) assert P.vel(B) == qd * B.x + q2d * B.y O.set_vel(N, 0) assert O.vel(N) == 0 assert P.a1pt_theory(O, N, B) == ((-25 * q + qdd) * B.x + (q2dd) * B.y + (-10 * qd) * B.z) B = N.orientnew('B', 'Axis', [q, N.z]) O = Point('O') P = O.locatenew('P', 10 * B.x) O.set_vel(N, 5 * N.x) assert O.vel(N) == 5 * N.x assert P.a2pt_theory(O, N, B) == (-10 * qd**2) * B.x + (10 * qdd) * B.y B.set_ang_vel(N, 5 * B.y) O = Point('O') P = O.locatenew('P', q * B.x) P.set_vel(B, qd * B.x + q2d * B.y) O.set_vel(N, 0) assert P.v1pt_theory(O, N, B) == qd * B.x + q2d * B.y - 5 * q * B.z
def test_point_funcs(): q, q2 = dynamicsymbols("q q2") qd, q2d = dynamicsymbols("q q2", 1) qdd, q2dd = dynamicsymbols("q q2", 2) N = ReferenceFrame("N") B = ReferenceFrame("B") B.set_ang_vel(N, 5 * B.y) O = Point("O") P = O.locatenew("P", q * B.x) assert P.pos_from(O) == q * B.x P.set_vel(B, qd * B.x + q2d * B.y) assert P.vel(B) == qd * B.x + q2d * B.y O.set_vel(N, 0) assert O.vel(N) == 0 assert P.a1pt_theory(O, N, B) == ((-25 * q + qdd) * B.x + (q2dd) * B.y + (-10 * qd) * B.z) B = N.orientnew("B", "Axis", [q, N.z]) O = Point("O") P = O.locatenew("P", 10 * B.x) O.set_vel(N, 5 * N.x) assert O.vel(N) == 5 * N.x assert P.a2pt_theory(O, N, B) == (-10 * qd ** 2) * B.x + (10 * qdd) * B.y B.set_ang_vel(N, 5 * B.y) O = Point("O") P = O.locatenew("P", q * B.x) P.set_vel(B, qd * B.x + q2d * B.y) O.set_vel(N, 0) assert P.v1pt_theory(O, N, B) == qd * B.x + q2d * B.y - 5 * q * B.z
def test_vector_latex(): a, b, c, d, omega = symbols('a, b, c, d, omega') v = (a ** 2 + b / c) * A.x + sqrt(d) * A.y + cos(omega) * A.z assert v._latex() == (r'(a^{2} + \frac{b}{c})\mathbf{\hat{a}_x} + ' r'\sqrt{d}\mathbf{\hat{a}_y} + ' r'\operatorname{cos}\left(\omega\right)' r'\mathbf{\hat{a}_z}') theta, omega, alpha, q = dynamicsymbols('theta, omega, alpha, q') v = theta * A.x + omega * omega * A.y + (q * alpha) * A.z assert v._latex() == (r'\theta\mathbf{\hat{a}_x} + ' r'\omega^{2}\mathbf{\hat{a}_y} + ' r'\alpha q\mathbf{\hat{a}_z}') phi1, phi2, phi3 = dynamicsymbols('phi1, phi2, phi3') theta1, theta2, theta3 = symbols('theta1, theta2, theta3') v = (sin(theta1) * A.x + cos(phi1) * cos(phi2) * A.y + cos(theta1 + phi3) * A.z) assert v._latex() == (r'\operatorname{sin}\left(\theta_{1}\right)' r'\mathbf{\hat{a}_x} + \operatorname{cos}' r'\left(\phi_{1}\right) \operatorname{cos}' r'\left(\phi_{2}\right)\mathbf{\hat{a}_y} + ' r'\operatorname{cos}\left(\theta_{1} + ' r'\phi_{3}\right)\mathbf{\hat{a}_z}') N = ReferenceFrame('N') a, b, c, d, omega = symbols('a, b, c, d, omega') v = (a ** 2 + b / c) * N.x + sqrt(d) * N.y + cos(omega) * N.z expected = (r'(a^{2} + \frac{b}{c})\mathbf{\hat{n}_x} + ' r'\sqrt{d}\mathbf{\hat{n}_y} + ' r'\operatorname{cos}\left(\omega\right)' r'\mathbf{\hat{n}_z}') assert v._latex() == expected lp = VectorLatexPrinter() assert lp.doprint(v) == expected # Try custom unit vectors. N = ReferenceFrame('N', latexs=(r'\hat{i}', r'\hat{j}', r'\hat{k}')) v = (a ** 2 + b / c) * N.x + sqrt(d) * N.y + cos(omega) * N.z expected = (r'(a^{2} + \frac{b}{c})\hat{i} + ' r'\sqrt{d}\hat{j} + ' r'\operatorname{cos}\left(\omega\right)\hat{k}') assert v._latex() == expected
def test_point_a2pt_theorys(): q = dynamicsymbols('q') qd = dynamicsymbols('q', 1) qdd = dynamicsymbols('q', 2) N = ReferenceFrame('N') B = N.orientnew('B', 'Axis', [q, N.z]) O = Point('O') P = O.locatenew('P', 0) O.set_vel(N, 0) assert P.a2pt_theory(O, N, B) == 0 P.set_pos(O, B.x) assert P.a2pt_theory(O, N, B) == (-qd**2) * B.x + (qdd) * B.y
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_point_a2pt_theorys(): q = dynamicsymbols("q") qd = dynamicsymbols("q", 1) qdd = dynamicsymbols("q", 2) N = ReferenceFrame("N") B = N.orientnew("B", "Axis", [q, N.z]) O = Point("O") P = O.locatenew("P", 0) O.set_vel(N, 0) assert P.a2pt_theory(O, N, B) == 0 P.set_pos(O, B.x) assert P.a2pt_theory(O, N, B) == (-qd ** 2) * B.x + (qdd) * B.y
def test_dyadic(): d1 = A.x | A.x d2 = A.y | A.y d3 = A.x | A.y assert d1 * 0 == 0 assert d1 != 0 assert d1 * 2 == 2 * A.x | A.x assert d1 / 2. == 0.5 * d1 assert d1 & (0 * d1) == 0 assert d1 & d2 == 0 assert d1 & A.x == A.x assert d1 ^ A.x == 0 assert d1 ^ A.y == A.x | A.z assert d1 ^ A.z == - A.x | A.y assert d2 ^ A.x == - A.y | A.z assert A.x ^ d1 == 0 assert A.y ^ d1 == - A.z | A.x assert A.z ^ d1 == A.y | A.x assert A.x & d1 == A.x assert A.y & d1 == 0 assert A.y & d2 == A.y assert d1 & d3 == A.x | A.y assert d3 & d1 == 0 assert d1.dt(A) == 0 q = dynamicsymbols('q') qd = dynamicsymbols('q', 1) B = A.orientnew('B', 'Axis', [q, A.z]) assert d1.express(B) == d1.express(B, B) assert d1.express(B) == ((cos(q)**2) * (B.x | B.x) + (-sin(q) * cos(q)) * (B.x | B.y) + (-sin(q) * cos(q)) * (B.y | B.x) + (sin(q)**2) * (B.y | B.y)) assert d1.express(B, A) == (cos(q)) * (B.x | A.x) + (-sin(q)) * (B.y | A.x) assert d1.express(A, B) == (cos(q)) * (A.x | B.x) + (-sin(q)) * (A.x | B.y) assert d1.dt(B) == (-qd) * (A.y | A.x) + (-qd) * (A.x | A.y) assert d1.to_matrix(A) == Matrix([[1, 0, 0], [0, 0, 0], [0, 0, 0]]) assert d1.to_matrix(A, B) == Matrix([[cos(q), -sin(q), 0], [0, 0, 0], [0, 0, 0]]) assert d3.to_matrix(A) == Matrix([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) a, b, c, d, e, f = symbols('a, b, c, d, e, f') v1 = a * A.x + b * A.y + c * A.z v2 = d * A.x + e * A.y + f * A.z d4 = v1.outer(v2) assert d4.to_matrix(A) == Matrix([[a * d, a * e, a * f], [b * d, b * e, b * f], [c * d, c * e, c * f]]) d5 = v1.outer(v1) C = A.orientnew('C', 'Axis', [q, A.x]) for expected, actual in zip(C.dcm(A) * d5.to_matrix(A) * C.dcm(A).T, d5.to_matrix(C)): assert (expected - actual).simplify() == 0
def test_point_v2pt_theorys(): q = dynamicsymbols("q") qd = dynamicsymbols("q", 1) N = ReferenceFrame("N") B = N.orientnew("B", "Axis", [q, N.z]) O = Point("O") P = O.locatenew("P", 0) O.set_vel(N, 0) assert P.v2pt_theory(O, N, B) == 0 P = O.locatenew("P", B.x) assert P.v2pt_theory(O, N, B) == (qd * B.z ^ B.x) O.set_vel(N, N.x) assert P.v2pt_theory(O, N, B) == N.x + qd * B.y
def test_point_v2pt_theorys(): q = dynamicsymbols('q') qd = dynamicsymbols('q', 1) N = ReferenceFrame('N') B = N.orientnew('B', 'Axis', [q, N.z]) O = Point('O') P = O.locatenew('P', 0) O.set_vel(N, 0) assert P.v2pt_theory(O, N, B) == 0 P = O.locatenew('P', B.x) assert P.v2pt_theory(O, N, B) == (qd * B.z ^ B.x) O.set_vel(N, N.x) assert P.v2pt_theory(O, N, B) == N.x + qd * B.y
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_orientnew_respects_input_latexs(): N = ReferenceFrame('N') q1 = dynamicsymbols('q1') A = N.orientnew('a', 'Axis', [q1, N.z]) #build default and alternate latex_vecs: def_latex_vecs = [(r"\mathbf{\hat{%s}_%s}" % (A.name.lower(), A.indices[0])), (r"\mathbf{\hat{%s}_%s}" % (A.name.lower(), A.indices[1])), (r"\mathbf{\hat{%s}_%s}" % (A.name.lower(), A.indices[2]))] name = 'b' indices = [x+'1' for x in N.indices] new_latex_vecs = [(r"\mathbf{\hat{%s}_{%s}}" % (name.lower(), indices[0])), (r"\mathbf{\hat{%s}_{%s}}" % (name.lower(), indices[1])), (r"\mathbf{\hat{%s}_{%s}}" % (name.lower(), indices[2]))] B = N.orientnew(name, 'Axis', [q1, N.z], latexs=new_latex_vecs) assert A.latex_vecs == def_latex_vecs assert B.latex_vecs == new_latex_vecs assert B.indices != indices
def test_point_v1pt_theorys(): q, q2 = dynamicsymbols("q q2") qd, q2d = dynamicsymbols("q q2", 1) qdd, q2dd = dynamicsymbols("q q2", 2) N = ReferenceFrame("N") B = ReferenceFrame("B") B.set_ang_vel(N, qd * B.z) O = Point("O") P = O.locatenew("P", B.x) P.set_vel(B, 0) O.set_vel(N, 0) assert P.v1pt_theory(O, N, B) == qd * B.y O.set_vel(N, N.x) assert P.v1pt_theory(O, N, B) == N.x + qd * B.y P.set_vel(B, B.z) assert P.v1pt_theory(O, N, B) == B.z + N.x + qd * 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 form_lagranges_equations(self): """Method to form Lagrange's equations of motion. Returns a vector of equations of motion using Lagrange's equations of the second kind. """ qds = self._qdots qdd_zero = dict((i, 0) for i in self._qdoubledots) n = len(self.q) # Internally we represent the EOM as four terms: # EOM = term1 - term2 - term3 - term4 = 0 # First term self._term1 = self._L.jacobian(qds) self._term1 = self._term1.diff(dynamicsymbols._t).T # Second term self._term2 = self._L.jacobian(self.q).T # Third term if self.coneqs: coneqs = self.coneqs m = len(coneqs) # Creating the multipliers self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1))) self.lam_coeffs = -coneqs.jacobian(qds) self._term3 = self.lam_coeffs.T * self.lam_vec # Extracting the coeffecients of the qdds from the diff coneqs diffconeqs = coneqs.diff(dynamicsymbols._t) self._m_cd = diffconeqs.jacobian(self._qdoubl
def test_point_a1pt_theorys(): q, q2 = dynamicsymbols("q q2") qd, q2d = dynamicsymbols("q q2", 1) qdd, q2dd = dynamicsymbols("q q2", 2) N = ReferenceFrame("N") B = ReferenceFrame("B") B.set_ang_vel(N, qd * B.z) O = Point("O") P = O.locatenew("P", B.x) P.set_vel(B, 0) O.set_vel(N, 0) assert P.a1pt_theory(O, N, B) == -(qd ** 2) * B.x + qdd * B.y P.set_vel(B, q2d * B.z) assert P.a1pt_theory(O, N, B) == -(qd ** 2) * B.x + qdd * B.y + q2dd * B.z O.set_vel(N, q2d * B.x) assert P.a1pt_theory(O, N, B) == ((q2dd - qd ** 2) * B.x + (q2d * qd + qdd) * B.y + q2dd * B.z)
def test_point_v1pt_theorys(): q, q2 = dynamicsymbols('q q2') qd, q2d = dynamicsymbols('q q2', 1) qdd, q2dd = dynamicsymbols('q q2', 2) N = ReferenceFrame('N') B = ReferenceFrame('B') B.set_ang_vel(N, qd * B.z) O = Point('O') P = O.locatenew('P', B.x) P.set_vel(B, 0) O.set_vel(N, 0) assert P.v1pt_theory(O, N, B) == qd * B.y O.set_vel(N, N.x) assert P.v1pt_theory(O, N, B) == N.x + qd * B.y P.set_vel(B, B.z) assert P.v1pt_theory(O, N, B) == B.z + N.x + qd * B.y
def test_point_a1pt_theorys(): q, q2 = dynamicsymbols('q q2') qd, q2d = dynamicsymbols('q q2', 1) qdd, q2dd = dynamicsymbols('q q2', 2) N = ReferenceFrame('N') B = ReferenceFrame('B') B.set_ang_vel(N, qd * B.z) O = Point('O') P = O.locatenew('P', B.x) P.set_vel(B, 0) O.set_vel(N, 0) assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y P.set_vel(B, q2d * B.z) assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y + q2dd * B.z O.set_vel(N, q2d * B.x) assert P.a1pt_theory(O, N, B) == ((q2dd - qd**2) * B.x + (q2d * qd + qdd) * B.y + q2dd * B.z)
def form_lagranges_equations(self): """Method to form Lagrange's equations of motion. Returns a vector of equations of motion using Lagrange's equations of the second kind. """ qds = self._qdots qdd_zero = dict((i, 0) for i in self._qdoubledots) n = len(self.q) # Internally we represent the EOM as four terms: # EOM = term1 - term2 - term3 - term4 = 0 # First term self._term1 = self._L.jacobian(qds) self._term1 = self._term1.diff(dynamicsymbols._t).T # Second term self._term2 = self._L.jacobian(self.q).T # Third term if self.coneqs: coneqs = self.coneqs m = len(coneqs) # Creating the multipliers self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1))) self.lam_coeffs = -coneqs.jacobian(qds) self._term3 = self.lam_coeffs.T * self.lam_vec # Extracting the coeffecients of the qdds from the diff coneqs diffconeqs = coneqs.diff(dynamicsymbols._t) self._m_cd = diffconeqs.jacobian(self._qdoubledots) # The remaining terms i.e. the 'forcing' terms in diff coneqs self._f_cd = -diffconeqs.subs(qdd_zero) else: self._term3 = zeros(n, 1) # Fourth term if self.forcelist: N = self.inertial self._term4 = zeros(n, 1) flist = zip(*_f_list_parser(self.forcelist, N)) for i, qd in enumerate(qds): for obj, force in self.forcelist: self._term4[i] = sum(v.diff(qd, N) & f for (v, f) in flist) else: self._term4 = zeros(n, 1) # Form the dynamic mass and forcing matrices without_lam = self._term1 - self._term2 - self._term4 self._m_d = without_lam.jacobian(self._qdoubledots) self._f_d = -without_lam.subs(qdd_zero) # Form the EOM self.eom = without_lam - self._term3 return self.eom
def test_orientnew_respects_input_indices(): N = ReferenceFrame('N') q1 = dynamicsymbols('q1') A = N.orientnew('a', 'Axis', [q1, N.z]) #modify default indices: minds = [x+'1' for x in N.indices] B = N.orientnew('b', 'Axis', [q1, N.z], indices=minds) assert N.indices == A.indices assert B.indices == minds
def test_orientnew_respects_input_indices(): N = ReferenceFrame("N") q1 = dynamicsymbols("q1") A = N.orientnew("a", "Axis", [q1, N.z]) # modify default indices: minds = [x + "1" for x in N.indices] B = N.orientnew("b", "Axis", [q1, N.z], indices=minds) assert N.indices == A.indices assert B.indices == minds
def test_orientnew_respects_input_indices(): N = ReferenceFrame('N') q1 = dynamicsymbols('q1') A = N.orientnew('a', 'Axis', [q1, N.z]) #modify default indices: minds = [x + '1' for x in N.indices] B = N.orientnew('b', 'Axis', [q1, N.z], indices=minds) assert N.indices == A.indices assert B.indices == minds
def test_vector_latex_with_functions(): # TODO : Get functions printing correctly. N = ReferenceFrame('N') omega = dynamicsymbols('omega') v = omega.diff() * N.x assert v._latex() == r'\dot{\omega}\mathbf{\hat{n}_x}'
def form_lagranges_equations(self): """Method to form Lagrange's equations of motion. Returns a vector of equations of motion using Lagrange's equations of the second kind. """ qds = self._qdots qdd_zero = dict((i, 0) for i in self._qdoubledots) n = len(self.q) # Internally we represent the EOM as four terms: # EOM = term1 - term2 - term3 - term4 = 0 # First term self._term1 = self._L.jacobian(qds) self._term1 = self._term1.diff(dynamicsymbols._t).T # Second term self._term2 = self._L.jacobian(self.q).T # Third term if self.coneqs: coneqs = self.coneqs m = len(coneqs) # Creating the multipliers self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1))) self.lam_coeffs = -coneqs.jacobian(qds) self._term3 = self.lam_coeffs.T * self.lam_vec # Extracting the coeffecients of the qdds from the diff coneqs diffconeqs = coneqs.diff(dynamicsymbols._t) self._m_cd = diffconeqs.jacobian(self._qdoubledots) # The remaining terms i.e. the 'forcing' terms in diff coneqs self._f_cd = -diffconeqs.subs(qdd_zero) else: self._term3 = zeros(n, 1) # Fourth term if self.forcelist: N = self.inertial self._term4 = zeros(n, 1) for i, qd in enumerate(qds): flist = zip(*_f_list_parser(self.forcelist, N)) self._term4[i] = sum(v.diff(qd, N) & f for (v, f) in flist) else: self._term4 = zeros(n, 1) # Form the dynamic mass and forcing matrices without_lam = self._term1 - self._term2 - self._term4 self._m_d = without_lam.jacobian(self._qdoubledots) self._f_d = -without_lam.subs(qdd_zero) # Form the EOM self.eom = without_lam - self._term3 return self.eom
def test_point_pos(): q = dynamicsymbols("q") N = ReferenceFrame("N") B = N.orientnew("B", "Axis", [q, N.z]) O = Point("O") P = O.locatenew("P", 10 * N.x + 5 * B.x) assert P.pos_from(O) == 10 * N.x + 5 * B.x Q = P.locatenew("Q", 10 * N.y + 5 * B.y) assert Q.pos_from(P) == 10 * N.y + 5 * B.y assert Q.pos_from(O) == 10 * N.x + 10 * N.y + 5 * B.x + 5 * B.y assert O.pos_from(Q) == -10 * N.x - 10 * N.y - 5 * B.x - 5 * B.y
def test_point_vel(): #Basic functionality q1, q2 = dynamicsymbols('q1 q2') N = ReferenceFrame('N') B = ReferenceFrame('B') Q = Point('Q') O = Point('O') Q.set_pos(O, q1 * N.x) raises(ValueError, lambda: Q.vel(N)) # Velocity of O in N is not defined O.set_vel(N, q2 * N.y) assert O.vel(N) == q2 * N.y raises(ValueError, lambda: O.vel(B)) #Velocity of O is not defined in B
def test_auto_point_vel_if_tree_has_vel_but_inappropriate_pos_vector(): q1, q2 = dynamicsymbols('q1 q2') B = ReferenceFrame('B') S = ReferenceFrame('S') P = Point('P') P.set_vel(B, q1 * B.x) P1 = Point('P1') P1.set_pos(P, S.y) raises(ValueError, lambda: P1.vel(B)) # P1.pos_from(P) can't be expressed in B raises(ValueError, lambda: P1.vel(S)) # P.vel(S) not defined
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_point_pos(): q = dynamicsymbols('q') N = ReferenceFrame('N') B = N.orientnew('B', 'Axis', [q, N.z]) O = Point('O') P = O.locatenew('P', 10 * N.x + 5 * B.x) assert P.pos_from(O) == 10 * N.x + 5 * B.x Q = P.locatenew('Q', 10 * N.y + 5 * B.y) assert Q.pos_from(P) == 10 * N.y + 5 * B.y assert Q.pos_from(O) == 10 * N.x + 10 * N.y + 5 * B.x + 5 * B.y assert O.pos_from(Q) == -10 * N.x - 10 * N.y - 5 * B.x - 5 * B.y
def test_auto_vel_dont_overwrite(): t = dynamicsymbols._t q1, q2, u1 = dynamicsymbols('q1, q2, u1') N = ReferenceFrame('N') P = Point('P1') P.set_vel(N, u1 * N.x) P1 = Point('P1') P1.set_pos(P, q2 * N.y) assert P1.vel(N) == q2.diff(t) * N.y + u1 * N.x assert P.vel(N) == u1 * N.x P1.set_vel(N, u1 * N.z) assert P1.vel(N) == u1 * N.z
def results(self): x_sym, y_sym = self.motion x_func = sp.lambdify(dynamicsymbols(self.var_name), x_sym, 'numpy') y_func = sp.lambdify(dynamicsymbols(self.var_name), y_sym, 'numpy') n_data_points = len(self.solution.time) data = { 'asset': [self.name] * n_data_points, 'variable': [self.var_name] * n_data_points, 'time': self.solution.time, 'displacement': self.solution.displacement, 'velocity': self.solution.velocity, 'acceleration': self.solution.acceleration, 'x': x_func(np.array(self.solution.displacement, dtype=np.float64)), 'y': y_func(np.array(self.solution.displacement, dtype=np.float64)), } return pd.DataFrame(data=data)
def test_simple_pedulum(): l, m, g = symbols('l m g') C = Body('C') b = Body('b', mass=m) q = dynamicsymbols('q') P = PinJoint('P', C, b, speeds=q.diff(t), coordinates=q, child_joint_pos = -l*b.x, parent_axis=C.z, child_axis=b.z) b.potential_energy = - m * g * l * cos(q) method = JointsMethod(C, P) method.form_eoms(LagrangesMethod) rhs = method.rhs() assert rhs[1] == -g*sin(q)/l
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_chaos_pendulum(): #https://www.pydy.org/examples/chaos_pendulum.html mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, g = symbols( 'mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, g') theta, phi, omega, alpha = dynamicsymbols('theta phi omega alpha') A = ReferenceFrame('A') B = ReferenceFrame('B') rod = Body('rod', mass=mA, frame=A, central_inertia=inertia(A, IAxx, IAxx, 0)) plate = Body('plate', mass=mB, frame=B, central_inertia=inertia(B, IBxx, IByy, IBzz)) C = Body('C') J1 = PinJoint('J1', C, rod, coordinates=theta, speeds=omega, child_joint_pos=-lA * rod.z, parent_axis=C.y, child_axis=rod.y) J2 = PinJoint('J2', rod, plate, coordinates=phi, speeds=alpha, parent_joint_pos=(lB - lA) * rod.z, parent_axis=rod.z, child_axis=plate.z) rod.apply_force(mA * g * C.z) plate.apply_force(mB * g * C.z) method = JointsMethod(C, J1, J2) method.form_eoms() MM = method.mass_matrix forcing = method.forcing rhs = MM.LUsolve(forcing) xd = (-2 * IBxx * alpha * omega * sin(phi) * cos(phi) + 2 * IByy * alpha * omega * sin(phi) * cos(phi) - g * lA * mA * sin(theta) - g * lB * mB * sin(theta)) / ( IAxx + IBxx * sin(phi)**2 + IByy * cos(phi)**2 + lA**2 * mA + lB**2 * mB) assert (rhs[0] - xd).simplify() == 0 xd = (IBxx - IByy) * omega**2 * sin(phi) * cos(phi) / IBzz assert (rhs[1] - xd).simplify() == 0
def test_vector_latex_with_functions(): N = ReferenceFrame("N") omega, alpha = dynamicsymbols("omega, alpha") v = omega.diff() * N.x assert v._latex() == r"\dot{\omega}\mathbf{\hat{n}_x}" v = omega.diff() ** alpha * N.x assert v._latex() == (r"\dot{\omega}^{\alpha}" r"\mathbf{\hat{n}_x}")
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_vector_latex_with_functions(): N = ReferenceFrame('N') omega, alpha = dynamicsymbols('omega, alpha') v = omega.diff() * N.x assert vlatex(v) == r'\dot{\omega}\mathbf{\hat{n}_x}' v = omega.diff()**alpha * N.x assert vlatex(v) == (r'\dot{\omega}^{\alpha}' r'\mathbf{\hat{n}_x}')
def test_free_dynamicsymbols(): A, B, C, D = symbols('A, B, C, D', cls=ReferenceFrame) a, b, c, d, e, f = dynamicsymbols('a, b, c, d, e, f') B.orient_axis(A, a, A.x) C.orient_axis(B, b, B.y) D.orient_axis(C, c, C.x) v = d*D.x + e*D.y + f*D.z assert set(ordered(v.free_dynamicsymbols(A))) == {a, b, c, d, e, f} assert set(ordered(v.free_dynamicsymbols(B))) == {b, c, d, e, f} assert set(ordered(v.free_dynamicsymbols(C))) == {c, d, e, f} assert set(ordered(v.free_dynamicsymbols(D))) == {d, e, f}
def test_vector_latex_with_functions(): N = ReferenceFrame('N') omega, alpha = dynamicsymbols('omega, alpha') v = omega.diff() * N.x assert v._latex() == r'\dot{\omega}\mathbf{\hat{n}_x}' v = omega.diff() ** alpha * N.x assert v._latex() == (r'\dot{\omega}^{\alpha}' r'\mathbf{\hat{n}_x}')
def test_point_partial_velocity(): N = ReferenceFrame('N') A = ReferenceFrame('A') p = Point('p') u1, u2 = dynamicsymbols('u1, u2') p.set_vel(N, u1 * A.x + u2 * N.y) assert p.partial_velocity(N, u1) == A.x assert p.partial_velocity(N, u1, u2) == (A.x, N.y) raises(ValueError, lambda: p.partial_velocity(A, u1))
def test_auto_point_vel_multiple_point_path(): t = dynamicsymbols._t q1, q2 = dynamicsymbols('q1 q2') B = ReferenceFrame('B') P = Point('P') P.set_vel(B, q1 * B.x) P1 = Point('P1') P1.set_pos(P, q2 * B.y) P1.set_vel(B, q1 * B.z) P2 = Point('P2') P2.set_pos(P1, q1 * B.z) P3 = Point('P3') P3.set_pos(P2, 10 * q1 * B.y) assert P3.vel(B) == 10 * q1.diff(t) * B.y + (q1 + q1.diff(t)) * B.z
def test_orientnew_respects_input_variables(): N = ReferenceFrame("N") q1 = dynamicsymbols("q1") A = N.orientnew("a", "Axis", [q1, N.z]) # build non-standard variable names name = "b" new_variables = ["notb_" + x + "1" for x in N.indices] B = N.orientnew(name, "Axis", [q1, N.z], variables=new_variables) for j, var in enumerate(A.varlist): assert var.name == A.name + "_" + A.indices[j] for j, var in enumerate(B.varlist): assert var.name == new_variables[j]
def test_orientnew_respects_input_variables(): N = ReferenceFrame('N') q1 = dynamicsymbols('q1') A = N.orientnew('a', 'Axis', [q1, N.z]) #build non-standard variable names name = 'b' new_variables = ['notb_'+x+'1' for x in N.indices] B = N.orientnew(name, 'Axis', [q1, N.z], variables=new_variables) for j,var in enumerate(A.varlist): assert var.name == A.name + '_' + A.indices[j] for j,var in enumerate(B.varlist): assert var.name == new_variables[j]
def __init__(self, mass=1, moi=1, ics=6 * [0]): self.size = 3 self.m = mass # kg self.I = moi # kgm^2 self.state = ics # Mass matrix self.M = np.array([[self.m, 0, 0], [0, self.m, 0], [0, 0, self.I]], np.float64) self.symbols = dynamicsymbols('x y h') self.forces = []
def test_orientnew_respects_input_variables(): N = ReferenceFrame('N') q1 = dynamicsymbols('q1') A = N.orientnew('a', 'Axis', [q1, N.z]) #build non-standard variable names name = 'b' new_variables = ['notb_' + x + '1' for x in N.indices] B = N.orientnew(name, 'Axis', [q1, N.z], variables=new_variables) for j, var in enumerate(A.varlist): assert var.name == A.name + '_' + A.indices[j] for j, var in enumerate(B.varlist): assert var.name == new_variables[j]
def test_apply_torque(): t = symbols('t') q = dynamicsymbols('q') B1 = Body('B1') B2 = Body('B2') N = ReferenceFrame('N') torque = t * q * N.x B1.apply_torque(torque, B2) #Applying equal and opposite torque assert B1.loads == [(B1.frame, torque)] assert B2.loads == [(B2.frame, -torque)] torque2 = t * N.y B1.apply_torque(torque2) assert B1.loads == [(B1.frame, torque + torque2)]
def translation(length: float, var_name: str): """This function creates the expressions of motions of a body. Parameters: length (float): Length of the connection. var_name (string): name of symbollic variable. Returns: x (Symbol): Symbollic variable. l (Symbol): Symbollic Generalised expression of motion in x-axis. """ x = dynamicsymbols(var_name) return x
def test_jointmethod_duplicate_coordinates_speeds(): P = Body('P') C = Body('C') T = Body('T') q, u = dynamicsymbols('q u') P1 = PinJoint('P1', P, C, q) P2 = PrismaticJoint('P2', C, T, q) raises(ValueError, lambda: JointsMethod(P, P1, P2)) P1 = PinJoint('P1', P, C, speeds=u) P2 = PrismaticJoint('P2', C, T, speeds=u) raises(ValueError, lambda: JointsMethod(P, P1, P2)) P1 = PinJoint('P1', P, C, q, u) P2 = PrismaticJoint('P2', C, T, q, u) raises(ValueError, lambda: JointsMethod(P, P1, P2))
def test_vector_var_in_dcm(): N = ReferenceFrame('N') A = ReferenceFrame('A') B = ReferenceFrame('B') u1, u2, u3, u4 = dynamicsymbols('u1 u2 u3 u4') v = u1 * u2 * A.x + u3 * N.y + u4**2 * N.z assert v.diff(u1, N, var_in_dcm=False) == u2 * A.x assert v.diff(u1, A, var_in_dcm=False) == u2 * A.x assert v.diff(u3, N, var_in_dcm=False) == N.y assert v.diff(u3, A, var_in_dcm=False) == N.y assert v.diff(u3, B, var_in_dcm=False) == N.y assert v.diff(u4, N, var_in_dcm=False) == 2 * u4 * N.z raises(ValueError, lambda: v.diff(u1, N))
def test_partial_velocity(): N = ReferenceFrame('N') A = ReferenceFrame('A') u1, u2 = dynamicsymbols('u1, u2') A.set_ang_vel(N, u1 * A.x + u2 * N.y) assert N.partial_velocity(A, u1) == -A.x assert N.partial_velocity(A, u1, u2) == (-A.x, -N.y) assert A.partial_velocity(N, u1) == A.x assert A.partial_velocity(N, u1, u2) == (A.x, N.y) assert N.partial_velocity(N, u1) == 0 assert A.partial_velocity(A, u1) == 0
def test_auto_point_vel_multiple_paths_warning_arises(): q, u = dynamicsymbols('q u') N = ReferenceFrame('N') O = Point('O') P = Point('P') Q = Point('Q') R = Point('R') P.set_vel(N, u * N.x) Q.set_vel(N, u * N.y) R.set_vel(N, u * N.z) O.set_pos(P, q * N.z) O.set_pos(Q, q * N.y) O.set_pos(R, q * N.x) with warnings.catch_warnings( ): #There are two possible paths in this point tree, thus a warning is raised warnings.simplefilter("error") raises(UserWarning, lambda: O.vel(N))
def test_auto_point_vel(): t = dynamicsymbols._t q1, q2 = dynamicsymbols('q1 q2') N = ReferenceFrame('N') B = ReferenceFrame('B') O = Point('O') Q = Point('Q') Q.set_pos(O, q1 * N.x) O.set_vel(N, q2 * N.y) assert Q.vel(N) == q1.diff(t) * N.x + q2 * N.y # Velocity of Q using O P1 = Point('P1') P1.set_pos(O, q1 * B.x) P2 = Point('P2') P2.set_pos(P1, q2 * B.z) raises(ValueError, lambda: P2.vel(B) ) # O's velocity is defined in different frame, and no #point in between has its velocity defined raises(ValueError, lambda: P2.vel(N)) # Velocity of O not defined in N
def test_orient_body_simple_ang_vel(): """orient_body_fixed() uses kinematic_equations() internally and solves those equations for the measure numbers of the angular velocity. This test ensures that the simplest form of that linear system solution is returned, thus the == for the expression comparison.""" psi, theta, phi = dynamicsymbols('psi, theta, varphi') t = dynamicsymbols._t A = ReferenceFrame('A') B = ReferenceFrame('B') B.orient_body_fixed(A, (psi, theta, phi), 'ZXZ') A_w_B = B.ang_vel_in(A) assert A_w_B.args[0][1] == B assert A_w_B.args[0][0][0] == (sin(theta) * sin(phi) * psi.diff(t) + cos(phi) * theta.diff(t)) assert A_w_B.args[0][0][1] == (sin(theta) * cos(phi) * psi.diff(t) - sin(phi) * theta.diff(t)) assert A_w_B.args[0][0][2] == cos(theta) * psi.diff(t) + phi.diff(t)
def test_Vector_diffs(): q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4') q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1) q1dd, q2dd, q3dd, q4dd = dynamicsymbols('q1 q2 q3 q4', 2) N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q3, N.z]) B = A.orientnew('B', 'Axis', [q2, A.x]) v1 = q2 * A.x + q3 * N.y v2 = q3 * B.x + v1 v3 = v1.dt(B) v4 = v2.dt(B) v5 = q1*A.x + q2*A.y + q3*A.z assert v1.dt(N) == q2d * A.x + q2 * q3d * A.y + q3d * N.y assert v1.dt(A) == q2d * A.x + q3 * q3d * N.x + q3d * N.y assert v1.dt(B) == (q2d * A.x + q3 * q3d * N.x + q3d *\ N.y - q3 * cos(q3) * q2d * N.z) assert v2.dt(N) == (q2d * A.x + (q2 + q3) * q3d * A.y + q3d * B.x + q3d * N.y) assert v2.dt(A) == q2d * A.x + q3d * B.x + q3 * q3d * N.x + q3d * N.y assert v2.dt(B) == (q2d * A.x + q3d * B.x + q3 * q3d * N.x + q3d * N.y - q3 * cos(q3) * q2d * N.z) assert v3.dt(N) == (q2dd * A.x + q2d * q3d * A.y + (q3d**2 + q3 * q3dd) * N.x + q3dd * N.y + (q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v3.dt(A) == (q2dd * A.x + (2 * q3d**2 + q3 * q3dd) * N.x + (q3dd - q3 * q3d**2) * N.y + (q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v3.dt(B) == (q2dd * A.x - q3 * cos(q3) * q2d**2 * A.y + (2 * q3d**2 + q3 * q3dd) * N.x + (q3dd - q3 * q3d**2) * N.y + (2 * q3 * sin(q3) * q2d * q3d - 2 * cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v4.dt(N) == (q2dd * A.x + q3d * (q2d + q3d) * A.y + q3dd * B.x + (q3d**2 + q3 * q3dd) * N.x + q3dd * N.y + (q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v4.dt(A) == (q2dd * A.x + q3dd * B.x + (2 * q3d**2 + q3 * q3dd) * N.x + (q3dd - q3 * q3d**2) * N.y + (q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v4.dt(B) == (q2dd * A.x - q3 * cos(q3) * q2d**2 * A.y + q3dd * B.x + (2 * q3d**2 + q3 * q3dd) * N.x + (q3dd - q3 * q3d**2) * N.y + (2 * q3 * sin(q3) * q2d * q3d - 2 * cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v5.dt(B) == q1d*A.x + (q3*q2d + q2d)*A.y + (-q2*q2d + q3d)*A.z assert v5.dt(A) == q1d*A.x + q2d*A.y + q3d*A.z assert v5.dt(N) == (-q2*q3d + q1d)*A.x + (q1*q3d + q2d)*A.y + q3d*A.z assert v3.diff(q1d, N) == 0 assert v3.diff(q2d, N) == A.x - q3 * cos(q3) * N.z assert v3.diff(q3d, N) == q3 * N.x + N.y assert v3.diff(q1d, A) == 0 assert v3.diff(q2d, A) == A.x - q3 * cos(q3) * N.z assert v3.diff(q3d, A) == q3 * N.x + N.y assert v3.diff(q1d, B) == 0 assert v3.diff(q2d, B) == A.x - q3 * cos(q3) * N.z assert v3.diff(q3d, B) == q3 * N.x + N.y assert v4.diff(q1d, N) == 0 assert v4.diff(q2d, N) == A.x - q3 * cos(q3) * N.z assert v4.diff(q3d, N) == B.x + q3 * N.x + N.y assert v4.diff(q1d, A) == 0 assert v4.diff(q2d, A) == A.x - q3 * cos(q3) * N.z assert v4.diff(q3d, A) == B.x + q3 * N.x + N.y assert v4.diff(q1d, B) == 0 assert v4.diff(q2d, B) == A.x - q3 * cos(q3) * N.z assert v4.diff(q3d, B) == B.x + q3 * N.x + N.y
def test_ang_vel(): q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4') q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1) 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]) u1, u2, u3 = dynamicsymbols('u1 u2 u3') assert A.ang_vel_in(N) == (q1d)*A.z assert B.ang_vel_in(N) == (q2d)*B.x + (q1d)*A.z assert C.ang_vel_in(N) == (q3d)*C.y + (q2d)*B.x + (q1d)*A.z A2 = N.orientnew('A2', 'Axis', [q4, N.y]) assert N.ang_vel_in(N) == 0 assert N.ang_vel_in(A) == -q1d*N.z assert N.ang_vel_in(B) == -q1d*A.z - q2d*B.x assert N.ang_vel_in(C) == -q1d*A.z - q2d*B.x - q3d*B.y assert N.ang_vel_in(A2) == -q4d*N.y assert A.ang_vel_in(N) == q1d*N.z assert A.ang_vel_in(A) == 0 assert A.ang_vel_in(B) == - q2d*B.x assert A.ang_vel_in(C) == - q2d*B.x - q3d*B.y assert A.ang_vel_in(A2) == q1d*N.z - q4d*N.y assert B.ang_vel_in(N) == q1d*A.z + q2d*A.x assert B.ang_vel_in(A) == q2d*A.x assert B.ang_vel_in(B) == 0 assert B.ang_vel_in(C) == -q3d*B.y assert B.ang_vel_in(A2) == q1d*A.z + q2d*A.x - q4d*N.y assert C.ang_vel_in(N) == q1d*A.z + q2d*A.x + q3d*B.y assert C.ang_vel_in(A) == q2d*A.x + q3d*C.y assert C.ang_vel_in(B) == q3d*B.y assert C.ang_vel_in(C) == 0 assert C.ang_vel_in(A2) == q1d*A.z + q2d*A.x + q3d*B.y - q4d*N.y assert A2.ang_vel_in(N) == q4d*A2.y assert A2.ang_vel_in(A) == q4d*A2.y - q1d*N.z assert A2.ang_vel_in(B) == q4d*N.y - q1d*A.z - q2d*A.x assert A2.ang_vel_in(C) == q4d*N.y - q1d*A.z - q2d*A.x - q3d*B.y assert A2.ang_vel_in(A2) == 0 C.set_ang_vel(N, u1*C.x + u2*C.y + u3*C.z) assert C.ang_vel_in(N) == (u1)*C.x + (u2)*C.y + (u3)*C.z assert N.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z assert C.ang_vel_in(D) == (u1)*C.x + (u2)*C.y + (u3)*C.z + (-q4d)*D.y assert D.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z + (q4d)*D.y q0 = dynamicsymbols('q0') q0d = dynamicsymbols('q0', 1) E = N.orientnew('E', 'Quaternion', (q0, q1, q2, q3)) assert E.ang_vel_in(N) == ( 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) * E.x + 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) * E.y + 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) * E.z) F = N.orientnew('F', 'Body', (q1, q2, q3), '313') assert F.ang_vel_in(N) == ((sin(q2)*sin(q3)*q1d + cos(q3)*q2d)*F.x + (sin(q2)*cos(q3)*q1d - sin(q3)*q2d)*F.y + (cos(q2)*q1d + q3d)*F.z) G = N.orientnew('G', 'Axis', (q1, N.x + N.y)) assert G.ang_vel_in(N) == q1d * (N.x + N.y).normalize() assert N.ang_vel_in(G) == -q1d * (N.x + N.y).normalize()