def test_rigidbody2():
M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
N = ReferenceFrame('N')
b = ReferenceFrame('b')
b.set_ang_vel(N, omega * b.x)
P = Point('P')
I = outer(b.x, b.x)
Inertia_tuple = (I, P)
B = RigidBody('B', P, b, M, Inertia_tuple)
P.set_vel(N, v * b.x)
assert B.angular_momentum(P, N) == omega * b.x
O = Point('O')
O.set_vel(N, v * b.x)
P.set_pos(O, r * b.y)
assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z
B.potential_energy = M * g * h
assert B.potential_energy == M * g * h
assert expand(2 * B.kinetic_energy(N)) == omega**2 + M * v**2
def test_rigidbody2():
M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
N = ReferenceFrame('N')
b = ReferenceFrame('b')
b.set_ang_vel(N, omega * b.x)
P = Point('P')
I = outer(b.x, b.x)
Inertia_tuple = (I, P)
B = RigidBody('B', P, b, M, Inertia_tuple)
P.set_vel(N, v * b.x)
assert B.angular_momentum(P, N) == omega * b.x
O = Point('O')
O.set_vel(N, v * b.x)
P.set_pos(O, r * b.y)
assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z
B.set_potential_energy(M * g * h)
assert B.potential_energy == M * g * h
assert B.kinetic_energy(N) == (omega**2 + M * v**2) / 2
m, a, b = symbols('m a b')
q1, q2 = dynamicsymbols('q1, q2')
q1d, q2d = dynamicsymbols('q1, q2', level=1)
# reference frames
# N.x parallel to horizontal line, N.y parallel to line AC
N = ReferenceFrame('N')
A = N.orientnew('A', 'axis', [-q1, N.y])
B = A.orientnew('B', 'axis', [-q2, A.x])
# points B*, O
pO = Point('O')
pB_star = pO.locatenew('B*', S(1)/3*(2*a*B.x - b*B.y))
pO.set_vel(N, 0)
pB_star.v2pt_theory(pO, N, B)
# rigidbody B
I_B_Bs = inertia(B, m*b**2/18, m*a**2/18, m*(a**2 + b**2)/18)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
# kinetic energy
K = rbB.kinetic_energy(N)
print('K = {0}'.format(msprint(trigsimp(K))))
K_expected = m/4*((a**2 + b**2*sin(q2)**2/3)*q1d**2 +
a*b*cos(q2)*q1d*q2d + b**2*q2d**2/3)
print('diff = {0}'.format(msprint(expand(trigsimp(K - K_expected)))))
assert expand(trigsimp(K - K_expected)) == 0
# velocity of the disk at the point of contact with the ground is not moving
# since the disk rolls without slipping.
pA = Point('pA') # ball bearing A
pB = pA.locatenew('pB', -R * F1.y) # ball bearing B
pA.set_vel(N, 0)
pA.set_vel(F1, 0)
pB.set_vel(F1, 0)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
#pC.v2pt_theory(pB, N, B)
#print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ')
#print(pC.vel(N).express(N).subs(q2d, q2d_val))
Ixx = m * r**2 / 4
Iyy = m * r**2 / 4
Izz = m * r**2 / 2
I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB))
#T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val}).subs({theta: theta_val})
T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val})
print('T = {}'.format(msprint(simplify(T))))
values = {R: 1, r: 1, m: 0.5, theta: theta_val}
q1d_val = solve([-1 - q2d_val], q1d)[q1d]
#print(msprint(q1d_val))
print('T = {}'.format(msprint(simplify(T.subs(q1d, q1d_val).subs(values)))))
kde = [x - y for x, y in zip(u, qd)]
kde_map = solve(kde, qd)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])
# define disc rigidbody
IC = inertia(C, m*R**2/4, m*R**2/4, m*R**2/2)
rbC = RigidBody('rbC', pC_star, C, m, (IC, pC_star))
rbC.set_potential_energy(m*g*dot(pC_star.pos_from(pR), A.z))
# potential energy
V = rbC.potential_energy
print('V = {0}'.format(msprint(V)))
# kinetic energy
K = trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map))
print('K = {0}'.format(msprint(K)))
u_indep = [u1, u2, u3]
Fr = generalized_active_forces_K(K, q, u_indep, kde_map, vc_map)
# Fr + Fr* = 0 but the dynamical equations cannot be formulated by only
# kinetic energy as Fr = -Fr* for r = 1, ..., p
print('\ngeneralized active forces, Fr')
for i, x in enumerate(Fr, 1):
print('F{0} = {1}'.format(i, msprint(x)))
L = K - V
le = lagrange_equations(L, q, u, kde_map)
print('\nLagrange\'s equations of the second kind')
for i, x in enumerate(le, 1):
print('eq{0}: {1} = 0'.format(i, msprint(x)))
m, I11, I22, I33 = symbols('m I11 I22 I33', real=True, positive=True)
# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')
# points B*, O
pB_star = Point('B*')
pB_star.set_vel(A, 0)
# rigidbody B
I_B_Bs = inertia(B, I11, I22, I33)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
# kinetic energy
K = rbB.kinetic_energy(A) # velocity of point B* is zero
print('K_ω = {0}'.format(msprint(K)))
print('\nSince I11, I22, I33 are the central principal moments of inertia')
print('let I_min = I11, I_max = I33')
I_min = I11
I_max = I33
H = rbB.angular_momentum(pB_star, A)
K_min = dot(H, H) / I_max / 2
K_max = dot(H, H) / I_min / 2
print('K_ω_min = {0}'.format(msprint(K_min)))
print('K_ω_max = {0}'.format(msprint(K_max)))
print('\nI11/I33, I22/I33 =< 1, since I33 >= I11, I22, so K_ω_min <= K_ω')
print('Similarly, I22/I11, I33/I11 >= 1, '
'since I11 <= I22, I33, so K_ω_max >= K_ω')
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
# Since S is rolling against C, v_S^_C = 0.
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
vc += [dot(pS_hat.vel(C), basis) for basis in A]
# Cone has only angular velocity ω in R.z direction.
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc += [omega - dot(C.ang_vel_in(R), R.z)]
vc_map = solve(vc, u)
# cone rigidbody
I_C = inertia(A, I11, I22, J)
rbC = RigidBody('rbC', pO, C, M, (I_C, pO))
# sphere rigidbody
I_S = inertia(A, 2 * m * r**2 / 5, 2 * m * r**2 / 5, 2 * m * r**2 / 5)
rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star))
# kinetic energy
K = radsimp(
expand((rbC.kinetic_energy(R) + 4 * rbS.kinetic_energy(R)).subs(vc_map)))
print('K = {0}'.format(msprint(collect(K, omega**2 / 2))))
K_expected = (J + 18 * m * r**2 * (2 + sqrt(3)) / 5) * omega**2 / 2
#print('K_expected = {0}'.format(msprint(collect(expand(K_expected),
# omega**2/2))))
assert expand(K - K_expected) == 0
kde = [x - y for x, y in zip(u, qd)]
kde_map = solve(kde, qd)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])
# define disc rigidbody
IC = inertia(C, m * R ** 2 / 4, m * R ** 2 / 4, m * R ** 2 / 2)
rbC = RigidBody("rbC", pC_star, C, m, (IC, pC_star))
rbC.set_potential_energy(m * g * dot(pC_star.pos_from(pR), A.z))
# potential energy
V = rbC.potential_energy
print("V = {0}".format(msprint(V)))
# kinetic energy
K = trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map))
print("K = {0}".format(msprint(K)))
u_indep = [u1, u2, u3]
Fr = generalized_active_forces_K(K, q, u_indep, kde_map, vc_map)
# Fr + Fr* = 0 but the dynamical equations cannot be formulated by only
# kinetic energy as Fr = -Fr* for r = 1, ..., p
print("\ngeneralized active forces, Fr")
for i, x in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(x)))
L = K - V
le = lagrange_equations(L, q, u, kde_map)
print("\nLagrange's equations of the second kind")
for i, x in enumerate(le, 1):
print("eq{0}: {1} = 0".format(i, msprint(x)))
from sympy.physics.mechanics import dynamicsymbols, inertia, msprint
m, a, b = symbols('m a b')
q1, q2 = dynamicsymbols('q1, q2')
q1d, q2d = dynamicsymbols('q1, q2', level=1)
# reference frames
# N.x parallel to horizontal line, N.y parallel to line AC
N = ReferenceFrame('N')
A = N.orientnew('A', 'axis', [-q1, N.y])
B = A.orientnew('B', 'axis', [-q2, A.x])
# points B*, O
pO = Point('O')
pB_star = pO.locatenew('B*', S(1) / 3 * (2 * a * B.x - b * B.y))
pO.set_vel(N, 0)
pB_star.v2pt_theory(pO, N, B)
# rigidbody B
I_B_Bs = inertia(B, m * b**2 / 18, m * a**2 / 18, m * (a**2 + b**2) / 18)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
# kinetic energy
K = rbB.kinetic_energy(N)
print('K = {0}'.format(msprint(trigsimp(K))))
K_expected = m / 4 * ((a**2 + b**2 * sin(q2)**2 / 3) * q1d**2 +
a * b * cos(q2) * q1d * q2d + b**2 * q2d**2 / 3)
print('diff = {0}'.format(msprint(expand(trigsimp(K - K_expected)))))
assert expand(trigsimp(K - K_expected)) == 0
pC_hat.set_vel(C, 0)
# C* is the point at the center of disk C.
pC_star = pC_hat.locatenew('C*', R * B.y)
pC_star.set_vel(C, 0)
pC_star.set_vel(B, 0)
# calculate velocities in A
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)
## --- Expressions for generalized speeds u1, u2, u3, u4, u5 ---
u_expr = map(lambda x: dot(C.ang_vel_in(A), x), B)
u_expr += qd[3:]
kde = [u_i - u_ex for u_i, u_ex in zip(u, u_expr)]
kde_map = solve(kde, qd)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])
# define disc rigidbody
I_C = inertia(C, m * R**2 / 4, m * R**2 / 4, m * R**2 / 2)
rbC = RigidBody('rbC', pC_star, C, m, (I_C, pC_star))
# kinetic energy
K = collect(trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map)),
m * R**2 / 8)
print('K = {0}'.format(msprint(K)))
K_expected = (m * R**2 / 8) * (5 * u1**2 + u2**2 + 6 * u3**2)
assert expand(K - K_expected) == 0
pB.set_vel(F1, 0)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
#pC.v2pt_theory(pB, N, B)
#print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ')
#print(pC.vel(N).express(N).subs(q2d, q2d_val))
Ixx = m*r**2/4
Iyy = m*r**2/4
Izz = m*r**2/2
I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB))
T = rb_disc.kinetic_energy(N).subs({theta: theta_val, q2d: q2d_val})
print('T = {}'.format(msprint(simplify(T))))
from sympy import S
#T2 = q1d**2*(m*r**2/4*(S(5)/8 - (2*R + r)/(2*r) + (2*R + r)**2/(2*r)**2) + m*R**2/2)
T2 = m*q1d**2*R**2/2 + m*r**2/2 * (
(2*R + r)**2/(2*r)**2 * q1d**2 / 2 -
(2*R + r)**2/(2*r) * q1d**2 / 2 +
5*q1d**2/16)
from sympy import expand
print('T - T2 = {}'.format(msprint(expand(T - T2).simplify())))
t = symbols('t')
dT = T.diff(symbols('t'))
print('dT/dt = {} = 0'.format(msprint(simplify(dT))))
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
# Since S is rolling against C, v_S^_C = 0.
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
vc += [dot(pS_hat.vel(C), basis) for basis in A]
# Cone has only angular velocity ω in R.z direction.
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc += [omega - dot(C.ang_vel_in(R), R.z)]
vc_map = solve(vc, u)
# cone rigidbody
I_C = inertia(A, I11, I22, J)
rbC = RigidBody('rbC', pO, C, M, (I_C, pO))
# sphere rigidbody
I_S = inertia(A, 2*m*r**2/5, 2*m*r**2/5, 2*m*r**2/5)
rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star))
# kinetic energy
K = radsimp(expand((rbC.kinetic_energy(R) +
4*rbS.kinetic_energy(R)).subs(vc_map)))
print('K = {0}'.format(msprint(collect(K, omega**2/2))))
K_expected = (J + 18*m*r**2*(2 + sqrt(3))/5) * omega**2/2
#print('K_expected = {0}'.format(msprint(collect(expand(K_expected),
# omega**2/2))))
assert expand(K - K_expected) == 0
pC_hat.set_vel(C, 0)
# C* is the point at the center of disk C.
pC_star = pC_hat.locatenew('C*', R*B.y)
pC_star.set_vel(C, 0)
pC_star.set_vel(B, 0)
# calculate velocities in A
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)
## --- Expressions for generalized speeds u1, u2, u3, u4, u5 ---
u_expr = map(lambda x: dot(C.ang_vel_in(A), x), B)
u_expr += qd[3:]
kde = [u_i - u_ex for u_i, u_ex in zip(u, u_expr)]
kde_map = solve(kde, qd)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])
# define disc rigidbody
I_C = inertia(C, m*R**2/4, m*R**2/4, m*R**2/2)
rbC = RigidBody('rbC', pC_star, C, m, (I_C, pC_star))
# kinetic energy
K = collect(trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map)),
m*R**2/8)
print('K = {0}'.format(msprint(K)))
K_expected = (m*R**2/8) * (5*u1**2 + u2**2 + 6*u3**2)
assert expand(K - K_expected) == 0
rbC = RigidBody('rod_C', pC_star, C, m0, (I_rod, pC_star))
I_discA = inertia(A, m*r**2/2, m*r**2/4, m*r**2/4, 0, 0, 0)
rbA = RigidBody('disc_A', pA_star, A, m, (I_discA, pA_star))
I_discB = inertia(B, m*r**2/2, m*r**2/4, m*r**2/4, 0, 0, 0)
rbB = RigidBody('disc_B', pB_star, B, m, (I_discB, pB_star))
print('omega_A_N = {}'.format(msprint(A.ang_vel_in(N).express(C))))
print('v_pA*_N = {}'.format(msprint(pA_hat.vel(N))))
qd_val = solve([dot(pA_hat.vel(N), C.y), dot(pB_hat.vel(N), C.y)],
[q2d, q3d])
print(msprint(qd_val))
print('T_A = {}'.format(msprint(simplify(rbA.kinetic_energy(N).subs(qd_val)))))
print('T_B = {}'.format(msprint(simplify(rbB.kinetic_energy(N).subs(qd_val)))))
T = expand(simplify(
rbA.kinetic_energy(N).subs(qd_val) +
rbB.kinetic_energy(N).subs(qd_val) +
rbC.kinetic_energy(N)))
print('T = {}'.format(msprint(T)))
#T = rb_disc.kinetic_energy(N).subs({theta: theta_val, q2d: q2d_val})
#print('T = {}'.format(msprint(simplify(T))))
#
#t = symbols('t')
#dT = T.diff(symbols('t'))
#print('dT/dt = {} = 0'.format(msprint(simplify(dT))))
T2 = (m0*v**2/2 + m/2*((v + L*q1d/2)**2 + (v - L*q1d/2)**2) +
I_rod = inertia(C, 0, m0 * L**2 / 12, m0 * L**2 / 12, 0, 0, 0)
rbC = RigidBody('rod_C', pC_star, C, m0, (I_rod, pC_star))
I_discA = inertia(A, m * r**2 / 2, m * r**2 / 4, m * r**2 / 4, 0, 0, 0)
rbA = RigidBody('disc_A', pA_star, A, m, (I_discA, pA_star))
I_discB = inertia(B, m * r**2 / 2, m * r**2 / 4, m * r**2 / 4, 0, 0, 0)
rbB = RigidBody('disc_B', pB_star, B, m, (I_discB, pB_star))
print('omega_A_N = {}'.format(msprint(A.ang_vel_in(N).express(C))))
print('v_pA*_N = {}'.format(msprint(pA_hat.vel(N))))
qd_val = solve([dot(pA_hat.vel(N), C.y), dot(pB_hat.vel(N), C.y)], [q2d, q3d])
print(msprint(qd_val))
print('T_A = {}'.format(msprint(simplify(rbA.kinetic_energy(N).subs(qd_val)))))
print('T_B = {}'.format(msprint(simplify(rbB.kinetic_energy(N).subs(qd_val)))))
T = expand(
simplify(
rbA.kinetic_energy(N).subs(qd_val) +
rbB.kinetic_energy(N).subs(qd_val) + rbC.kinetic_energy(N)))
print('T = {}'.format(msprint(T)))
#T = rb_disc.kinetic_energy(N).subs({theta: theta_val, q2d: q2d_val})
#print('T = {}'.format(msprint(simplify(T))))
#
#t = symbols('t')
#dT = T.diff(symbols('t'))
#print('dT/dt = {} = 0'.format(msprint(simplify(dT))))
T2 = (m0 * v**2 / 2 + m / 2 * ((v + L * q1d / 2)**2 + (v - L * q1d / 2)**2) +
m, I11, I22, I33 = symbols('m I11 I22 I33', real=True, positive=True)
# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')
# points B*, O
pB_star = Point('B*')
pB_star.set_vel(A, 0)
# rigidbody B
I_B_Bs = inertia(B, I11, I22, I33)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
# kinetic energy
K = rbB.kinetic_energy(A) # velocity of point B* is zero
print('K_ω = {0}'.format(msprint(K)))
print('\nSince I11, I22, I33 are the central principal moments of inertia')
print('let I_min = I11, I_max = I33')
I_min = I11
I_max = I33
H = rbB.angular_momentum(pB_star, A)
K_min = dot(H, H) / I_max / 2
K_max = dot(H, H) / I_min / 2
print('K_ω_min = {0}'.format(msprint(K_min)))
print('K_ω_max = {0}'.format(msprint(K_max)))
print('\nI11/I33, I22/I33 =< 1, since I33 >= I11, I22, so K_ω_min <= K_ω')
print('Similarly, I22/I11, I33/I11 >= 1, '
'since I11 <= I22, I33, so K_ω_max >= K_ω')
pA.set_vel(F1, 0)
pB.set_vel(F1, 0)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
#pC.v2pt_theory(pB, N, B)
#print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ')
#print(pC.vel(N).express(N).subs(q2d, q2d_val))
Ixx = m * r**2 / 4
Iyy = m * r**2 / 4
Izz = m * r**2 / 2
I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB))
T = rb_disc.kinetic_energy(N).subs({theta: theta_val, q2d: q2d_val})
print('T = {}'.format(msprint(simplify(T))))
from sympy import S
#T2 = q1d**2*(m*r**2/4*(S(5)/8 - (2*R + r)/(2*r) + (2*R + r)**2/(2*r)**2) + m*R**2/2)
T2 = m * q1d**2 * R**2 / 2 + m * r**2 / 2 * (
(2 * R + r)**2 / (2 * r)**2 * q1d**2 / 2 - (2 * R + r)**2 /
(2 * r) * q1d**2 / 2 + 5 * q1d**2 / 16)
from sympy import expand
print('T - T2 = {}'.format(msprint(expand(T - T2).simplify())))
t = symbols('t')
dT = T.diff(symbols('t'))
print('dT/dt = {} = 0'.format(msprint(simplify(dT))))
# since the disk rolls without slipping.
pA = Point('pA') # ball bearing A
pB = pA.locatenew('pB', -R*F1.y) # ball bearing B
pA.set_vel(N, 0)
pA.set_vel(F1, 0)
pB.set_vel(F1, 0)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
#pC.v2pt_theory(pB, N, B)
#print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ')
#print(pC.vel(N).express(N).subs(q2d, q2d_val))
Ixx = m*r**2/4
Iyy = m*r**2/4
Izz = m*r**2/2
I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB))
#T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val}).subs({theta: theta_val})
T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val})
print('T = {}'.format(msprint(simplify(T))))
values = {R: 1, r: 1, m: 0.5, theta: theta_val}
q1d_val = solve([-1 - q2d_val], q1d)[q1d]
#print(msprint(q1d_val))
print('T = {}'.format(msprint(simplify(T.subs(q1d, q1d_val).subs(values)))))
