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_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, )[0] == Matrix([[0, 1], [-k/m, -c/m]]))
def test_linearize_pendulum_lagrange_minimal():
q1 = dynamicsymbols('q1') # angle of pendulum
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, q1d*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)
# Solve for eom with Lagranges method
Lag = Lagrangian(N, pP)
LM = LagrangesMethod(Lag, [q1], forcelist=[(P, m*g*N.x)], frame=N)
LM.form_lagranges_equations()
# Linearize
A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=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 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_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)
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_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 center of mass.
# 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 the
# center of mass 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_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of Kane. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 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])
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^
R.ang_vel_in(N)))
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [q1d - u3/cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]
ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody()
BodyD.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
BodyList = [BodyD]
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3, u4, u5])
KM.kindiffeq(kd)
kdd = KM.kindiffdict()
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
KM2 = Kane(N)
KM2.coords([q1, q2, q3])
KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5])
KM2.kindiffeq(kd)
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
assert fr.expand() == fr2.expand()
assert frstar.expand() == frstar2.expand()
def test_rigidbody():
m, m2, v1, v2, v3, omega = symbols('m m2 v1 v2 v3 omega')
A = ReferenceFrame('A')
A2 = ReferenceFrame('A2')
P = Point('P')
P2 = Point('P2')
I = Dyadic([])
I2 = Dyadic([])
B = RigidBody('B', P, A, m, (I, P))
assert B.mass == m
assert B.frame == A
assert B.masscenter == P
assert B.inertia == (I, B.masscenter)
B.mass = m2
B.frame = A2
B.masscenter = P2
B.inertia = (I2, B.masscenter)
assert B.mass == m2
assert B.frame == A2
assert B.masscenter == P2
assert B.inertia == (I2, B.masscenter)
assert B.masscenter == P2
assert B.inertia == (I2, B.masscenter)
# Testing linear momentum function assuming A2 is the inertial frame
N = ReferenceFrame('N')
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
assert B.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
def test_pendulum_angular_momentum():
"""Consider a pendulum of length OA = 2a, of mass m as a rigid body of
center of mass G (OG = a) which turn around (O,z). The angle between the
reference frame R and the rod is q. The inertia of the body is I =
(G,0,ma^2/3,ma^2/3). """
m, a = symbols('m, a')
q = dynamicsymbols('q')
R = ReferenceFrame('R')
R1 = R.orientnew('R1', 'Axis', [q, R.z])
R1.set_ang_vel(R, q.diff() * R.z)
I = inertia(R1, 0, m * a**2 / 3, m * a**2 / 3)
O = Point('O')
A = O.locatenew('A', 2*a * R1.x)
G = O.locatenew('G', a * R1.x)
S = RigidBody('S', G, R1, m, (I, G))
O.set_vel(R, 0)
A.v2pt_theory(O, R, R1)
G.v2pt_theory(O, R, R1)
assert (4 * m * a**2 / 3 * q.diff() * R.z -
S.angular_momentum(O, R).express(R)) == 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)
assert linear_momentum(N, A, Pa) == 10 * N.x + 500 * N.y
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 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_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of KanesMethod. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 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])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5],
kd_eqs=kd)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd,
u_auxiliary=[u4, u5])
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar.simplify()
frstar2.simplify()
assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# 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 the
# center of mass 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 = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
def test_point_a2pt_theorys():
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
qdd = dynamicsymbols('q', 2)
N = ReferenceFrame('N')
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 0)
O.set_vel(N, 0)
assert P.a2pt_theory(O, N, B) == 0
P.set_pos(O, B.x)
assert P.a2pt_theory(O, N, B) == (-qd**2) * B.x + (qdd) * B.y
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
class RootLink(Link):
"""TODO
"""
def __init__(self, linkage):
super(RootLink, self).__init__(linkage, 'root')
# TODO rename to avoid name conflicts, or inform the user that 'N' is
# taken.
self._frame = ReferenceFrame('N')
self._origin = Point('NO')
self._origin.set_vel(self._frame, 0)
# TODO need to set_acc?
self._origin.set_acc(self._frame, 0)
def get_equations(m_val, g_val, l_val):
# This function body is copyied from:
# http://www.pydy.org/examples/double_pendulum.html
# Retrieved 2015-09-29
from sympy import symbols
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
Particle, KanesMethod)
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
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, u1 * N.z)
B.set_ang_vel(N, u2 * N.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)
kd = [q1d - u1, q2d - u2]
FL = [(P, m * g * N.x), (R, m * g * N.x)]
BL = [ParP, ParR]
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
try:
(fr, frstar) = KM.kanes_equations(bodies=BL, loads=FL)
except TypeError:
(fr, frstar) = KM.kanes_equations(FL, BL)
kdd = KM.kindiffdict()
mm = KM.mass_matrix_full
fo = KM.forcing_full
qudots = mm.inv() * fo
qudots = qudots.subs(kdd)
qudots.simplify()
# Edit:
depv = [q1, q2, u1, u2]
subs = list(zip([m, g, l], [m_val, g_val, l_val]))
return zip(depv, [expr.subs(subs) for expr in qudots])
def test_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of Kane. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 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])
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
R.set_ang_acc(N,
R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^ R.ang_vel_in(N)))
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [q1d - u3 / cos(q2), q2d - u1, q3d - u2 + u3 * tan(q2)]
ForceList = [(Dmc, -m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3, u4, u5])
KM.kindiffeq(kd)
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
KM2 = Kane(N)
KM2.coords([q1, q2, q3])
KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5])
KM2.kindiffeq(kd)
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
assert fr.expand() == fr2.expand()
assert frstar.expand() == frstar2.expand()
def test_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of KanesMethod. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 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])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5],
kd_eqs=kd)
with warns_deprecated_sympy():
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd,
u_auxiliary=[u4, u5])
with warns_deprecated_sympy():
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar.simplify()
frstar2.simplify()
assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
def test_point_v2pt_theorys():
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
N = ReferenceFrame('N')
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 0)
O.set_vel(N, 0)
assert P.v2pt_theory(O, N, B) == 0
P = O.locatenew('P', B.x)
assert P.v2pt_theory(O, N, B) == (qd * B.z ^ B.x)
O.set_vel(N, N.x)
assert P.v2pt_theory(O, N, B) == N.x + qd * B.y
def test_point_v2pt_theorys():
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
N = ReferenceFrame('N')
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 0)
O.set_vel(N, 0)
assert P.v2pt_theory(O, N, B) == 0
P = O.locatenew('P', B.x)
assert P.v2pt_theory(O, N, B) == (qd * B.z ^ B.x)
O.set_vel(N, N.x)
assert P.v2pt_theory(O, N, B) == N.x + qd * B.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.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 = 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)
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 get_equations(m_val, g_val, l_val):
# This function body is copyied from:
# http://www.pydy.org/examples/double_pendulum.html
# Retrieved 2015-09-29
from sympy import symbols
from sympy.physics.mechanics import (
dynamicsymbols, ReferenceFrame, Point, Particle, KanesMethod
)
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
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, u1 * N.z)
B.set_ang_vel(N, u2 * N.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)
kd = [q1d - u1, q2d - u2]
FL = [(P, m * g * N.x), (R, m * g * N.x)]
BL = [ParP, ParR]
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
(fr, frstar) = KM.kanes_equations(FL, BL)
kdd = KM.kindiffdict()
mm = KM.mass_matrix_full
fo = KM.forcing_full
qudots = mm.inv() * fo
qudots = qudots.subs(kdd)
qudots.simplify()
# Edit:
depv = [q1, q2, u1, u2]
subs = list(zip([m, g, l], [m_val, g_val, l_val]))
return zip(depv, [expr.subs(subs) for expr in qudots])
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]
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_rigidbody2():
M, v, r, omega = dynamicsymbols('M v r omega')
N = ReferenceFrame('N')
b = ReferenceFrame('b')
b.set_ang_vel(N, omega * b.x)
P = Point('P')
I = outer (b.x, b.x)
Inertia_tuple = (I, P)
B = RigidBody('B', P, b, M, Inertia_tuple)
P.set_vel(N, v * b.x)
assert B.angularmomentum(P, N) == omega * b.x
O = Point('O')
O.set_vel(N, v * b.x)
P.set_pos(O, r * b.y)
assert B.angularmomentum(O, N) == omega * b.x - M*v*r*b.z
def test_point_v1pt_theorys():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, qd * B.z)
O = Point('O')
P = O.locatenew('P', B.x)
P.set_vel(B, 0)
O.set_vel(N, 0)
assert P.v1pt_theory(O, N, B) == qd * B.y
O.set_vel(N, N.x)
assert P.v1pt_theory(O, N, B) == N.x + qd * B.y
P.set_vel(B, B.z)
assert P.v1pt_theory(O, N, B) == B.z + N.x + qd * B.y
def test_point_v1pt_theorys():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, qd * B.z)
O = Point('O')
P = O.locatenew('P', B.x)
P.set_vel(B, 0)
O.set_vel(N, 0)
assert P.v1pt_theory(O, N, B) == qd * B.y
O.set_vel(N, N.x)
assert P.v1pt_theory(O, N, B) == N.x + qd * B.y
P.set_vel(B, B.z)
assert P.v1pt_theory(O, N, B) == B.z + N.x + qd * B.y
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
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]
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
RHS = l.rhs()
RHS.simplify()
assert (l.mass_matrix[3:6] == [0, 5 * m * r**2 / 4, 0])
assert (RHS[4] == (-4 * g * sin(q2) + 5 * r * sin(q2) * cos(q2) * q1d**2 +
6 * r * cos(q2) * q1d * q3d) / (5 * r))
assert RHS[5] == (5 * sin(q2)**2 * q1d + 6 * sin(q2) * q3d -
q1d) * q2d / cos(q2)
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_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_point_a1pt_theorys():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, qd * B.z)
O = Point('O')
P = O.locatenew('P', B.x)
P.set_vel(B, 0)
O.set_vel(N, 0)
assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y
P.set_vel(B, q2d * B.z)
assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y + q2dd * B.z
O.set_vel(N, q2d * B.x)
assert P.a1pt_theory(O, N, B) == ((q2dd - qd**2) * B.x +
(q2d * qd + qdd) * B.y + q2dd * B.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_point_a1pt_theorys():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, qd * B.z)
O = Point('O')
P = O.locatenew('P', B.x)
P.set_vel(B, 0)
O.set_vel(N, 0)
assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y
P.set_vel(B, q2d * B.z)
assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y + q2dd * B.z
O.set_vel(N, q2d * B.x)
assert P.a1pt_theory(O, N, B) == ((q2dd - qd**2) * B.x + (q2d * qd + qdd) * B.y +
q2dd * B.z)
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_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_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_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_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_rigidbody2():
M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
N = ReferenceFrame('N')
b = ReferenceFrame('b')
b.set_ang_vel(N, omega * b.x)
P = Point('P')
I = outer(b.x, b.x)
Inertia_tuple = (I, P)
B = RigidBody('B', P, b, M, Inertia_tuple)
P.set_vel(N, v * b.x)
assert B.angular_momentum(P, N) == omega * b.x
O = Point('O')
O.set_vel(N, v * b.x)
P.set_pos(O, r * b.y)
assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z
B.potential_energy = M * g * h
assert B.potential_energy == M * g * h
assert expand(2 * B.kinetic_energy(N)) == omega**2 + M * v**2
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_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_rigidbody3():
q1, q2, q3, q4 = dynamicsymbols('q1:5')
p1, p2, p3 = symbols('p1:4')
m = symbols('m')
A = ReferenceFrame('A')
B = A.orientnew('B', 'axis', [q1, A.x])
O = Point('O')
O.set_vel(A, q2 * A.x + q3 * A.y + q4 * A.z)
P = O.locatenew('P', p1 * B.x + p2 * B.y + p3 * B.z)
I = outer(B.x, B.x)
rb1 = RigidBody('rb1', P, B, m, (I, P))
# I_S/O = I_S/S* + I_S*/O
rb2 = RigidBody('rb2', P, B, m,
(I + inertia_of_point_mass(m, P.pos_from(O), B), O))
assert rb1.central_inertia == rb2.central_inertia
assert rb1.angular_momentum(O, A) == rb2.angular_momentum(O, A)
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)
KM.kanes_equations(BL, FL)
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)
# Make sure an error is raised if nonlinear kinematic differential
# equations are supplied.
kd = [q1d - u1**2, sin(q2d) - cos(u2)]
raises(ValueError, lambda: KanesMethod(N, q_ind=[q1, q2],
u_ind=[u1, u2], kd_eqs=kd))
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)
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_rigidbody3():
q1, q2, q3, q4 = dynamicsymbols("q1:5")
p1, p2, p3 = symbols("p1:4")
m = symbols("m")
A = ReferenceFrame("A")
B = A.orientnew("B", "axis", [q1, A.x])
O = Point("O")
O.set_vel(A, q2 * A.x + q3 * A.y + q4 * A.z)
P = O.locatenew("P", p1 * B.x + p2 * B.y + p3 * B.z)
P.v2pt_theory(O, A, B)
I = outer(B.x, B.x)
rb1 = RigidBody("rb1", P, B, m, (I, P))
# I_S/O = I_S/S* + I_S*/O
rb2 = RigidBody("rb2", P, B, m,
(I + inertia_of_point_mass(m, P.pos_from(O), B), O))
assert rb1.central_inertia == rb2.central_inertia
assert rb1.angular_momentum(O, A) == rb2.angular_momentum(O, A)
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_point_funcs():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, 5 * B.y)
O = Point('O')
P = O.locatenew('P', q * B.x)
assert P.pos_from(O) == q * B.x
P.set_vel(B, qd * B.x + q2d * B.y)
assert P.vel(B) == qd * B.x + q2d * B.y
O.set_vel(N, 0)
assert O.vel(N) == 0
assert P.a1pt_theory(O, N, B) == ((-25 * q + qdd) * B.x + (q2dd) * B.y +
(-10 * qd) * B.z)
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 10 * B.x)
O.set_vel(N, 5 * N.x)
assert O.vel(N) == 5 * N.x
assert P.a2pt_theory(O, N, B) == (-10 * qd**2) * B.x + (10 * qdd) * B.y
B.set_ang_vel(N, 5 * B.y)
O = Point('O')
P = O.locatenew('P', q * B.x)
P.set_vel(B, qd * B.x + q2d * B.y)
O.set_vel(N, 0)
assert P.v1pt_theory(O, N, B) == qd * B.x + q2d * B.y - 5 * q * B.z
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 LM.solve_multipliers(sol_type='Matrix') == lam_sol
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_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(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 >= 0.7.7.
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_partial_velocity():
q1, q2, q3, u1, u2, u3 = dynamicsymbols("q1 q2 q3 u1 u2 u3")
u4, u5 = dynamicsymbols("u4, u5")
r = symbols("r")
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])
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
C = Point("C")
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew("Dmc", r * L.z)
Dmc.v2pt_theory(C, N, R)
vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)]
u_list = [u1, u2, u3, u4, u5]
assert partial_velocity(vel_list, u_list) == [
[-r * L.y, 0, L.x],
[r * L.x, 0, L.y],
[0, 0, L.z],
[L.x, L.x, 0],
[cos(q2) * L.y - sin(q2) * L.z, cos(q2) * L.y - sin(q2) * L.z, 0],
]
