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_dcm_diff_16824():
# NOTE : This is a regression test for the bug introduced in PR 14758,
# identified in 16824, and solved by PR 16828.
# This is the solution to Problem 2.2 on page 264 in Kane & Lenvinson's
# 1985 book.
q1, q2, q3 = dynamicsymbols('q1:4')
s1 = sin(q1)
c1 = cos(q1)
s2 = sin(q2)
c2 = cos(q2)
s3 = sin(q3)
c3 = cos(q3)
dcm = Matrix([[c2 * c3, s1 * s2 * c3 - s3 * c1, c1 * s2 * c3 + s3 * s1],
[c2 * s3, s1 * s2 * s3 + c3 * c1, c1 * s2 * s3 - c3 * s1],
[-s2, s1 * c2, c1 * c2]])
A = ReferenceFrame('A')
B = ReferenceFrame('B')
B.orient(A, 'DCM', dcm)
AwB = B.ang_vel_in(A)
alpha2 = s3 * c2 * q1.diff() + c3 * q2.diff()
beta2 = s1 * c2 * q3.diff() + c1 * q2.diff()
assert simplify(AwB.dot(A.y) - alpha2) == 0
assert simplify(AwB.dot(B.y) - beta2) == 0
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_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_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_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_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_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_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_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()
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()
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()
