def test_rolling_disc():
# Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and 3
# speed variables are need to describe this system, along with the
# disc's mass and radius, and the local gravity.
q1, q2, q3 = dynamicsymbols('q1 q2 q3')
q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1)
r, m, g = symbols('r m g')
# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
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])
# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc's center of mass.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
# Forming the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
# Finally we form the equations of motion, using the same steps we did
# before. Supply the Lagrangian, the generalized speeds.
BodyD.potential_energy = -m * g * r * cos(q2)
Lag = Lagrangian(N, BodyD)
q = [q1, q2, q3]
q1 = Function('q1')
q2 = Function('q2')
q3 = Function('q3')
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
RHS = l.rhs().as_mutable()
RHS.simplify()
t = symbols('t')
assert tuple(l.mass_matrix[3:6]) == (0, 5 * m * r**2 / 4, 0)
assert RHS[4].simplify() == (
(-8 * g * sin(q2(t)) + r *
(5 * sin(2 * q2(t)) * Derivative(q1(t), t) +
12 * cos(q2(t)) * Derivative(q3(t), t)) * Derivative(q1(t), t)) /
(10 * r))
assert RHS[5] == (-5 * cos(q2(t)) * Derivative(q1(t), t) +
6 * tan(q2(t)) * Derivative(q3(t), t) +
4 * Derivative(q1(t), t) / cos(q2(t))) * Derivative(
q2(t), t)
def test_rolling_disc():
# Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and 3
# speed variables are need to describe this system, along with the
# disc's mass and radius, and the local gravity.
q1, q2, q3 = dynamicsymbols('q1 q2 q3')
q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1)
r, m, g = symbols('r m g')
# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
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])
# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc's center of mass.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
# Forming the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
# Finally we form the equations of motion, using the same steps we did
# before. Supply the Lagrangian, the generalized speeds.
BodyD.set_potential_energy(- m * g * r * cos(q2))
Lag = Lagrangian(N, BodyD)
q = [q1, q2, q3]
q1 = Function('q1')
q2 = Function('q2')
q3 = Function('q3')
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
RHS = l.rhs()
RHS.simplify()
t = symbols('t')
assert (l.mass_matrix[3:6] == [0, 5*m*r**2/4, 0])
assert RHS[4].simplify() == (-8*g*sin(q2(t)) + 5*r*sin(2*q2(t)
)*Derivative(q1(t), t)**2 + 12*r*cos(q2(t))*Derivative(q1(t), t
)*Derivative(q3(t), t))/(10*r)
assert RHS[5] == (-5*cos(q2(t))*Derivative(q1(t), t) + 6*tan(q2(t)
)*Derivative(q3(t), t) + 4*Derivative(q1(t), t)/cos(q2(t))
)*Derivative(q2(t), t)
def test_disc_on_an_incline_plane():
# Disc rolling on an inclined plane
# First the generalized coordinates are created. The mass center of the
# disc is located from top vertex of the inclined plane by the generalized
# coordinate 'y'. The orientation of the disc is defined by the angle
# 'theta'. The mass of the disc is 'm' and its radius is 'R'. The length of
# the inclined path is 'l', the angle of inclination is 'alpha'. 'g' is the
# gravitational constant.
y, theta = dynamicsymbols('y theta')
yd, thetad = dynamicsymbols('y theta', 1)
m, g, R, l, alpha = symbols('m g R l alpha')
# Next, we create the inertial reference frame 'N'. A reference frame 'A'
# is attached to the inclined plane. Finally a frame is created which is attached to the disk.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [pi/2 - alpha, N.z])
B = A.orientnew('B', 'Axis', [-theta, A.z])
# Creating the disc 'D'; we create the point that represents the mass
# center of the disc and set its velocity. The inertia dyadic of the disc
# is created. Finally, we create the disc.
Do = Point('Do')
Do.set_vel(N, yd * A.x)
I = m * R**2 / 2 * B.z | B.z
D = RigidBody('D', Do, B, m, (I, Do))
# To construct the Lagrangian, 'L', of the disc, we determine its kinetic
# and potential energies, T and U, respectively. L is defined as the
# difference between T and U.
D.set_potential_energy(m * g * (l - y) * sin(alpha))
L = Lagrangian(N, D)
# We then create the list of generalized coordinates and constraint
# equations. The constraint arises due to the disc rolling without slip on
# on the inclined path. Also, the constraint is holonomic but we supply the
# differentiated holonomic equation as the 'LagrangesMethod' class requires
# that. We then invoke the 'LagrangesMethod' class and supply it the
# necessary arguments and generate the equations of motion. The'rhs' method
# solves for the q_double_dots (i.e. the second derivative with respect to
# time of the generalized coordinates and the lagrange multiplers.
q = [y, theta]
coneq = [yd - R * thetad]
m = LagrangesMethod(L, q, coneq)
m.form_lagranges_equations()
rhs = m.rhs()
rhs.simplify()
assert rhs[2] == 2*g*sin(alpha)/3
def test_disc_on_an_incline_plane():
# Disc rolling on an inclined plane
# First the generalized coordinates are created. The mass center of the
# disc is located from top vertex of the inclined plane by the generalized
# coordinate 'y'. The orientation of the disc is defined by the angle
# 'theta'. The mass of the disc is 'm' and its radius is 'R'. The length of
# the inclined path is 'l', the angle of inclination is 'alpha'. 'g' is the
# gravitational constant.
y, theta = dynamicsymbols('y theta')
yd, thetad = dynamicsymbols('y theta', 1)
m, g, R, l, alpha = symbols('m g R l alpha')
# Next, we create the inertial reference frame 'N'. A reference frame 'A'
# is attached to the inclined plane. Finally a frame is created which is attached to the disk.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [pi / 2 - alpha, N.z])
B = A.orientnew('B', 'Axis', [-theta, A.z])
# Creating the disc 'D'; we create the point that represents the mass
# center of the disc and set its velocity. The inertia dyadic of the disc
# is created. Finally, we create the disc.
Do = Point('Do')
Do.set_vel(N, yd * A.x)
I = m * R**2 / 2 * B.z | B.z
D = RigidBody('D', Do, B, m, (I, Do))
# To construct the Lagrangian, 'L', of the disc, we determine its kinetic
# and potential energies, T and U, respectively. L is defined as the
# difference between T and U.
D.potential_energy = m * g * (l - y) * sin(alpha)
L = Lagrangian(N, D)
# We then create the list of generalized coordinates and constraint
# equations. The constraint arises due to the disc rolling without slip on
# on the inclined path. We then invoke the 'LagrangesMethod' class and
# supply it the necessary arguments and generate the equations of motion.
# The'rhs' method solves for the q_double_dots (i.e. the second derivative
# with respect to time of the generalized coordinates and the lagrange
# multipliers.
q = [y, theta]
hol_coneqs = [y - R * theta]
m = LagrangesMethod(L, q, hol_coneqs=hol_coneqs)
m.form_lagranges_equations()
rhs = m.rhs()
rhs.simplify()
assert rhs[2] == 2 * g * sin(alpha) / 3
def test_simp_pen():
# This tests that the equations generated by LagrangesMethod are identical
# to those obtained by hand calculations. The system under consideration is
# the simple pendulum.
# We begin by creating the generalized coordinates as per the requirements
# of LagrangesMethod. Also we created the associate symbols
# that characterize the system: 'm' is the mass of the bob, l is the length
# of the massless rigid rod connecting the bob to a point O fixed in the
# inertial frame.
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u ', 1)
l, m, g = symbols('l m g')
# We then create the inertial frame and a frame attached to the massless
# string following which we define the inertial angular velocity of the
# string.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q, N.z])
A.set_ang_vel(N, qd * N.z)
# Next, we create the point O and fix it in the inertial frame. We then
# locate the point P to which the bob is attached. Its corresponding
# velocity is then determined by the 'two point formula'.
O = Point('O')
O.set_vel(N, 0)
P = O.locatenew('P', l * A.x)
P.v2pt_theory(O, N, A)
# The 'Particle' which represents the bob is then created and its
# Lagrangian generated.
Pa = Particle('Pa', P, m)
Pa.potential_energy = -m * g * l * cos(q)
L = Lagrangian(N, Pa)
# The 'LagrangesMethod' class is invoked to obtain equations of motion.
lm = LagrangesMethod(L, [q])
lm.form_lagranges_equations()
RHS = lm.rhs()
assert RHS[1] == -g * sin(q) / l
def test_simp_pen():
# This tests that the equations generated by LagrangesMethod are identical
# to those obtained by hand calculations. The system under consideration is
# the simple pendulum.
# We begin by creating the generalized coordinates as per the requirements
# of LagrangesMethod. Also we created the associate symbols
# that characterize the system: 'm' is the mass of the bob, l is the length
# of the massless rigid rod connecting the bob to a point O fixed in the
# inertial frame.
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u ', 1)
l, m, g = symbols('l m g')
# We then create the inertial frame and a frame attached to the massless
# string following which we define the inertial angular velocity of the
# string.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q, N.z])
A.set_ang_vel(N, qd * N.z)
# Next, we create the point O and fix it in the inertial frame. We then
# locate the point P to which the bob is attached. Its corresponding
# velocity is then determined by the 'two point formula'.
O = Point('O')
O.set_vel(N, 0)
P = O.locatenew('P', l * A.x)
P.v2pt_theory(O, N, A)
# The 'Particle' which represents the bob is then created and its
# Lagrangian generated.
Pa = Particle('Pa', P, m)
Pa.set_potential_energy(- m * g * l * cos(q))
L = Lagrangian(N, Pa)
# The 'LagrangesMethod' class is invoked to obtain equations of motion.
lm = LagrangesMethod(L, [q])
lm.form_lagranges_equations()
RHS = lm.rhs()
assert RHS[1] == -g*sin(q)/l
v0: 20}
N = ReferenceFrame('N')
B = N.orientnew('B', 'axis', [q3, N.z])
O = Point('O')
S = O.locatenew('S', q1*N.x + q2*N.y)
S.set_vel(N, S.pos_from(O).dt(N))
#Is = m/12*(l**2 + w**2)
Is = symbols('Is')
I = inertia(B, 0, 0, Is, 0, 0, 0)
rb = RigidBody('rb', S, B, m, (I, S))
rb.set_potential_energy(0)
L = Lagrangian(N, rb)
lm = LagrangesMethod(
L, q, nonhol_coneqs = [q1d*sin(q3) - q2d*cos(q3) + l/2*q3d])
lm.form_lagranges_equations()
rhs = lm.rhs()
print('{} = {}'.format(msprint(q1d.diff(t)),
msprint(rhs[3].simplify())))
print('{} = {}'.format(msprint(q2d.diff(t)),
msprint(rhs[4].simplify())))
print('{} = {}'.format(msprint(q3d.diff(t)),
msprint(rhs[5].simplify())))
print('{} = {}'.format('λ', msprint(rhs[6].simplify())))
# Definition of mass and velocity
Pa1 = Particle('Pa1', P1, m1)
Pa2 = Particle('Pa2', P2, m2)
vx1 = diff(x1, t)
vy1 = diff(y1, t)
vx2 = diff(x2, t)
vy2 = diff(y2, t)
P1.set_vel(N, vx1 * N.x + vy1 * N.y)
P2.set_vel(N, vx2 * N.x + vy2 * N.y)
# Potential Energy
Pa1.potential_energy = m1 * g * y1
Pa2.potential_energy = m2 * g * y2
# External Force
fl = [(P1, F * N.x), (P2, 0 * N.x)]
# Lagrangian
LL = Lagrangian(N, Pa1, Pa2)
LM = LagrangesMethod(LL, q, forcelist=fl, frame=N)
eom = LM.form_lagranges_equations()
f = LM.rhs()
# Linearization
linearizer = 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 = linearizer.linearize(A_and_B=True, op_point=op_point)
O.set_vel(N, 0)
p1.set_vel(N, p1.pos_from(O).dt(N))
p2.set_vel(N, p2.pos_from(O).dt(N))
P1 = Particle('P1', p1, 2*m)
P2 = Particle('P2', p2, m)
P1.set_potential_energy(0)
P2.set_potential_energy(P2.mass * g * (p2.pos_from(O) & N.y))
L1 = Lagrangian(N, P1, P2)
print('{} = {}'.format('L1', msprint(L1)))
lm1 = LagrangesMethod(L1, [s, theta])
lm1.form_lagranges_equations()
rhs = lm1.rhs()
t = symbols('t')
print('{} = {}'.format(msprint(sd.diff(t)), msprint(rhs[2].simplify())))
print('{} = {}\n'.format(msprint(thetad.diff(t)), msprint(rhs[3].simplify())))
# part b
r1 = s*N.x + h*N.y
r2 = (s + l*cos(theta))*N.x + (h + l*sin(theta))*N.y
p1 = O.locatenew('p1', r1)
p2 = O.locatenew('p2', r2)
p1.set_vel(N, p1.pos_from(O).dt(N))
p2.set_vel(N, p2.pos_from(O).dt(N))
P1 = Particle('P1', p1, 2*m)
P2 = Particle('P2', p2, m)
# Corpos Rígidos
I_1 = inertia(B1, 0, 0, I_1_ZZ)
# Elo 1
E_1 = RigidBody('Elo_1', CM_1, B1, M_1, (I_1, CM_1))
I_2 = inertia(B1, 0, 0, I_1_ZZ)
# Elo 2
E_2 = RigidBody('Elo_2', CM_2, B2, M_2, (I_2, CM_2))
# Energia Potencial
P_1 = -M_1 * G * B0.z
R_1_CM = CM_1.pos_from(O).express(B0)
E_1.potential_energy = R_1_CM.dot(P_1)
P_2 = -M_2 * G * B0.z
R_2_CM = CM_2.pos_from(O).express(B0).simplify()
E_2.potential_energy = R_2_CM.dot(P_2)
# Forças/Momentos Generalizados
FL = [(B1, TAU_1 * B0.z), (B2, TAU_2 * B0.z)]
# Método de Lagrange
L = Lagrangian(B0, E_1, E_2)
L = L.simplify()
pprint(L)
LM = LagrangesMethod(L, [THETA_1, THETA_2], frame=B0, forcelist=FL)
# Equações do Movimento
L_EQ = LM.form_lagranges_equations()
pprint(L_EQ)
# Equações Prontas para Solução Numérica
RHS = LM.rhs()
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)
O.set_vel(N, 0)
p1.set_vel(N, p1.pos_from(O).dt(N))
p2.set_vel(N, p2.pos_from(O).dt(N))
P1 = Particle('P1', p1, 2 * m)
P2 = Particle('P2', p2, m)
P1.set_potential_energy(0)
P2.set_potential_energy(P2.mass * g * (p2.pos_from(O) & N.y))
L1 = Lagrangian(N, P1, P2)
print('{} = {}'.format('L1', msprint(L1)))
lm1 = LagrangesMethod(L1, [s, theta])
lm1.form_lagranges_equations()
rhs = lm1.rhs()
t = symbols('t')
print('{} = {}'.format(msprint(sd.diff(t)), msprint(rhs[2].simplify())))
print('{} = {}\n'.format(msprint(thetad.diff(t)), msprint(rhs[3].simplify())))
# part b
r1 = s * N.x + h * N.y
r2 = (s + l * cos(theta)) * N.x + (h + l * sin(theta)) * N.y
p1 = O.locatenew('p1', r1)
p2 = O.locatenew('p2', r2)
p1.set_vel(N, p1.pos_from(O).dt(N))
p2.set_vel(N, p2.pos_from(O).dt(N))
P1 = Particle('P1', p1, 2 * m)
P2 = Particle('P2', p2, m)
N = ReferenceFrame('N') # 参照座標系
pt = Point("P")
pt.set_vel(N, v_x * N.x + v_y * N.y)
m = symbols("m") # 質量
pcl = Particle("mass", pt, m)
f_x, f_y = dynamicsymbols("f_x f_y")
LL = Lagrangian(N, pcl)
LM = LagrangesMethod(LL, [q])
eom = LM.form_lagranges_equations().simplify()
rhs = LM.rhs()
import pdb
pdb.set_trace()
def get_path(t_i):
"""
:param t_i: 時間
:return: 時間t_iでのx, y座標
"""
x_t = x.subs({q: theta, t: t_i, length: 1}).evalf()
y_t = y.subs({q: theta, t: t_i, length: 1}).evalf()
return x_t, y_t
