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_input_format():
# 1 dof problem from test_one_dof
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)
# test for input format kane.kanes_equations((body1, body2, particle1))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body2), loads=[])
assert KM.kanes_equations(BL, [])[0] == Matrix([0])
# test for error raised when a wrong force list (in this case a string) is provided
raises(ValueError, lambda: KM._form_fr('bad input'))
# 1 dof problem from test_one_dof with FL & BL in instance
KM = KanesMethod(N, [q], [u], kd, bodies=BL, forcelist=FL)
assert KM.kanes_equations()[0] == Matrix([-c*u - k*q])
# 2 dof problem from test_two_dof
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]
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)
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
# test for input format
# kane.kanes_equations((body1, body2), (load1, load2))
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)
def test_input_format():
# 1 dof problem from test_one_dof
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)
# test for input format kane.kanes_equations((body1, body2, particle1))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for error raised when a wrong force list (in this case a string) is provided
from sympy.testing.pytest import raises
raises(ValueError, lambda: KM._form_fr("bad input"))
# 2 dof problem from test_two_dof
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]
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)
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
# test for input format
# kane.kanes_equations((body1, body2), (load1, load2))
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)
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_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_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 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_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(BL, FL)
assert KM.bodies == 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_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 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', P, m)
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([]), Matrix([]))
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 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_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_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
raises(TypeError, lambda: angular_momentum(N, N, A, Pa))
raises(TypeError, lambda: angular_momentum(O, O, A, Pa))
raises(TypeError, lambda: angular_momentum(O, N, O, Pa))
expected = (I + M * l**2 + 4 * m * l**2) * omega * N.z
assert angular_momentum(O, N, A, Pa) == expected
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 warns_deprecated_sympy():
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 __init__(self):
#We define some quantities required for tests here..
self.p = dynamicsymbols('p:3')
self.q = dynamicsymbols('q:3')
self.dynamic = list(self.p) + list(self.q)
self.states = [radians(45) for x in self.p] + \
[radians(30) for x in self.q]
self.I = ReferenceFrame('I')
self.A = self.I.orientnew('A', 'space', self.p, 'XYZ')
self.B = self.A.orientnew('B', 'space', self.q, 'XYZ')
self.O = Point('O')
self.P1 = self.O.locatenew('P1', 10 * self.I.x + \
10 * self.I.y + 10 * self.I.z)
self.P2 = self.P1.locatenew('P2', 10 * self.I.x + \
10 * self.I.y + 10 * self.I.z)
self.point_list1 = [[2, 3, 1], [4, 6, 2], [5, 3, 1], [5, 3, 6]]
self.point_list2 = [[3, 1, 4], [3, 8, 2], [2, 1, 6], [2, 1, 1]]
self.shape1 = Cylinder(1.0, 1.0)
self.shape2 = Cylinder(1.0, 1.0)
self.Ixx, self.Iyy, self.Izz = symbols('Ixx Iyy Izz')
self.mass = symbols('mass')
self.parameters = [self.Ixx, self.Iyy, self.Izz, self.mass]
self.param_vals = [0, 0, 0, 0]
self.inertia = inertia(self.A, self.Ixx, self.Iyy, self.Izz)
self.rigid_body = RigidBody('rigid_body1', self.P1, self.A, \
self.mass, (self.inertia, self.P1))
self.global_frame1 = VisualizationFrame('global_frame1', \
self.A, self.P1, self.shape1)
self.global_frame2 = VisualizationFrame('global_frame2', \
self.B, self.P2, self.shape2)
self.scene1 = Scene(self.I, self.O, \
(self.global_frame1, self.global_frame2), \
name='scene')
self.particle = Particle('particle1', self.P1, self.mass)
#To make it more readable
p = self.p
q = self.q
#Here is the dragon ..
self.transformation_matrix = \
[[cos(p[1])*cos(p[2]), sin(p[2])*cos(p[1]), -sin(p[1]), 0], \
[sin(p[0])*sin(p[1])*cos(p[2]) - sin(p[2])*cos(p[0]), \
sin(p[0])*sin(p[1])*sin(p[2]) + cos(p[0])*cos(p[2]), \
sin(p[0])*cos(p[1]), 0], \
[sin(p[0])*sin(p[2]) + sin(p[1])*cos(p[0])*cos(p[2]), \
-sin(p[0])*cos(p[2]) + sin(p[1])*sin(p[2])*cos(p[0]), \
cos(p[0])*cos(p[1]), 0], \
[10, 10, 10, 1]]
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 warns_deprecated_sympy():
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
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_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_parallel_axis():
N = ReferenceFrame('N')
m, a, b = symbols('m, a, b')
o = Point('o')
p = o.locatenew('p', a * N.x + b * N.y)
P = Particle('P', o, m)
Ip = P.parallel_axis(p, N)
Ip_expected = inertia(N, m * b**2, m * a**2, m * (a**2 + b**2),
ixy=-m * a * b)
assert Ip == Ip_expected
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)
assert linear_momentum(N, A, Pa) == 10 * N.x + 500 * N.y
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_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_parallel_axis():
N = ReferenceFrame("N")
m, a, b = symbols("m, a, b")
o = Point("o")
p = o.locatenew("p", a * N.x + b * N.y)
P = Particle("P", o, m)
Ip = P.parallel_axis(p, N)
Ip_expected = inertia(N,
m * b**2,
m * a**2,
m * (a**2 + b**2),
ixy=-m * a * b)
assert Ip == Ip_expected
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_particle():
m, m2, v1, v2, v3, r, g, h = symbols("m m2 v1 v2 v3 r g h")
P = Point("P")
P2 = Point("P2")
p = Particle("pa", P, m)
assert p.__str__() == "pa"
assert p.mass == m
assert p.point == P
# Test the mass setter
p.mass = m2
assert p.mass == m2
# Test the point setter
p.point = P2
assert p.point == P2
# Test the linear momentum function
N = ReferenceFrame("N")
O = Point("O")
P2.set_pos(O, r * N.y)
P2.set_vel(N, v1 * N.x)
raises(TypeError, lambda: Particle(P, P, m))
raises(TypeError, lambda: Particle("pa", m, m))
assert p.linear_momentum(N) == m2 * v1 * N.x
assert p.angular_momentum(O, N) == -m2 * r * v1 * N.z
P2.set_vel(N, v2 * N.y)
assert p.linear_momentum(N) == m2 * v2 * N.y
assert p.angular_momentum(O, N) == 0
P2.set_vel(N, v3 * N.z)
assert p.linear_momentum(N) == m2 * v3 * N.z
assert p.angular_momentum(O, N) == m2 * r * v3 * N.x
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
p.potential_energy = m * g * h
assert p.potential_energy == m * g * h
# TODO make the result not be system-dependent
assert p.kinetic_energy(N) in [
m2 * (v1**2 + v2**2 + v3**2) / 2,
m2 * v1**2 / 2 + m2 * v2**2 / 2 + m2 * v3**2 / 2,
]
def test_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