def test_dub_pen():
# The system considered is the double pendulum. Like in the
# test of the simple pendulum above, we begin by creating the generalized
# coordinates and the simple generalized speeds and accelerations which
# will be used later. Following this we create frames and points necessary
# for the kinematics. The procedure isn't explicitly explained as this is
# similar to the simple pendulum. Also this is documented on the pydy.org
# website.
q1, q2 = dynamicsymbols("q1 q2")
q1d, q2d = dynamicsymbols("q1 q2", 1)
q1dd, q2dd = dynamicsymbols("q1 q2", 2)
u1, u2 = dynamicsymbols("u1 u2")
u1d, u2d = dynamicsymbols("u1 u2", 1)
l, m, g = symbols("l m g")
N = ReferenceFrame("N")
A = N.orientnew("A", "Axis", [q1, N.z])
B = N.orientnew("B", "Axis", [q2, N.z])
A.set_ang_vel(N, q1d * A.z)
B.set_ang_vel(N, q2d * A.z)
O = Point("O")
P = O.locatenew("P", l * A.x)
R = P.locatenew("R", l * B.x)
O.set_vel(N, 0)
P.v2pt_theory(O, N, A)
R.v2pt_theory(P, N, B)
ParP = Particle("ParP", P, m)
ParR = Particle("ParR", R, m)
ParP.potential_energy = -m * g * l * cos(q1)
ParR.potential_energy = -m * g * l * cos(q1) - m * g * l * cos(q2)
L = Lagrangian(N, ParP, ParR)
lm = LagrangesMethod(L, [q1, q2], bodies=[ParP, ParR])
lm.form_lagranges_equations()
assert (simplify(l * m *
(2 * g * sin(q1) + l * sin(q1) * sin(q2) * q2dd +
l * sin(q1) * cos(q2) * q2d**2 - l * sin(q2) * cos(q1) *
q2d**2 + l * cos(q1) * cos(q2) * q2dd + 2 * l * q1dd) -
lm.eom[0]) == 0)
assert (simplify(l * m *
(g * sin(q2) + l * sin(q1) * sin(q2) * q1dd -
l * sin(q1) * cos(q2) * q1d**2 + l * sin(q2) * cos(q1) *
q1d**2 + l * cos(q1) * cos(q2) * q1dd + l * q2dd) -
lm.eom[1]) == 0)
assert lm.bodies == [ParP, ParR]
def test_particle():
m, m2, v1, v2, v3, r, g, h = symbols('m m2 v1 v2 v3 r g h')
P = Point('P')
P2 = Point('P2')
p = Particle('pa', P, m)
assert p.mass == m
assert p.point == P
# Test the mass setter
p.mass = m2
assert p.mass == m2
# Test the point setter
p.point = P2
assert p.point == P2
# Test the linear momentum function
N = ReferenceFrame('N')
O = Point('O')
P2.set_pos(O, r * N.y)
P2.set_vel(N, v1 * N.x)
assert p.linear_momentum(N) == m2 * v1 * N.x
assert p.angular_momentum(O, N) == -m2 * r *v1 * N.z
P2.set_vel(N, v2 * N.y)
assert p.linear_momentum(N) == m2 * v2 * N.y
assert p.angular_momentum(O, N) == 0
P2.set_vel(N, v3 * N.z)
assert p.linear_momentum(N) == m2 * v3 * N.z
assert p.angular_momentum(O, N) == m2 * r * v3 * N.x
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
p.potential_energy = m * g * h
assert p.potential_energy == m * g * h
# TODO make the result not be system-dependent
assert p.kinetic_energy(
N) in [m2*(v1**2 + v2**2 + v3**2)/2,
m2 * v1**2 / 2 + m2 * v2**2 / 2 + m2 * v3**2 / 2]
def test_dub_pen():
# The system considered is the double pendulum. Like in the
# test of the simple pendulum above, we begin by creating the generalized
# coordinates and the simple generalized speeds and accelerations which
# will be used later. Following this we create frames and points necessary
# for the kinematics. The procedure isn't explicitly explained as this is
# similar to the simple pendulum. Also this is documented on the pydy.org
# website.
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
q1dd, q2dd = dynamicsymbols('q1 q2', 2)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
l, m, g = symbols('l m g')
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = N.orientnew('B', 'Axis', [q2, N.z])
A.set_ang_vel(N, q1d * A.z)
B.set_ang_vel(N, q2d * A.z)
O = Point('O')
P = O.locatenew('P', l * A.x)
R = P.locatenew('R', l * B.x)
O.set_vel(N, 0)
P.v2pt_theory(O, N, A)
R.v2pt_theory(P, N, B)
ParP = Particle('ParP', P, m)
ParR = Particle('ParR', R, m)
ParP.potential_energy = - m * g * l * cos(q1)
ParR.potential_energy = - m * g * l * cos(q1) - m * g * l * cos(q2)
L = Lagrangian(N, ParP, ParR)
lm = LagrangesMethod(L, [q1, q2], bodies=[ParP, ParR])
lm.form_lagranges_equations()
assert simplify(l*m*(2*g*sin(q1) + l*sin(q1)*sin(q2)*q2dd
+ l*sin(q1)*cos(q2)*q2d**2 - l*sin(q2)*cos(q1)*q2d**2
+ l*cos(q1)*cos(q2)*q2dd + 2*l*q1dd) - lm.eom[0]) == 0
assert simplify(l*m*(g*sin(q2) + l*sin(q1)*sin(q2)*q1dd
- l*sin(q1)*cos(q2)*q1d**2 + l*sin(q2)*cos(q1)*q1d**2
+ l*cos(q1)*cos(q2)*q1dd + l*q2dd) - lm.eom[1]) == 0
assert lm.bodies == [ParP, ParR]
def test_lagrange_2forces():
### Equations for two damped springs in serie with two forces
### generalized coordinates
qs = q1, q2 = dynamicsymbols('q1, q2')
### generalized speeds
qds = q1d, q2d = dynamicsymbols('q1, q2', 1)
### Mass, spring strength, friction coefficient
m, k, nu = symbols('m, k, nu')
N = ReferenceFrame('N')
O = Point('O')
### Two points
P1 = O.locatenew('P1', q1 * N.x)
P1.set_vel(N, q1d * N.x)
P2 = O.locatenew('P1', q2 * N.x)
P2.set_vel(N, q2d * N.x)
pP1 = Particle('pP1', P1, m)
pP1.potential_energy = k * q1**2 / 2
pP2 = Particle('pP2', P2, m)
pP2.potential_energy = k * (q1 - q2)**2 / 2
#### Friction forces
forcelist = [(P1, - nu * q1d * N.x),
(P2, - nu * q2d * N.x)]
lag = Lagrangian(N, pP1, pP2)
l_method = LagrangesMethod(lag, (q1, q2), forcelist=forcelist, frame=N)
l_method.form_lagranges_equations()
eq1 = l_method.eom[0]
assert eq1.diff(q1d) == nu
eq2 = l_method.eom[1]
assert eq2.diff(q2d) == nu
def _convert_bodies(self):
# Convert `Body` to `Particle` and `RigidBody`
bodylist = []
for body in self.bodies:
if body.is_rigidbody:
rb = RigidBody(body.name, body.masscenter, body.frame, body.mass,
(body.central_inertia, body.masscenter))
rb.potential_energy = body.potential_energy
bodylist.append(rb)
else:
part = Particle(body.name, body.masscenter, body.mass)
part.potential_energy = body.potential_energy
bodylist.append(part)
return bodylist
def test_lagrange_2forces():
### Equations for two damped springs in serie with two forces
### generalized coordinates
qs = q1, q2 = dynamicsymbols('q1, q2')
### generalized speeds
qds = q1d, q2d = dynamicsymbols('q1, q2', 1)
### Mass, spring strength, friction coefficient
m, k, nu = symbols('m, k, nu')
N = ReferenceFrame('N')
O = Point('O')
### Two points
P1 = O.locatenew('P1', q1 * N.x)
P1.set_vel(N, q1d * N.x)
P2 = O.locatenew('P1', q2 * N.x)
P2.set_vel(N, q2d * N.x)
pP1 = Particle('pP1', P1, m)
pP1.potential_energy = k * q1**2 / 2
pP2 = Particle('pP2', P2, m)
pP2.potential_energy = k * (q1 - q2)**2 / 2
#### Friction forces
forcelist = [(P1, -nu * q1d * N.x), (P2, -nu * q2d * N.x)]
lag = Lagrangian(N, pP1, pP2)
l_method = LagrangesMethod(lag, (q1, q2), forcelist=forcelist, frame=N)
l_method.form_lagranges_equations()
eq1 = l_method.eom[0]
assert eq1.diff(q1d) == nu
eq2 = l_method.eom[1]
assert eq2.diff(q2d) == nu
def test_Lagrangian():
M, m, g, h = symbols("M m g h")
N = ReferenceFrame("N")
O = Point("O")
O.set_vel(N, 0 * N.x)
P = O.locatenew("P", 1 * N.x)
P.set_vel(N, 10 * N.x)
Pa = Particle("Pa", P, 1)
Ac = O.locatenew("Ac", 2 * N.y)
Ac.set_vel(N, 5 * N.y)
a = ReferenceFrame("a")
a.set_ang_vel(N, 10 * N.z)
I = outer(N.z, N.z)
A = RigidBody("A", Ac, a, 20, (I, Ac))
Pa.potential_energy = m * g * h
A.potential_energy = M * g * h
raises(TypeError, lambda: Lagrangian(A, A, Pa))
raises(TypeError, lambda: Lagrangian(N, N, Pa))
def test_potential_energy():
m, M, l1, g, h, H = symbols("m M l1 g h H")
omega = dynamicsymbols("omega")
N = ReferenceFrame("N")
O = Point("O")
O.set_vel(N, 0 * N.x)
Ac = O.locatenew("Ac", l1 * N.x)
P = Ac.locatenew("P", l1 * N.x)
a = ReferenceFrame("a")
a.set_ang_vel(N, omega * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle("Pa", P, m)
I = outer(N.z, N.z)
A = RigidBody("A", Ac, a, M, (I, Ac))
Pa.potential_energy = m * g * h
A.potential_energy = M * g * H
assert potential_energy(A, Pa) == m * g * h + M * g * H
def test_Lagrangian():
M, m, g, h = symbols('M m g h')
N = ReferenceFrame('N')
O = Point('O')
O.set_vel(N, 0 * N.x)
P = O.locatenew('P', 1 * N.x)
P.set_vel(N, 10 * N.x)
Pa = Particle('Pa', P, 1)
Ac = O.locatenew('Ac', 2 * N.y)
Ac.set_vel(N, 5 * N.y)
a = ReferenceFrame('a')
a.set_ang_vel(N, 10 * N.z)
I = outer(N.z, N.z)
A = RigidBody('A', Ac, a, 20, (I, Ac))
Pa.potential_energy = m * g * h
A.potential_energy = M * g * h
raises(TypeError, lambda: Lagrangian(A, A, Pa))
raises(TypeError, lambda: Lagrangian(N, N, Pa))
def test_potential_energy():
m, M, l1, g, h, H = symbols("m M l1 g h H")
omega = dynamicsymbols("omega")
N = ReferenceFrame("N")
O = Point("O")
O.set_vel(N, 0 * N.x)
Ac = O.locatenew("Ac", l1 * N.x)
P = Ac.locatenew("P", l1 * N.x)
a = ReferenceFrame("a")
a.set_ang_vel(N, omega * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle("Pa", P, m)
I = outer(N.z, N.z)
A = RigidBody("A", Ac, a, M, (I, Ac))
Pa.potential_energy = m * g * h
A.potential_energy = M * g * H
assert potential_energy(A, Pa) == m * g * h + M * g * H
def test_potential_energy():
m, M, l1, g, h, H = symbols('m M l1 g h H')
omega = dynamicsymbols('omega')
N = ReferenceFrame('N')
O = Point('O')
O.set_vel(N, 0 * N.x)
Ac = O.locatenew('Ac', l1 * N.x)
P = Ac.locatenew('P', l1 * N.x)
a = ReferenceFrame('a')
a.set_ang_vel(N, omega * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle('Pa', P, m)
I = outer(N.z, N.z)
A = RigidBody('A', Ac, a, M, (I, Ac))
Pa.potential_energy = m * g * h
A.potential_energy = M * g * H
assert potential_energy(A, Pa) == m * g * h + M * g * H
def test_particle():
m, m2, v1, v2, v3, r, g, h = symbols("m m2 v1 v2 v3 r g h")
P = Point("P")
P2 = Point("P2")
p = Particle("pa", P, m)
assert p.__str__() == "pa"
assert p.mass == m
assert p.point == P
# Test the mass setter
p.mass = m2
assert p.mass == m2
# Test the point setter
p.point = P2
assert p.point == P2
# Test the linear momentum function
N = ReferenceFrame("N")
O = Point("O")
P2.set_pos(O, r * N.y)
P2.set_vel(N, v1 * N.x)
raises(TypeError, lambda: Particle(P, P, m))
raises(TypeError, lambda: Particle("pa", m, m))
assert p.linear_momentum(N) == m2 * v1 * N.x
assert p.angular_momentum(O, N) == -m2 * r * v1 * N.z
P2.set_vel(N, v2 * N.y)
assert p.linear_momentum(N) == m2 * v2 * N.y
assert p.angular_momentum(O, N) == 0
P2.set_vel(N, v3 * N.z)
assert p.linear_momentum(N) == m2 * v3 * N.z
assert p.angular_momentum(O, N) == m2 * r * v3 * N.x
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
p.potential_energy = m * g * h
assert p.potential_energy == m * g * h
# TODO make the result not be system-dependent
assert p.kinetic_energy(N) in [
m2 * (v1**2 + v2**2 + v3**2) / 2,
m2 * v1**2 / 2 + m2 * v2**2 / 2 + m2 * v3**2 / 2,
]
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.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
vy1 = diff(y1,t)
# Setting Reference Frames
N = ReferenceFrame("N")
# Point mass assumption
P = Point("P")
# Velocity of Point mass in X-Y plane
P.set_vel(N,vx1 * N.x + vy1 * N.y)
# Making a particle from point mass
Pa = Particle("P",P,m)
# Potential energy of system
Pa.potential_energy = m * g * y1
# Non-restorative forces
fl = [(P,friction*cos(alpha)*N.x + friction*sin(alpha)*N.y)]
# Setting-up Lagrangian
L = Lagrangian(N,Pa)
# 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)) )
N = ReferenceFrame("N")
# Lumped mass abstraction
P_1 = Point("P_1")
P_2 = Point("P_2")
# Velocity of Point mass in X-Y plane
P_1.set_vel(N, vx1 * N.x + vy1 * N.y)
P_2.set_vel(N, vx2 * N.x + vy2 * N.y)
# Making a particle from point mass
Pa_1 = Particle("P_1", P_1, m_1)
Pa_2 = Particle("P_2", P_2, m_2)
# Potential energy of system
Pa_1.potential_energy = m_1 * g * y1
Pa_2.potential_energy = m_2 * g * y2
# Non-restorative forces
f_1 = (P_1, torque_1 * (-sin(theta_1) * N.x + cos(theta_1) * N.y) / lg_1
) #torque_1*( -sin(theta_1) * N.x + cos(theta_1) * N.y)/lg_1)
f_2 = (
P_2, 0 * N.x
) #torque_2*( -sin(theta_1 + theta_2) * N.x + cos(theta_1 + theta_2) * N.y)/lg_2)
fl = [f_1, f_2]
# Setting-up Lagrangian
L = Lagrangian(N, Pa_1, Pa_2)
# Generation Equation of Motion
LM = LagrangesMethod(L, var, forcelist=fl, frame=N)
y2 = l * cos(theta)
# 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}
N = ReferenceFrame("N")
# Lumped mass abstraction
P_1 = Point("P_1")
P_2 = Point("P_2")
# Velocity of Point mass in X-Y plane
P_1.set_vel(N,vx1 * N.x + vy1 * N.y)
P_2.set_vel(N,vx2 * N.x + vy2 * N.y)
# 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()
N = ReferenceFrame("N")
# Lumped mass abstraction
P_1 = Point("P_1")
P_2 = Point("P_2")
# Velocity of Point mass in X-Y plane
P_1.set_vel(N, vx1 * N.x + vy1 * N.y)
P_2.set_vel(N, vx2 * N.x + vy2 * N.y)
# Making a particle from point mass
Pa_1 = Particle("P_1", P_1, m_theta)
Pa_2 = Particle("P_2", P_2, m_x)
# Potential energy of system
Pa_1.potential_energy = m_theta * g * y1
Pa_2.potential_energy = 0
# Non-restorative forces
f_theta = (P_1, 0 * N.x)
f_sliding = (P_2, F_x * N.x
) #torque*( -sin(theta) * N.x + cos(theta) * N.y)/l_g)
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 """
