def test_partial_velocity(): q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') u4, u5 = dynamicsymbols('u4, u5') r = symbols('r') N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)] u_list = [u1, u2, u3, u4, u5] assert (partial_velocity(vel_list, u_list, N) == [[ -r * L.y, r * L.x, 0, L.x, cos(q2) * L.y - sin(q2) * L.z ], [0, 0, 0, L.x, cos(q2) * L.y - sin(q2) * L.z], [L.x, L.y, L.z, 0, 0]]) # Make sure that partial velocities can be computed regardless if the # orientation between frames is defined or not. A = ReferenceFrame('A') B = ReferenceFrame('B') v = u4 * A.x + u5 * B.y assert partial_velocity((v, ), (u4, u5), A) == [[A.x, B.y]] raises(TypeError, lambda: partial_velocity(Dmc.vel(N), u_list, N)) raises(TypeError, lambda: partial_velocity(vel_list, u1, N))
def test_partial_velocity(): q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') u4, u5 = dynamicsymbols('u4, u5') r = symbols('r') N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)] u_list = [u1, u2, u3, u4, u5] assert (partial_velocity(vel_list, u_list, N) == [[- r*L.y, r*L.x, 0, L.x, cos(q2)*L.y - sin(q2)*L.z], [0, 0, 0, L.x, cos(q2)*L.y - sin(q2)*L.z], [L.x, L.y, L.z, 0, 0]]) # Make sure that partial velocities can be computed regardless if the # orientation between frames is defined or not. A = ReferenceFrame('A') B = ReferenceFrame('B') v = u4 * A.x + u5 * B.y assert partial_velocity((v, ), (u4, u5), A) == [[A.x, B.y]] raises(TypeError, lambda: partial_velocity(Dmc.vel(N), u_list, N)) raises(TypeError, lambda: partial_velocity(vel_list, u1, N))
def test_partial_velocity(): q1, q2, q3, u1, u2, u3 = dynamicsymbols("q1 q2 q3 u1 u2 u3") u4, u5 = dynamicsymbols("u4, u5") r = symbols("r") N = ReferenceFrame("N") Y = N.orientnew("Y", "Axis", [q1, N.z]) L = Y.orientnew("L", "Axis", [q2, Y.x]) R = L.orientnew("R", "Axis", [q3, L.y]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point("C") C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew("Dmc", r * L.z) Dmc.v2pt_theory(C, N, R) vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)] u_list = [u1, u2, u3, u4, u5] assert partial_velocity(vel_list, u_list, N) == [ [-r * L.y, r * L.x, 0, L.x, cos(q2) * L.y - sin(q2) * L.z], [0, 0, 0, L.x, cos(q2) * L.y - sin(q2) * L.z], [L.x, L.y, L.z, 0, 0], ] # Make sure that partial velocities can be computed regardless if the # orientation between frames is defined or not. A = ReferenceFrame("A") B = ReferenceFrame("B") v = u4 * A.x + u5 * B.y assert partial_velocity((v, ), (u4, u5), A) == [[A.x, B.y]] raises(TypeError, lambda: partial_velocity(Dmc.vel(N), u_list, N)) raises(TypeError, lambda: partial_velocity(vel_list, u1, N))
def test_get_motion_methods(): #Initialization t = dynamicsymbols._t s1, s2, s3 = symbols('s1 s2 s3') S1, S2, S3 = symbols('S1 S2 S3') S4, S5, S6 = symbols('S4 S5 S6') t1, t2 = symbols('t1 t2') a, b, c = dynamicsymbols('a b c') ad, bd, cd = dynamicsymbols('a b c', 1) a2d, b2d, c2d = dynamicsymbols('a b c', 2) v0 = S1 * N.x + S2 * N.y + S3 * N.z v01 = S4 * N.x + S5 * N.y + S6 * N.z v1 = s1 * N.x + s2 * N.y + s3 * N.z v2 = a * N.x + b * N.y + c * N.z v2d = ad * N.x + bd * N.y + cd * N.z v2dd = a2d * N.x + b2d * N.y + c2d * N.z #Test position parameter assert get_motion_params(frame=N) == (0, 0, 0) assert get_motion_params(N, position=v1) == (0, 0, v1) assert get_motion_params(N, position=v2) == (v2dd, v2d, v2) #Test velocity parameter assert get_motion_params(N, velocity=v1) == (0, v1, v1 * t) assert get_motion_params(N, velocity=v1, position=v0, timevalue1=t1) == \ (0, v1, v0 + v1*(t - t1)) answer = get_motion_params(N, velocity=v1, position=v2, timevalue1=t1) answer_expected = (0, v1, v1 * t - v1 * t1 + v2.subs(t, t1)) assert answer == answer_expected answer = get_motion_params(N, velocity=v2, position=v0, timevalue1=t1) integral_vector = Integral(a, (t, t1, t))*N.x + Integral(b, (t, t1, t))*N.y \ + Integral(c, (t, t1, t))*N.z answer_expected = (v2d, v2, v0 + integral_vector) assert answer == answer_expected #Test acceleration parameter assert get_motion_params(N, acceleration=v1) == \ (v1, v1 * t, v1 * t**2/2) assert get_motion_params(N, acceleration=v1, velocity=v0, position=v2, timevalue1=t1, timevalue2=t2) == \ (v1, (v0 + v1*t - v1*t2), -v0*t1 + v1*t**2/2 + v1*t2*t1 - \ v1*t1**2/2 + t*(v0 - v1*t2) + \ v2.subs(t, t1)) assert get_motion_params(N, acceleration=v1, velocity=v0, position=v01, timevalue1=t1, timevalue2=t2) == \ (v1, v0 + v1*t - v1*t2, -v0*t1 + v01 + v1*t**2/2 + \ v1*t2*t1 - v1*t1**2/2 + \ t*(v0 - v1*t2)) answer = get_motion_params(N, acceleration=a * N.x, velocity=S1 * N.x, position=S2 * N.x, timevalue1=t1, timevalue2=t2) i1 = Integral(a, (t, t2, t)) answer_expected = (a*N.x, (S1 + i1)*N.x, \ (S2 + Integral(S1 + i1, (t, t1, t)))*N.x) assert answer == answer_expected
def test_kin_eqs(): q0, q1, q2, q3 = dynamicsymbols('q0 q1 q2 q3') q0d, q1d, q2d, q3d = dynamicsymbols('q0 q1 q2 q3', 1) u1, u2, u3 = dynamicsymbols('u1 u2 u3') kds = kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'quaternion') assert kds == [-0.5 * q0 * u1 - 0.5 * q2 * u3 + 0.5 * q3 * u2 + q1d, -0.5 * q0 * u2 + 0.5 * q1 * u3 - 0.5 * q3 * u1 + q2d, -0.5 * q0 * u3 - 0.5 * q1 * u2 + 0.5 * q2 * u1 + q3d, 0.5 * q1 * u1 + 0.5 * q2 * u2 + 0.5 * q3 * u3 + q0d]
def test_get_motion_methods(): #Initialization t = dynamicsymbols._t s1, s2, s3 = symbols('s1 s2 s3') S1, S2, S3 = symbols('S1 S2 S3') S4, S5, S6 = symbols('S4 S5 S6') t1, t2 = symbols('t1 t2') a, b, c = dynamicsymbols('a b c') ad, bd, cd = dynamicsymbols('a b c', 1) a2d, b2d, c2d = dynamicsymbols('a b c', 2) v0 = S1*N.x + S2*N.y + S3*N.z v01 = S4*N.x + S5*N.y + S6*N.z v1 = s1*N.x + s2*N.y + s3*N.z v2 = a*N.x + b*N.y + c*N.z v2d = ad*N.x + bd*N.y + cd*N.z v2dd = a2d*N.x + b2d*N.y + c2d*N.z #Test position parameter assert get_motion_params(frame = N) == (0, 0, 0) assert get_motion_params(N, position=v1) == (0, 0, v1) assert get_motion_params(N, position=v2) == (v2dd, v2d, v2) #Test velocity parameter assert get_motion_params(N, velocity=v1) == (0, v1, v1 * t) assert get_motion_params(N, velocity=v1, position=v0, timevalue1=t1) == \ (0, v1, v0 + v1*(t - t1)) answer = get_motion_params(N, velocity=v1, position=v2, timevalue1=t1) answer_expected = (0, v1, v1*t - v1*t1 + v2.subs(t, t1)) assert answer == answer_expected answer = get_motion_params(N, velocity=v2, position=v0, timevalue1=t1) integral_vector = Integral(a, (t, t1, t))*N.x + Integral(b, (t, t1, t))*N.y \ + Integral(c, (t, t1, t))*N.z answer_expected = (v2d, v2, v0 + integral_vector) assert answer == answer_expected #Test acceleration parameter assert get_motion_params(N, acceleration=v1) == \ (v1, v1 * t, v1 * t**2/2) assert get_motion_params(N, acceleration=v1, velocity=v0, position=v2, timevalue1=t1, timevalue2=t2) == \ (v1, (v0 + v1*t - v1*t2), -v0*t1 + v1*t**2/2 + v1*t2*t1 - \ v1*t1**2/2 + t*(v0 - v1*t2) + \ v2.subs(t, t1)) assert get_motion_params(N, acceleration=v1, velocity=v0, position=v01, timevalue1=t1, timevalue2=t2) == \ (v1, v0 + v1*t - v1*t2, -v0*t1 + v01 + v1*t**2/2 + \ v1*t2*t1 - v1*t1**2/2 + \ t*(v0 - v1*t2)) answer = get_motion_params(N, acceleration=a*N.x, velocity=S1*N.x, position=S2*N.x, timevalue1=t1, timevalue2=t2) i1 = Integral(a, (t, t2, t)) answer_expected = (a*N.x, (S1 + i1)*N.x, \ (S2 + Integral(S1 + i1, (t, t1, t)))*N.x) assert answer == answer_expected
def test_dynamicsymbols(): # Tests to check the assumptions applied to dynamicsymbols f1 = dynamicsymbols("f1") f2 = dynamicsymbols("f2", real=True) f3 = dynamicsymbols("f3", positive=True) f4, f5 = dynamicsymbols("f4,f5", commutative=False) f6 = dynamicsymbols("f6", integer=True) assert f1.is_real is None assert f2.is_real assert f3.is_positive assert f4 * f5 != f5 * f4 assert f6.is_integer
def test_dynamicsymbols(): #Tests to check the assumptions applied to dynamicsymbols f1 = dynamicsymbols('f1') f2 = dynamicsymbols('f2', real=True) f3 = dynamicsymbols('f3', positive=True) f4, f5 = dynamicsymbols('f4,f5', commutative=False) f6 = dynamicsymbols('f6', integer=True) assert f1.is_real is None assert f2.is_real assert f3.is_positive assert f4 * f5 != f5 * f4 assert f6.is_integer
def test_kin_eqs(): q0, q1, q2, q3 = dynamicsymbols("q0 q1 q2 q3") q0d, q1d, q2d, q3d = dynamicsymbols("q0 q1 q2 q3", 1) u1, u2, u3 = dynamicsymbols("u1 u2 u3") ke = kinematic_equations([u1, u2, u3], [q1, q2, q3], "body", 313) assert ke == kinematic_equations([u1, u2, u3], [q1, q2, q3], "body", "313") kds = kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], "quaternion") assert kds == [ -0.5 * q0 * u1 - 0.5 * q2 * u3 + 0.5 * q3 * u2 + q1d, -0.5 * q0 * u2 + 0.5 * q1 * u3 - 0.5 * q3 * u1 + q2d, -0.5 * q0 * u3 - 0.5 * q1 * u2 + 0.5 * q2 * u1 + q3d, 0.5 * q1 * u1 + 0.5 * q2 * u2 + 0.5 * q3 * u3 + q0d, ] raises( ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2], "quaternion"), ) raises( ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], "quaternion", "123"), ) raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], "foo")) raises(TypeError, lambda: kinematic_equations(u1, [q0, q1, q2, q3], "quaternion")) raises(TypeError, lambda: kinematic_equations([u1], [q0, q1, q2, q3], "quaternion")) raises(TypeError, lambda: kinematic_equations([u1, u2, u3], q0, "quaternion")) raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], "body")) raises( ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], "space")) raises( ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2], "body", "222"), ) assert kinematic_equations([0, 0, 0], [q0, q1, q2], "space") == [ S.Zero, S.Zero, S.Zero, ]
def test_time_derivative(): #The use of time_derivative for calculations pertaining to scalar #fields has been tested in test_coordinate_vars in test_essential.py A = ReferenceFrame('A') q = dynamicsymbols('q') qd = dynamicsymbols('q', 1) B = A.orientnew('B', 'Axis', [q, A.z]) d = A.x | A.x assert time_derivative(d, B) == (-qd) * (A.y | A.x) + \ (-qd) * (A.x | A.y) d1 = A.x | B.y assert time_derivative(d1, A) == - qd*(A.x|B.x) assert time_derivative(d1, B) == - qd*(A.y|B.y) d2 = A.x | B.x assert time_derivative(d2, A) == qd*(A.x|B.y) assert time_derivative(d2, B) == - qd*(A.y|B.x) d3 = A.x | B.z assert time_derivative(d3, A) == 0 assert time_derivative(d3, B) == - qd*(A.y|B.z) 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) C = B.orientnew('C', 'Axis', [q4, B.x]) v1 = q1 * A.z v2 = q2*A.x + q3*B.y v3 = q1*A.x + q2*A.y + q3*A.z assert time_derivative(B.x, C) == 0 assert time_derivative(B.y, C) == - q4d*B.z assert time_derivative(B.z, C) == q4d*B.y assert time_derivative(v1, B) == q1d*A.z assert time_derivative(v1, C) == - q1*sin(q)*q4d*A.x + \ q1*cos(q)*q4d*A.y + q1d*A.z assert time_derivative(v2, A) == q2d*A.x - q3*qd*B.x + q3d*B.y assert time_derivative(v2, C) == q2d*A.x - q2*qd*A.y + \ q2*sin(q)*q4d*A.z + q3d*B.y - q3*q4d*B.z assert time_derivative(v3, B) == (q2*qd + q1d)*A.x + \ (-q1*qd + q2d)*A.y + q3d*A.z assert time_derivative(d, C) == - qd*(A.y|A.x) + \ sin(q)*q4d*(A.z|A.x) - qd*(A.x|A.y) + sin(q)*q4d*(A.x|A.z) raises(ValueError, lambda: time_derivative(B.x, C, order=0.5)) raises(ValueError, lambda: time_derivative(B.x, C, order=-1))
def test_time_derivative(): #The use of time_derivative for calculations pertaining to scalar #fields has been tested in test_coordinate_vars in test_essential.py A = ReferenceFrame('A') q = dynamicsymbols('q') qd = dynamicsymbols('q', 1) B = A.orientnew('B', 'Axis', [q, A.z]) d = A.x | A.x assert time_derivative(d, B) == (-qd) * (A.y | A.x) + \ (-qd) * (A.x | A.y) d1 = A.x | B.y assert time_derivative(d1, A) == -qd * (A.x | B.x) assert time_derivative(d1, B) == -qd * (A.y | B.y) d2 = A.x | B.x assert time_derivative(d2, A) == qd * (A.x | B.y) assert time_derivative(d2, B) == -qd * (A.y | B.x) d3 = A.x | B.z assert time_derivative(d3, A) == 0 assert time_derivative(d3, B) == -qd * (A.y | B.z) 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) C = B.orientnew('C', 'Axis', [q4, B.x]) v1 = q1 * A.z v2 = q2 * A.x + q3 * B.y v3 = q1 * A.x + q2 * A.y + q3 * A.z assert time_derivative(B.x, C) == 0 assert time_derivative(B.y, C) == -q4d * B.z assert time_derivative(B.z, C) == q4d * B.y assert time_derivative(v1, B) == q1d * A.z assert time_derivative(v1, C) == - q1*sin(q)*q4d*A.x + \ q1*cos(q)*q4d*A.y + q1d*A.z assert time_derivative(v2, A) == q2d * A.x - q3 * qd * B.x + q3d * B.y assert time_derivative(v2, C) == q2d*A.x - q2*qd*A.y + \ q2*sin(q)*q4d*A.z + q3d*B.y - q3*q4d*B.z assert time_derivative(v3, B) == (q2*qd + q1d)*A.x + \ (-q1*qd + q2d)*A.y + q3d*A.z assert time_derivative(d, C) == - qd*(A.y|A.x) + \ sin(q)*q4d*(A.z|A.x) - qd*(A.x|A.y) + sin(q)*q4d*(A.x|A.z) raises(ValueError, lambda: time_derivative(B.x, C, order=0.5)) raises(ValueError, lambda: time_derivative(B.x, C, order=-1))
def test_kin_eqs(): q0, q1, q2, q3 = dynamicsymbols('q0 q1 q2 q3') q0d, q1d, q2d, q3d = dynamicsymbols('q0 q1 q2 q3', 1) u1, u2, u3 = dynamicsymbols('u1 u2 u3') ke = kinematic_equations([u1,u2,u3], [q1,q2,q3], 'body', 313) assert ke == kinematic_equations([u1,u2,u3], [q1,q2,q3], 'body', '313') kds = kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'quaternion') assert kds == [-0.5 * q0 * u1 - 0.5 * q2 * u3 + 0.5 * q3 * u2 + q1d, -0.5 * q0 * u2 + 0.5 * q1 * u3 - 0.5 * q3 * u1 + q2d, -0.5 * q0 * u3 - 0.5 * q1 * u2 + 0.5 * q2 * u1 + q3d, 0.5 * q1 * u1 + 0.5 * q2 * u2 + 0.5 * q3 * u3 + q0d] raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2], 'quaternion')) raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'quaternion', '123')) raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'foo')) raises(TypeError, lambda: kinematic_equations(u1, [q0, q1, q2, q3], 'quaternion')) raises(TypeError, lambda: kinematic_equations([u1], [q0, q1, q2, q3], 'quaternion')) raises(TypeError, lambda: kinematic_equations([u1, u2, u3], q0, 'quaternion')) raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'body')) raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2, q3], 'space')) raises(ValueError, lambda: kinematic_equations([u1, u2, u3], [q0, q1, q2], 'body', '222')) assert kinematic_equations([0, 0, 0], [q0, q1, q2], 'space') == [S.Zero, S.Zero, S.Zero]
def test_partial_velocity(): q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') u4, u5 = dynamicsymbols('u4, u5') r = symbols('r') N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)] u_list = [u1, u2, u3, u4, u5] assert (partial_velocity(vel_list, u_list, N) == [[- r*L.y, r*L.x, 0, L.x, cos(q2)*L.y - sin(q2)*L.z], [0, 0, 0, L.x, cos(q2)*L.y - sin(q2)*L.z], [L.x, L.y, L.z, 0, 0]])
def test_partial_velocity(): q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') u4, u5 = dynamicsymbols('u4, u5') r = symbols('r') N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)] u_list = [u1, u2, u3, u4, u5] assert (partial_velocity(vel_list, u_list, N) == [[ -r * L.y, r * L.x, 0, L.x, cos(q2) * L.y - sin(q2) * L.z ], [0, 0, 0, L.x, cos(q2) * L.y - sin(q2) * L.z], [L.x, L.y, L.z, 0, 0]])
def orient(self, parent, rot_type, amounts, rot_order=''): """Defines the orientation of this frame relative to a parent frame. Parameters ========== parent : ReferenceFrame The frame that this ReferenceFrame will have its orientation matrix defined in relation to. rot_type : str The type of orientation matrix that is being created. Supported types are 'Body', 'Space', 'Quaternion', and 'Axis'. See examples for correct usage. amounts : list OR value The quantities that the orientation matrix will be defined by. rot_order : str If applicable, the order of a series of rotations. Examples ======== >>> from sympy.physics.vector import ReferenceFrame, Vector >>> from sympy import symbols >>> q0, q1, q2, q3, q4 = symbols('q0 q1 q2 q3 q4') >>> N = ReferenceFrame('N') >>> B = ReferenceFrame('B') Now we have a choice of how to implement the orientation. First is Body. Body orientation takes this reference frame through three successive simple rotations. Acceptable rotation orders are of length 3, expressed in XYZ or 123, and cannot have a rotation about about an axis twice in a row. >>> B.orient(N, 'Body', [q1, q2, q3], '123') >>> B.orient(N, 'Body', [q1, q2, 0], 'ZXZ') >>> B.orient(N, 'Body', [0, 0, 0], 'XYX') Next is Space. Space is like Body, but the rotations are applied in the opposite order. >>> B.orient(N, 'Space', [q1, q2, q3], '312') Next is Quaternion. This orients the new ReferenceFrame with Quaternions, defined as a finite rotation about lambda, a unit vector, by some amount theta. This orientation is described by four parameters: q0 = cos(theta/2) q1 = lambda_x sin(theta/2) q2 = lambda_y sin(theta/2) q3 = lambda_z sin(theta/2) Quaternion does not take in a rotation order. >>> B.orient(N, 'Quaternion', [q0, q1, q2, q3]) Last is Axis. This is a rotation about an arbitrary, non-time-varying axis by some angle. The axis is supplied as a Vector. This is how simple rotations are defined. >>> B.orient(N, 'Axis', [q1, N.x + 2 * N.y]) """ from sympy.physics.vector.functions import dynamicsymbols _check_frame(parent) amounts = list(amounts) for i, v in enumerate(amounts): if not isinstance(v, Vector): amounts[i] = sympify(v) def _rot(axis, angle): """DCM for simple axis 1,2,or 3 rotations. """ if axis == 1: return Matrix([[1, 0, 0], [0, cos(angle), -sin(angle)], [0, sin(angle), cos(angle)]]) elif axis == 2: return Matrix([[cos(angle), 0, sin(angle)], [0, 1, 0], [-sin(angle), 0, cos(angle)]]) elif axis == 3: return Matrix([[cos(angle), -sin(angle), 0], [sin(angle), cos(angle), 0], [0, 0, 1]]) approved_orders = ('123', '231', '312', '132', '213', '321', '121', '131', '212', '232', '313', '323', '') rot_order = str( rot_order).upper() # Now we need to make sure XYZ = 123 rot_type = rot_type.upper() rot_order = [i.replace('X', '1') for i in rot_order] rot_order = [i.replace('Y', '2') for i in rot_order] rot_order = [i.replace('Z', '3') for i in rot_order] rot_order = ''.join(rot_order) if not rot_order in approved_orders: raise TypeError('The supplied order is not an approved type') parent_orient = [] if rot_type == 'AXIS': if not rot_order == '': raise TypeError('Axis orientation takes no rotation order') if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 2)): raise TypeError('Amounts are a list or tuple of length 2') theta = amounts[0] axis = amounts[1] axis = _check_vector(axis) if not axis.dt(parent) == 0: raise ValueError('Axis cannot be time-varying') axis = axis.express(parent).normalize() axis = axis.args[0][0] parent_orient = ((eye(3) - axis * axis.T) * cos(theta) + Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T) elif rot_type == 'QUATERNION': if not rot_order == '': raise TypeError( 'Quaternion orientation takes no rotation order') if not (isinstance(amounts, (list, tuple)) & (len(amounts) == 4)): raise TypeError('Amounts are a list or tuple of length 4') q0, q1, q2, q3 = amounts parent_orient = (Matrix([[q0 ** 2 + q1 ** 2 - q2 ** 2 - q3 ** 2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3)], [2 * (q1 * q2 + q0 * q3), q0 ** 2 - q1 ** 2 + q2 ** 2 - q3 ** 2, 2 * (q2 * q3 - q0 * q1)], [2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), q0 ** 2 - q1 ** 2 - q2 ** 2 + q3 ** 2]])) elif rot_type == 'BODY': if not (len(amounts) == 3 & len(rot_order) == 3): raise TypeError('Body orientation takes 3 values & 3 orders') a1 = int(rot_order[0]) a2 = int(rot_order[1]) a3 = int(rot_order[2]) parent_orient = (_rot(a1, amounts[0]) * _rot(a2, amounts[1]) * _rot(a3, amounts[2])) elif rot_type == 'SPACE': if not (len(amounts) == 3 & len(rot_order) == 3): raise TypeError('Space orientation takes 3 values & 3 orders') a1 = int(rot_order[0]) a2 = int(rot_order[1]) a3 = int(rot_order[2]) parent_orient = (_rot(a3, amounts[2]) * _rot(a2, amounts[1]) * _rot(a1, amounts[0])) else: raise NotImplementedError('That is not an implemented rotation') #Reset the _dcm_cache of this frame, and remove it from the _dcm_caches #of the frames it is linked to. Also remove it from the _dcm_dict of #its parent frames = self._dcm_cache.keys() for frame in frames: if frame in self._dcm_dict: del frame._dcm_dict[self] del frame._dcm_cache[self] #Add the dcm relationship to _dcm_dict self._dcm_dict = self._dlist[0] = {} self._dcm_dict.update({parent: parent_orient.T}) parent._dcm_dict.update({self: parent_orient}) #Also update the dcm cache after resetting it self._dcm_cache = {} self._dcm_cache.update({parent: parent_orient.T}) parent._dcm_cache.update({self: parent_orient}) if rot_type == 'QUATERNION': t = dynamicsymbols._t q0, q1, q2, q3 = amounts q0d = diff(q0, t) q1d = diff(q1, t) q2d = diff(q2, t) q3d = diff(q3, t) w1 = 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) w2 = 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) w3 = 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) wvec = Vector([(Matrix([w1, w2, w3]), self)]) elif rot_type == 'AXIS': thetad = (amounts[0]).diff(dynamicsymbols._t) wvec = thetad * amounts[1].express(parent).normalize() else: try: from sympy.polys.polyerrors import CoercionFailed from sympy.physics.vector.functions import kinematic_equations q1, q2, q3 = amounts u1, u2, u3 = dynamicsymbols('u1, u2, u3') templist = kinematic_equations([u1, u2, u3], [q1, q2, q3], rot_type, rot_order) templist = [expand(i) for i in templist] td = solve(templist, [u1, u2, u3]) u1 = expand(td[u1]) u2 = expand(td[u2]) u3 = expand(td[u3]) wvec = u1 * self.x + u2 * self.y + u3 * self.z except (CoercionFailed, AssertionError): wvec = self._w_diff_dcm(parent) self._ang_vel_dict.update({parent: wvec}) parent._ang_vel_dict.update({self: -wvec}) self._var_dict = {}