def print_equations(model: Model, pretty_print: bool) -> None:
mass_mat = model.kanes.mass_matrix
forcing_mat = model.kanes.forcing
rhs = mass_mat.inv() * forcing_mat
kdd = model.kanes.kindiffdict()
rhs = rhs.subs(kdd)
rhs.simplify()
if pretty_print:
mechanics.mpprint(rhs)
else:
mechanics.mprint(rhs)
B_prime = A.orientnew('B_prime', 'Axis', [q1, A.z])
B = B_prime.orientnew('B', 'Axis', [pi/2 - q2, B_prime.x])
C = B.orientnew('C', 'Axis', [q3, B.z])
# Angular velocity based on coordinate time derivatives
w_C_in_A_qd = C.ang_vel_in(A)
# First definition of Angular velocity
w_C_in_A_uxuyuz = ux * A.x + uy * A.y + uz * A.z
print("Using w_C_A as")
print(w_C_in_A_uxuyuz)
kinematic_eqs = [(w_C_in_A_qd - w_C_in_A_uxuyuz) & uv for uv in A]
print("The kinematic equations are:")
soln = solve(kinematic_eqs, [q1d, q2d, q3d])
for qd in [q1d, q2d, q3d]:
mprint(qd)
print("=")
mprint(soln[qd])
# Second definition of Angular velocity
w_C_in_A_u1u2u3 = u1 * B.x + u2 * B.y + u3 * B.z
print("Using w_C_A as")
print(w_C_in_A_u1u2u3)
kinematic_eqs = [(w_C_in_A_qd - w_C_in_A_u1u2u3) & uv for uv in A]
print("The kinematic equations are:")
soln = solve(kinematic_eqs, [q1d, q2d, q3d])
for qd in [q1d, q2d, q3d]:
mprint(qd)
print("=")
mprint(soln[qd])
from sympy.physics.vector import ReferenceFrame, Vector
from sympy.physics.vector import dot, dynamicsymbols
from sympy.physics.mechanics import mprint, msprint
from sympy import solve
# define euler angle symbols and reference frames
ea = dynamicsymbols('α β γ')
A = ReferenceFrame('A')
A_1 = A.orientnew('A_1', 'axis', [ea[0], A.x])
A_2 = A_1.orientnew('A_2', 'axis', [ea[1], A_1.y])
B = A_2.orientnew('B', 'axis', [ea[2], A_2.z])
# display the rotation matrix C from frame A to frame B
print('Rotation matrix C:')
mprint(B.dcm(A))
# define angular velocity vector
w = dynamicsymbols('ω_x ω_y ω_z')
omega_A = sum((a * uv for a, uv in zip(w, A)), Vector(0))
omega_B = sum((a * uv for a, uv in zip(w, B)), Vector(0))
# display angular velocity vector omega
print('\nangular velocity:')
omega = B.ang_vel_in(A)
print('ω = {}'.format(msprint(omega)))
print('ω_A = {}'.format(msprint(omega.express(A))))
print('ω_B = {}'.format(msprint(omega.express(B))))
# solve for alpha, beta, gamma in terms of angular velocity components
dea = [a.diff() for a in ea]
# forces #
# add the gravitional force to each body
rodGravity = (Ao, N.z * gravity * mA)
plateGravity = (Bo, N.z * gravity * mB)
## equations of motion with Kane's method ##
# make a list of the bodies and forces
bodyList = [rod, plate]
forceList = [rodGravity, plateGravity]
# create a Kane object with respect to the Newtonian reference frame
kane = me.KanesMethod(N,
q_ind=[theta, phi],
u_ind=[omega, alpha],
kd_eqs=kinDiffs)
# calculate Kane's equations
fr, frstar = kane.kanes_equations(forceList, bodyList)
zero = fr + frstar
# solve Kane's equations for the derivatives of the speeds
eom = sym.solvers.solve(zero, omegad, alphad)
# add the kinematical differential equations to get the equations of motion
eom.update(kane.kindiffdict())
# print the results
me.mprint(eom)
B_prime = A.orientnew('B_prime', 'Axis', [q1, A.z])
B = B_prime.orientnew('B', 'Axis', [pi / 2 - q2, B_prime.x])
C = B.orientnew('C', 'Axis', [q3, B.z])
# Angular velocity based on coordinate time derivatives
w_C_in_A_qd = C.ang_vel_in(A)
# First definition of Angular velocity
w_C_in_A_uxuyuz = ux * A.x + uy * A.y + uz * A.z
print("Using w_C_A as")
print(w_C_in_A_uxuyuz)
kinematic_eqs = [(w_C_in_A_qd - w_C_in_A_uxuyuz) & uv for uv in A]
print("The kinematic equations are:")
soln = solve(kinematic_eqs, [q1d, q2d, q3d])
for qd in [q1d, q2d, q3d]:
mprint(qd)
print("=")
mprint(soln[qd])
# Second definition of Angular velocity
w_C_in_A_u1u2u3 = u1 * B.x + u2 * B.y + u3 * B.z
print("Using w_C_A as")
print(w_C_in_A_u1u2u3)
kinematic_eqs = [(w_C_in_A_qd - w_C_in_A_u1u2u3) & uv for uv in A]
print("The kinematic equations are:")
soln = solve(kinematic_eqs, [q1d, q2d, q3d])
for qd in [q1d, q2d, q3d]:
mprint(qd)
print("=")
mprint(soln[qd])
from sympy.physics.vector import ReferenceFrame, Vector
from sympy.physics.vector import dot, dynamicsymbols
from sympy.physics.mechanics import mprint, msprint
from sympy import pi
# define euler angle symbols and reference frames
ea = dynamicsymbols('α β γ') # zyx
s_i = dynamicsymbols('s_x s_y s_z')
p_i = dynamicsymbols('p_x p_y p_z')
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', ea, 'zyx')
# display the rotation matrix C from frame A to frame B
print('Rotation matrix C:')
mprint(B.dcm(A))
vals = [pi, pi/3, -pi/4]
print('\nsubstitute in euler angle values')
print(' α = {}'.format(vals[0]))
print(' β = {}'.format(vals[1]))
print(' γ = {}'.format(vals[2]))
ea_dict = dict(zip(ea, vals))
R = B.dcm(A).subs(ea_dict)
print('R = ')
mprint(R)
p = s_i[0]*A.x + s_i[1]*A.y + s_i[2]*A.z
p += p_i[0]*B.x + p_i[2]*B.z
print('\nvector p = ')
mprint(p.express(A).subs(ea_dict))
from sympy.physics.vector import ReferenceFrame, Vector
from sympy.physics.vector import dot, dynamicsymbols
from sympy.physics.mechanics import mprint, msprint
from sympy import solve
# define euler angle symbols and reference frames
ea = dynamicsymbols('α β γ')
A = ReferenceFrame('A')
A_1 = A.orientnew('A_1', 'axis', [ea[0], A.x])
A_2 = A_1.orientnew('A_2', 'axis', [ea[1], A_1.y])
B = A_2.orientnew('B', 'axis', [ea[2], A_2.z])
# display the rotation matrix C from frame A to frame B
print('Rotation matrix C:')
mprint(B.dcm(A))
# define angular velocity vector
w = dynamicsymbols('ω_x ω_y ω_z')
omega_A = sum((a*uv for a, uv in zip(w, A)), Vector(0))
omega_B = sum((a*uv for a, uv in zip(w, B)), Vector(0))
# display angular velocity vector omega
print('\nangular velocity:')
omega = B.ang_vel_in(A)
print('ω = {}'.format(msprint(omega)))
print('ω_A = {}'.format(msprint(omega.express(A))))
print('ω_B = {}'.format(msprint(omega.express(B))))
# solve for alpha, beta, gamma in terms of angular velocity components
dea = [a.diff() for a in ea]
# Alternatively, f_a can be formed as
#v_co_n = cross(C.ang_vel_in(N), CO.pos_from(P))
#a_co_n = v_co_n.dt(B) + cross(B.ang_vel_in(N), v_co_n)
#f_a = Matrix([((a_co_n - CO.acc(N)) & uv).expand() for uv in B])
# Kane's dynamic equations via elbow grease
# Partial angular velocities and velocities
partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u]
partial_v_CO = [CO.vel(N).diff(ui, N) for ui in u]
# Active forces
F_CO = m*g*A.z
# Generalized active forces (unconstrained)
gaf = [dot(F_CO, pv) for pv in partial_v_CO]
print("Generalized active forces (unconstrained)")
mprint(gaf)
# Inertia force
R_star_CO = -m*CO.acc(N)
I = (m * r**2) / 4
J = (m * r**2) / 2
# Inertia torque
I_C_CO = inertia(C, I, J, I) # Inertia of disc C about point CO
T_star_C = -(dot(I_C_CO, C.ang_acc_in(N))
+ cross(C.ang_vel_in(N), dot(I_C_CO, C.ang_vel_in(N))))
# Generalized inertia forces (unconstrained)
gif = [dot(R_star_CO, pv) + dot(T_star_C, pav) for pv, pav in
zip(partial_v_CO, partial_w_C)]
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())
# Making a particle from point mass
Pa_1 = Particle("P_1",P_1,m_theta)
Pa_2 = Particle("P_2",P_2,m_l)
# Potential energy of system
Pa_1.potential_energy = m_theta * g * y1
Pa_2.potential_energy = m_l * g * y2
# Non-restorative forces
f_theta = (P_1, torque*( -sin(theta) * N.x + cos(theta) * N.y)/lg_1)
f_sliding = (P_2, sliding_force * (cos(theta) * N.x + sin(theta) * N.y))
fl = [f_theta, f_sliding]
# Setting-up Lagrangian
L = Lagrangian(N,Pa_1,Pa_2)
# Generation Equation of Motion
LM = LagrangesMethod(L,var,forcelist = fl,frame=N)
EOM = LM.form_lagranges_equations()
""" Printing results """
mprint( simplify(EOM) )
#print( mlatex(simplify(me)) )
#print( mlatex(LM.rhs()) )
#simplyfied
#eq1 = simplify( LM.rhs() )
#mprint( eq1 )
# Form equations of motion
KM = KanesMethod(A, q, u, kindiffs)
fr, frstar = KM.kanes_equations(torque_force_list, body_list)
# Simplifying assumptions
simplifications = {IFxx: symbols('I'), # Flywheel transverse inertias
IFyy: symbols('I'), # assumed to be equal
IFzz: symbols('J'),
Ixx: 0,
Iyy: 0,
Izz: 0}
frstar = frstar.subs(simplifications)
MM = -frstar.jacobian(ud)
MM.simplify()
forcing = fr + frstar.subs({ud[0]: 0, ud[1]:0, ud[2]:0}).expand()
forcing.simplify()
print("Mass matrix:")
mprint(MM)
print("forcing vector:")
mprint(forcing)
eqns = MM * Matrix(ud) - forcing
udots = solve(eqns, ud)
for udi in ud:
mprint(udi)
print(":")
mprint(udots[udi])
rod = me.RigidBody("rod", Ao, A, mA, (me.inertia(A, IAxx, IAxx, 0.0), Ao))
# create the empty plate object
plate = me.RigidBody("plate", Bo, B, mB, (me.inertia(B, IBxx, IByy, IBzz), Bo))
# forces #
# add the gravitional force to each body
rodGravity = (Ao, N.z * gravity * mA)
plateGravity = (Bo, N.z * gravity * mB)
## equations of motion with Kane's method ##
# make a list of the bodies and forces
bodyList = [rod, plate]
forceList = [rodGravity, plateGravity]
# create a Kane object with respect to the Newtonian reference frame
kane = me.KanesMethod(N, q_ind=[theta, phi], u_ind=[omega, alpha], kd_eqs=kinDiffs)
# calculate Kane's equations
fr, frstar = kane.kanes_equations(forceList, bodyList)
zero = fr + frstar
# solve Kane's equations for the derivatives of the speeds
eom = sym.solvers.solve(zero, omegad, alphad)
# add the kinematical differential equations to get the equations of motion
eom.update(kane.kindiffdict())
# print the results
me.mprint(eom)
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())
Pa1 = Particle('Pa1', P1, m1)
Pa1.potential_energy = m1 * g * y1
# 質点2（振子）の質点、位置
P2 = Point('P2')
x2 = p - l * sin(theta)
y2 = cos(theta)
vx2 = diff(x2, t)
vy2 = diff(y2, t)
P2.set_vel(N, vx2 * N.x + vy2 * N.y)
Pa2 = Particle('Pa2', P2, m2)
Pa2.potential_energy = m1 * g * y2
# 外力
fl = [(P1, F * N.x), (P2, 0 * N.x)]
# 運動方程式
LL = Lagrangian(N, Pa1, Pa2)
LM = LagrangesMethod(LL, q, forcelist=fl, frame=N)
eom = LM.form_lagranges_equations().simplify()
f = LM.rhs().simplify()
# 線形化
linearlizer = LM.to_linearizer(q_ind=q, qd_ind=qd)
op_point = {p: 0, theta: 0, p.diff(t): 0, theta.diff(t): 0}
A, B = linearlizer.linearize(A_and_B=True, op_point=op_point)
mprint(A)
mprint(B)
# Define rigid body objects
Enclosure = RigidBody('Enclosure', O, B, 0, (I_enclosure, O))
Flywheel = RigidBody('Flywheel', O, C, 0, (I_flywheel, O))
body_list = [Enclosure, Flywheel]
# List of forces and torques
tau = symbols('tau') # Gimbal torque scalar
torque_force_list = [(O, m*g*A.z), # Gravitational force
(B, tau*A.y)] # Gimbal torque vector
# Form equations of motion
KM = KanesMethod(A, q, u, kindiffs)
fr, frstar = KM.kanes_equations(torque_force_list, body_list)
frstar.simplify()
print("Pre simplification")
mprint(fr)
mprint(frstar)
# Simplifying assumptions
simplifications = {IFxx: symbols('I'), # Flywheel transverse inertias
IFyy: symbols('I')} # assumed to be equal
frstar = frstar.subs(simplifications)
frstar.simplify()
print("Post simplification")
mprint(fr)
mprint(frstar)
