def test_sub_qdot2():
# This test solves exercises 8.3 from Kane 1985 and defines
# all velocities in terms of q, qdot. We check that the generalized active
# forces are correctly computed if u terms are only defined in the
# kinematic differential equations.
#
# This functionality was added in PR 8948. Without qdot/u substitution, the
# KanesMethod constructor will fail during the constraint initialization as
# the B matrix will be poorly formed and inversion of the dependent part
# will fail.
g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t')
q = dynamicsymbols('q:5')
qd = dynamicsymbols('q:5', level=1)
u = dynamicsymbols('u:5')
## Define inertial, intermediate, and rigid body reference frames
A = ReferenceFrame('A')
B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z])
B = B_prime.orientnew('B', 'Axis', [pi / 2 - q[1], B_prime.x])
C = B.orientnew('C', 'Axis', [q[2], B.z])
## Define points of interest and their velocities
pO = Point('O')
pO.set_vel(A, 0)
# R is the point in plane H that comes into contact with disk C.
pR = pO.locatenew('R', q[3] * A.x + q[4] * A.y)
pR.set_vel(A, pR.pos_from(pO).diff(t, A))
pR.set_vel(B, 0)
# C^ is the point in disk C that comes into contact with plane H.
pC_hat = pR.locatenew('C^', 0)
pC_hat.set_vel(C, 0)
# C* is the point at the center of disk C.
pCs = pC_hat.locatenew('C*', R * B.y)
pCs.set_vel(C, 0)
pCs.set_vel(B, 0)
# calculate velocites of points C* and C^ in frame A
pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B
pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C
## Define forces on each point of the system
R_C_hat = Px * A.x + Py * A.y + Pz * A.z
R_Cs = -m * g * A.z
forces = [(pC_hat, R_C_hat), (pCs, R_Cs)]
## Define kinematic differential equations
# let ui = omega_C_A & bi (i = 1, 2, 3)
# u4 = qd4, u5 = qd5
u_expr = [C.ang_vel_in(A) & uv for uv in B]
u_expr += qd[3:]
kde = [ui - e for ui, e in zip(u, u_expr)]
km1 = KanesMethod(A, q, u, kde)
with warns_deprecated_sympy():
fr1, _ = km1.kanes_equations(forces, [])
## Calculate generalized active forces if we impose the condition that the
# disk C is rolling without slipping
u_indep = u[:3]
u_dep = list(set(u) - set(u_indep))
vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]]
km2 = KanesMethod(A,
q,
u_indep,
kde,
u_dependent=u_dep,
velocity_constraints=vc)
with warns_deprecated_sympy():
fr2, _ = km2.kanes_equations(forces, [])
fr1_expected = Matrix([
-R * g * m * sin(q[1]),
-R * (Px * cos(q[0]) + Py * sin(q[0])) * tan(q[1]),
R * (Px * cos(q[0]) + Py * sin(q[0])), Px, Py
])
fr2_expected = Matrix([-R * g * m * sin(q[1]), 0, 0])
assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand()))
assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
def test_sub_qdot():
# This test solves exercises 8.12, 8.17 from Kane 1985 and defines
# some velocities in terms of q, qdot.
## --- Declare symbols ---
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
Q1, Q2, Q3 = symbols('Q1 Q2 Q3')
# --- Reference Frames ---
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
A.set_ang_vel(F, u1 * A.x + u3 * A.z)
# define frames for wheels
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
## --- define points D, S*, Q on frame A and their velocities ---
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll w/o slip
pD.set_vel(F, u2 * A.y)
pS_star = pD.locatenew('S*', e * A.y)
pQ = pD.locatenew('Q', f * A.y - R * A.x)
# masscenters of bodies A, B, C
pA_star = pD.locatenew('A*', a * A.y)
pB_star = pD.locatenew('B*', b * A.z)
pC_star = pD.locatenew('C*', -b * A.z)
for p in [pS_star, pQ, pA_star, pB_star, pC_star]:
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R * A.x)
pC_hat = pC_star.locatenew('C^', -R * A.x)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# --- relate qdot, u ---
# the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip
kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
kde += [u1 - q1d]
kde_map = solve(kde, [q1d, q2d, q3d])
for k, v in list(kde_map.items()):
kde_map[k.diff(t)] = v.diff(t)
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
## --- use kanes method ---
km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3])
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
bodies = [rbA, rbB, rbC]
# Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2)
# -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2
fr_expected = Matrix([
f * Q3 + M * g * e * sin(theta) * cos(q1),
Q2 + M * g * sin(theta) * sin(q1),
e * M * g * cos(theta) - Q1 * f - Q2 * R
])
#Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))])
fr_star_expected = Matrix([
-(IA + 2 * J * b**2 / R**2 + 2 * K + mA * a**2 + 2 * mB * b**2) *
u1.diff(t) - mA * a * u1 * u2,
-(mA + 2 * mB + 2 * J / R**2) * u2.diff(t) + mA * a * u1**2, 0
])
with warns_deprecated_sympy():
fr, fr_star = km.kanes_equations(forces, bodies)
assert (fr.expand() == fr_expected.expand())
assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
def test_sub_qdot():
# This test solves exercises 8.12, 8.17 from Kane 1985 and defines
# some velocities in terms of q, qdot.
## --- Declare symbols ---
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
Q1, Q2, Q3 = symbols('Q1 Q2 Q3')
# --- Reference Frames ---
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
A.set_ang_vel(F, u1*A.x + u3*A.z)
# define frames for wheels
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
## --- define points D, S*, Q on frame A and their velocities ---
pD = Point('D')
pD.set_vel(A, 0)
# u3 will not change v_D_F since wheels are still assumed to roll w/o slip
pD.set_vel(F, u2 * A.y)
pS_star = pD.locatenew('S*', e*A.y)
pQ = pD.locatenew('Q', f*A.y - R*A.x)
# masscenters of bodies A, B, C
pA_star = pD.locatenew('A*', a*A.y)
pB_star = pD.locatenew('B*', b*A.z)
pC_star = pD.locatenew('C*', -b*A.z)
for p in [pS_star, pQ, pA_star, pB_star, pC_star]:
p.v2pt_theory(pD, F, A)
# points of B, C touching the plane P
pB_hat = pB_star.locatenew('B^', -R*A.x)
pC_hat = pC_star.locatenew('C^', -R*A.x)
pB_hat.v2pt_theory(pB_star, F, B)
pC_hat.v2pt_theory(pC_star, F, C)
# --- relate qdot, u ---
# the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip
kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
kde += [u1 - q1d]
kde_map = solve(kde, [q1d, q2d, q3d])
for k, v in list(kde_map.items()):
kde_map[k.diff(t)] = v.diff(t)
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
## --- use kanes method ---
km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3])
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
bodies = [rbA, rbB, rbC]
# Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2)
# -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2
fr_expected = Matrix([
f*Q3 + M*g*e*sin(theta)*cos(q1),
Q2 + M*g*sin(theta)*sin(q1),
e*M*g*cos(theta) - Q1*f - Q2*R])
#Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))])
fr_star_expected = Matrix([
-(IA + 2*J*b**2/R**2 + 2*K +
mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2,
-(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2,
0])
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
fr, fr_star = km.kanes_equations(forces, bodies)
assert (fr.expand() == fr_expected.expand())
assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
def test_sub_qdot2():
# This test solves exercises 8.3 from Kane 1985 and defines
# all velocities in terms of q, qdot. We check that the generalized active
# forces are correctly computed if u terms are only defined in the
# kinematic differential equations.
#
# This functionality was added in PR 8948. Without qdot/u substitution, the
# KanesMethod constructor will fail during the constraint initialization as
# the B matrix will be poorly formed and inversion of the dependent part
# will fail.
g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t')
q = dynamicsymbols('q:5')
qd = dynamicsymbols('q:5', level=1)
u = dynamicsymbols('u:5')
## Define inertial, intermediate, and rigid body reference frames
A = ReferenceFrame('A')
B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z])
B = B_prime.orientnew('B', 'Axis', [pi/2 - q[1], B_prime.x])
C = B.orientnew('C', 'Axis', [q[2], B.z])
## Define points of interest and their velocities
pO = Point('O')
pO.set_vel(A, 0)
# R is the point in plane H that comes into contact with disk C.
pR = pO.locatenew('R', q[3]*A.x + q[4]*A.y)
pR.set_vel(A, pR.pos_from(pO).diff(t, A))
pR.set_vel(B, 0)
# C^ is the point in disk C that comes into contact with plane H.
pC_hat = pR.locatenew('C^', 0)
pC_hat.set_vel(C, 0)
# C* is the point at the center of disk C.
pCs = pC_hat.locatenew('C*', R*B.y)
pCs.set_vel(C, 0)
pCs.set_vel(B, 0)
# calculate velocites of points C* and C^ in frame A
pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B
pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C
## Define forces on each point of the system
R_C_hat = Px*A.x + Py*A.y + Pz*A.z
R_Cs = -m*g*A.z
forces = [(pC_hat, R_C_hat), (pCs, R_Cs)]
## Define kinematic differential equations
# let ui = omega_C_A & bi (i = 1, 2, 3)
# u4 = qd4, u5 = qd5
u_expr = [C.ang_vel_in(A) & uv for uv in B]
u_expr += qd[3:]
kde = [ui - e for ui, e in zip(u, u_expr)]
km1 = KanesMethod(A, q, u, kde)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
fr1, _ = km1.kanes_equations(forces, [])
## Calculate generalized active forces if we impose the condition that the
# disk C is rolling without slipping
u_indep = u[:3]
u_dep = list(set(u) - set(u_indep))
vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]]
km2 = KanesMethod(A, q, u_indep, kde,
u_dependent=u_dep, velocity_constraints=vc)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
fr2, _ = km2.kanes_equations(forces, [])
fr1_expected = Matrix([
-R*g*m*sin(q[1]),
-R*(Px*cos(q[0]) + Py*sin(q[0]))*tan(q[1]),
R*(Px*cos(q[0]) + Py*sin(q[0])),
Px,
Py])
fr2_expected = Matrix([
-R*g*m*sin(q[1]),
0,
0])
assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand()))
assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
def test_replace_qdots_in_force():
# Test PR 16700 "Replaces qdots with us in force-list in kanes.py"
# The new functionality allows one to specify forces in qdots which will
# automatically be replaced with u:s which are defined by the kde supplied
# to KanesMethod. The test case is the double pendulum with interacting
# forces in the example of chapter 4.7 "CONTRIBUTING INTERACTION FORCES"
# in Ref. [1]. Reference list at end test function.
q1, q2 = dynamicsymbols("q1, q2")
qd1, qd2 = dynamicsymbols("q1, q2", level=1)
u1, u2 = dynamicsymbols("u1, u2")
l, m = symbols("l, m")
N = ReferenceFrame("N") # Inertial frame
A = N.orientnew("A", "Axis", (q1, N.z)) # Rod A frame
B = A.orientnew("B", "Axis", (q2, N.z)) # Rod B frame
O = Point("O") # Origo
O.set_vel(N, 0)
P = O.locatenew("P", (l * A.x)) # Point @ end of rod A
P.v2pt_theory(O, N, A)
Q = P.locatenew("Q", (l * B.x)) # Point @ end of rod B
Q.v2pt_theory(P, N, B)
Ap = Particle("Ap", P, m)
Bp = Particle("Bp", Q, m)
# The forces are specified below. sigma is the torsional spring stiffness
# and delta is the viscous damping coefficient acting between the two
# bodies. Here, we specify the viscous damper as function of qdots prior
# forming the kde. In more complex systems it not might be obvious which
# kde is most efficient, why it is convenient to specify viscous forces in
# qdots independently of the kde.
sig, delta = symbols("sigma, delta")
Ta = (sig * q2 + delta * qd2) * N.z
forces = [(A, Ta), (B, -Ta)]
# Try different kdes.
kde1 = [u1 - qd1, u2 - qd2]
kde2 = [u1 - qd1, u2 - (qd1 + qd2)]
KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1)
fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces)
KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2)
fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces)
# Check EOM for KM2:
# Mass and force matrix from p.6 in Ref. [2] with added forces from
# example of chapter 4.7 in [1] and without gravity.
forcing_matrix_expected = Matrix(
[
[m * l ** 2 * sin(q2) * u2 ** 2 + sig * q2 + delta * (u2 - u1)],
[m * l ** 2 * sin(q2) * -(u1 ** 2) - sig * q2 - delta * (u2 - u1)],
]
)
mass_matrix_expected = Matrix(
[[2 * m * l ** 2, m * l ** 2 * cos(q2)], [m * l ** 2 * cos(q2), m * l ** 2]]
)
assert KM2.mass_matrix.expand() == mass_matrix_expected.expand()
assert KM2.forcing.expand() == forcing_matrix_expected.expand()
# Check fr1 with reference fr_expected from [1] with u:s instead of qdots.
fr1_expected = Matrix([0, -(sig * q2 + delta * u2)])
assert fr1.expand() == fr1_expected.expand()
# Check fr2
fr2_expected = Matrix([sig * q2 + delta * (u2 - u1), -sig * q2 - delta * (u2 - u1)])
assert fr2.expand() == fr2_expected.expand()
# Specifying forces in u:s should stay the same:
Ta = (sig * q2 + delta * u2) * N.z
forces = [(A, Ta), (B, -Ta)]
KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1)
fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces)
assert fr1.expand() == fr1_expected.expand()
Ta = (sig * q2 + delta * (u2 - u1)) * N.z
forces = [(A, Ta), (B, -Ta)]
KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2)
fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces)
assert fr2.expand() == fr2_expected.expand()
# Test if we have a qubic qdot force:
Ta = (sig * q2 + delta * qd2 ** 3) * N.z
forces = [(A, Ta), (B, -Ta)]
KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1)
fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces)
fr1_cubic_expected = Matrix([0, -(sig * q2 + delta * u2 ** 3)])
assert fr1.expand() == fr1_cubic_expected.expand()
KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2)
fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces)
fr2_cubic_expected = Matrix(
[sig * q2 + delta * (u2 - u1) ** 3, -sig * q2 - delta * (u2 - u1) ** 3]
)
assert fr2.expand() == fr2_cubic_expected.expand()
