248 lines
9.9 KiB
Python
248 lines
9.9 KiB
Python
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
|
|
RigidBody, LagrangesMethod, Particle,
|
|
inertia, Lagrangian)
|
|
from sympy.core.function import (Derivative, Function)
|
|
from sympy.core.numbers import pi
|
|
from sympy.core.symbol import symbols
|
|
from sympy.functions.elementary.trigonometric import (cos, sin, tan)
|
|
from sympy.matrices.dense import Matrix
|
|
from sympy.simplify.simplify import simplify
|
|
from sympy.testing.pytest import raises
|
|
|
|
|
|
def test_invalid_coordinates():
|
|
# Simple pendulum, but use symbol instead of dynamicsymbol
|
|
l, m, g = symbols('l m g')
|
|
q = symbols('q') # Generalized coordinate
|
|
N, O = ReferenceFrame('N'), Point('O')
|
|
O.set_vel(N, 0)
|
|
P = Particle('P', Point('P'), m)
|
|
P.point.set_pos(O, l * (sin(q) * N.x - cos(q) * N.y))
|
|
P.potential_energy = m * g * P.point.pos_from(O).dot(N.y)
|
|
L = Lagrangian(N, P)
|
|
raises(ValueError, lambda: LagrangesMethod(L, [q], bodies=P))
|
|
|
|
|
|
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_nonminimal_pendulum():
|
|
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)
|
|
# 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()
|
|
# Check solution
|
|
lam1 = LM.lam_vec[0, 0]
|
|
eom_sol = Matrix([[m*Derivative(q1, t, t) - 9.8*m + 2*lam1*q1],
|
|
[m*Derivative(q2, t, t) + 2*lam1*q2]])
|
|
assert LM.eom == eom_sol
|
|
# Check multiplier solution
|
|
lam_sol = Matrix([(19.6*q1 + 2*q1d**2 + 2*q2d**2)/(4*q1**2/m + 4*q2**2/m)])
|
|
assert simplify(LM.solve_multipliers(sol_type='Matrix')) == simplify(lam_sol)
|
|
|
|
|
|
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_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()
|
|
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)) + 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)
|