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_w_diff_dcm2():
q1, q2, q3 = dynamicsymbols('q1:4')
N = ReferenceFrame('N')
A = N.orientnew('A', 'axis', [q1, N.x])
B = A.orientnew('B', 'axis', [q2, A.y])
C = B.orientnew('C', 'axis', [q3, B.z])
DCM = C.dcm(N).T
D = N.orientnew('D', 'DCM', DCM)
# Frames D and C are the same ReferenceFrame,
# since they have equal DCM respect to frame N.
# Therefore, D and C should have same angle velocity in N.
assert D.dcm(N) == C.dcm(N) == Matrix(
[[
cos(q2) * cos(q3),
sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1),
sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)
],
[
-sin(q3) * cos(q2),
-sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1)
], [sin(q2), -sin(q1) * cos(q2),
cos(q1) * cos(q2)]])
assert (D.ang_vel_in(N) - C.ang_vel_in(N)).express(N).simplify() == 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_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_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_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_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_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_w_diff_dcm2():
q1, q2, q3 = dynamicsymbols("q1:4")
N = ReferenceFrame("N")
A = N.orientnew("A", "axis", [q1, N.x])
B = A.orientnew("B", "axis", [q2, A.y])
C = B.orientnew("C", "axis", [q3, B.z])
DCM = C.dcm(N).T
D = N.orientnew("D", "DCM", DCM)
# Frames D and C are the same ReferenceFrame,
# since they have equal DCM respect to frame N.
# Therefore, D and C should have same angle velocity in N.
assert (D.dcm(N) == C.dcm(N) == Matrix([
[
cos(q2) * cos(q3),
sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1),
sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3),
],
[
-sin(q3) * cos(q2),
-sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
],
[sin(q2), -sin(q1) * cos(q2),
cos(q1) * cos(q2)],
]))
assert (D.ang_vel_in(N) - C.ang_vel_in(N)).express(N).simplify() == 0
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_vector_angle():
A = ReferenceFrame('A')
v1 = A.x + A.y
v2 = A.z
assert v1.angle_between(v2) == pi/2
B = ReferenceFrame('B')
B.orient_axis(A, A.x, pi)
v3 = A.x
v4 = B.x
assert v3.angle_between(v4) == 0
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_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_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_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_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_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_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_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_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_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_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_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_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_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_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_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_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 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_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_body_add_force():
# Body with RigidBody.
rigidbody_masscenter = Point('rigidbody_masscenter')
rigidbody_mass = Symbol('rigidbody_mass')
rigidbody_frame = ReferenceFrame('rigidbody_frame')
body_inertia = inertia(rigidbody_frame, 1, 0, 0)
rigid_body = Body('rigidbody_body', rigidbody_masscenter, rigidbody_mass,
rigidbody_frame, body_inertia)
l = Symbol('l')
Fa = Symbol('Fa')
point = rigid_body.masscenter.locatenew('rigidbody_body_point0',
l * rigid_body.frame.x)
point.set_vel(rigid_body.frame, 0)
force_vector = Fa * rigid_body.frame.z
# apply_force with point
rigid_body.apply_force(force_vector, point)
assert len(rigid_body.loads) == 1
force_point = rigid_body.loads[0][0]
frame = rigid_body.frame
assert force_point.vel(frame) == point.vel(frame)
assert force_point.pos_from(force_point) == point.pos_from(force_point)
assert rigid_body.loads[0][1] == force_vector
# apply_force without point
rigid_body.apply_force(force_vector)
assert len(rigid_body.loads) == 2
assert rigid_body.loads[1][1] == force_vector
# passing something else than point
raises(TypeError, lambda: rigid_body.apply_force(force_vector, 0))
raises(TypeError, lambda: rigid_body.apply_force(0))
def test_apply_force():
f, g = symbols('f g')
q, x, v1, v2 = dynamicsymbols('q x v1 v2')
P1 = Point('P1')
P2 = Point('P2')
B1 = Body('B1')
B2 = Body('B2')
N = ReferenceFrame('N')
P1.set_vel(B1.frame, v1 * B1.x)
P2.set_vel(B2.frame, v2 * B2.x)
force = f * q * N.z # time varying force
B1.apply_force(force, P1, B2,
P2) #applying equal and opposite force on moving points
assert B1.loads == [(P1, force)]
assert B2.loads == [(P2, -force)]
g1 = B1.mass * g * N.y
g2 = B2.mass * g * N.y
B1.apply_force(g1) #applying gravity on B1 masscenter
B2.apply_force(g2) #applying gravity on B2 masscenter
assert B1.loads == [(P1, force), (B1.masscenter, g1)]
assert B2.loads == [(P2, -force), (B2.masscenter, g2)]
force2 = x * N.x
B1.apply_force(
force2, reaction_body=B2) #Applying time varying force on masscenter
assert B1.loads == [(P1, force), (B1.masscenter, force2 + g1)]
assert B2.loads == [(P2, -force), (B2.masscenter, -force2 + g2)]
def test_body_masscenter_vel():
A = Body('A')
N = ReferenceFrame('N')
B = Body('B', frame=N)
A.masscenter.set_vel(N, N.z)
assert A.masscenter_vel(B) == N.z
assert A.masscenter_vel(N) == N.z
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_dyadic_evalf():
N = ReferenceFrame('N')
a = pi * (N.x | N.x)
assert a.evalf(3) == Float('3.1416', 3) * (N.x | N.x)
s = symbols('s')
a = 5 * s * pi * (N.x | N.x)
assert a.evalf(2) == Float('5', 2) * Float('3.1416', 2) * s * (N.x | N.x)
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_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_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_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_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_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_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_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_issue_11503():
A = ReferenceFrame("A")
B = A.orientnew("B", "Axis", [35, A.y])
C = ReferenceFrame("C")
A.orient(C, "Axis", [70, C.z])
def test_issue_10348():
u = dynamicsymbols('u:3')
I = ReferenceFrame('I')
A = I.orientnew('A', 'space', u, 'XYZ')
from sympy import S, Integral, sin, cos, pi, sqrt, symbols
from sympy.physics.vector import Dyadic, Point, ReferenceFrame, Vector
from sympy.physics.vector.functions import (cross, dot, express,
time_derivative,
kinematic_equations, outer,
partial_velocity,
get_motion_params, dynamicsymbols)
from sympy.utilities.pytest import raises
Vector.simp = True
q1, q2, q3, q4, q5 = symbols('q1 q2 q3 q4 q5')
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])
def test_dot():
assert dot(A.x, A.x) == 1
assert dot(A.x, A.y) == 0
assert dot(A.x, A.z) == 0
assert dot(A.y, A.x) == 0
assert dot(A.y, A.y) == 1
assert dot(A.y, A.z) == 0
assert dot(A.z, A.x) == 0
assert dot(A.z, A.y) == 0
assert dot(A.z, A.z) == 1
from sympy.physics.mechanics import Lagrangian, LagrangesMethod
from sympy import symbols, sin, cos
q = q1, q2, q3 = dynamicsymbols('q1:4') # x, y, theta
qd = q1d, q2d, q3d = dynamicsymbols('q1:4', 1)
t, g, m, l, w, f, v0 = symbols('t g m l w f v0')
Fx, Fy = symbols('Fx Fy')
values = {
g: 9.81,
m: 20,
l: 2,
w: 1,
f: 2,
v0: 20}
N = ReferenceFrame('N')
B = N.orientnew('B', 'axis', [q3, N.z])
O = Point('O')
S = O.locatenew('S', q1*N.x + q2*N.y)
S.set_vel(N, S.pos_from(O).dt(N))
#Is = m/12*(l**2 + w**2)
Is = symbols('Is')
I = inertia(B, 0, 0, Is, 0, 0, 0)
rb = RigidBody('rb', S, B, m, (I, S))
rb.set_potential_energy(0)
L = Lagrangian(N, rb)
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_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_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_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)
