def test_linearize_pendulum_lagrange_nonminimal():
q1, q2 = dynamicsymbols('q1:3')
q1d, q2d = dynamicsymbols('q1:3', level=1)
L, m, t = symbols('L, m, t')
g = 9.8
# Compose World Frame
N = ReferenceFrame('N')
pN = Point('N*')
pN.set_vel(N, 0)
# A.x is along the pendulum
theta1 = atan(q2 / q1)
A = N.orientnew('A', 'axis', [theta1, N.z])
# Create point P, the pendulum mass
P = pN.locatenew('P1', q1 * N.x + q2 * N.y)
P.set_vel(N, P.pos_from(pN).dt(N))
pP = Particle('pP', P, m)
# Constraint Equations
f_c = Matrix([q1**2 + q2**2 - L**2])
# Calculate the lagrangian, and form the equations of motion
Lag = Lagrangian(N, pP)
LM = LagrangesMethod(Lag, [q1, q2],
hol_coneqs=f_c,
forcelist=[(P, m * g * N.x)],
frame=N)
LM.form_lagranges_equations()
# Compose operating point
op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0}
# Solve for multiplier operating point
lam_op = LM.solve_multipliers(op_point=op_point)
op_point.update(lam_op)
# Perform the Linearization
A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d],
op_point=op_point,
A_and_B=True)
assert A == Matrix([[0, 1], [-9.8 / L, 0]])
assert B == Matrix([])
def test_linearize_rolling_disc_lagrange():
q1, q2, q3 = q = dynamicsymbols('q1 q2 q3')
q1d, q2d, q3d = qd = dynamicsymbols('q1 q2 q3', 1)
r, m, g = symbols('r m g')
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])
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyD.potential_energy = - m * g * r * cos(q2)
Lag = Lagrangian(N, BodyD)
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
# Linearize about steady-state upright rolling
op_point = {q1: 0, q2: 0, q3: 0,
q1d: 0, q2d: 0,
q1d.diff(): 0, q2d.diff(): 0, q3d.diff(): 0}
A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0]
sol = Matrix([[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, -6*q3d, 0],
[0, -4*g/(5*r), 0, 6*q3d/5, 0, 0],
[0, 0, 0, 0, 0, 0]])
assert A == sol
# Other system variables
m, l, g = symbols('m l g')
# Set up the reference frames
# Reference frame A set up in the plane perpendicular to the page containing
# segment OP
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [theta, N.z])
# Set up the points and particles
O = Point('O')
P = O.locatenew('P', l * A.x)
Pa = Particle('Pa', P, m)
# Set up velocities
A.set_ang_vel(N, thetad * N.z)
O.set_vel(N, 0)
P.v2pt_theory(O, N, A)
# Set up the lagrangian
L = Lagrangian(N, Pa)
# Create the list of forces acting on the system
fl = [(P, g * m * N.x)]
# Create the equations of motion using lagranges method
l = LagrangesMethod(L, [theta], forcelist=fl, frame=N)
pprint(l.form_lagranges_equations())
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)
# 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 )
# 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()
O = Point('O')
p1 = O.locatenew('p1', r1)
p2 = O.locatenew('p2', r2)
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)
# v_x = symbols('v_x') # x方向の速度
# v_y = symbols('v_y') # y方向の速度
v_x = x.diff(t)
v_y = y.diff(t)
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座標