def test_one_dof():
# This is for a 1 dof spring-mass-damper case.
# It is described in more detail in the Kane docstring.
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, c, k = symbols('m c k')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, u * N.x)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle()
pa.mass = m
pa.point = P
BL = [pa]
KM = Kane(N)
KM.coords([q])
KM.speeds([u])
KM.kindiffeq(kd)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
assert KM.linearize() == (Matrix([[0, 1], [k, c]]), Matrix([]))
def test_pend():
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, l, g = symbols('m l g')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y)
kd = [qd - u]
FL = [(P, m * g * N.x)]
pa = Particle()
pa.mass = m
pa.point = P
BL = [pa]
KM = Kane(N)
KM.coords([q])
KM.speeds([u])
KM.kindiffeq(kd)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
rhs.simplify()
assert expand(rhs[0]) == expand(-g / l * sin(q))
def test_particle():
m = Symbol('m')
P = Point('P')
p = Particle()
assert p.mass == None
assert p.point == None
# Test the mass setter
p.mass = m
assert p.mass == m
# Test the point setter
p.point = P
assert p.point == P
def test_particle():
m, m2 = symbols("m m2")
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
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_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.set_potential_energy(m * g * h)
A.set_potential_energy(M * g * H)
assert potential_energy(A, Pa) == m * g * h + M * g * H
def test_linear_momentum():
N = ReferenceFrame('N')
Ac = Point('Ac')
Ac.set_vel(N, 25 * N.y)
I = outer(N.x, N.x)
A = RigidBody('A', Ac, N, 20, (I, Ac))
P = Point('P')
Pa = Particle('Pa', P, 1)
Pa.point.set_vel(N, 10 * N.x)
assert linear_momentum(N, A, Pa) == 10 * N.x + 500 * N.y
def __init__(self,
name,
masscenter=None,
mass=None,
frame=None,
central_inertia=None):
self.name = name
self.loads = []
if frame is None:
frame = ReferenceFrame(name + '_frame')
if masscenter is None:
masscenter = Point(name + '_masscenter')
if central_inertia is None and mass is None:
ixx = Symbol(name + '_ixx')
iyy = Symbol(name + '_iyy')
izz = Symbol(name + '_izz')
izx = Symbol(name + '_izx')
ixy = Symbol(name + '_ixy')
iyz = Symbol(name + '_iyz')
_inertia = (inertia(frame, ixx, iyy, izz, ixy, iyz,
izx), masscenter)
else:
_inertia = (central_inertia, masscenter)
if mass is None:
_mass = Symbol(name + '_mass')
else:
_mass = mass
masscenter.set_vel(frame, 0)
# If user passes masscenter and mass then a particle is created
# otherwise a rigidbody. As a result a body may or may not have inertia.
if central_inertia is None and mass is not None:
self.frame = frame
self.masscenter = masscenter
Particle.__init__(self, name, masscenter, _mass)
else:
RigidBody.__init__(self, name, masscenter, frame, _mass, _inertia)
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in Kane. The inertia of the pendulum is
# defined about the hinge, not about the mass center.
# Defining the constants and knowns of the system
gravity = symbols('g')
k, ls = symbols('k ls')
a, mA, mC = symbols('a mA mC')
F = dynamicsymbols('F')
Ix, Iy, Iz = symbols('Ix Iy Iz')
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
# Creating reference frames
N = ReferenceFrame('N')
A = ReferenceFrame('A')
A.orient(N, 'Axis', [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point('O')
# Creating and Locating the positions of the cart,C, and mass center of the pendulum, A
C = O.locatenew('C', q1 * N.x)
Ao = C.locatenew('Ao', a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle('Cart', C, mC)
Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)), (C, N.x * F)]
km = Kane(N)
km.coords([q1, q2])
km.speeds([u1, u2])
km.kindiffeq(kindiffs)
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == -Iz
def test_two_dof():
# This is for a 2 d.o.f., 2 particle spring-mass-damper.
# The first coordinate is the displacement of the first particle, and the
# second is the relative displacement between the first and second
# particles. Speeds are defined as the time derivatives of the particles.
q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
N = ReferenceFrame('N')
P1 = Point('P1')
P2 = Point('P2')
P1.set_vel(N, u1 * N.x)
P2.set_vel(N, (u1 + u2) * N.x)
kd = [q1d - u1, q2d - u2]
# Now we create the list of forces, then assign properties to each
# particle, then create a list of all particles.
FL = [(P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x),
(P2, (-k2 * q2 - c2 * u2) * N.x)]
pa1 = Particle('pa1', P1, m)
pa2 = Particle('pa2', P2, m)
BL = [pa1, pa2]
# Finally we create the KanesMethod object, specify the inertial frame,
# pass relevant information, and form Fr & Fr*. Then we calculate the mass
# matrix and forcing terms, and finally solve for the udots.
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
# The old input format raises a deprecation warning, so catch it here so
# it doesn't cause py.test to fail.
with warns_deprecated_sympy():
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(
(-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) / m)
assert expand(rhs[1]) == expand(
(k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m)
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
4, 1)
def test_center_of_mass():
a = ReferenceFrame('a')
m = symbols('m', real=True)
p1 = Particle('p1', Point('p1_pt'), S.One)
p2 = Particle('p2', Point('p2_pt'), S(2))
p3 = Particle('p3', Point('p3_pt'), S(3))
p4 = Particle('p4', Point('p4_pt'), m)
b_f = ReferenceFrame('b_f')
b_cm = Point('b_cm')
mb = symbols('mb')
b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o = Point('o')
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = 5 / (m + mb + 6) * a.x + (m + mb + 3) / (m + mb + 6) * a.y + mb / (
m + mb + 6) * a.z
assert point_o.pos_from(p1.point) - expr == 0
def test_center_of_mass():
a = ReferenceFrame("a")
m = symbols("m", real=True)
p1 = Particle("p1", Point("p1_pt"), S.One)
p2 = Particle("p2", Point("p2_pt"), S(2))
p3 = Particle("p3", Point("p3_pt"), S(3))
p4 = Particle("p4", Point("p4_pt"), m)
b_f = ReferenceFrame("b_f")
b_cm = Point("b_cm")
mb = symbols("mb")
b = RigidBody("b", b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o = Point("o")
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = (5 / (m + mb + 6) * a.x + (m + mb + 3) / (m + mb + 6) * a.y + mb /
(m + mb + 6) * a.z)
assert point_o.pos_from(p1.point) - expr == 0
def test_linear_momentum():
N = ReferenceFrame("N")
Ac = Point("Ac")
Ac.set_vel(N, 25 * N.y)
I = outer(N.x, N.x)
A = RigidBody("A", Ac, N, 20, (I, Ac))
P = Point("P")
Pa = Particle("Pa", P, 1)
Pa.point.set_vel(N, 10 * N.x)
raises(TypeError, lambda: linear_momentum(A, A, Pa))
raises(TypeError, lambda: linear_momentum(N, N, Pa))
assert linear_momentum(N, A, Pa) == 10 * N.x + 500 * N.y
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.set_potential_energy(-m * g * l * cos(q1))
ParR.set_potential_energy(-m * g * l * cos(q1) - m * g * l * cos(q2))
L = Lagrangian(N, ParP, ParR)
lm = LagrangesMethod(L, [q1, q2])
lm.form_lagranges_equations()
assert expand(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) -
(simplify(lm.eom[0]))) == 0
assert expand((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)) -
(simplify(lm.eom[1]))) == 0
def __init__(self, name, masscenter=None, mass=None, frame=None,
central_inertia=None):
self.name = name
self.loads = []
if frame is None:
frame = ReferenceFrame(name + '_frame')
if masscenter is None:
masscenter = Point(name + '_masscenter')
if central_inertia is None and mass is None:
ixx = Symbol(name + '_ixx')
iyy = Symbol(name + '_iyy')
izz = Symbol(name + '_izz')
izx = Symbol(name + '_izx')
ixy = Symbol(name + '_ixy')
iyz = Symbol(name + '_iyz')
_inertia = (inertia(frame, ixx, iyy, izz, ixy, iyz, izx),
masscenter)
else:
_inertia = (central_inertia, masscenter)
if mass is None:
_mass = Symbol(name + '_mass')
else:
_mass = mass
masscenter.set_vel(frame, 0)
# If user passes masscenter and mass then a particle is created
# otherwise a rigidbody. As a result a body may or may not have inertia.
if central_inertia is None and mass is not None:
self.frame = frame
self.masscenter = masscenter
Particle.__init__(self, name, masscenter, _mass)
else:
RigidBody.__init__(self, name, masscenter, frame, _mass, _inertia)
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
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_two_dof():
# This is for a 2 d.o.f., 2 particle spring-mass-damper.
# The first coordinate is the displacement of the first particle, and the
# second is the relative displacement between the first and second
# particles. Speeds are defined as the time derivatives of the particles.
q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
N = ReferenceFrame('N')
P1 = Point('P1')
P2 = Point('P2')
P1.set_vel(N, u1 * N.x)
P2.set_vel(N, (u1 + u2) * N.x)
kd = [q1d - u1, q2d - u2]
# Now we create the list of forces, then assign properties to each
# particle, then create a list of all particles.
FL = [(P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x),
(P2, (-k2 * q2 - c2 * u2) * N.x)]
pa1 = Particle('pa1', P1, m)
pa2 = Particle('pa2', P2, m)
BL = [pa1, pa2]
# Finally we create the Kane object, specify the inertial frame, pass
# relevant information, and form Fr & Fr*. Then we calculate the mass
# matrix and forcing terms, and finally solve for the udots.
KM = Kane(N)
KM.coords([q1, q2])
KM.speeds([u1, u2])
KM.kindiffeq(kd)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(
(-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) / m)
assert expand(rhs[1]) == expand(
(k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 * c2 * u2) / m)
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 second_order_system():
# from sympy.printing.pycode import NumPyPrinter, pycode
coordinates = dynamicsymbols('q:1') # Generalized coordinates
speeds = dynamicsymbols('u:1') # Generalized speeds
# Force applied to the cart
cart_thrust = dynamicsymbols('thrust')
m = sp.symbols('m:1') # Mass of each bob
g, t = sp.symbols('g t')
# Gravity and time
ref_frame = ReferenceFrame('I') # Inertial reference frame
origin = Point('O') # Origin point
origin.set_vel(ref_frame, 0) # Origin's velocity is zero
P0 = Point('P0') # Hinge point of top link
# Set the position of P0
P0.set_pos(origin, coordinates[0] * ref_frame.x)
P0.set_vel(ref_frame, speeds[0] * ref_frame.x) # Set the velocity of P0
Pa0 = Particle('Pa0', P0, m[0]) # Define a particle at P0
# List to hold the n + 1 frames
frames = [ref_frame]
points = [P0] # List to hold the n + 1 points
# List to hold the n + 1 particles
particles = [Pa0]
# List to hold the n + 1 applied forces, including the input force, f
applied_forces = [(P0, cart_thrust * ref_frame.x - m[0] * g *
ref_frame.y)]
# List to hold kinematic ODE's
kindiffs = [coordinates[0].diff(t) - speeds[0]]
# Initialize the object
kane = KanesMethod(ref_frame, q_ind=coordinates,
u_ind=speeds, kd_eqs=kindiffs)
# Generate EoM's fr + frstar = 0
fr, frstar = kane.kanes_equations(particles, applied_forces)
state = coordinates + speeds
gain = [cart_thrust]
kindiff_dict = kane.kindiffdict()
M = kane.mass_matrix_full.subs(kindiff_dict)
F = kane.forcing_full.subs(kindiff_dict)
static_parameters = [g, m[0]]
transfer = M.inv() * F
return DynamicSystem(state, gain, static_parameters, transfer)
def test_angular_momentum_and_linear_momentum():
m, M, l1 = symbols('m M l1')
q1d = dynamicsymbols('q1d')
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, q1d * 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))
assert linear_momentum(N, A, Pa) == 2 * m * q1d* l1 * N.y + M * l1 * q1d * N.y
assert angular_momentum(O, N, A, Pa) == 4 * m * q1d * l1**2 * N.z + q1d * N.z
def test_kinetic_energy():
m, M, l1 = symbols('m M l1')
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))
assert 0 == kinetic_energy(N, Pa, A) - (M*l1**2*omega**2/2
+ 2*l1**2*m*omega**2 + omega**2/2)
def test_one_dof():
# This is for a 1 dof spring-mass-damper case.
# It is described in more detail in the KanesMethod docstring.
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, c, k = symbols('m c k')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, u * N.x)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
# The old input format raises a deprecation warning, so catch it here so
# it doesn't cause py.test to fail.
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
2, 1)
assert (KM.linearize(A_and_B=True,
new_method=True)[0] == Matrix([[0, 1],
[-k / m, -c / m]]))
# Ensure that the old linearizer still works and that the new linearizer
# gives the same results. The old linearizer is deprecated and should be
# removed in >= 1.0.
M_old = KM.mass_matrix_full
# The old linearizer raises a deprecation warning, so catch it here so
# it doesn't cause py.test to fail.
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
F_A_old, F_B_old, r_old = KM.linearize()
M_new, F_A_new, F_B_new, r_new = KM.linearize(new_method=True)
assert simplify(M_new.inv() * F_A_new - M_old.inv() * F_A_old) == zeros(2)
def test_gravity():
N = ReferenceFrame("N")
m, M, g = symbols("m M g")
F1, F2 = dynamicsymbols("F1 F2")
po = Point("po")
pa = Particle("pa", po, m)
A = ReferenceFrame("A")
P = Point("P")
I = outer(A.x, A.x)
B = RigidBody("B", P, A, M, (I, P))
forceList = [(po, F1), (P, F2)]
forceList.extend(gravity(g * N.y, pa, B))
l = [(po, F1), (P, F2), (po, g * m * N.y), (P, g * M * N.y)]
for i in range(len(l)):
for j in range(len(l[i])):
assert forceList[i][j] == l[i][j]
def test_gravity():
N = ReferenceFrame('N')
m, M, g = symbols('m M g')
F1, F2 = dynamicsymbols('F1 F2')
po = Point('po')
pa = Particle('pa', po, m)
A = ReferenceFrame('A')
P = Point('P')
I = outer(A.x, A.x)
B = RigidBody('B', P, A, M, (I, P))
forceList = [(po, F1), (P, F2)]
forceList.extend(gravity(g*N.y, pa, B))
l = [(po, F1), (P, F2), (po, g*m*N.y), (P, g*M*N.y)]
for i in range(len(l)):
for j in range(len(l[i])):
assert forceList[i][j] == l[i][j]
def kinetic_energy(self, frame):
"""Kinetic energy of the body.
Parameters
==========
frame : ReferenceFrame or Body
The Body's angular velocity and the velocity of it's mass
center are typically defined with respect to an inertial frame but
any relevant frame in which the velocities are known can be supplied.
Examples
========
>>> from sympy.physics.mechanics import Body, ReferenceFrame, Point
>>> from sympy import symbols
>>> m, v, r, omega = symbols('m v r omega')
>>> N = ReferenceFrame('N')
>>> O = Point('O')
>>> P = Body('P', masscenter=O, mass=m)
>>> P.masscenter.set_vel(N, v * N.y)
>>> P.kinetic_energy(N)
m*v**2/2
>>> N = ReferenceFrame('N')
>>> b = ReferenceFrame('b')
>>> b.set_ang_vel(N, omega * b.x)
>>> P = Point('P')
>>> P.set_vel(N, v * N.x)
>>> B = Body('B', masscenter=P, frame=b)
>>> B.kinetic_energy(N)
B_ixx*omega**2/2 + B_mass*v**2/2
See Also
========
sympy.physics.mechanics : Particle, RigidBody
"""
if isinstance(frame, Body):
frame = Body.frame
if self.is_rigidbody:
return RigidBody(self.name, self.masscenter, self.frame, self.mass,
(self.central_inertia, self.masscenter)).kinetic_energy(frame)
return Particle(self.name, self.masscenter, self.mass).kinetic_energy(frame)
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_kinetic_energy():
m, M, l1 = symbols("m M l1")
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))
raises(TypeError, lambda: kinetic_energy(Pa, Pa, A))
raises(TypeError, lambda: kinetic_energy(N, N, A))
assert (0 == (kinetic_energy(N, Pa, A) -
(M * l1**2 * omega**2 / 2 + 2 * l1**2 * m * omega**2 +
omega**2 / 2)).expand())
def test_pend():
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, l, g = symbols('m l g')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y)
kd = [qd - u]
FL = [(P, m * g * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
rhs.simplify()
assert expand(rhs[0]) == expand(-g / l * sin(q))
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.set_potential_energy(k * q1**2 / 2)
pP2 = Particle('pP2', P2, m)
pP2.set_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_angular_momentum_and_linear_momentum():
"""A rod with length 2l, centroidal inertia I, and mass M along with a
particle of mass m fixed to the end of the rod rotate with an angular rate
of omega about point O which is fixed to the non-particle end of the rod.
The rod's reference frame is A and the inertial frame is N."""
m, M, l, I = symbols('m, M, l, I')
omega = dynamicsymbols('omega')
N = ReferenceFrame('N')
a = ReferenceFrame('a')
O = Point('O')
Ac = O.locatenew('Ac', l * N.x)
P = Ac.locatenew('P', l * N.x)
O.set_vel(N, 0 * N.x)
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)
A = RigidBody('A', Ac, a, M, (I * outer(N.z, N.z), Ac))
expected = 2 * m * omega * l * N.y + M * l * omega * N.y
assert linear_momentum(N, A, Pa) == expected
expected = (I + M * l**2 + 4 * m * l**2) * omega * N.z
assert angular_momentum(O, N, A, Pa) == expected
def test_linearize_pendulum_kane_minimal():
q1 = dynamicsymbols('q1') # angle of pendulum
u1 = dynamicsymbols('u1') # Angular velocity
q1d = dynamicsymbols('q1', 1) # Angular velocity
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
A = N.orientnew('A', 'axis', [q1, N.z])
A.set_ang_vel(N, u1 * N.z)
# Locate point P relative to the origin N*
P = pN.locatenew('P', L * A.x)
P.v2pt_theory(pN, N, A)
pP = Particle('pP', P, m)
# Create Kinematic Differential Equations
kde = Matrix([q1d - u1])
# Input the force resultant at P
R = m * g * N.x
# Solve for eom with kanes method
KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Linearize
A, B, inp_vec = KM.linearize(A_and_B=True, new_method=True, simplify=True)
assert A == Matrix([[0, 1], [-9.8 * cos(q1) / L, 0]])
assert B == Matrix([])
def test_one_dof():
# This is for a 1 dof spring-mass-damper case.
# It is described in more detail in the KanesMethod docstring.
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, c, k = symbols('m c k')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, u * N.x)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
assert KM.linearize() == (Matrix([[0, 1], [-k,
-c]]), Matrix([]), Matrix([]))
def test_linearize_pendulum_kane_minimal():
q1 = dynamicsymbols("q1") # angle of pendulum
u1 = dynamicsymbols("u1") # Angular velocity
q1d = dynamicsymbols("q1", 1) # Angular velocity
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
A = N.orientnew("A", "axis", [q1, N.z])
A.set_ang_vel(N, u1 * N.z)
# Locate point P relative to the origin N*
P = pN.locatenew("P", L * A.x)
P.v2pt_theory(pN, N, A)
pP = Particle("pP", P, m)
# Create Kinematic Differential Equations
kde = Matrix([q1d - u1])
# Input the force resultant at P
R = m * g * N.x
# Solve for eom with kanes method
KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde)
with warns_deprecated_sympy():
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Linearize
A, B, inp_vec = KM.linearize(A_and_B=True, simplify=True)
assert A == Matrix([[0, 1], [-9.8 * cos(q1) / L, 0]])
assert B == Matrix([])
def test_pend():
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, l, g = symbols('m l g')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y)
kd = [qd - u]
FL = [(P, m * g * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
rhs.simplify()
assert expand(rhs[0]) == expand(-g / l * sin(q))
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1)
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_pend():
q, u = dynamicsymbols("q u")
qd, ud = dynamicsymbols("q u", 1)
m, l, g = symbols("m l g")
N = ReferenceFrame("N")
P = Point("P")
P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y)
kd = [qd - u]
FL = [(P, m * g * N.x)]
pa = Particle("pa", P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
with warns_deprecated_sympy():
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
rhs.simplify()
assert expand(rhs[0]) == expand(-g / l * sin(q))
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
2, 1)
def test_two_dof():
# This is for a 2 d.o.f., 2 particle spring-mass-damper.
# The first coordinate is the displacement of the first particle, and the
# second is the relative displacement between the first and second
# particles. Speeds are defined as the time derivatives of the particles.
q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
N = ReferenceFrame('N')
P1 = Point('P1')
P2 = Point('P2')
P1.set_vel(N, u1 * N.x)
P2.set_vel(N, (u1 + u2) * N.x)
kd = [q1d - u1, q2d - u2]
# Now we create the list of forces, then assign properties to each
# particle, then create a list of all particles.
FL = [(P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
q2 - c2 * u2) * N.x)]
pa1 = Particle()
pa2 = Particle()
pa1.mass = m
pa2.mass = m
pa1.point = P1
pa2.point = P2
BL = [pa1, pa2]
# Finally we create the Kane object, specify the inertial frame, pass
# relevant information, and form Fr & Fr*. Then we calculate the mass
# matrix and forcing terms, and finally solve for the udots.
KM = Kane(N)
KM.coords([q1, q2])
KM.speeds([u1, u2])
KM.kindiffeq(kd)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
c2 * u2) / m)
thetad = dynamicsymbols('theta', 1)
# 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)
pDs = pP1.locatenew('D*', L * E.x)
pP1.set_vel(E, 0)
pP1.set_vel(B, pP1.pos_from(pO).diff(t, B))
pP1.v1pt_theory(pO, A, B)
pDs.set_vel(E, 0)
pDs.v2pt_theory(pP1, B, E)
pDs.v2pt_theory(pP1, A, E)
# X*B.z, (Y*E.y + Z*E.z) are forces the panes of glass
# exert on P1, D* respectively
R1 = X * B.z + C * E.x - m1 * g * B.y
R2 = Y * E.y + Z * E.z - C * E.x - m2 * g * B.y
resultants = [R1, R2]
points = [pP1, pDs]
forces = [(pP1, R1), (pDs, R2)]
system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
# kinematic differential equations
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# use nonholonomic partial velocities to find the nonholonomic
# generalized active forces
vc = [dot(pDs.vel(B), E.y).subs(kde_map)]
vc_map = solve(vc, [u3])
partials = partial_velocities(points, [u1, u2], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, system, kde_map, vc_map)
m, g, l = symbols('m g l')
N = ReferenceFrame('N')
# part a
r1 = s*N.x
r2 = (s + l*cos(theta))*N.x + l*sin(theta)*N.y
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())))
def test_particle():
m, m2, v1, v2, v3, r = symbols('m m2 v1 v2 v3 r')
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.linearmomentum(N) == m2 * v1 * N.x
assert p.angularmomentum(O, N) == -m2 * r *v1 * N.z
P2.set_vel(N, v2 * N.y)
assert p.linearmomentum(N) == m2 * v2 * N.y
assert p.angularmomentum(O, N) == 0
P2.set_vel(N, v3 * N.z)
assert p.linearmomentum(N) == m2 * v3 * N.z
assert p.angularmomentum(O, N) == m2 * r * v3 * N.x
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
assert p.linearmomentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
assert p.angularmomentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
one_frame.ang_vel_in(inertial_frame)
two_frame.set_ang_vel(one_frame, omega2*inertial_frame.z)
two_frame.ang_vel_in(inertial_frame)
#Sets up the linear velocities of the points on the linkages
#one.set_vel(inertial_frame, 0)
two.v2pt_theory(one, inertial_frame, one_frame)
two.vel(inertial_frame)
three.v2pt_theory(two, inertial_frame, two_frame)
three.vel(inertial_frame)
#Sets up the masses of the linkages
one_mass, two_mass = symbols('m_1, m_2')
#Defines the linkages as particles
twoP = Particle('twoP', two, one_mass)
threeP = Particle('threeP', three, two_mass)
#Sets up gravity information and assigns gravity to act on mass centers
g = symbols('g')
two_grav_force_vector = -1*one_mass*g*inertial_frame.y
two_grav_force = (two, two_grav_force_vector)
three_grav_force_vector = -1*two_mass*g*inertial_frame.y
three_grav_force = (three, three_grav_force_vector)
#Sets up joint torques
one_torque, two_torque = dynamicsymbols('T_1, T_2')
one_torque_vector = one_torque*inertial_frame.z - two_torque*inertial_frame.z
one_link_torque = (one_frame, one_torque_vector)
two_torque_vector = two_torque*inertial_frame.z
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.set_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]
c_frame.ang_vel_in(inertial_frame)
#Sets up the linear velocities of the points on the linkages
A.set_vel(inertial_frame, 0)
B.v2pt_theory(A, inertial_frame, a_frame)
B.vel(inertial_frame)
C.v2pt_theory(B, inertial_frame, b_frame)
C.vel(inertial_frame)
D.v2pt_theory(C, inertial_frame, c_frame)
D.vel(inertial_frame)
#Sets up the masses of the linkages
a_mass, b_mass, c_mass = symbols('m_a, m_b, m_c')
#Defines the linkages as particles
bP = Particle('bP', B, a_mass)
cP = Particle('cP', C, b_mass)
dP = Particle('dP', D, c_mass)
#Sets up gravity information and assigns gravity to act on mass centers
g = symbols('g')
b_grav_force_vector = -1*a_mass*g*inertial_frame.y
b_grav_force = (B, b_grav_force_vector)
c_grav_force_vector = -1*b_mass*g*inertial_frame.y
c_grav_force = (C, c_grav_force_vector)
d_grav_force_vector = -1*c_mass*g*inertial_frame.y
d_grav_force = (D, d_grav_force_vector)
#Sets up joint torques
a_torque, b_torque, c_torque = dynamicsymbols('T_a, T_b, T_c')
a_torque_vector = a_torque*inertial_frame.z - b_torque*inertial_frame.z
leg_frame.ang_vel_in(inertial_frame)
body_frame.set_ang_vel(leg_frame, omega2*inertial_frame.z)
body_frame.ang_vel_in(inertial_frame)
#Sets up the linear velocities of the points on the linkages
ankle.set_vel(inertial_frame, 0)
waist.v2pt_theory(ankle, inertial_frame, leg_frame)
waist.vel(inertial_frame)
body.v2pt_theory(waist, inertial_frame, body_frame)
body.vel(inertial_frame)
#Sets up the masses of the linkages
leg_mass, body_mass = symbols('m_L, m_B')
#Defines the linkages as particles
waistP = Particle('waistP', waist, leg_mass)
bodyP = Particle('bodyP', body, body_mass)
#Sets up gravity information and assigns gravity to act on mass centers
g = symbols('g')
leg_grav_force_vector = -1*leg_mass*g*inertial_frame.y
leg_grav_force = (waist, leg_grav_force_vector)
body_grav_force_vector = -1*body_mass*g*inertial_frame.y
body_grav_force = (body,body_grav_force_vector)
#Sets up joint torques
ankle_torque, waist_torque = dynamicsymbols('T_a, T_w')
leg_torque_vector = ankle_torque*inertial_frame.z - waist_torque*inertial_frame.z
leg_torque = (leg_frame, leg_torque_vector)
body_torque_vector = waist_torque*inertial_frame.z
