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_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_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 = {}
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 = {}
