def test_inertia_of_point_mass():
r, s, t, m = symbols("r s t m")
N = ReferenceFrame("N")
px = r * N.x
I = inertia_of_point_mass(m, px, N)
assert I == m * r ** 2 * (N.y | N.y) + m * r ** 2 * (N.z | N.z)
py = s * N.y
I = inertia_of_point_mass(m, py, N)
assert I == m * s ** 2 * (N.x | N.x) + m * s ** 2 * (N.z | N.z)
pz = t * N.z
I = inertia_of_point_mass(m, pz, N)
assert I == m * t ** 2 * (N.x | N.x) + m * t ** 2 * (N.y | N.y)
p = px + py + pz
I = inertia_of_point_mass(m, p, N)
assert I == (
m * (s ** 2 + t ** 2) * (N.x | N.x)
- m * r * s * (N.x | N.y)
- m * r * t * (N.x | N.z)
- m * r * s * (N.y | N.x)
+ m * (r ** 2 + t ** 2) * (N.y | N.y)
- m * s * t * (N.y | N.z)
- m * r * t * (N.z | N.x)
- m * s * t * (N.z | N.y)
+ m * (r ** 2 + s ** 2) * (N.z | N.z)
)
def test_inertia_of_point_mass():
r, s, t, m = symbols('r s t m')
N = ReferenceFrame('N')
px = r * N.x
I = inertia_of_point_mass(m, px, N)
assert I == m * r**2 * (N.y | N.y) + m * r**2 * (N.z | N.z)
py = s * N.y
I = inertia_of_point_mass(m, py, N)
assert I == m * s**2 * (N.x | N.x) + m * s**2 * (N.z | N.z)
pz = t * N.z
I = inertia_of_point_mass(m, pz, N)
assert I == m * t**2 * (N.x | N.x) + m * t**2 * (N.y | N.y)
p = px + py + pz
I = inertia_of_point_mass(m, p, N)
assert I == (m * (s**2 + t**2) * (N.x | N.x) -
m * r * s * (N.x | N.y) -
m * r * t * (N.x | N.z) -
m * r * s * (N.y | N.x) +
m * (r**2 + t**2) * (N.y | N.y) -
m * s * t * (N.y | N.z) -
m * r * t * (N.z | N.x) -
m * s * t * (N.z | N.y) +
m * (r**2 + s**2) * (N.z | N.z))
def inertia_torque(rb, frame, kde_map=None, constraint_map=None, uaux=None):
# get central inertia
# I_S/O = I_S/S* + I_S*/O
I = rb.inertia[0] - inertia_of_point_mass(
rb.mass, rb.masscenter.pos_from(rb.inertia[1]), rb.frame)
alpha = rb.frame.ang_acc_in(frame)
omega = rb.frame.ang_vel_in(frame)
if not isinstance(alpha, Vector) and alpha == 0:
alpha = Vector([])
if not isinstance(omega, Vector) and omega == 0:
omega = Vector([])
if uaux is not None:
# auxilliary speeds do not change alpha, omega
# use doit() to evaluate terms such as
# Derivative(0, t) to 0.
uaux_zero = dict(zip(uaux, [0] * len(uaux)))
alpha = subs(alpha, uaux_zero)
omega = subs(omega, uaux_zero)
if kde_map is not None:
alpha = subs(alpha, kde_map)
omega = subs(omega, kde_map)
if constraint_map is not None:
alpha = subs(alpha, constraint_map)
omega = subs(omega, constraint_map)
return -dot(alpha, I) - dot(cross(omega, I), omega)
def inertia_torque(rb, frame, kde_map=None, constraint_map=None, uaux=None):
# get central inertia
# I_S/O = I_S/S* + I_S*/O
I = rb.inertia[0] - inertia_of_point_mass(rb.mass,
rb.masscenter.pos_from(rb.inertia[1]), rb.frame)
alpha = rb.frame.ang_acc_in(frame)
omega = rb.frame.ang_vel_in(frame)
if not isinstance(alpha, Vector) and alpha == 0:
alpha = Vector([])
if not isinstance(omega, Vector) and omega == 0:
omega = Vector([])
if uaux is not None:
# auxilliary speeds do not change alpha, omega
# use doit() to evaluate terms such as
# Derivative(0, t) to 0.
uaux_zero = dict(zip(uaux, [0] * len(uaux)))
alpha = subs(alpha, uaux_zero)
omega = subs(omega, uaux_zero)
if kde_map is not None:
alpha = subs(alpha, kde_map)
omega = subs(omega, kde_map)
if constraint_map is not None:
alpha = subs(alpha, constraint_map)
omega = subs(omega, constraint_map)
return -dot(alpha, I) - dot(cross(omega, I), omega)
def test_rigidbody3():
q1, q2, q3, q4 = dynamicsymbols('q1:5')
p1, p2, p3 = symbols('p1:4')
m = symbols('m')
A = ReferenceFrame('A')
B = A.orientnew('B', 'axis', [q1, A.x])
O = Point('O')
O.set_vel(A, q2 * A.x + q3 * A.y + q4 * A.z)
P = O.locatenew('P', p1 * B.x + p2 * B.y + p3 * B.z)
I = outer(B.x, B.x)
rb1 = RigidBody('rb1', P, B, m, (I, P))
# I_S/O = I_S/S* + I_S*/O
rb2 = RigidBody('rb2', P, B, m,
(I + inertia_of_point_mass(m, P.pos_from(O), B), O))
assert rb1.central_inertia == rb2.central_inertia
assert rb1.angular_momentum(O, A) == rb2.angular_momentum(O, A)
def test_rigidbody3():
q1, q2, q3, q4 = dynamicsymbols('q1:5')
p1, p2, p3 = symbols('p1:4')
m = symbols('m')
A = ReferenceFrame('A')
B = A.orientnew('B', 'axis', [q1, A.x])
O = Point('O')
O.set_vel(A, q2*A.x + q3*A.y + q4*A.z)
P = O.locatenew('P', p1*B.x + p2*B.y + p3*B.z)
I = outer(B.x, B.x)
rb1 = RigidBody('rb1', P, B, m, (I, P))
# I_S/O = I_S/S* + I_S*/O
rb2 = RigidBody('rb2', P, B, m,
(I + inertia_of_point_mass(m, P.pos_from(O), B), O))
assert rb1.central_inertia == rb2.central_inertia
assert rb1.angular_momentum(O, A) == rb2.angular_momentum(O, A)
def test_rigidbody3():
q1, q2, q3, q4 = dynamicsymbols("q1:5")
p1, p2, p3 = symbols("p1:4")
m = symbols("m")
A = ReferenceFrame("A")
B = A.orientnew("B", "axis", [q1, A.x])
O = Point("O")
O.set_vel(A, q2 * A.x + q3 * A.y + q4 * A.z)
P = O.locatenew("P", p1 * B.x + p2 * B.y + p3 * B.z)
P.v2pt_theory(O, A, B)
I = outer(B.x, B.x)
rb1 = RigidBody("rb1", P, B, m, (I, P))
# I_S/O = I_S/S* + I_S*/O
rb2 = RigidBody("rb2", P, B, m,
(I + inertia_of_point_mass(m, P.pos_from(O), B), O))
assert rb1.central_inertia == rb2.central_inertia
assert rb1.angular_momentum(O, A) == rb2.angular_momentum(O, A)
def parallel_axis(self, point, frame):
"""Returns an inertia dyadic of the particle with respect to another
point and frame.
Parameters
==========
point : sympy.physics.vector.Point
The point to express the inertia dyadic about.
frame : sympy.physics.vector.ReferenceFrame
The reference frame used to construct the dyadic.
Returns
=======
inertia : sympy.physics.vector.Dyadic
The inertia dyadic of the particle expressed about the provided
point and frame.
"""
# circular import issue
from sympy.physics.mechanics import inertia_of_point_mass
return inertia_of_point_mass(self.mass, self.point.pos_from(point),
frame)
from sympy.physics.mechanics import dot
from sympy import symbols
from sympy import S
m = symbols('m')
m_val = 12
N = ReferenceFrame('N')
pO = Point('O')
pBs = pO.locatenew('B*', -3*N.x + 2*N.y - 4*N.z)
I_B_O = inertia(N, 260*m/m_val, 325*m/m_val, 169*m/m_val,
72*m/m_val, 96*m/m_val, -144*m/m_val)
print("I_B_rel_O = {0}".format(I_B_O))
I_Bs_O = inertia_of_point_mass(m, pBs.pos_from(pO), N)
print("\nI_B*_rel_O = {0}".format(I_Bs_O))
I_B_Bs = I_B_O - I_Bs_O
print("\nI_B_rel_B* = {0}".format(I_B_Bs))
pQ = pO.locatenew('Q', -4*N.z)
I_Bs_Q = inertia_of_point_mass(m, pBs.pos_from(pQ), N)
print("\nI_B*_rel_Q = {0}".format(I_Bs_Q))
I_B_Q = I_B_Bs + I_Bs_Q
print("\nI_B_rel_Q = {0}".format(I_B_Q))
# n_a is a vector parallel to line PQ
n_a = S(3)/5 * N.x - S(4)/5 * N.z
I_a_a_B_Q = dot(dot(n_a, I_B_Q), n_a)
from sympy.physics.mechanics import cross, dot
from sympy import solve, sqrt, symbols, integrate
from sympy import sin, cos, tan, pi, S
m = symbols('m')
m_val = 12
N = ReferenceFrame('N')
pO = Point('O')
pBs = pO.locatenew('B*', -3 * N.x + 2 * N.y - 4 * N.z)
I_B_O = inertia(N, 260 * m / m_val, 325 * m / m_val, 169 * m / m_val,
72 * m / m_val, 96 * m / m_val, -144 * m / m_val)
print("I_B_rel_O = {0}".format(I_B_O))
I_Bs_O = inertia_of_point_mass(m, pBs.pos_from(pO), N)
print("\nI_B*_rel_O = {0}".format(I_Bs_O))
I_B_Bs = I_B_O - I_Bs_O
print("\nI_B_rel_B* = {0}".format(I_B_Bs))
pQ = pO.locatenew('Q', -4 * N.z)
I_Bs_Q = inertia_of_point_mass(m, pBs.pos_from(pQ), N)
print("\nI_B*_rel_Q = {0}".format(I_Bs_Q))
I_B_Q = I_B_Bs + I_Bs_Q
print("\nI_B_rel_Q = {0}".format(I_B_Q))
# n_a is a vector parallel to line PQ
n_a = S(3) / 5 * N.x - S(4) / 5 * N.z
I_a_a_B_Q = dot(dot(n_a, I_B_Q), n_a)
N2 = N.orientnew('N2', 'Axis', [theta, N.x])
# calculate the mass center of the assembly
# Point O is located at the right angle corner of triangle 1.
pCs_1 = pO.locatenew('C*_1', 1/S(3) * (a*N.y + b*N.x))
pCs_2 = pO.locatenew('C*_2', 1/S(3) * (a*N2.y + b*N2.x))
pBs = pO.locatenew('B*', 1/(2*m) * m * (pCs_1.pos_from(pO) +
pCs_2.pos_from(pO)))
print("\nB* = {0}".format(pBs.pos_from(pO).express(N)))
# central inertia of each triangle
I1_C_Cs = inertia_rt(N, b, a, m)
I2_C_Cs = inertia_rt(N2, b, a, m)
# inertia of mass center of each triangle about Point B*
I1_Cs_Bs = inertia_of_point_mass(m, pCs_1.pos_from(pBs), N)
I2_Cs_Bs = inertia_of_point_mass(m, pCs_2.pos_from(pBs), N)
I_B_Bs = I1_C_Cs + I1_Cs_Bs + I2_C_Cs + I2_Cs_Bs
print("\nI_B_Bs = {0}".format(I_B_Bs.express(N)))
# central principal moments of inertia
evals = inertia_matrix(I_B_Bs, N).eigenvals()
print("\neigenvalues:")
for e in evals.iterkeys():
print(e)
print("\nuse a/b = r")
evals_sub_a = [simplify(e.subs(a, r*b)) for e in evals.iterkeys()]
for e in evals_sub_a:
def test_non_central_inertia():
# This tests that the calculation of Fr* does not depend the point
# about which the inertia of a rigid body is defined. This test solves
# exercises 8.12, 8.17 from Kane 1985.
# Declare symbols
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
Q1, Q2, Q3 = symbols('Q1, Q2 Q3')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
# Reference Frames
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
A.set_ang_vel(F, u1*A.x + u3*A.z)
# define frames for wheels
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
B.set_ang_vel(A, u4 * A.z)
C.set_ang_vel(A, u5 * A.z)
# define points D, S*, Q on frame A and their velocities
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll without slip.
pD.set_vel(F, u2 * A.y)
pS_star = pD.locatenew('S*', e*A.y)
pQ = pD.locatenew('Q', f*A.y - R*A.x)
for p in [pS_star, pQ]:
p.v2pt_theory(pD, F, A)
# masscenters of bodies A, B, C
pA_star = pD.locatenew('A*', a*A.y)
pB_star = pD.locatenew('B*', b*A.z)
pC_star = pD.locatenew('C*', -b*A.z)
for p in [pA_star, pB_star, pC_star]:
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R*A.x)
pC_hat = pC_star.locatenew('C^', -R*A.x)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# the velocities of B^, C^ are zero since B, C are assumed to roll without slip
kde = [q1d - u1, q2d - u4, q3d - u5]
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
km = KanesMethod(F, q_ind=[q1, q2, q3], u_ind=[u1, u2], kd_eqs=kde,
u_dependent=[u4, u5], velocity_constraints=vc,
u_auxiliary=[u3])
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
bodies = [rbA, rbB, rbC]
fr, fr_star = km.kanes_equations(forces, bodies)
vc_map = solve(vc, [u4, u5])
# KanesMethod returns the negative of Fr, Fr* as defined in Kane1985.
fr_star_expected = Matrix([
-(IA + 2*J*b**2/R**2 + 2*K +
mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2,
-(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2,
0])
assert (trigsimp(fr_star.subs(vc_map).subs(u3, 0)).doit().expand() ==
fr_star_expected.expand())
# define inertias of rigid bodies A, B, C about point D
# I_S/O = I_S/S* + I_S*/O
bodies2 = []
for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]):
I = I_star + inertia_of_point_mass(rb.mass,
rb.masscenter.pos_from(pD),
rb.frame)
bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass,
(I, pD)))
fr2, fr_star2 = km.kanes_equations(forces, bodies2)
assert (trigsimp(fr_star2.subs(vc_map).subs(u3, 0)).doit().expand() ==
fr_star_expected.expand())
from sympy import symbols, simplify, latex
from sympy.physics.mechanics import (inertia, inertia_of_point_mass, mprint,
mlatex, Point, ReferenceFrame)
Ixx, Iyy, Izz, Ixz, I, J = symbols("I_xx I_yy I_zz I_xz I J")
mA, mB, lx, lz = symbols("m_A m_B l_x l_z")
A = ReferenceFrame("A")
IA_AO = inertia(A, Ixx, Iyy, Izz, 0, 0, Ixz)
IB_BO = inertia(A, I, J, I)
BO = Point('BO')
AO = BO.locatenew('AO', lx * A.x + lz * A.z)
CO = BO.locatenew('CO', mA / (mA + mB) * AO.pos_from(BO))
print(mlatex(CO.pos_from(BO)))
IC_CO = IA_AO + IB_BO +\
inertia_of_point_mass(mA, AO.pos_from(CO), A) +\
inertia_of_point_mass(mB, BO.pos_from(CO), A)
mprint((A.x & IC_CO & A.x).expand())
mprint((A.y & IC_CO & A.y).expand())
mprint((A.z & IC_CO & A.z).expand())
mprint((A.x & IC_CO & A.z).expand())
from sympy import sin, cos, tan, pi, S, Matrix
from sympy import simplify, Abs
b, m, k_a = symbols('b m k_a', real=True, nonnegative=True)
theta = symbols('theta', real=True)
N = ReferenceFrame('N')
N2 = N.orientnew('N2', 'Axis', [theta, N.z])
pO = Point('O')
pP1s = pO.locatenew('P1*', b/2 * (N.x + N.y))
pP2s = pO.locatenew('P2*', b/2 * (2*N.x + N.y + N.z))
pP3s = pO.locatenew('P3*', b/2 * ((2 + sin(theta))*N.x +
(2 - cos(theta))*N.y +
N.z))
I_1s_O = inertia_of_point_mass(m, pP1s.pos_from(pO), N)
I_2s_O = inertia_of_point_mass(m, pP2s.pos_from(pO), N)
I_3s_O = inertia_of_point_mass(m, pP3s.pos_from(pO), N)
print("\nI_1s_rel_O = {0}".format(I_1s_O))
print("\nI_2s_rel_O = {0}".format(I_2s_O))
print("\nI_3s_rel_O = {0}".format(I_3s_O))
I_1_1s = inertia(N, m*b**2/12, m*b**2/12, 2*m*b**2/12)
I_2_2s = inertia(N, 2*m*b**2/12, m*b**2/12, m*b**2/12)
I_3_3s_N2 = Matrix([[2, 0, 0],
[0, 1, 0],
[0, 0, 1]])
I_3_3s_N = m*b**2/12 * simplify(N.dcm(N2) * I_3_3s_N2 * N2.dcm(N))
I_3_3s = inertia(N, I_3_3s_N[0, 0], I_3_3s_N[1, 1], I_3_3s_N[2, 2],
angle = (acos(dot(a, b) / (a.magnitude() * b.magnitude())) * 180 /
pi).evalf()
return min(angle, 180 - angle)
m, m_R, m_C, rho, r = symbols('m m_R m_C rho r', real=True, nonnegative=True)
N = ReferenceFrame('N')
pA = Point('A')
pPs = pA.locatenew('P*', 3 * r * N.x - 2 * r * N.y)
m_R = rho * 24 * r**2
m_C = rho * pi * r**2
m = m_R - m_C
I_Cs_A = inertia_of_point_mass(m, pPs.pos_from(pA), N)
I_C_Cs = inertia(N,
m_R * (4 * r)**2 / 12 - m_C * r**2 / 4,
m_R * (6 * r)**2 / 12 - m_C * r**2 / 4,
m_R * ((4 * r)**2 + (6 * r)**2) / 12 - m_C * r**2 / 2)
I_C_A = I_C_Cs + I_Cs_A
print("\nI_C_rel_A = {0}".format(I_C_A))
# Eigenvectors of I_C_A are the parallel to the principal axis for point A
# of Body C.
evecs_m = [triple[2] for triple in inertia_matrix(I_C_A, N).eigenvects()]
# Convert eigenvectors from Matrix type to Vector type.
evecs = [
sum(simplify(v[0][i]).evalf() * n for i, n in enumerate(N))
from sympy import solve, sqrt, symbols, integrate, diff
from sympy import sin, cos, tan, pi, S, Matrix
from sympy import simplify, Abs
b, m, k_a = symbols('b m k_a', real=True, nonnegative=True)
theta = symbols('theta', real=True)
N = ReferenceFrame('N')
N2 = N.orientnew('N2', 'Axis', [theta, N.z])
pO = Point('O')
pP1s = pO.locatenew('P1*', b / 2 * (N.x + N.y))
pP2s = pO.locatenew('P2*', b / 2 * (2 * N.x + N.y + N.z))
pP3s = pO.locatenew(
'P3*', b / 2 * ((2 + sin(theta)) * N.x + (2 - cos(theta)) * N.y + N.z))
I_1s_O = inertia_of_point_mass(m, pP1s.pos_from(pO), N)
I_2s_O = inertia_of_point_mass(m, pP2s.pos_from(pO), N)
I_3s_O = inertia_of_point_mass(m, pP3s.pos_from(pO), N)
print("\nI_1s_rel_O = {0}".format(I_1s_O))
print("\nI_2s_rel_O = {0}".format(I_2s_O))
print("\nI_3s_rel_O = {0}".format(I_3s_O))
I_1_1s = inertia(N, m * b**2 / 12, m * b**2 / 12, 2 * m * b**2 / 12)
I_2_2s = inertia(N, 2 * m * b**2 / 12, m * b**2 / 12, m * b**2 / 12)
I_3_3s_N2 = Matrix([[2, 0, 0], [0, 1, 0], [0, 0, 1]])
I_3_3s_N = m * b**2 / 12 * simplify(N.dcm(N2) * I_3_3s_N2 * N2.dcm(N))
I_3_3s = inertia(N, I_3_3s_N[0, 0], I_3_3s_N[1, 1], I_3_3s_N[2, 2],
I_3_3s_N[0, 1], I_3_3s_N[1, 2], I_3_3s_N[2, 0])
I_1_O = I_1_1s + I_1s_O
N2 = N.orientnew('N2', 'Axis', [theta, N.x])
# calculate the mass center of the assembly
# Point O is located at the right angle corner of triangle 1.
pCs_1 = pO.locatenew('C*_1', 1 / S(3) * (a * N.y + b * N.x))
pCs_2 = pO.locatenew('C*_2', 1 / S(3) * (a * N2.y + b * N2.x))
pBs = pO.locatenew('B*',
1 / (2 * m) * m * (pCs_1.pos_from(pO) + pCs_2.pos_from(pO)))
print("\nB* = {0}".format(pBs.pos_from(pO).express(N)))
# central inertia of each triangle
I1_C_Cs = inertia_rt(N, b, a, m)
I2_C_Cs = inertia_rt(N2, b, a, m)
# inertia of mass center of each triangle about Point B*
I1_Cs_Bs = inertia_of_point_mass(m, pCs_1.pos_from(pBs), N)
I2_Cs_Bs = inertia_of_point_mass(m, pCs_2.pos_from(pBs), N)
I_B_Bs = I1_C_Cs + I1_Cs_Bs + I2_C_Cs + I2_Cs_Bs
print("\nI_B_Bs = {0}".format(I_B_Bs.express(N)))
# central principal moments of inertia
evals = inertia_matrix(I_B_Bs, N).eigenvals()
print("\neigenvalues:")
for e in evals.iterkeys():
print(e)
print("\nuse a/b = r")
evals_sub_a = [simplify(e.subs(a, r * b)) for e in evals.iterkeys()]
for e in evals_sub_a:
# Mass
# ====
a_mass, b_mass, c_mass = symbols("m_a, m_b, m_c")
# Inertia
# =======
rotI = lambda I, f: Matrix([j & I & i for i in f for j in f]).reshape(3, 3)
one_inertia_dyadic_dp1 = inertia(
one_frame_dp1,
0,
0,
rotI(inertia_of_point_mass(a_mass, one_mass_center_dp1.pos_from(one_dp1), one_frame_dp1), one_frame_dp1)[8],
)
one_central_inertia_dp1 = (one_inertia_dyadic_dp1, one_mass_center_dp1)
two_inertia_dyadic_dp1 = inertia(
two_frame_dp1,
0,
0,
rotI(inertia_of_point_mass(b_mass + c_mass, two_mass_center_dp1.pos_from(one_dp1), one_frame_dp1), one_frame_dp1)[
8
],
)
two_central_inertia_dp1 = (two_inertia_dyadic_dp1, two_mass_center_dp1)
IB_BO = (R.x & IB_BO & R.x)*(R.x|R.x) +\
(R.y & IB_BO & R.y)*(R.y|R.y) +\
(R.z & IB_BO & R.z)*(R.z|R.z) +\
(R.x & IB_BO & R.z)*(R.x|R.z) +\
(R.z & IB_BO & R.x)*(R.z|R.x)
IH_HO = (R.x & IH_HO & R.x)*(R.x|R.x) +\
(R.y & IH_HO & R.y)*(R.y|R.y) +\
(R.z & IH_HO & R.z)*(R.z|R.z) +\
(R.x & IH_HO & R.z)*(R.x|R.z) +\
(R.z & IH_HO & R.x)*(R.z|R.x)
# Inertia of rear and front assemblies, expressed in R frame (same as F frame when
# steer is zero, as is the case in reference configuration)
IRear = IR_RO + IB_BO +\
inertia_of_point_mass(mR, RO.pos_from(RO_BO_mc).express(R), R) +\
inertia_of_point_mass(mB, BO.pos_from(RO_BO_mc).express(R), R)
IFront = IF_FO + IH_HO +\
inertia_of_point_mass(mF, FO.pos_from(FO_HO_mc).express(R), R) +\
inertia_of_point_mass(mH, HO.pos_from(FO_HO_mc).express(R), R)
expressions = [
("rear_.Ixx", R.x & IRear & R.x),
("rear_.Iyy", R.y & IRear & R.y),
("rear_.Izz", R.z & IRear & R.z),
("rear_.Ixz", R.x & IRear & R.z),
("rear_.J", IRyy),
("rear_.m", mR + mB),
("rear_.R", rR),
("rear_.r", tR),
frame_a.orient(body_r_f, 'DCM', _sm.Matrix([1,1,1,1,1,0,0,0,1]).reshape(3, 3))
point_o = _me.Point('o')
m1 = _sm.symbols('m1')
particle_p1.mass = m1
m2 = _sm.symbols('m2')
particle_p2.mass = m2
mr = _sm.symbols('mr')
body_r.mass = mr
i1 = _sm.symbols('i1')
i2 = _sm.symbols('i2')
i3 = _sm.symbols('i3')
body_r.inertia = (_me.inertia(body_r_f, i1, i2, i3, 0, 0, 0), body_r_cm)
point_o.set_pos(particle_p1.point, c1*frame_a.x)
point_o.set_pos(particle_p2.point, c2*frame_a.y)
point_o.set_pos(body_r_cm, c3*frame_a.z)
a = _me.inertia_of_point_mass(particle_p1.mass, particle_p1.point.pos_from(point_o), frame_a)
a = _me.inertia_of_point_mass(particle_p2.mass, particle_p2.point.pos_from(point_o), frame_a)
a = body_r.inertia[0] + _me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
a = _me.inertia_of_point_mass(particle_p1.mass, particle_p1.point.pos_from(point_o), frame_a) + _me.inertia_of_point_mass(particle_p2.mass, particle_p2.point.pos_from(point_o), frame_a) + body_r.inertia[0] + _me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
a = _me.inertia_of_point_mass(particle_p1.mass, particle_p1.point.pos_from(point_o), frame_a) + body_r.inertia[0] + _me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
a = body_r.inertia[0] + _me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
a = body_r.inertia[0]
particle_p2.point.set_pos(particle_p1.point, c1*frame_a.x+c2*frame_a.y)
body_r_cm.set_pos(particle_p1.point, c3*frame_a.x)
body_r_cm.set_pos(particle_p2.point, c3*frame_a.y)
b = _me.functions.center_of_mass(point_o,particle_p1, particle_p2, body_r)
b = _me.functions.center_of_mass(point_o,particle_p1, body_r)
b = _me.functions.center_of_mass(particle_p1.point,particle_p1, particle_p2, body_r)
u1, u2, u3 = _me.dynamicsymbols('u1 u2 u3')
v = u1*frame_a.x+u2*frame_a.y+u3*frame_a.z
u = (v+c1*frame_a.x).normalize()
r_a = sm.Matrix([1,1,1,1,1,0,0,0,1]).reshape(3, 3)
point_o=me.Point('o')
m1=sm.symbols('m1')
particle_p1.mass = m1
m2=sm.symbols('m2')
particle_p2.mass = m2
mr=sm.symbols('mr')
body_r.mass = mr
i1 = sm.symbols('i1')
i2 = sm.symbols('i2')
i3 = sm.symbols('i3')
body_r.inertia = (me.inertia(body_r_f, i1, i2, i3, 0, 0, 0), body_r_cm)
point_o.set_pos(particle_p1.point, c1*frame_a.x)
point_o.set_pos(particle_p2.point, c2*frame_a.y)
point_o.set_pos(body_r_cm, c3*frame_a.z)
a=me.inertia_of_point_mass(particle_p1.mass, particle_p1.point.pos_from(point_o), frame_a)
a=me.inertia_of_point_mass(particle_p2.mass, particle_p2.point.pos_from(point_o), frame_a)
a=body_r.inertia[0] + me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
a=me.inertia_of_point_mass(particle_p1.mass, particle_p1.point.pos_from(point_o), frame_a) + me.inertia_of_point_mass(particle_p2.mass, particle_p2.point.pos_from(point_o), frame_a) + body_r.inertia[0] + me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
a=me.inertia_of_point_mass(particle_p1.mass, particle_p1.point.pos_from(point_o), frame_a) + body_r.inertia[0] + me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
a=body_r.inertia[0] + me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
a=body_r.inertia[0]
particle_p2.point.set_pos(particle_p1.point, c1*frame_a.x+c2*frame_a.y)
body_r_cm.set_pos(particle_p1.point, c3*frame_a.x)
body_r_cm.set_pos(particle_p2.point, c3*frame_a.y)
b=me.functions.center_of_mass(point_o,particle_p1, particle_p2, body_r)
b=me.functions.center_of_mass(point_o,particle_p1, body_r)
b=me.functions.center_of_mass(particle_p1.point,particle_p1, particle_p2, body_r)
u1, u2, u3 = me.dynamicsymbols('u1 u2 u3')
v=u1*frame_a.x+u2*frame_a.y+u3*frame_a.z
u=(v+c1*frame_a.x).normalize()
IB_BO = (R.x & IB_BO & R.x)*(R.x|R.x) +\
(R.y & IB_BO & R.y)*(R.y|R.y) +\
(R.z & IB_BO & R.z)*(R.z|R.z) +\
(R.x & IB_BO & R.z)*(R.x|R.z) +\
(R.z & IB_BO & R.x)*(R.z|R.x)
IH_HO = (R.x & IH_HO & R.x)*(R.x|R.x) +\
(R.y & IH_HO & R.y)*(R.y|R.y) +\
(R.z & IH_HO & R.z)*(R.z|R.z) +\
(R.x & IH_HO & R.z)*(R.x|R.z) +\
(R.z & IH_HO & R.x)*(R.z|R.x)
# Inertia of rear and front assemblies, expressed in R frame (same as F frame when
# steer is zero, as is the case in reference configuration)
IRear = IR_RO + IB_BO +\
inertia_of_point_mass(mR, RO.pos_from(RO_BO_mc).express(R), R) +\
inertia_of_point_mass(mB, BO.pos_from(RO_BO_mc).express(R), R)
IFront = IF_FO + IH_HO +\
inertia_of_point_mass(mF, FO.pos_from(FO_HO_mc).express(R), R) +\
inertia_of_point_mass(mH, HO.pos_from(FO_HO_mc).express(R), R)
expressions = [("rear_.Ixx", R.x & IRear & R.x),
("rear_.Iyy", R.y & IRear & R.y),
("rear_.Izz", R.z & IRear & R.z),
("rear_.Ixz", R.x & IRear & R.z), ("rear_.J", IRyy),
("rear_.m", mR + mB), ("rear_.R", rR), ("rear_.r", tR),
("rear_.a", RO_BO_mc.pos_from(RO) & R.x),
("rear_.b", RO_BO_mc.pos_from(RO) & R.z),
("rear_.c",
((w + c) * N.x + ((rR + tR) * N.z & R.x) * R.x) & R.x),
m = symbols('m', real=True, nonnegative=True)
m_val = 1
N = ReferenceFrame('N')
pO = Point('O')
pP = pO.locatenew('P', -3 * N.y)
pQ = pO.locatenew('Q', -4 * N.z)
pR = pO.locatenew('R', 2 * N.x)
points = [pO, pP, pQ, pR]
# center of mass of assembly
pCs = pO.locatenew('C*', sum(p.pos_from(pO) for p in points) / S(len(points)))
print(pCs.pos_from(pO))
I_C_Cs = sum(inertia_of_point_mass(m, p.pos_from(pCs), N) for p in points)
print("I_C_Cs = {0}".format(I_C_Cs))
# calculate the principal moments of inertia and the principal axes
M = inertia_matrix(I_C_Cs, N)
# use numpy to find eigenvalues/eigenvectors since sympy failed
# note that the eigenvlaues/eigenvectors are the
# prinicpal moments of inertia/principal axes
eigvals, eigvecs_np = np.linalg.eigh(np.matrix(M.subs(m, m_val).n().tolist()))
eigvecs = [sum(eigvecs_np[i, j] * n for i, n in enumerate(N))
for j in range(3)]
# get the minimum moment of inertia and its associated principal axis
e, v = min(zip(eigvals, eigvecs))
from sympy import symbols, simplify, latex
from sympy.physics.mechanics import (inertia, inertia_of_point_mass, mprint,
mlatex, Point, ReferenceFrame)
Ixx, Iyy, Izz, Ixz, I, J = symbols("I_xx I_yy I_zz I_xz I J")
mA, mB, lx, lz = symbols("m_A m_B l_x l_z")
A = ReferenceFrame("A")
IA_AO = inertia(A, Ixx, Iyy, Izz, 0, 0, Ixz)
IB_BO = inertia(A, I, J, I)
BO = Point('BO')
AO = BO.locatenew('AO', lx*A.x + lz*A.z)
CO = BO.locatenew('CO', mA / (mA + mB) * AO.pos_from(BO))
print(mlatex(CO.pos_from(BO)))
IC_CO = IA_AO + IB_BO +\
inertia_of_point_mass(mA, AO.pos_from(CO), A) +\
inertia_of_point_mass(mB, BO.pos_from(CO), A)
mprint((A.x & IC_CO & A.x).expand())
mprint((A.y & IC_CO & A.y).expand())
mprint((A.z & IC_CO & A.z).expand())
mprint((A.x & IC_CO & A.z).expand())
m = symbols('m', real=True, nonnegative=True)
m_val = 1
N = ReferenceFrame('N')
pO = Point('O')
pP = pO.locatenew('P', -3 * N.y)
pQ = pO.locatenew('Q', -4 * N.z)
pR = pO.locatenew('R', 2 * N.x)
points = [pO, pP, pQ, pR]
# center of mass of assembly
pCs = pO.locatenew('C*', sum(p.pos_from(pO) for p in points) / S(len(points)))
print(pCs.pos_from(pO))
I_C_Cs = sum(inertia_of_point_mass(m, p.pos_from(pCs), N) for p in points)
print("I_C_Cs = {0}".format(I_C_Cs))
# calculate the principal moments of inertia and the principal axes
M = inertia_matrix(I_C_Cs, N)
# use numpy to find eigenvalues/eigenvectors since sympy failed
# note that the eigenvlaues/eigenvectors are the
# prinicpal moments of inertia/principal axes
eigvals, eigvecs_np = np.linalg.eigh(np.matrix(M.subs(m, m_val).n().tolist()))
eigvecs = [
sum(eigvecs_np[i, j] * n for i, n in enumerate(N)) for j in range(3)
]
# get the minimum moment of inertia and its associated principal axis
e, v = min(zip(eigvals, eigvecs))
def test_non_central_inertia():
# This tests that the calculation of Fr* does not depend the point
# about which the inertia of a rigid body is defined. This test solves
# exercises 8.12, 8.17 from Kane 1985.
# Declare symbols
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
Q1, Q2, Q3 = symbols('Q1, Q2 Q3')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
# Reference Frames
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
A.set_ang_vel(F, u1 * A.x + u3 * A.z)
# define frames for wheels
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
B.set_ang_vel(A, u4 * A.z)
C.set_ang_vel(A, u5 * A.z)
# define points D, S*, Q on frame A and their velocities
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll without slip.
pD.set_vel(F, u2 * A.y)
pS_star = pD.locatenew('S*', e * A.y)
pQ = pD.locatenew('Q', f * A.y - R * A.x)
for p in [pS_star, pQ]:
p.v2pt_theory(pD, F, A)
# masscenters of bodies A, B, C
pA_star = pD.locatenew('A*', a * A.y)
pB_star = pD.locatenew('B*', b * A.z)
pC_star = pD.locatenew('C*', -b * A.z)
for p in [pA_star, pB_star, pC_star]:
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R * A.x)
pC_hat = pC_star.locatenew('C^', -R * A.x)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# the velocities of B^, C^ are zero since B, C are assumed to roll without slip
kde = [q1d - u1, q2d - u4, q3d - u5]
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
km = KanesMethod(F,
q_ind=[q1, q2, q3],
u_ind=[u1, u2],
kd_eqs=kde,
u_dependent=[u4, u5],
velocity_constraints=vc,
u_auxiliary=[u3])
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
bodies = [rbA, rbB, rbC]
with warns_deprecated_sympy():
fr, fr_star = km.kanes_equations(forces, bodies)
vc_map = solve(vc, [u4, u5])
# KanesMethod returns the negative of Fr, Fr* as defined in Kane1985.
fr_star_expected = Matrix([
-(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) *
u1.diff(t) - mA * a * u1 * u2,
-(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0
])
t = trigsimp(fr_star.subs(vc_map).subs({u3: 0})).doit().expand()
assert ((fr_star_expected - t).expand() == zeros(3, 1))
# define inertias of rigid bodies A, B, C about point D
# I_S/O = I_S/S* + I_S*/O
bodies2 = []
for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]):
I = I_star + inertia_of_point_mass(rb.mass, rb.masscenter.pos_from(pD),
rb.frame)
bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass,
(I, pD)))
with warns_deprecated_sympy():
fr2, fr_star2 = km.kanes_equations(forces, bodies2)
t = trigsimp(fr_star2.subs(vc_map).subs({u3: 0})).doit()
assert (fr_star_expected - t).expand() == zeros(3, 1)
angle = (acos(dot(a, b)/(a.magnitude() * b.magnitude())) *
180 / pi).evalf()
return min(angle, 180 - angle)
m, m_R, m_C, rho, r = symbols('m m_R m_C rho r', real=True, nonnegative=True)
N = ReferenceFrame('N')
pA = Point('A')
pPs = pA.locatenew('P*', 3*r*N.x - 2*r*N.y)
m_R = rho * 24 * r**2
m_C = rho * pi * r**2
m = m_R - m_C
I_Cs_A = inertia_of_point_mass(m, pPs.pos_from(pA), N)
I_C_Cs = inertia(N, m_R*(4*r)**2/12 - m_C*r**2/4,
m_R*(6*r)**2/12 - m_C*r**2/4,
m_R*((4*r)**2+(6*r)**2)/12 - m_C*r**2/2)
I_C_A = I_C_Cs + I_Cs_A
print("\nI_C_rel_A = {0}".format(I_C_A))
# Eigenvectors of I_C_A are the parallel to the principal axis for point A
# of Body C.
evecs_m = [triple[2]
for triple in inertia_matrix(I_C_A, N).eigenvects()]
# Convert eigenvectors from Matrix type to Vector type.
evecs = [sum(simplify(v[0][i]).evalf() * n for i, n in enumerate(N))
for v in evecs_m]
two.v2pt_theory(one, inertial_frame, one_frame)
two.vel(inertial_frame)
two_mass_center.v2pt_theory(two, inertial_frame, two_frame)
# Mass
# ====
one_mass, two_mass = symbols('m_1, m_2')
# Inertia
# =======
rotI = lambda I, f: Matrix([j & I & i for i in f for j in
f]).reshape(3,3)
one_inertia_dyadic = inertia(one_frame, 0, 0, rotI(inertia_of_point_mass(one_mass, one_mass_center.pos_from(one), one_frame), one_frame)[8])
one_central_inertia = (one_inertia_dyadic, one_mass_center)
two_inertia_dyadic = inertia(two_frame, 0, 0, rotI(inertia_of_point_mass(two_mass, two_mass_center.pos_from(one), one_frame), one_frame)[8])
two_central_inertia = (two_inertia_dyadic, two_mass_center)
# Rigid Bodies
# ============
oneB = RigidBody('One', one_mass_center, one_frame,
one_mass, one_central_inertia)
twoB = RigidBody('Upper Leg', two_mass_center, two_frame,
two_mass, two_central_inertia)
