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_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_dcm():
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = A.orientnew('B', 'Axis', [q2, A.x])
C = B.orientnew('C', 'Axis', [q3, B.y])
D = N.orientnew('D', 'Axis', [q4, N.y])
E = N.orientnew('E', 'Space', [q1, q2, q3], '123')
assert N.dcm(C) == Matrix([
[- sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), - sin(q1) *
cos(q2), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1)], [sin(q1) *
cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) *
sin(q3) - sin(q2) * cos(q1) * cos(q3)], [- sin(q3) * cos(q2), sin(q2),
cos(q2) * cos(q3)]])
# This is a little touchy. Is it ok to use simplify in assert?
assert D.dcm(C) == Matrix(
[[cos(q1) * cos(q3) * cos(q4) - sin(q3) * (- sin(q4) * cos(q2) +
sin(q1) * sin(q2) * cos(q4)), - sin(q2) * sin(q4) - sin(q1) *
cos(q2) * cos(q4), sin(q3) * cos(q1) * cos(q4) + cos(q3) * (- sin(q4) *
cos(q2) + sin(q1) * sin(q2) * cos(q4))], [sin(q1) * cos(q3) +
sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) * sin(q3) -
sin(q2) * cos(q1) * cos(q3)], [sin(q4) * cos(q1) * cos(q3) -
sin(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)), sin(q2) *
cos(q4) - sin(q1) * sin(q4) * cos(q2), sin(q3) * sin(q4) * cos(q1) +
cos(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4))]])
assert E.dcm(N) == Matrix(
[[cos(q2)*cos(q3), sin(q3)*cos(q2), -sin(q2)],
[sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) +
cos(q1)*cos(q3), sin(q1)*cos(q2)], [sin(q1)*sin(q3) +
sin(q2)*cos(q1)*cos(q3), - sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1),
cos(q1)*cos(q2)]])
def test_dcm():
q1, q2, q3, q4 = dynamicsymbols("q1 q2 q3 q4")
N = ReferenceFrame("N")
A = N.orientnew("A", "Axis", [q1, N.z])
B = A.orientnew("B", "Axis", [q2, A.x])
C = B.orientnew("C", "Axis", [q3, B.y])
D = N.orientnew("D", "Axis", [q4, N.y])
E = N.orientnew("E", "Space", [q1, q2, q3], "123")
assert N.dcm(C) == Matrix(
[
[
-sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
-sin(q1) * cos(q2),
sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1),
],
[
sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
cos(q1) * cos(q2),
sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3),
],
[-sin(q3) * cos(q2), sin(q2), cos(q2) * cos(q3)],
]
)
# This is a little touchy. Is it ok to use simplify in assert?
test_mat = D.dcm(C) - Matrix(
[
[
cos(q1) * cos(q3) * cos(q4) - sin(q3) * (-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)),
-sin(q2) * sin(q4) - sin(q1) * cos(q2) * cos(q4),
sin(q3) * cos(q1) * cos(q4) + cos(q3) * (-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)),
],
[
sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
cos(q1) * cos(q2),
sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3),
],
[
sin(q4) * cos(q1) * cos(q3) - sin(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)),
sin(q2) * cos(q4) - sin(q1) * sin(q4) * cos(q2),
sin(q3) * sin(q4) * cos(q1) + cos(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)),
],
]
)
assert test_mat.expand() == zeros(3, 3)
assert E.dcm(N) == Matrix(
[
[cos(q2) * cos(q3), sin(q3) * cos(q2), -sin(q2)],
[
sin(q1) * sin(q2) * cos(q3) - sin(q3) * cos(q1),
sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
sin(q1) * cos(q2),
],
[
sin(q1) * sin(q3) + sin(q2) * cos(q1) * cos(q3),
-sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
cos(q1) * cos(q2),
],
]
)
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_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_point_pos():
q = dynamicsymbols('q')
N = ReferenceFrame('N')
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 10 * N.x + 5 * B.x)
assert P.pos_from(O) == 10 * N.x + 5 * B.x
Q = P.locatenew('Q', 10 * N.y + 5 * B.y)
assert Q.pos_from(P) == 10 * N.y + 5 * B.y
assert Q.pos_from(O) == 10 * N.x + 10 * N.y + 5 * B.x + 5 * B.y
assert O.pos_from(Q) == -10 * N.x - 10 * N.y - 5 * B.x - 5 * B.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_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_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 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_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_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_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]])
def test_simp_pen():
# This tests that the equations generated by LagrangesMethod are identical
# to those obtained by hand calculations. The system under consideration is
# the simple pendulum.
# We begin by creating the generalized coordinates as per the requirements
# of LagrangesMethod. Also we created the associate symbols
# that characterize the system: 'm' is the mass of the bob, l is the length
# of the massless rigid rod connecting the bob to a point O fixed in the
# inertial frame.
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u ', 1)
l, m, g = symbols('l m g')
# We then create the inertial frame and a frame attached to the massless
# string following which we define the inertial angular velocity of the
# string.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q, N.z])
A.set_ang_vel(N, qd * N.z)
# Next, we create the point O and fix it in the inertial frame. We then
# locate the point P to which the bob is attached. Its corresponding
# velocity is then determined by the 'two point formula'.
O = Point('O')
O.set_vel(N, 0)
P = O.locatenew('P', l * A.x)
P.v2pt_theory(O, N, A)
# The 'Particle' which represents the bob is then created and its
# Lagrangian generated.
Pa = Particle('Pa', P, m)
Pa.set_potential_energy(- m * g * l * cos(q))
L = Lagrangian(N, Pa)
# The 'LagrangesMethod' class is invoked to obtain equations of motion.
lm = LagrangesMethod(L, [q])
lm.form_lagranges_equations()
RHS = lm.rhs()
assert RHS[1] == -g*sin(q)/l
def test_time_derivative():
#The use of time_derivative for calculations pertaining to scalar
#fields has been tested in test_coordinate_vars in test_essential.py
A = ReferenceFrame('A')
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
B = A.orientnew('B', 'Axis', [q, A.z])
d = A.x | A.x
assert time_derivative(d, B) == (-qd) * (A.y | A.x) + \
(-qd) * (A.x | A.y)
d1 = A.x | B.y
assert time_derivative(d1, A) == - qd*(A.x|B.x)
assert time_derivative(d1, B) == - qd*(A.y|B.y)
d2 = A.x | B.x
assert time_derivative(d2, A) == qd*(A.x|B.y)
assert time_derivative(d2, B) == - qd*(A.y|B.x)
d3 = A.x | B.z
assert time_derivative(d3, A) == 0
assert time_derivative(d3, B) == - qd*(A.y|B.z)
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1)
q1dd, q2dd, q3dd, q4dd = dynamicsymbols('q1 q2 q3 q4', 2)
C = B.orientnew('C', 'Axis', [q4, B.x])
v1 = q1 * A.z
v2 = q2*A.x + q3*B.y
v3 = q1*A.x + q2*A.y + q3*A.z
assert time_derivative(B.x, C) == 0
assert time_derivative(B.y, C) == - q4d*B.z
assert time_derivative(B.z, C) == q4d*B.y
assert time_derivative(v1, B) == q1d*A.z
assert time_derivative(v1, C) == - q1*sin(q)*q4d*A.x + \
q1*cos(q)*q4d*A.y + q1d*A.z
assert time_derivative(v2, A) == q2d*A.x - q3*qd*B.x + q3d*B.y
assert time_derivative(v2, C) == q2d*A.x - q2*qd*A.y + \
q2*sin(q)*q4d*A.z + q3d*B.y - q3*q4d*B.z
assert time_derivative(v3, B) == (q2*qd + q1d)*A.x + \
(-q1*qd + q2d)*A.y + q3d*A.z
assert time_derivative(d, C) == - qd*(A.y|A.x) + \
sin(q)*q4d*(A.z|A.x) - qd*(A.x|A.y) + sin(q)*q4d*(A.x|A.z)
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_linearize_pendulum_kane_minimal():
q1 = dynamicsymbols('q1') # angle of pendulum
u1 = dynamicsymbols('u1') # Angular velocity
q1d = dynamicsymbols('q1', 1) # Angular velocity
L, m, t = symbols('L, m, t')
g = 9.8
# Compose world frame
N = ReferenceFrame('N')
pN = Point('N*')
pN.set_vel(N, 0)
# A.x is along the pendulum
A = N.orientnew('A', 'axis', [q1, N.z])
A.set_ang_vel(N, u1*N.z)
# Locate point P relative to the origin N*
P = pN.locatenew('P', L*A.x)
P.v2pt_theory(pN, N, A)
pP = Particle('pP', P, m)
# Create Kinematic Differential Equations
kde = Matrix([q1d - u1])
# Input the force resultant at P
R = m*g*N.x
# Solve for eom with kanes method
KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Linearize
A, B, inp_vec = KM.linearize(A_and_B=True, new_method=True, simplify=True)
assert A == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
assert B == Matrix([])
def test_linearize_pendulum_kane_minimal():
q1 = dynamicsymbols("q1") # angle of pendulum
u1 = dynamicsymbols("u1") # Angular velocity
q1d = dynamicsymbols("q1", 1) # Angular velocity
L, m, t = symbols("L, m, t")
g = 9.8
# Compose world frame
N = ReferenceFrame("N")
pN = Point("N*")
pN.set_vel(N, 0)
# A.x is along the pendulum
A = N.orientnew("A", "axis", [q1, N.z])
A.set_ang_vel(N, u1 * N.z)
# Locate point P relative to the origin N*
P = pN.locatenew("P", L * A.x)
P.v2pt_theory(pN, N, A)
pP = Particle("pP", P, m)
# Create Kinematic Differential Equations
kde = Matrix([q1d - u1])
# Input the force resultant at P
R = m * g * N.x
# Solve for eom with kanes method
KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde)
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Linearize
A, B, inp_vec = KM.linearize(A_and_B=True, new_method=True, simplify=True)
assert A == Matrix([[0, 1], [-9.8 * cos(q1) / L, 0]])
assert B == Matrix([])
"""Exercise 8.1 from Kane 1985."""
from __future__ import division
from sympy import factor, solve, simplify, symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import dot, dynamicsymbols, partial_velocity
from util import msprint, subs
g, L, m1, m2, omega, t = symbols('g L m1 m2 omega t')
C, f1, f2 = symbols('C f1 f2')
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
A = ReferenceFrame('A')
B = A.orientnew('B', 'Axis', [omega * t, A.y])
E = B.orientnew('E', 'Axis', [q3, B.z])
pO = Point('O')
pO.set_vel(A, 0)
pO.set_vel(B, 0)
pP1 = pO.locatenew('P1', q1 * B.x + q2 * B.y)
pP2 = pP1.locatenew('P2', L * E.x)
pP1.set_vel(E, 0)
pP1.set_vel(B, pP1.pos_from(pO).diff(t, B))
pP1.v1pt_theory(pO, A, B)
pP2.set_vel(E, 0)
pP2.v2pt_theory(pP1, A, E)
print(
"velocities of points P1, P2 in rf A:\nv_P1_A = {0}\nv_P2_A = {1}".format(
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 three
# speed variables are need to describe this system, along with the disc's
# mass and radius, and the local gravity (note that mass will drop out).
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)
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])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
# 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)
# This is a simple way to form the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
# Kinematic differential equations; how the generalized coordinate time
# derivatives relate to generalized speeds.
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
# Creation of the force list; it is the gravitational force at the mass
# center of the disc. Then we create the disc by assigning a Point to the
# center of mass attribute, a ReferenceFrame to the frame attribute, and mass
# and inertia. Then we form the body list.
ForceList = [(Dmc, -m * g * Y.z)]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
# Finally we form the equations of motion, using the same steps we did
# before. Specify inertial frame, supply generalized speeds, supply
# kinematic differential equation dictionary, compute Fr from the force
# list and Fr* from the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
KM.kanes_equations(BodyList, ForceList)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
kdd = KM.kindiffdict()
rhs = rhs.subs(kdd)
rhs.simplify()
assert rhs.expand() == Matrix([
(6 * u2 * u3 * r - u3**2 * r * tan(q2) + 4 * g * sin(q2)) / (5 * r),
-2 * u1 * u3 / 3, u1 * (-2 * u2 + u3 * tan(q2))
]).expand()
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(
6, 1)
# This code tests our output vs. benchmark values. When r=g=m=1, the
# critical speed (where all eigenvalues of the linearized equations are 0)
# is 1 / sqrt(3) for the upright case.
A = KM.linearize(A_and_B=True)[0]
A_upright = A.subs({
r: 1,
g: 1,
m: 1
}).subs({
q1: 0,
q2: 0,
q3: 0,
u1: 0,
u3: 0
})
import sympy
assert sympy.sympify(A_upright.subs({u2: 1 / sqrt(3)})).eigenvals() == {
S.Zero: 6
}
class YAMSBody(object):
def __init__(self, name):
"""
Origin point have no velocities in the body frame!
"""
self.frame = ReferenceFrame('e_' + name)
self.origin = Point('O_' + name)
self.masscenter = Point('G_' + name)
self.name = name
self.origin.set_vel(self.frame, 0 * self.frame.x)
self.parent = None # Parent body, assuming a tree structure
self.children = [] # children bodies
self.inertial_frame = None # storing the typical inertial frame use for computation
self.inertial_origin = None # storing the typical inertial frame use for computation
def __repr__(self):
s = '<{} object "{}" with attributes:>\n'.format(
type(self).__name__, self.name)
s += ' - origin: {}\n'.format(self.origin)
s += ' - frame: {}\n'.format(self.frame)
return s
def ang_vel_in(self, frame_or_body):
""" Angular velocity of body wrt to another frame or body
This is just a wrapper for the ReferenceFrame ang_vel_in function
"""
if isinstance(frame_or_body, ReferenceFrame):
return self.frame.ang_vel_in(frame_or_body)
else:
if issubclass(type(frame_or_body), YAMSBody):
return self.frame.ang_vel_in(frame_or_body.frame)
else:
raise Exception(
'Unknown class type, use ReferenceFrame of YAMSBody as argument'
)
def connectTo(parent,
child,
type='Rigid',
rel_pos=None,
rot_type='Body',
rot_amounts=None,
rot_order=None,
dynamicAllowed=False):
# register parent/child relationship
child.parent = parent
parent.children.append(child)
if isinstance(parent, YAMSInertialBody):
parent.inertial_frame = parent.frame
child.inertial_frame = parent.frame
parent.inertial_origin = parent.origin
child.inertial_origin = parent.origin
else:
if parent.inertial_frame is None:
raise Exception(
'Parent body was not connected to an inertial frame. Bodies needs to be connected in order, starting from inertial frame.'
)
else:
child.inertial_frame = parent.inertial_frame # the same frame is used for all connected bodies
child.inertial_origin = parent.origin
if rel_pos is None or len(rel_pos) != 3:
raise Exception('rel_pos needs to be an array of size 3')
pos = 0 * parent.frame.x
vel = 0 * parent.frame.x
t = dynamicsymbols._t
if type == 'Free':
# --- "Free", "floating" connection
if not isinstance(parent, YAMSInertialBody):
raise Exception(
'Parent needs to be inertial body for a free connection')
# Defining relative position and velocity of child wrt parent
for d, e in zip(rel_pos[0:3],
(parent.frame.x, parent.frame.y, parent.frame.z)):
if d is not None:
pos += d * e
vel += diff(d, t) * e
elif type == 'Rigid':
# Defining relative position and velocity of child wrt parent
for d, e in zip(rel_pos[0:3],
(parent.frame.x, parent.frame.y, parent.frame.z)):
if d is not None:
pos += d * e
if exprHasFunction(d) and not dynamicAllowed:
raise Exception(
'Position variable cannot be a dynamic variable for a rigid connection: variable {}'
.format(d))
if dynamicAllowed:
vel += diff(d, t) * e
elif type == 'Joint':
# Defining relative position and velocity of child wrt parent
for d, e in zip(rel_pos[0:3],
(parent.frame.x, parent.frame.y, parent.frame.z)):
if d is not None:
pos += d * e
if exprHasFunction(d) and not dynamicAllowed:
raise Exception(
'Position variable cannot be a dynamic variable for a joint connection, variable: {}'
.format(d))
if dynamicAllowed:
vel += diff(d, t) * e
# Orientation
if rot_amounts is None:
raise Exception(
'rot_amounts needs to be provided with Joint connection')
for d in rot_amounts:
if d != 0 and not exprHasFunction(d):
raise Exception(
'Rotation amount variable should be a dynamic variable for a joint connection, variable: {}'
.format(d))
else:
raise Exception('Unsupported joint type: {}'.format(type))
# Orientation (creating a path connecting frames together)
if rot_amounts is None:
child.frame.orient(parent.frame, 'Axis', (0, parent.frame.x))
else:
if rot_type in ['Body', 'Space']:
child.frame.orient(parent.frame, rot_type, rot_amounts,
rot_order) # <<<
else:
child.frame.orient(parent.frame, rot_type, rot_amounts) # <<<
# Position of child origin wrt parent origin
child.origin.set_pos(parent.origin, pos)
# Velocity of child origin frame wrt parent frame (0 for rigid or joint)
child.origin.set_vel(parent.frame, vel)
# Velocity of child masscenter wrt parent frame, based on origin vel (NOTE: for rigid body only, should be overriden for flexible body)
child.masscenter.v2pt_theory(child.origin, parent.frame, child.frame)
# Velocity of child origin wrt inertial frame, using parent origin/frame as intermediate
child.origin.v1pt_theory(parent.origin, child.inertial_frame,
parent.frame)
# Velocity of child masscenter wrt inertial frame, using parent origin/frame as intermediate
child.masscenter.v1pt_theory(parent.origin, child.inertial_frame,
parent.frame)
#r_OB = child.origin.pos_from(child.inertial_origin)
#vel_OB = r_OB.diff(t, child.inertial_frame)
# --------------------------------------------------------------------------------}
# --- Visualization
# --------------------------------------------------------------------------------{
def vizOrigin(self, radius=1.0, color='black', format='pydy'):
if format == 'pydy':
from pydy.viz.shapes import Sphere
from pydy.viz.visualization_frame import VisualizationFrame
return VisualizationFrame(self.frame, self.origin,
Sphere(color=color, radius=radius))
def vizCOG(self, radius=1.0, color='red', format='pydy'):
if format == 'pydy':
from pydy.viz.shapes import Sphere
from pydy.viz.visualization_frame import VisualizationFrame
return VisualizationFrame(self.frame, self.masscenter,
Sphere(color=color, radius=radius))
def vizFrame(self, radius=0.1, length=1.0, format='pydy'):
if format == 'pydy':
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
from sympy.physics.mechanics import Point
X_frame = self.frame.orientnew(
'ffx', 'Axis', (-np.pi / 2, self.frame.z)) # Make y be x
Z_frame = self.frame.orientnew(
'ffz', 'Axis', (+np.pi / 2, self.frame.x)) # Make y be z
X_shape = Cylinder(radius=radius, length=length,
color='red') # Cylinder are along y
Y_shape = Cylinder(radius=radius, length=length, color='green')
Z_shape = Cylinder(radius=radius, length=length, color='blue')
X_center = Point('X')
X_center.set_pos(self.origin, length / 2 * X_frame.y)
Y_center = Point('Y')
Y_center.set_pos(self.origin, length / 2 * self.frame.y)
Z_center = Point('Z')
Z_center.set_pos(self.origin, length / 2 * Z_frame.y)
X_viz_frame = VisualizationFrame(X_frame, X_center, X_shape)
Y_viz_frame = VisualizationFrame(self.frame, Y_center, Y_shape)
Z_viz_frame = VisualizationFrame(Z_frame, Z_center, Z_shape)
return X_viz_frame, Y_viz_frame, Z_viz_frame
def vizAsCylinder(self,
radius,
length,
axis='z',
color='blue',
offset=0,
format='pydy'):
""" """
if format == 'pydy':
# pydy cylinder is along y and centered at the middle of the cylinder
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
if axis == 'y':
e = self.frame
a = self.frame.y
elif axis == 'z':
e = self.frame.orientnew('CF_' + self.name, 'Axis',
(np.pi / 2, self.frame.x))
a = self.frame.z
elif axis == 'x':
e = self.frame.orientnew('CF_' + self.name, 'Axis',
(np.pi / 2, self.frame.z))
a = self.frame.x
shape = Cylinder(radius=radius, length=length, color=color)
center = Point('CC_' + self.name)
center.set_pos(self.origin, (length / 2 + offset) * a)
return VisualizationFrame(e, center, shape)
else:
raise NotImplementedError()
def vizAsRotor(self,
radius=0.1,
length=1,
nB=3,
axis='x',
color='white',
format='pydy'):
# --- Bodies visualization
if format == 'pydy':
from pydy.viz.shapes import Cylinder
from pydy.viz.visualization_frame import VisualizationFrame
blade_shape = Cylinder(radius=radius, length=length, color=color)
viz = []
if axis == 'x':
for iB in np.arange(nB):
frame = self.frame.orientnew(
'b', 'Axis', (-np.pi / 2 + (iB - 1) * 2 * np.pi / nB,
self.frame.x)) # Y pointing along blade
center = Point('RB')
center.set_pos(self.origin, length / 2 * frame.y)
viz.append(VisualizationFrame(frame, center, blade_shape))
return viz
else:
raise NotImplementedError()
class TestVisualizationFrameScene(object):
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_vframe_with_rframe(self):
self.frame1 = VisualizationFrame('frame1', self.I, self.O, \
self.shape1)
assert self.frame1.name == 'frame1'
assert self.frame1.reference_frame == self.I
assert self.frame1.origin == self.O
assert self.frame1.shape is self.shape1
self.frame1.name = 'frame1_'
assert self.frame1.name == 'frame1_'
self.frame1.reference_frame = self.A
assert self.frame1.reference_frame == self.A
self.frame1.origin = self.P1
assert self.frame1.origin == self.P1
self.frame1.shape = self.shape2
assert self.frame1.shape is self.shape2
assert self.frame1.generate_transformation_matrix(self.I, self.O).tolist() == \
self.transformation_matrix
def test_vframe_with_rbody(self):
self.frame2 = VisualizationFrame('frame2', self.rigid_body, \
self.shape1)
assert self.frame2.name == 'frame2'
assert self.frame2.reference_frame == self.A
assert self.frame2.origin == self.P1
assert self.frame2.shape == self.shape1
self.frame2.name = 'frame2_'
assert self.frame2.name == 'frame2_'
self.frame2.reference_frame = self.B
assert self.frame2.reference_frame == self.B
self.frame2.origin = self.P2
assert self.frame2.origin == self.P2
self.frame2.shape = self.shape2
assert self.frame2.shape is self.shape2
self.frame2.reference_frame = self.A
self.frame2.origin = self.P1
assert self.frame2.generate_transformation_matrix(self.I, self.O).tolist() == \
self.transformation_matrix
def test_vframe_with_particle(self):
self.frame3 = VisualizationFrame('frame3', \
self.A, self.particle, \
self.shape1)
assert self.frame3.name == 'frame3'
assert self.frame3.reference_frame == self.A
assert self.frame3.origin == self.P1
assert self.frame3.shape is self.shape1
self.frame3.name = 'frame3_'
assert self.frame3.name == 'frame3_'
self.frame3.reference_frame = self.B
assert self.frame3.reference_frame == self.B
self.frame3.origin = self.P2
assert self.frame3.origin == self.P2
self.frame3.shape = self.shape2
assert self.frame3.shape is self.shape2
self.frame3.reference_frame = self.A
self.frame3.origin = self.P1
assert self.frame3.generate_transformation_matrix(self.I, self.O).tolist() == \
self.transformation_matrix
def test_vframe_without_name(self):
self.frame4 = VisualizationFrame(self.I, self.O, \
self.shape1)
assert self.frame4.name == 'unnamed'
#To check if referenceframe and origin are defined
#properly without name arg
assert self.frame4.reference_frame == self.I
assert self.frame4.origin == self.O
assert self.frame4.shape is self.shape1
self.frame4.name = 'frame1_'
assert self.frame4.name == 'frame1_'
def test_numeric_transform(self):
self.list1 = [[0.5000000000000001, 0.5, \
-0.7071067811865475, 0.0, \
-0.14644660940672627, 0.8535533905932737, \
0.5, 0.0, \
0.8535533905932737, -0.14644660940672627, \
0.5000000000000001, 0.0, \
10.0, 10.0, 10.0, 1.0]]
self.list2 = [[-0.11518993731879767, 0.8178227645734215, \
-0.563823734943801, 0.0, \
0.1332055011661179, 0.5751927992738988, \
0.8070994598700584, 0.0, \
0.984371663956036, 0.017865313009926137, \
-0.17519491371464685, 0.0, \
20.0, 20.0, 20.0, 1.0]]
self.global_frame1.generate_transformation_matrix(self.I, self.O)
self.global_frame1.generate_numeric_transform_function(self.dynamic, \
self.parameters)
assert_allclose(self.global_frame1.\
evaluate_transformation_matrix(self.states, \
self.param_vals), self.list1)
self.global_frame2.generate_transformation_matrix(self.I, self.O)
self.global_frame2.generate_numeric_transform_function(self.dynamic, \
self.parameters)
assert_allclose(self.global_frame2.\
evaluate_transformation_matrix(self.states, \
self.param_vals), self.list2)
def test_perspective_camera(self):
#Camera is a subclass of VisualizationFrame, but without any
#specific shape attached. We supply only ReferenceFrame,Point
#to camera. and it inherits methods from VisualizationFrame
#Testing with rigid-body ..
camera = PerspectiveCamera('camera', self.rigid_body, fov=45, \
near=1, far=1000)
assert camera.name == 'camera'
assert camera.reference_frame == self.A
assert camera.origin == self.P1
assert camera.fov == 45
assert camera.near == 1
assert camera.far == 1000
#Testing with reference_frame, particle ..
camera = PerspectiveCamera('camera', self.I, self.particle, \
fov=45, near=1, far=1000)
assert camera.name == 'camera'
assert camera.reference_frame == self.I
assert camera.origin == self.P1
assert camera.fov == 45
assert camera.near == 1
assert camera.far == 1000
#Testing with reference_frame, point ..
camera = PerspectiveCamera('camera', self.I, self.O, \
fov=45, near=1, far=1000)
assert camera.name == 'camera'
assert camera.reference_frame == self.I
assert camera.origin == self.O
assert camera.fov == 45
assert camera.near == 1
assert camera.far == 1000
camera.name = 'camera1'
assert camera.name == 'camera1'
assert camera.__str__() == 'PerspectiveCamera: camera1'
assert camera.__repr__() == 'PerspectiveCamera'
camera.reference_frame = self.A
assert camera.reference_frame == self.A
camera.origin = self.P1
assert camera.origin == self.P1
camera.fov = 30
assert camera.fov == 30
camera.near = 10
assert camera.near == 10
camera.far = 500
assert camera.far == 500
#Test unnamed
camera1 = PerspectiveCamera(self.I, self.O)
assert camera1.name == 'unnamed'
assert camera1.reference_frame == self.I
assert camera1.origin == self.O
assert camera1.fov == 45
assert camera1.near == 1
assert camera1.far == 1000
def test_orthographic_camera(self):
#As compared to Perspective Camera, Orthographic Camera doesnt
#have fov, instead the left,right,top and bottom faces are
#adjusted by the Scene width, and height
#Testing with rigid_body
camera = OrthoGraphicCamera('camera', self.rigid_body, \
near=1, far=1000)
assert camera.name == 'camera'
assert camera.reference_frame == self.A
assert camera.origin == self.P1
assert camera.near == 1
assert camera.far == 1000
#Testing with reference_frame, particle
camera = OrthoGraphicCamera('camera', self.I, self.particle, \
near=1, far=1000)
assert camera.name == 'camera'
assert camera.reference_frame == self.I
assert camera.origin == self.P1
assert camera.near == 1
assert camera.far == 1000
#Testing with reference_frame, point ...
camera = OrthoGraphicCamera('camera', self.I, self.O, near=1, \
far=1000)
assert camera.name == 'camera'
assert camera.reference_frame == self.I
assert camera.origin == self.O
assert camera.near == 1
assert camera.far == 1000
camera.name = 'camera1'
assert camera.name == 'camera1'
assert camera.__str__() == 'OrthoGraphicCamera: camera1'
assert camera.__repr__() == 'OrthoGraphicCamera'
camera.reference_frame = self.A
assert camera.reference_frame == self.A
camera.origin = self.P1
assert camera.origin == self.P1
camera.near = 10
assert camera.near == 10
camera.far = 500
assert camera.far == 500
camera1 = OrthoGraphicCamera(self.I, self.O)
assert camera1.name == 'unnamed'
assert camera1.reference_frame == self.I
assert camera1.origin == self.O
assert camera1.near == 1
assert camera1.far == 1000
def test_point_light(self):
#Testing with rigid-body ..
light = PointLight('light', self.rigid_body, color='blue')
assert light.name == 'light'
assert light.reference_frame == self.A
assert light.origin == self.P1
assert light.color == 'blue'
#Testing with reference_frame, particle ..
light = PointLight('light', self.I, self.particle, color='blue')
assert light.name == 'light'
assert light.reference_frame == self.I
assert light.origin == self.P1
assert light.color == 'blue'
#Testing with reference_frame, point ..
light = PointLight('light', self.I, self.O, color='blue')
assert light.name == 'light'
assert light.reference_frame == self.I
assert light.origin == self.O
assert light.color == 'blue'
light.name = 'light1'
assert light.name == 'light1'
assert light.__str__() == 'PointLight: light1'
assert light.__repr__() == 'PointLight'
light.reference_frame = self.A
assert light.reference_frame == self.A
light.origin = self.P1
assert light.origin == self.P1
light.color = 'red'
assert light.color == 'red'
#Test unnamed
light1 = PointLight(self.I, self.O)
assert light1.name == 'unnamed'
assert light1.reference_frame == self.I
assert light1.origin == self.O
assert light1.color == 'white'
def test_scene_init(self):
self.scene2 = Scene(self.I, self.O, \
self.global_frame1, self.global_frame2, \
name='scene')
assert self.scene2.name == 'scene'
assert self.scene2.reference_frame == self.I
assert self.scene2.origin == self.O
assert self.scene2.visualization_frames[0] is self.global_frame1
assert self.scene2.visualization_frames[1] is self.global_frame2
self.scene2.name = 'scene1'
assert self.scene2.name == 'scene1'
self.scene2.reference_frame = self.A
assert self.scene2.reference_frame == self.A
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy import solve, symbols, pi, sin, cos
from sympy.simplify.simplify import trigsimp
def msprint(expr):
pr = MechanicsStrPrinter()
return pr.doprint(expr)
theta = symbols('theta:3')
x = symbols('x:3')
q = symbols('q')
A = ReferenceFrame('A')
B = A.orientnew('B', 'SPACE', theta, 'xyz')
O = Point('O')
P = O.locatenew('P', x[0] * A.x + x[1] * A.y + x[2] * A.z)
p = P.pos_from(O)
# From problem, point P is on L (span(B.x)) when:
constraint_eqs = {x[0] : q*cos(theta[1])*cos(theta[2]),
x[1] : q*cos(theta[1])*sin(theta[2]),
x[2] : -q*sin(theta[1])}
# If point P is on line L then r^{P/O} will have no components in the B.y or
# B.z directions since point O is also on line L and B.x is parallel to L.
assert(trigsimp(dot(P.pos_from(O), B.y).subs(constraint_eqs)) == 0)
assert(trigsimp(dot(P.pos_from(O), B.z).subs(constraint_eqs)) == 0)
cR, cF, ls = symbols('cR cF ls')
benchmark_parameters = {
rR: 0.3,
rF: 0.35,
cR: 0.9534570696121847,
ls: 0.2676445084476887,
cF: 0.0320714267276193,
}
## define reference frames
# N: inertial frame
# B: rear aseembly frame
# H: front assembly frame
N = ReferenceFrame('N')
B = N.orientnew('B', 'body', [0, phi, theta], 'zxy') # yaw is ignored
H = B.orientnew('H', 'axis', [delta, B.z])
## define points
# rear wheel/ground contact point
pP = Point('P')
# define unit vectors from rear/front wheel centers to ground
# along the wheel plane
R_z = ((B.y ^ N.z) ^ B.y).normalize()
F_z = ((H.y ^ N.z) ^ H.y).normalize()
# define rear wheel center point
pRs = pP.locatenew('R*', -rR * R_z)
# "top" of steer axis, point of SA closest to R*
def test_linearize_pendulum_kane_nonminimal():
# Create generalized coordinates and speeds for this non-minimal realization
# q1, q2 = N.x and N.y coordinates of pendulum
# u1, u2 = N.x and N.y velocities of pendulum
q1, q2 = dynamicsymbols('q1:3')
q1d, q2d = dynamicsymbols('q1:3', level=1)
u1, u2 = dynamicsymbols('u1:3')
u1d, u2d = dynamicsymbols('u1: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])
# Locate the pendulum mass
P = pN.locatenew('P1', q1 * N.x + q2 * N.y)
pP = Particle('pP', P, m)
# Calculate the kinematic differential equations
kde = Matrix([q1d - u1, q2d - u2])
dq_dict = solve(kde, [q1d, q2d])
# Set velocity of point P
P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
# Configuration constraint is length of pendulum
f_c = Matrix([P.pos_from(pN).magnitude() - L])
# Velocity constraint is that the velocity in the A.x direction is
# always zero (the pendulum is never getting longer).
f_v = Matrix([P.vel(N).express(A).dot(A.x)])
f_v.simplify()
# Acceleration constraints is the time derivative of the velocity constraint
f_a = f_v.diff(t)
f_a.simplify()
# Input the force resultant at P
R = m * g * N.x
# Derive the equations of motion using the KanesMethod class.
KM = KanesMethod(N,
q_ind=[q2],
u_ind=[u2],
q_dependent=[q1],
u_dependent=[u1],
configuration_constraints=f_c,
velocity_constraints=f_v,
acceleration_constraints=f_a,
kd_eqs=kde)
with warns_deprecated_sympy():
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Set the operating point to be straight down, and non-moving
q_op = {q1: L, q2: 0}
u_op = {u1: 0, u2: 0}
ud_op = {u1d: 0, u2d: 0}
A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op],
A_and_B=True,
simplify=True)
assert A.expand() == Matrix([[0, 1], [-9.8 / L, 0]])
assert B == Matrix([])
def __init__(self, parameters=None, samples=None):
## coordinates
# theta: pitch
# psi: yaw
# phi: roll
# delta: steer
# these correspond to x_aux[:] + x[0:3] in the auxiliary state and
# state vectors
x, y, theta, psi, phi, delta = symbols('x y θ ψ φ δ')
# rr: rear radius
# rf: front radius
rr, rf = symbols('rr rf')
# d1: orthogonal distance from rear wheel center to steer axis
# d3: orthogonal distance from front wheel center to steer axis
# d2: steer axis separation
d1, d2, d3 = symbols('d1:4')
## reference frames
# N: inertial frame
# D: rear wheel frame
# C: rear assembly frame - yaw, roll, pitch from inertial frame
# E: front assembly frame
# F: front wheel frame
N = ReferenceFrame('N')
C = N.orientnew('C', 'body', [psi, phi, theta], 'zxy')
E = C.orientnew('E', 'axis', [delta, C.z]) # steer
# Since it will not matter if circles (wheels) rotate, we can attach
# them to the respective assembly frames
# However, we need to rotate the rear/front assembly frames for a
# cylinder shape to be attached correctly. We can simply rotate pi/2
# about the z-axis for C and about the x-axis for E.
Cy = C.orientnew('Cy', 'axis', [pi/2, C.z])
Ey = C.orientnew('Ey', 'axis', [pi/2, E.x])
## points
# define unit vectors from rear/front wheel centers to ground in the
# wheel plane which will be used to describe points
Dz = ((C.y ^ N.z) ^ C.y).normalize()
Fz = ((E.y ^ N.z) ^ E.y).normalize()
# origin of inertial frame
O = Point('O')
# rear wheel/ground contact point
dn = O.locatenew('dn', x*N.x + y*N.y)
# rear wheel center point
do = dn.locatenew('do', -rr*Dz)
# orthogonal projection of rear wheel center on steer axis
ce = do.locatenew('ce', d1*C.x)
# front wheel center point
fo = ce.locatenew('fo', d2*C.z + d3*E.x)
# front wheel/ground contact point
fn = fo.locatenew('fn', rf*Fz)
## define additional points for visualization
# rear assembly center
cc = do.locatenew('cc', d1/2*C.x)
# front assembly center
ec = ce.locatenew('ec', d2/2*C.z)
## shapes
Ds = Cylinder(name='rear wheel', radius=rr, length=0.01)
Cs = Cylinder(name='rear assembly', radius=0.02, length=d1,
color='lightseagreen')
Es = Cylinder(name='front assembly', radius=0.02, length=d2,
color='lightseagreen')
Fs = Cylinder(name='front wheel', radius=rf, length=0.01)
## visualization frames
Dv = VisualizationFrame('Rear Wheel', C, do, Ds)
Cv = VisualizationFrame('Rear Assembly', Cy, cc, Cs)
Ev = VisualizationFrame('Front Assembly', Ey, ec, Es)
Fv = VisualizationFrame('Front Wheel', E, fo, Fs)
# V: visualization frame
V = N.orientnew('V', 'axis', [pi, N.x])
self.coordinate = Coordinates(x, y, theta, psi, phi, delta)
self.parameter = Parameters(rr, rf, d1, d2, d3)
self.frame = Frames(N, V, C, E)
self.shape = Visual(Ds, Cs, Es, Fs)
self.point = Visual(do, cc, ec, fo)
self.vframe = Visual(Dv, Cv, Ev, Fv)
self.scene = Scene(V, dn) # scene moves with rear contact point
self.scene.visualization_frames = list(self.vframe.__dict__.values())
self.scene.states_symbols = list(self.coordinate.__dict__.values())
if parameters is not None:
self.set_parameters(parameters)
if samples is not None:
self.set_trajectory(samples)
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_non_central_inertia():
# This tests that the calculation of Fr* does not depend the point
# about which the inertia of a rigid body is defined. This test solves
# exercises 8.12, 8.17 from Kane 1985.
# Declare symbols
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
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')
Q1, Q2, Q3 = symbols('Q1, Q2 Q3')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
# 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])
B.set_ang_vel(A, u4 * A.z)
C.set_ang_vel(A, u5 * 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 without 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)
for p in [pS_star, pQ]:
p.v2pt_theory(pD, F, A)
# 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 [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)
# the velocities of B^, C^ are zero since B, C are assumed to roll without slip
kde = [q1d - u1, q2d - u4, q3d - u5]
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
# 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))
km = KanesMethod(F,
q_ind=[q1, q2, q3],
u_ind=[u1, u2],
kd_eqs=kde,
u_dependent=[u4, u5],
velocity_constraints=vc,
u_auxiliary=[u3])
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
bodies = [rbA, rbB, rbC]
with warns_deprecated_sympy():
fr, fr_star = km.kanes_equations(forces, bodies)
vc_map = solve(vc, [u4, u5])
# KanesMethod returns the negative of Fr, Fr* as defined in Kane1985.
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
])
t = trigsimp(fr_star.subs(vc_map).subs({u3: 0})).doit().expand()
assert ((fr_star_expected - t).expand() == zeros(3, 1))
# define inertias of rigid bodies A, B, C about point D
# I_S/O = I_S/S* + I_S*/O
bodies2 = []
for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]):
I = I_star + inertia_of_point_mass(rb.mass, rb.masscenter.pos_from(pD),
rb.frame)
bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass,
(I, pD)))
with warns_deprecated_sympy():
fr2, fr_star2 = km.kanes_equations(forces, bodies2)
t = trigsimp(fr_star2.subs(vc_map).subs({u3: 0})).doit()
assert (fr_star_expected - t).expand() == zeros(3, 1)
def test_aux_dep():
# This test is about rolling disc dynamics, comparing the results found
# with KanesMethod to those found when deriving the equations "manually"
# with SymPy.
# The terms Fr, Fr*, and Fr*_steady are all compared between the two
# methods. Here, Fr*_steady refers to the generalized inertia forces for an
# equilibrium configuration.
# Note: comparing to the test of test_rolling_disc() in test_kane.py, this
# test also tests auxiliary speeds and configuration and motion constraints
#, seen in the generalized dependent coordinates q[3], and depend speeds
# u[3], u[4] and u[5].
# First, manual derivation of Fr, Fr_star, Fr_star_steady.
# Symbols for time and constant parameters.
# Symbols for contact forces: Fx, Fy, Fz.
t, r, m, g, I, J = symbols('t r m g I J')
Fx, Fy, Fz = symbols('Fx Fy Fz')
# Configuration variables and their time derivatives:
# q[0] -- yaw
# q[1] -- lean
# q[2] -- spin
# q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in
# A.z direction
# Generalized speeds and their time derivatives:
# u[0] -- disc angular velocity component, disc fixed x direction
# u[1] -- disc angular velocity component, disc fixed y direction
# u[2] -- disc angular velocity component, disc fixed z direction
# u[3] -- disc velocity component, A.x direction
# u[4] -- disc velocity component, A.y direction
# u[5] -- disc velocity component, A.z direction
# Auxiliary generalized speeds:
# ua[0] -- contact point auxiliary generalized speed, A.x direction
# ua[1] -- contact point auxiliary generalized speed, A.y direction
# ua[2] -- contact point auxiliary generalized speed, A.z direction
q = dynamicsymbols('q:4')
qd = [qi.diff(t) for qi in q]
u = dynamicsymbols('u:6')
ud = [ui.diff(t) for ui in u]
ud_zero = dict(zip(ud, [0.] * len(ud)))
ua = dynamicsymbols('ua:3')
ua_zero = dict(zip(ua, [0.] * len(ua))) # noqa:F841
# Reference frames:
# Yaw intermediate frame: A.
# Lean intermediate frame: B.
# Disc fixed frame: C.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q[0], N.z])
B = A.orientnew('B', 'Axis', [q[1], A.x])
C = B.orientnew('C', 'Axis', [q[2], B.y])
# Angular velocity and angular acceleration of disc fixed frame
# u[0], u[1] and u[2] are generalized independent speeds.
C.set_ang_vel(N, u[0] * B.x + u[1] * B.y + u[2] * B.z)
C.set_ang_acc(
N,
C.ang_vel_in(N).diff(t, B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Velocity and acceleration of points:
# Disc-ground contact point: P.
# Center of disc: O, defined from point P with depend coordinate: q[3]
# u[3], u[4] and u[5] are generalized dependent speeds.
P = Point('P')
P.set_vel(N, ua[0] * A.x + ua[1] * A.y + ua[2] * A.z)
O = P.locatenew('O', q[3] * A.z + r * sin(q[1]) * A.y)
O.set_vel(N, u[3] * A.x + u[4] * A.y + u[5] * A.z)
O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N)))
# Kinematic differential equations:
# Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates
# directions of B, for qd0, qd1 and qd2.
# the other is v_o_n_qd = O.vel(N) in A.z direction for qd3.
# Then, solve for dq/dt's in terms of u's: qd_kd.
w_c_n_qd = qd[0] * A.z + qd[1] * B.x + qd[2] * B.y
v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P))
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv)
for uv in B] + [dot(v_o_n_qd - O.vel(N), A.z)])
qd_kd = solve(kindiffs, qd) # noqa:F841
# Values of generalized speeds during a steady turn for later substitution
# into the Fr_star_steady.
steady_conditions = solve(kindiffs.subs({qd[1]: 0, qd[3]: 0}), u)
steady_conditions.update({qd[1]: 0, qd[3]: 0})
# Partial angular velocities and velocities.
partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua]
partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua]
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
# Configuration constraint: f_c, the projection of radius r in A.z direction
# is q[3].
# Velocity constraints: f_v, for u3, u4 and u5.
# Acceleration constraints: f_a.
f_c = Matrix([dot(-r * B.z, A.z) - q[3]])
f_v = Matrix([
dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N), O.pos_from(P))),
ai).expand() for ai in A
])
v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A]) # noqa:F841
# Solve for constraint equations in the form of
# u_dependent = A_rs * [u_i; u_aux].
# First, obtain constraint coefficient matrix: M_v * [u; ua] = 0;
# Second, taking u[0], u[1], u[2] as independent,
# taking u[3], u[4], u[5] as dependent,
# rearranging the matrix of M_v to be A_rs for u_dependent.
# Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
M_v = zeros(3, 9)
for i in range(3):
for j, ui in enumerate(u + ua):
M_v[i, j] = f_v[i].diff(ui)
M_v_i = M_v[:, :3]
M_v_d = M_v[:, 3:6]
M_v_aux = M_v[:, 6:]
M_v_i_aux = M_v_i.row_join(M_v_aux)
A_rs = -M_v_d.inv() * M_v_i_aux
u_dep = A_rs[:, :3] * Matrix(u[:3])
u_dep_dict = dict(zip(u[3:], u_dep))
# Active forces: F_O acting on point O; F_P acting on point P.
# Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
F_O = m * g * A.z
F_P = Fx * A.x + Fy * A.y + Fz * A.z
Fr_u = Matrix([
dot(F_O, pv_o) + dot(F_P, pv_p)
for pv_o, pv_p in zip(partial_v_O, partial_v_P)
])
# Inertia force: R_star_O.
# Inertia of disc: I_C_O, where J is a inertia component about principal axis.
# Inertia torque: T_star_C.
# Generalized inertia forces (unconstrained): Fr_star_u.
R_star_O = -m * O.acc(N)
I_C_O = inertia(B, I, J, I)
T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
+ cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
Fr_star_u = Matrix([
dot(R_star_O, pv) + dot(T_star_C, pav)
for pv, pav in zip(partial_v_O, partial_w_C)
])
# Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
# Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
+ A_rs.T * Fr_star_u[3:6, :]
Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
.subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()
# Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.
# Rigid Bodies: disc, with inertia I_C_O.
iner_tuple = (I_C_O, O)
disc = RigidBody('disc', O, C, m, iner_tuple)
bodyList = [disc]
# Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
F_o = (O, F_O)
F_p = (P, F_P)
forceList = [F_o, F_p]
# KanesMethod.
kane = KanesMethod(N,
q_ind=q[:3],
u_ind=u[:3],
kd_eqs=kindiffs,
q_dependent=q[3:],
configuration_constraints=f_c,
u_dependent=u[3:],
velocity_constraints=f_v,
u_auxiliary=ua)
# fr, frstar, frstar_steady and kdd(kinematic differential equations).
with warns_deprecated_sympy():
(fr, frstar) = kane.kanes_equations(forceList, bodyList)
frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
.subs({q[3]: -r*cos(q[1])}).expand()
kdd = kane.kindiffdict()
assert Matrix(Fr_c).expand() == fr.expand()
assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand()
assert (simplify(Matrix(Fr_star_steady).expand()) == simplify(
frstar_steady.expand()))
syms_in_forcing = find_dynamicsymbols(kane.forcing)
for qdi in qd:
assert qdi not in syms_in_forcing
from sympy.physics.mechanics import Point, ReferenceFrame, RigidBody
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint
from util import inertia_coefficient_matrix, partial_velocities, subs
q1, q2, q3, q4, q5 = dynamicsymbols('q1:6')
q1d, q2d, q3d, q4d, q5d = dynamicsymbols('q1:6', level=1)
u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
R, e, f, theta = symbols('R e f theta')
a, b, mA, mB, IA = symbols('a b mA mB IA')
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
# reference frames
F = ReferenceFrame('F')
P = F.orientnew('P', 'axis', [-theta, F.y])
A = P.orientnew('A', 'axis', [q1, P.x])
# define frames for wheels
B = A.orientnew('B', 'axis', [q4, A.z])
C = A.orientnew('C', 'axis', [q5, A.z])
# define points
pO = Point('O')
pO.set_vel(F, 0)
pD = pO.locatenew('D', q2*P.y + q3*P.z)
pD.set_vel(A, 0)
pD.set_vel(F, pD.pos_from(pO).dt(F))
pS_star = pD.locatenew('S*', e*A.y)
pQ = pD.locatenew('Q', f*A.y - R*A.x)
for p in [pS_star, pQ]:
from sympy.core.backend import sin, cos, tan, pi, symbols, Matrix, zeros
from sympy.physics.mechanics import (Particle, Point, ReferenceFrame,
RigidBody, Vector)
from sympy.physics.mechanics import (angular_momentum, dynamicsymbols, inertia,
inertia_of_point_mass, kinetic_energy,
linear_momentum, outer, potential_energy,
msubs, find_dynamicsymbols)
from sympy.physics.mechanics.functions import gravity
from sympy.physics.vector.vector import Vector
from sympy.utilities.pytest import raises
Vector.simp = True
q1, q2, q3, q4, q5 = symbols('q1 q2 q3 q4 q5')
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = A.orientnew('B', 'Axis', [q2, A.x])
C = B.orientnew('C', 'Axis', [q3, B.y])
def test_inertia():
N = ReferenceFrame('N')
ixx, iyy, izz = symbols('ixx iyy izz')
ixy, iyz, izx = symbols('ixy iyz izx')
assert inertia(N, ixx, iyy, izz) == (ixx * (N.x | N.x) + iyy *
(N.y | N.y) + izz * (N.z | N.z))
assert inertia(N, 0, 0, 0) == 0 * (N.x | N.x)
assert inertia(
N, ixx, iyy, izz, ixy, iyz,
izx) == (ixx * (N.x | N.x) + ixy * (N.x | N.y) + izx * (N.x | N.z) +
ixy * (N.y | N.x) + iyy * (N.y | N.y) + iyz * (N.y | N.z) +
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 three
# speed variables are need to describe this system, along with the disc's
# mass and radius, and the local graivty (note that mass will drop out).
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)
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])
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)))
# 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 mass center.
# 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)
Dmc.a2pt_theory(C, N, R)
# This is a simple way to form the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
# Kinematic differential equations; how the generalized coordinate time
# derivatives relate to generalized speeds.
kd = [q1d - u3 / cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]
# Creation of the force list; it is the gravitational force at the mass
# center of the disc. Then we create the disc by assigning a Point to the
# mass center attribute, a ReferenceFrame to the frame attribute, and mass
# and inertia. Then we form the body list.
ForceList = [(Dmc, -m * g * Y.z)]
BodyD = RigidBody()
BodyD.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
BodyList = [BodyD]
# Finally we form the equations of motion, using the same steps we did
# before. Specify inertial frame, supply generalized speeds, supply
# kinematic differential equation dictionary, compute Fr from the force
# list and Fr* fromt the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3])
KM.kindiffeq(kd)
KM.kanes_equations(ForceList, BodyList)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
kdd = KM.kindiffdict()
rhs = rhs.subs(kdd)
assert rhs.expand() == Matrix([
(10 * u2 * u3 * r - 5 * u3**2 * r * tan(q2) + 4 * g * sin(q2)) /
(5 * r), -2 * u1 * u3 / 3, u1 * (-2 * u2 + u3 * tan(q2))
]).expand()
from __future__ import division
from sympy import Dummy
from sympy import expand, symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import dynamicsymbols
from util import msprint, subs, partial_velocities
from util import generalized_active_forces, potential_energy
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
# L' is the natural length of the springs
a, k, L_prime = symbols('a k L\'', real=True, positive=True)
# reference frames
X = ReferenceFrame('X')
C = X.orientnew('C', 'body', [q4, q5, q6], 'xyz')
# define points
pO = Point('O') # point O is fixed in X
pC_star = pO.locatenew('C*', a * (q1 * X.x + q2 * X.y + q3 * X.z))
# define points of the cube connected to springs
pC1 = pC_star.locatenew('C1', a * (C.x + C.y - C.z))
pC2 = pC_star.locatenew('C2', a * (C.y + C.z - C.x))
pC3 = pC_star.locatenew('C3', a * (C.z + C.x - C.y))
# define fixed spring points
pk1 = pO.locatenew('k1', L_prime * X.x + a * (X.x + X.y - X.z))
pk2 = pO.locatenew('k2', L_prime * X.y + a * (X.y + X.z - X.x))
pk3 = pO.locatenew('k3', L_prime * X.z + a * (X.z + X.x - X.y))
pC_star.set_vel(X, pC_star.pos_from(pO).dt(X))
pC1.v2pt_theory(pC_star, X, C)
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import inertia
from sympy.physics.mechanics import cross, dot, dynamicsymbols
from util import msprint, subs, partial_velocities
from util import generalized_active_forces, potential_energy
from util import generalized_active_forces_V
q1, q2, q3 = dynamicsymbols('q1:4')
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
m, M, G, R = symbols('m M G R')
I1, I2, I3 = symbols('I1:4')
# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')
# define points
pP = Point('P')
pP.set_vel(A, 0)
pB_star = pP.locatenew('B*', R * A.x)
pB_star.set_vel(B, 0)
pB_star.set_vel(A, pB_star.pos_from(pP).dt(A))
# kinematic differential equations
kde = [x - y for x, y in zip([u1, u2, u3], map(B.ang_vel_in(A).dot, B))]
kde_map = solve(kde, [q1d, q2d, q3d])
I = inertia(B, I1, I2, I3) # central inertia dyadic of B
from sympy import solve, symbols, pi
def msprint(expr):
pr = MechanicsStrPrinter()
return pr.doprint(expr)
# Define generalized coordinates, speeds, and constants:
qi = dynamicsymbols('q0 q1 q2 q3 q4 q5')
qid = dynamicsymbols('q0 q1 q2 q3 q4 q5', level=1)
ui = dynamicsymbols('u0 u1 u2 u3 u4 u5')
R = symbols('R')
A = ReferenceFrame('A')
A_1 = A.orientnew("A'", 'Axis', [qi[1], A.z])
B = A_1.orientnew('B', 'Axis', [pi/2 - qi[2], A_1.x])
C = B.orientnew('C', 'Axis', [qi[3], B.z])
pO = Point('O')
pCs = pO.locatenew('C*', qi[4] * A.x + qi[5] * A.y + R * B.y)
pO.set_vel(A, 0) # Point O is fixed in Reference Frame A
pCs.v2pt_theory(pO, A, B) # Point C* is fixed in Reference Frame B
# Set ui = qid
kinematic_eqs = []
for u, qd in zip(ui, qid):
kinematic_eqs.append(u - qd)
soln = solve(kinematic_eqs, qid)
print("kinematic equations:")
Rear = WheelAssemblyGyrostat('r')
Front = WheelAssemblyGyrostat('f')
### Parameters used in Meijaard formulation
w, c, l = symbols('w.w w.c w.lambda')
rR, rF, tR, tF = symbols('w.rR w.rF w.tR w.tF')
mR, mF, mH, mB = symbols('w.mR w.mF w.mH w.mB')
IRxx, IRyy = symbols('w.IRxx w.IRyy')
IBxx, IByy, IBzz, IBxz = symbols('w.IBxx w.IByy w.IBzz w.IBxz')
IHxx, IHyy, IHzz, IHxz = symbols('w.IHxx w.IHyy w.IHzz w.IHxz')
IFxx, IFyy = symbols('w.IFxx w.IFyy')
xB, zB, xH, zH = symbols('w.xB w.zB w.xH w.zH')
# Orient reference frames
N = ReferenceFrame('N') # Newtonian frame
R = N.orientnew('R', 'Axis', [l, N.y]) # Rear frame, z aligned with steer axis
# Location mass centers using Meijaard parameters
P = Point('P') # Rear contact
RO = P.locatenew('RO', -(rR + tR) * N.z) # Rear wheel center
BO = P.locatenew('BO', xB * N.x + zB * N.z) # Rear frame center
HO = P.locatenew('HO', xH * N.x + zH * N.z) # Front frame center
FO = P.locatenew('FO', w * N.x - (rF + tF) * N.z) # Front wheel center
Q = P.locatenew('Q', w * N.x)
# Mass centers of rear and front assembly relative to respective wheel centers
RO_BO_mc = RO.locatenew('RO_BO_mc', (mB * BO.pos_from(RO)) / (mR + mB))
FO_HO_mc = FO.locatenew('FO_HO_mc', (mH * HO.pos_from(FO)) / (mF + mH))
# Inertia dyads of bodies in Meijaard et al. 2007
IR_RO = inertia(R, IRxx, IRyy, IRxx)
s = symbols('s') # coefficient of viscous friction
# Mass and Inertia scalars
m, Ixx, Iyy, Izz, Ixy, Iyz, Ixz = symbols('m Ixx Iyy Izz Ixy Iyz Ixz')
q = dynamicsymbols('q:5') # Generalized coordinates
qd = [qi.diff(t) for qi in q] # Generalized coordinate time derivatives
u = dynamicsymbols('u:3') # Generalized speeds
ud = [ui.diff(t) for ui in u] # Generalized speeds time derivatives
ua = dynamicsymbols('ua:3') # Auxiliary generalized speeds
CF = dynamicsymbols('Rx Ry Rz') # Contact forces
r = dynamicsymbols('r:3') # Coordinates, in R frame, from O to P
rd = [ri.diff(t) for ri in r] # Time derivatives of xi
N = ReferenceFrame('N') # Inertial Reference Frame
Y = N.orientnew('Y', 'Axis', [q[0], N.z]) # Yaw Frame
L = Y.orientnew('L', 'Axis', [q[1], Y.x]) # Lean Frame
R = L.orientnew('R', 'Axis', [q[2], L.y]) # Rattleback body fixed frame
I = inertia(R, Ixx, Iyy, Izz, Ixy, Iyz, Ixz) # Inertia dyadic
# Angular velocity using u's as body fixed measure numbers of angular velocity
R.set_ang_vel(N, u[0]*R.x + u[1]*R.y + u[2]*R.z)
# Rattleback ground contact point
P = Point('P')
P.set_vel(N, ua[0]*Y.x + ua[1]*Y.y + ua[2]*Y.z)
# Rattleback ellipsoid center location, see:
# "Realistic mathematical modeling of the rattleback", Kane, Thomas R. and
# David A. Levinson, 1982, International Journal of Non-Linear Mechanics
symbol_values = {
R_SQ: 4.5e5,
R_QE: 1.5e11,
R_EM: 4.0e8,
R_Ee: 7.0e6,
R_Mm: 2.0e6,
omega_E: 2e-7,
omega_M: 24e-7,
omega_e: 12e-4,
omega_m: 10e-4
}
# reference frames
S = ReferenceFrame('S')
Q = S.orientnew('Q', 'axis', [0, S.x])
E = Q.orientnew('E', 'axis', [0, S.x])
M = E.orientnew('M', 'axis', [0, S.x])
frames = [S, Q, E, M]
pS = Point('S')
pS.set_acc(S, 0)
pQ = Point('Q')
pQ.set_acc(Q, 0)
pE = Point('E')
pE.set_acc(E, 0)
pM = Point('M')
pM.set_acc(M, 0)
pe = Point('e')
pm = Point('m')
points = [pS, pQ, pE, pM, pe, pm]
def test_linearize_rolling_disc_kane():
# Symbols for time and constant parameters
t, r, m, g, v = symbols('t r m g v')
# Configuration variables and their time derivatives
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
# Generalized speeds and their time derivatives
u = dynamicsymbols('u:6')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
# Reference frames
N = ReferenceFrame('N') # Inertial frame
NO = Point('NO') # Inertial origin
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
CO = NO.locatenew('CO', q4 * N.x + q5 * N.y + q6 * N.z) # Disc center
# Disc angular velocity in N expressed using time derivatives of coordinates
w_c_n_qd = C.ang_vel_in(N)
w_b_n_qd = B.ang_vel_in(N)
# Inertial angular velocity and angular acceleration of disc fixed frame
C.set_ang_vel(N, u1 * B.x + u2 * B.y + u3 * B.z)
# Disc center velocity in N expressed using time derivatives of coordinates
v_co_n_qd = CO.pos_from(NO).dt(N)
# Disc center velocity in N expressed using generalized speeds
CO.set_vel(N, u4 * C.x + u5 * C.y + u6 * C.z)
# Disc Ground Contact Point
P = CO.locatenew('P', r * B.z)
P.v2pt_theory(CO, N, C)
# Configuration constraint
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
# Velocity level constraints
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
# Set angular velocity of remaining frames
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(
N,
C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Active forces
F_CO = m * g * A.z
# Create inertia dyadic of disc C about point CO
I = (m * r**2) / 4
J = (m * r**2) / 2
I_C_CO = inertia(C, I, J, I)
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
BL = [Disc]
FL = [(CO, F_CO)]
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3],
kd_eqs=kindiffs,
q_dependent=[q6],
configuration_constraints=f_c,
u_dependent=[u4, u5, u6],
velocity_constraints=f_v)
(fr, fr_star) = KM.kanes_equations(BL, FL)
# Test generalized form equations
linearizer = KM.to_linearizer()
assert linearizer.f_c == f_c
assert linearizer.f_v == f_v
assert linearizer.f_a == f_v.diff(t).subs(KM.kindiffdict())
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
for qi in qdots.keys():
assert sol[qi] == qdots[qi]
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix(
[0, 0, 0])
# Perform the linearization
# Precomputed operating point
q_op = {q6: -r * cos(q2)}
u_op = {
u1: 0,
u2: sin(q2) * q1d + q3d,
u3: cos(q2) * q1d,
u4: -r * (sin(q2) * q1d + q3d) * cos(q3),
u5: 0,
u6: -r * (sin(q2) * q1d + q3d) * sin(q3)
}
qd_op = {
q2d: 0,
q4d: -r * (sin(q2) * q1d + q3d) * cos(q1),
q5d: -r * (sin(q2) * q1d + q3d) * sin(q1),
q6d: 0
}
ud_op = {
u1d:
4 * g * sin(q2) / (5 * r) + sin(2 * q2) * q1d**2 / 2 +
6 * cos(q2) * q1d * q3d / 5,
u2d:
0,
u3d:
0,
u4d:
r * (sin(q2) * sin(q3) * q1d * q3d + sin(q3) * q3d**2),
u5d:
r * (4 * g * sin(q2) /
(5 * r) + sin(2 * q2) * q1d**2 / 2 + 6 * cos(q2) * q1d * q3d / 5),
u6d:
-r * (sin(q2) * cos(q3) * q1d * q3d + cos(q3) * q3d**2)
}
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op],
A_and_B=True,
simplify=True)
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
# Precomputed solution
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[sin(q1) * q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
[-cos(q1) * q3d, 0, 0, 0, 0,
cos(q1), -sin(q1), 0],
[0, Rational(4, 5), 0, 0, 0, 0, 0, 6 * q3d / 5],
[0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, -2 * q3d, 0, 0]])
B_sol = Matrix([])
# Check that linearization is correct
assert A.subs(upright_nominal) == A_sol
assert B.subs(upright_nominal) == B_sol
# Check eigenvalues at critical speed are all zero:
assert sympify(A.subs(upright_nominal).subs(q3d,
1 / sqrt(3))).eigenvals() == {
0: 8
}
"""Exercise 10.2 from Kane 1985."""
from __future__ import division
from sympy import collect, expand, pi, solve, symbols, trigsimp
from sympy.physics.mechanics import ReferenceFrame, RigidBody, Point
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint
from util import subs
g, m, Px, Py, Pz, R = symbols('g m Px Py Pz R')
q = dynamicsymbols('q1:6')
qd = dynamicsymbols('q1:6', level=1)
u1, u2, u3, u4, u5 = u = dynamicsymbols('u1:6')
## --- Define ReferenceFrames ---
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 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).dt(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)
def test_bicycle():
# Code to get equations of motion for a bicycle modeled as in:
# J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized
# dynamics equations for the balance and steer of a bicycle: a benchmark
# and review. Proceedings of The Royal Society (2007) 463, 1955-1982
# doi: 10.1098/rspa.2007.1857
# Note that this code has been crudely ported from Autolev, which is the
# reason for some of the unusual naming conventions. It was purposefully as
# similar as possible in order to aide debugging.
# Declare Coordinates & Speeds
# Simple definitions for qdots - qd = u
# Speeds are: yaw frame ang. rate, roll frame ang. rate, rear wheel frame
# ang. rate (spinning motion), frame ang. rate (pitching motion), steering
# frame ang. rate, and front wheel ang. rate (spinning motion).
# Wheel positions are ignorable coordinates, so they are not introduced.
q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5')
q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)
# Declare System's Parameters
WFrad, WRrad, htangle, forkoffset = symbols(
'WFrad WRrad htangle forkoffset')
forklength, framelength, forkcg1 = symbols(
'forklength framelength forkcg1')
forkcg3, framecg1, framecg3, Iwr11 = symbols(
'forkcg3 framecg1 framecg3 Iwr11')
Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11')
Iframe22, Iframe33, Iframe31, Ifork11 = symbols(
'Iframe22 Iframe33 Iframe31 Ifork11')
Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')
# Set up reference frames for the system
# N - inertial
# Y - yaw
# R - roll
# WR - rear wheel, rotation angle is ignorable coordinate so not oriented
# Frame - bicycle frame
# TempFrame - statically rotated frame for easier reference inertia definition
# Fork - bicycle fork
# TempFork - statically rotated frame for easier reference inertia definition
# WF - front wheel, again posses a ignorable coordinate
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
R = Y.orientnew('R', 'Axis', [q2, Y.x])
Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y])
WR = ReferenceFrame('WR')
TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y])
Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x])
TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y])
WF = ReferenceFrame('WF')
# Kinematics of the Bicycle First block of code is forming the positions of
# the relevant points
# rear wheel contact -> rear wheel mass center -> frame mass center +
# frame/fork connection -> fork mass center + front wheel mass center ->
# front wheel contact point
WR_cont = Point('WR_cont')
WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z)
Steer = WR_mc.locatenew('Steer', framelength * Frame.z)
Frame_mc = WR_mc.locatenew('Frame_mc',
-framecg1 * Frame.x + framecg3 * Frame.z)
Fork_mc = Steer.locatenew('Fork_mc', -forkcg1 * Fork.x + forkcg3 * Fork.z)
WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z)
WF_cont = WF_mc.locatenew(
'WF_cont',
WFrad * (dot(Fork.y, Y.z) * Fork.y - Y.z).normalize())
# Set the angular velocity of each frame.
# Angular accelerations end up being calculated automatically by
# differentiating the angular velocities when first needed.
# u1 is yaw rate
# u2 is roll rate
# u3 is rear wheel rate
# u4 is frame pitch rate
# u5 is fork steer rate
# u6 is front wheel rate
Y.set_ang_vel(N, u1 * Y.z)
R.set_ang_vel(Y, u2 * R.x)
WR.set_ang_vel(Frame, u3 * Frame.y)
Frame.set_ang_vel(R, u4 * Frame.y)
Fork.set_ang_vel(Frame, u5 * Fork.x)
WF.set_ang_vel(Fork, u6 * Fork.y)
# Form the velocities of the previously defined points, using the 2 - point
# theorem (written out by hand here). Accelerations again are calculated
# automatically when first needed.
WR_cont.set_vel(N, 0)
WR_mc.v2pt_theory(WR_cont, N, WR)
Steer.v2pt_theory(WR_mc, N, Frame)
Frame_mc.v2pt_theory(WR_mc, N, Frame)
Fork_mc.v2pt_theory(Steer, N, Fork)
WF_mc.v2pt_theory(Steer, N, Fork)
WF_cont.v2pt_theory(WF_mc, N, WF)
# Sets the inertias of each body. Uses the inertia frame to construct the
# inertia dyadics. Wheel inertias are only defined by principle moments of
# inertia, and are in fact constant in the frame and fork reference frames;
# it is for this reason that the orientations of the wheels does not need
# to be defined. The frame and fork inertias are defined in the 'Temp'
# frames which are fixed to the appropriate body frames; this is to allow
# easier input of the reference values of the benchmark paper. Note that
# due to slightly different orientations, the products of inertia need to
# have their signs flipped; this is done later when entering the numerical
# value.
Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0,
Iframe31), Frame_mc)
Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0,
Ifork31), Fork_mc)
WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)
# Declaration of the RigidBody containers. ::
BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)
# The kinematic differential equations; they are defined quite simply. Each
# entry in this list is equal to zero.
kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
# The nonholonomic constraints are the velocity of the front wheel contact
# point dotted into the X, Y, and Z directions; the yaw frame is used as it
# is "closer" to the front wheel (1 less DCM connecting them). These
# constraints force the velocity of the front wheel contact point to be 0
# in the inertial frame; the X and Y direction constraints enforce a
# "no-slip" condition, and the Z direction constraint forces the front
# wheel contact point to not move away from the ground frame, essentially
# replicating the holonomic constraint which does not allow the frame pitch
# to change in an invalid fashion.
conlist_speed = [
WF_cont.vel(N) & Y.x,
WF_cont.vel(N) & Y.y,
WF_cont.vel(N) & Y.z
]
# The holonomic constraint is that the position from the rear wheel contact
# point to the front wheel contact point when dotted into the
# normal-to-ground plane direction must be zero; effectively that the front
# and rear wheel contact points are always touching the ground plane. This
# is actually not part of the dynamic equations, but instead is necessary
# for the lineraization process.
conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z]
# The force list; each body has the appropriate gravitational force applied
# at its mass center.
FL = [(Frame_mc, -mframe * g * Y.z), (Fork_mc, -mfork * g * Y.z),
(WF_mc, -mwf * g * Y.z), (WR_mc, -mwr * g * Y.z)]
BL = [BodyFrame, BodyFork, BodyWR, BodyWF]
# The N frame is the inertial frame, coordinates are supplied in the order
# of independent, dependent coordinates, as are the speeds. The kinematic
# differential equation are also entered here. Here the dependent speeds
# are specified, in the same order they were provided in earlier, along
# with the non-holonomic constraints. The dependent coordinate is also
# provided, with the holonomic constraint. Again, this is only provided
# for the linearization process.
KM = KanesMethod(N,
q_ind=[q1, q2, q5],
q_dependent=[q4],
configuration_constraints=conlist_coord,
u_ind=[u2, u3, u5],
u_dependent=[u1, u4, u6],
velocity_constraints=conlist_speed,
kd_eqs=kd)
(fr, frstar) = KM.kanes_equations(FL, BL)
# This is the start of entering in the numerical values from the benchmark
# paper to validate the eigen values of the linearized equations from this
# model to the reference eigen values. Look at the aforementioned paper for
# more information. Some of these are intermediate values, used to
# transform values from the paper into the coordinate systems used in this
# model.
PaperRadRear = 0.3
PaperRadFront = 0.35
HTA = evalf.N(pi / 2 - pi / 10)
TrailPaper = 0.08
rake = evalf.N(-(TrailPaper * sin(HTA) - (PaperRadFront * cos(HTA))))
PaperWb = 1.02
PaperFrameCgX = 0.3
PaperFrameCgZ = 0.9
PaperForkCgX = 0.9
PaperForkCgZ = 0.7
FrameLength = evalf.N(PaperWb * sin(HTA) -
(rake - (PaperRadFront - PaperRadRear) * cos(HTA)))
FrameCGNorm = evalf.N((PaperFrameCgZ - PaperRadRear -
(PaperFrameCgX / sin(HTA)) * cos(HTA)) * sin(HTA))
FrameCGPar = evalf.N(
(PaperFrameCgX / sin(HTA) +
(PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) *
cos(HTA)))
tempa = evalf.N((PaperForkCgZ - PaperRadFront))
tempb = evalf.N((PaperWb - PaperForkCgX))
tempc = evalf.N(sqrt(tempa**2 + tempb**2))
PaperForkL = evalf.N(
(PaperWb * cos(HTA) - (PaperRadFront - PaperRadRear) * sin(HTA)))
ForkCGNorm = evalf.N(rake +
(tempc * sin(pi / 2 - HTA - acos(tempa / tempc))))
ForkCGPar = evalf.N(tempc * cos((pi / 2 - HTA) - acos(tempa / tempc)) -
PaperForkL)
# Here is the final assembly of the numerical values. The symbol 'v' is the
# forward speed of the bicycle (a concept which only makes sense in the
# upright, static equilibrium case?). These are in a dictionary which will
# later be substituted in. Again the sign on the *product* of inertia
# values is flipped here, due to different orientations of coordinate
# systems.
v = symbols('v')
val_dict = {
WFrad: PaperRadFront,
WRrad: PaperRadRear,
htangle: HTA,
forkoffset: rake,
forklength: PaperForkL,
framelength: FrameLength,
forkcg1: ForkCGPar,
forkcg3: ForkCGNorm,
framecg1: FrameCGNorm,
framecg3: FrameCGPar,
Iwr11: 0.0603,
Iwr22: 0.12,
Iwf11: 0.1405,
Iwf22: 0.28,
Ifork11: 0.05892,
Ifork22: 0.06,
Ifork33: 0.00708,
Ifork31: 0.00756,
Iframe11: 9.2,
Iframe22: 11,
Iframe33: 2.8,
Iframe31: -2.4,
mfork: 4,
mframe: 85,
mwf: 3,
mwr: 2,
g: 9.81,
q1: 0,
q2: 0,
q4: 0,
q5: 0,
u1: 0,
u2: 0,
u3: v / PaperRadRear,
u4: 0,
u5: 0,
u6: v / PaperRadFront
}
# Linearizes the forcing vector; the equations are set up as MM udot =
# forcing, where MM is the mass matrix, udot is the vector representing the
# time derivatives of the generalized speeds, and forcing is a vector which
# contains both external forcing terms and internal forcing terms, such as
# centripital or coriolis forces. This actually returns a matrix with as
# many rows as *total* coordinates and speeds, but only as many columns as
# independent coordinates and speeds.
e = KM._fr[0]
#import symengine
#print "Converting to symengine..."
#f = symengine.sympify(e)
print "Saving to expr.txt"
s = str(e)
open("expr.txt", "w").write(s)
return
import IPython
IPython.embed()
forcing_lin = KM.linearize()[0]
# As mentioned above, the size of the linearized forcing terms is expanded
# to include both q's and u's, so the mass matrix must have this done as
# well. This will likely be changed to be part of the linearized process,
# for future reference.
MM_full = KM.mass_matrix_full
MM_full_s = MM_full.subs(val_dict)
forcing_lin_s = forcing_lin.subs(KM.kindiffdict()).subs(val_dict)
MM_full_s = MM_full_s.evalf()
forcing_lin_s = forcing_lin_s.evalf()
# Finally, we construct an "A" matrix for the form xdot = A x (x being the
# state vector, although in this case, the sizes are a little off). The
# following line extracts only the minimum entries required for eigenvalue
# analysis, which correspond to rows and columns for lean, steer, lean
# rate, and steer rate.
Amat = MM_full_s.inv() * forcing_lin_s
A = Amat.extract([1, 2, 4, 6], [1, 2, 3, 5])
# Precomputed for comparison
Res = Matrix([[0, 0, 1.0, 0], [0, 0, 0, 1.0],
[
9.48977444677355,
-0.891197738059089 * v**2 - 0.571523173729245,
-0.105522449805691 * v, -0.330515398992311 * v
],
[
11.7194768719633,
-1.97171508499972 * v**2 + 30.9087533932407,
3.67680523332152 * v, -3.08486552743311 * v
]])
# Actual eigenvalue comparison
for i in range(6):
# Pull out eigenvalues, put in a list
e_valsA = list(A.subs(v, i).eigenvals(rational=False).keys())
e_valsR = list(Res.subs(v, i).eigenvals(rational=False).keys())
# sort the list
e_valsA.sort()
e_valsR.sort()
# numerically compare eigenvalues
for i in range(4):
assert abs(e_valsA[i] - e_valsR[i]) < 1e-12
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()
def test_ang_vel():
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1)
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = A.orientnew('B', 'Axis', [q2, A.x])
C = B.orientnew('C', 'Axis', [q3, B.y])
D = N.orientnew('D', 'Axis', [q4, N.y])
u1, u2, u3 = dynamicsymbols('u1 u2 u3')
assert A.ang_vel_in(N) == (q1d) * A.z
assert B.ang_vel_in(N) == (q2d) * B.x + (q1d) * A.z
assert C.ang_vel_in(N) == (q3d) * C.y + (q2d) * B.x + (q1d) * A.z
A2 = N.orientnew('A2', 'Axis', [q4, N.y])
assert N.ang_vel_in(N) == 0
assert N.ang_vel_in(A) == -q1d * N.z
assert N.ang_vel_in(B) == -q1d * A.z - q2d * B.x
assert N.ang_vel_in(C) == -q1d * A.z - q2d * B.x - q3d * B.y
assert N.ang_vel_in(A2) == -q4d * N.y
assert A.ang_vel_in(N) == q1d * N.z
assert A.ang_vel_in(A) == 0
assert A.ang_vel_in(B) == -q2d * B.x
assert A.ang_vel_in(C) == -q2d * B.x - q3d * B.y
assert A.ang_vel_in(A2) == q1d * N.z - q4d * N.y
assert B.ang_vel_in(N) == q1d * A.z + q2d * A.x
assert B.ang_vel_in(A) == q2d * A.x
assert B.ang_vel_in(B) == 0
assert B.ang_vel_in(C) == -q3d * B.y
assert B.ang_vel_in(A2) == q1d * A.z + q2d * A.x - q4d * N.y
assert C.ang_vel_in(N) == q1d * A.z + q2d * A.x + q3d * B.y
assert C.ang_vel_in(A) == q2d * A.x + q3d * C.y
assert C.ang_vel_in(B) == q3d * B.y
assert C.ang_vel_in(C) == 0
assert C.ang_vel_in(A2) == q1d * A.z + q2d * A.x + q3d * B.y - q4d * N.y
assert A2.ang_vel_in(N) == q4d * A2.y
assert A2.ang_vel_in(A) == q4d * A2.y - q1d * N.z
assert A2.ang_vel_in(B) == q4d * N.y - q1d * A.z - q2d * A.x
assert A2.ang_vel_in(C) == q4d * N.y - q1d * A.z - q2d * A.x - q3d * B.y
assert A2.ang_vel_in(A2) == 0
C.set_ang_vel(N, u1 * C.x + u2 * C.y + u3 * C.z)
assert C.ang_vel_in(N) == (u1) * C.x + (u2) * C.y + (u3) * C.z
assert N.ang_vel_in(C) == (-u1) * C.x + (-u2) * C.y + (-u3) * C.z
assert C.ang_vel_in(
D) == (u1) * C.x + (u2) * C.y + (u3) * C.z + (-q4d) * D.y
assert D.ang_vel_in(
C) == (-u1) * C.x + (-u2) * C.y + (-u3) * C.z + (q4d) * D.y
q0 = dynamicsymbols('q0')
q0d = dynamicsymbols('q0', 1)
E = N.orientnew('E', 'Quaternion', (q0, q1, q2, q3))
assert E.ang_vel_in(N) == (
2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) * E.x + 2 *
(q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) * E.y + 2 *
(q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) * E.z)
F = N.orientnew('F', 'Body', (q1, q2, q3), '313')
assert F.ang_vel_in(N) == (
(sin(q2) * sin(q3) * q1d + cos(q3) * q2d) * F.x +
(sin(q2) * cos(q3) * q1d - sin(q3) * q2d) * F.y +
(cos(q2) * q1d + q3d) * F.z)
G = N.orientnew('G', 'Axis', (q1, N.x + N.y))
assert G.ang_vel_in(N) == q1d * (N.x + N.y).normalize()
assert N.ang_vel_in(G) == -q1d * (N.x + N.y).normalize()
dyn_implicit_rhs = Matrix([0, 0, u**2 + v**2 - g * y])
comb_implicit_mat = Matrix([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0],
[0, 0, 1, 0, -x / m], [0, 0, 0, 1, -y / m],
[0, 0, 0, 0, l**2 / m]])
comb_implicit_rhs = Matrix([u, v, 0, 0, u**2 + v**2 - g * y])
kin_explicit_rhs = Matrix([u, v])
comb_explicit_rhs = comb_implicit_mat.LUsolve(comb_implicit_rhs)
# Set up a body and load to pass into the system
theta = atan(x / y)
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [theta, N.z])
O = Point('O')
P = O.locatenew('P', l * A.x)
Pa = Particle('Pa', P, m)
bodies = [Pa]
loads = [(P, g * m * N.x)]
# Set up some output equations to be given to SymbolicSystem
# Change to make these fit the pendulum
PE = symbols("PE")
out_eqns = {PE: m * g * (l + y)}
# Set up remaining arguments that can be passed to SymbolicSystem
alg_con = [2]
# Define first derivatives of generalized coordinates
theta0dot, theta1dot, theta2dot, theta3dot = dynamicsymbols('theta_0 theta_1 theta_2 theta_3', 1)
# Define symbols for other parameters
# Link lengths
l0, l1, l2, l3, l4 = sympy.symbols('l_0 l_1 l_2 l_3 l_4', real=True, nonzero=True)
# Time and gravity
t, g = sympy.symbols('t g', real=True, nonzero=True)
# ----- Define reference frames -----
N = ReferenceFrame('N')
# Define body fixed frame that rotates with Theta 0
XYZ1 = N.orientnew('XYZ1', 'Axis', [theta0, N.z])
# Define new frame to align new z-axis along the link l1
XYZ2 = XYZ1.orientnew('XYZ2', 'Axis', [-theta1, XYZ1.x])
# Define new frame to align new z-axis along the link l2
XYZ3 = XYZ2.orientnew('XYZ3', 'Axis', [-theta2, XYZ2.x])
# Define new frame to align new z-axis along the link l3
XYZ4 = XYZ3.orientnew('XYZ4', 'Axis', [-theta3, XYZ3.x])
# ----- Define points of interest and their velocities -----
# Define Origin and its velocity
O = Point('O')
O.set_vel(N, 0 * N.x)
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()))
# -*- coding: utf-8 -*-
"""Exercise 6.7 from Kane 1985."""
from __future__ import division
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import inertia, inertia_of_point_mass
from sympy.physics.mechanics import cross, dot
from sympy import solve, sqrt, symbols, integrate, diff
from sympy import sin, cos, tan, pi, S, Matrix
from sympy import simplify, Abs
b, m, k_a = symbols('b m k_a', real=True, nonnegative=True)
theta = symbols('theta', real=True)
N = ReferenceFrame('N')
N2 = N.orientnew('N2', 'Axis', [theta, N.z])
pO = Point('O')
pP1s = pO.locatenew('P1*', b / 2 * (N.x + N.y))
pP2s = pO.locatenew('P2*', b / 2 * (2 * N.x + N.y + N.z))
pP3s = pO.locatenew(
'P3*', b / 2 * ((2 + sin(theta)) * N.x + (2 - cos(theta)) * N.y + N.z))
I_1s_O = inertia_of_point_mass(m, pP1s.pos_from(pO), N)
I_2s_O = inertia_of_point_mass(m, pP2s.pos_from(pO), N)
I_3s_O = inertia_of_point_mass(m, pP3s.pos_from(pO), N)
print("\nI_1s_rel_O = {0}".format(I_1s_O))
print("\nI_2s_rel_O = {0}".format(I_2s_O))
print("\nI_3s_rel_O = {0}".format(I_3s_O))
I_1_1s = inertia(N, m * b**2 / 12, m * b**2 / 12, 2 * m * b**2 / 12)
I_2_2s = inertia(N, 2 * m * b**2 / 12, m * b**2 / 12, m * b**2 / 12)
from sympy import sin, cos, pi, expand, simplify, solve, symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import dynamicsymbols
from util import msprint, subs, partial_velocities, potential_energy
from util import generalized_active_forces, generalized_active_forces_V
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', level=1)
u1, u2 = dynamicsymbols('u1 u2')
b, g, m, L, t = symbols('b g m L t')
E, I = symbols('E I')
# reference frames, points, velocities
N = ReferenceFrame('N')
B = N.orientnew('B', 'Axis', [-(q2 - q1)/(2*b), N.y]) # small angle approx.
pO = Point('O') # Point O is where B* would be with zero displacement.
pO.set_vel(N, 0)
# small angle approx.
pB_star = pO.locatenew('B*', -(q1 + q2)/2 * N.x)
pP1 = pO.locatenew('P1', -q1*N.x - b*N.z)
pP2 = pO.locatenew('P2', -q2*N.x + b*N.z)
for p in [pB_star, pP1, pP2]:
p.set_vel(N, p.pos_from(pO).diff(t, N))
# kinematic differential equations
kde = [u1 - q1d, u2 - q2d]
kde_map = solve(kde, [q1d, q2d])
def test_Vector_diffs():
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1)
q1dd, q2dd, q3dd, q4dd = dynamicsymbols('q1 q2 q3 q4', 2)
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q3, N.z])
B = A.orientnew('B', 'Axis', [q2, A.x])
v1 = q2 * A.x + q3 * N.y
v2 = q3 * B.x + v1
v3 = v1.dt(B)
v4 = v2.dt(B)
assert v1.dt(N) == q2d * A.x + q2 * q3d * A.y + q3d * N.y
assert v1.dt(A) == q2d * A.x + q3 * q3d * N.x + q3d * N.y
assert v1.dt(B) == (q2d * A.x + q3 * q3d * N.x + q3d * N.y -
q3 * cos(q3) * q2d * N.z)
assert v2.dt(N) == (q2d * A.x + (q2 + q3) * q3d * A.y + q3d * B.x +
q3d * N.y)
assert v2.dt(A) == q2d * A.x + q3d * B.x + q3 * q3d * N.x + q3d * N.y
assert v2.dt(B) == (q2d * A.x + q3d * B.x + q3 * q3d * N.x + q3d * N.y -
q3 * cos(q3) * q2d * N.z)
assert v3.dt(N) == (q2dd * A.x + q2d * q3d * A.y +
(q3d**2 + q3 * q3dd) * N.x + q3dd * N.y +
(q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d -
q3 * cos(q3) * q2dd) * N.z)
assert v3.dt(A) == (q2dd * A.x + (2 * q3d**2 + q3 * q3dd) * N.x +
(q3dd - q3 * q3d**2) * N.y +
(q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d -
q3 * cos(q3) * q2dd) * N.z)
assert v3.dt(B) == (q2dd * A.x - q3 * cos(q3) * q2d**2 * A.y +
(2 * q3d**2 + q3 * q3dd) * N.x +
(q3dd - q3 * q3d**2) * N.y +
(2 * q3 * sin(q3) * q2d * q3d -
2 * cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z)
assert v4.dt(N) == (q2dd * A.x + q3d * (q2d + q3d) * A.y + q3dd * B.x +
(q3d**2 + q3 * q3dd) * N.x + q3dd * N.y +
(q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d -
q3 * cos(q3) * q2dd) * N.z)
assert v4.dt(A) == (q2dd * A.x + q3dd * B.x +
(2 * q3d**2 + q3 * q3dd) * N.x +
(q3dd - q3 * q3d**2) * N.y +
(q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d -
q3 * cos(q3) * q2dd) * N.z)
assert v4.dt(B) == (q2dd * A.x - q3 * cos(q3) * q2d**2 * A.y + q3dd * B.x +
(2 * q3d**2 + q3 * q3dd) * N.x +
(q3dd - q3 * q3d**2) * N.y +
(2 * q3 * sin(q3) * q2d * q3d -
2 * cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z)
assert v3.diff(q1d, N) == 0
assert v3.diff(q2d, N) == A.x - q3 * cos(q3) * N.z
assert v3.diff(q3d, N) == q3 * N.x + N.y
assert v3.diff(q1d, A) == 0
assert v3.diff(q2d, A) == A.x - q3 * cos(q3) * N.z
assert v3.diff(q3d, A) == q3 * N.x + N.y
assert v3.diff(q1d, B) == 0
assert v3.diff(q2d, B) == A.x - q3 * cos(q3) * N.z
assert v3.diff(q3d, B) == q3 * N.x + N.y
assert v4.diff(q1d, N) == 0
assert v4.diff(q2d, N) == A.x - q3 * cos(q3) * N.z
assert v4.diff(q3d, N) == B.x + q3 * N.x + N.y
assert v4.diff(q1d, A) == 0
assert v4.diff(q2d, A) == A.x - q3 * cos(q3) * N.z
assert v4.diff(q3d, A) == B.x + q3 * N.x + N.y
assert v4.diff(q1d, B) == 0
assert v4.diff(q2d, B) == A.x - q3 * cos(q3) * N.z
assert v4.diff(q3d, B) == B.x + q3 * N.x + N.y
def test_linearize_pendulum_kane_nonminimal():
# Create generalized coordinates and speeds for this non-minimal realization
# q1, q2 = N.x and N.y coordinates of pendulum
# u1, u2 = N.x and N.y velocities of pendulum
q1, q2 = dynamicsymbols('q1:3')
q1d, q2d = dynamicsymbols('q1:3', level=1)
u1, u2 = dynamicsymbols('u1:3')
u1d, u2d = dynamicsymbols('u1: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])
# Locate the pendulum mass
P = pN.locatenew('P1', q1*N.x + q2*N.y)
pP = Particle('pP', P, m)
# Calculate the kinematic differential equations
kde = Matrix([q1d - u1,
q2d - u2])
dq_dict = solve(kde, [q1d, q2d])
# Set velocity of point P
P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
# Configuration constraint is length of pendulum
f_c = Matrix([P.pos_from(pN).magnitude() - L])
# Velocity constraint is that the velocity in the A.x direction is
# always zero (the pendulum is never getting longer).
f_v = Matrix([P.vel(N).express(A).dot(A.x)])
f_v.simplify()
# Acceleration constraints is the time derivative of the velocity constraint
f_a = f_v.diff(t)
f_a.simplify()
# Input the force resultant at P
R = m*g*N.x
# Derive the equations of motion using the KanesMethod class.
KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1],
u_dependent=[u1], configuration_constraints=f_c,
velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde)
(fr, frstar) = KM.kanes_equations([(P, R)], [pP])
# Set the operating point to be straight down, and non-moving
q_op = {q1: L, q2: 0}
u_op = {u1: 0, u2: 0}
ud_op = {u1d: 0, u2d: 0}
A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
new_method=True, simplify=True)
assert A == Matrix([[0, 1], [-9.8/L, 0]])
assert B == Matrix([])
def test_linearize_rolling_disc_kane():
# Symbols for time and constant parameters
t, r, m, g, v = symbols('t r m g v')
# Configuration variables and their time derivatives
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
# Generalized speeds and their time derivatives
u = dynamicsymbols('u:6')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
# Reference frames
N = ReferenceFrame('N') # Inertial frame
NO = Point('NO') # Inertial origin
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center
# Disc angular velocity in N expressed using time derivatives of coordinates
w_c_n_qd = C.ang_vel_in(N)
w_b_n_qd = B.ang_vel_in(N)
# Inertial angular velocity and angular acceleration of disc fixed frame
C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z)
# Disc center velocity in N expressed using time derivatives of coordinates
v_co_n_qd = CO.pos_from(NO).dt(N)
# Disc center velocity in N expressed using generalized speeds
CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z)
# Disc Ground Contact Point
P = CO.locatenew('P', r*B.z)
P.v2pt_theory(CO, N, C)
# Configuration constraint
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
# Velocity level constraints
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
# Set angular velocity of remaining frames
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Active forces
F_CO = m*g*A.z
# Create inertia dyadic of disc C about point CO
I = (m * r**2) / 4
J = (m * r**2) / 2
I_C_CO = inertia(C, I, J, I)
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
BL = [Disc]
FL = [(CO, F_CO)]
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs,
q_dependent=[q6], configuration_constraints=f_c,
u_dependent=[u4, u5, u6], velocity_constraints=f_v)
(fr, fr_star) = KM.kanes_equations(FL, BL)
# Test generalized form equations
linearizer = KM.to_linearizer()
assert linearizer.f_c == f_c
assert linearizer.f_v == f_v
assert linearizer.f_a == f_v.diff(t)
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
for qi in qd:
assert sol[qi] == qdots[qi]
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0])
# Perform the linearization
# Precomputed operating point
q_op = {q6: -r*cos(q2)}
u_op = {u1: 0,
u2: sin(q2)*q1d + q3d,
u3: cos(q2)*q1d,
u4: -r*(sin(q2)*q1d + q3d)*cos(q3),
u5: 0,
u6: -r*(sin(q2)*q1d + q3d)*sin(q3)}
qd_op = {q2d: 0,
q4d: -r*(sin(q2)*q1d + q3d)*cos(q1),
q5d: -r*(sin(q2)*q1d + q3d)*sin(q1),
q6d: 0}
ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5,
u2d: 0,
u3d: 0,
u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2),
u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5),
u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)}
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True)
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
# Precomputed solution
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
[-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0],
[0, 4/5, 0, 0, 0, 0, 0, 6*q3d/5],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, -2*q3d, 0, 0]])
B_sol = Matrix([])
# Check that linearization is correct
assert A.subs(upright_nominal) == A_sol
assert B.subs(upright_nominal) == B_sol
# Check eigenvalues at critical speed are all zero:
assert A.subs(upright_nominal).subs(q3d, 1/sqrt(3)).eigenvals() == {0: 8}
from sympy.physics.mechanics import LagrangesMethod, Lagrangian
from sympy.physics.mechanics import ReferenceFrame, Particle, Point
from sympy.physics.mechanics import dynamicsymbols
# System state variables
theta = dynamicsymbols('theta')
thetad = dynamicsymbols('theta', 1)
# Other system variables
m, l, g = symbols('m l g')
# Set up the reference frames
# Reference frame A set up in the plane perpendicular to the page containing
# segment OP
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [theta, N.z])
# Set up the points and particles
O = Point('O')
P = O.locatenew('P', l * A.x)
Pa = Particle('Pa', P, m)
# Set up velocities
A.set_ang_vel(N, thetad * N.z)
O.set_vel(N, 0)
P.v2pt_theory(O, N, A)
# Set up the lagrangian
L = Lagrangian(N, Pa)
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 three
# speed variables are need to describe this system, along with the disc's
# mass and radius, and the local gravity (note that mass will drop out).
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)
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])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
# 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)
# This is a simple way to form the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
# Kinematic differential equations; how the generalized coordinate time
# derivatives relate to generalized speeds.
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
# Creation of the force list; it is the gravitational force at the mass
# center of the disc. Then we create the disc by assigning a Point to the
# center of mass attribute, a ReferenceFrame to the frame attribute, and mass
# and inertia. Then we form the body list.
ForceList = [(Dmc, - m * g * Y.z)]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
# Finally we form the equations of motion, using the same steps we did
# before. Specify inertial frame, supply generalized speeds, supply
# kinematic differential equation dictionary, compute Fr from the force
# list and Fr* from the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
KM.kanes_equations(ForceList, BodyList)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
kdd = KM.kindiffdict()
rhs = rhs.subs(kdd)
rhs.simplify()
assert rhs.expand() == Matrix([(6*u2*u3*r - u3**2*r*tan(q2) +
4*g*sin(q2))/(5*r), -2*u1*u3/3, u1*(-2*u2 + u3*tan(q2))]).expand()
# This code tests our output vs. benchmark values. When r=g=m=1, the
# critical speed (where all eigenvalues of the linearized equations are 0)
# is 1 / sqrt(3) for the upright case.
A = KM.linearize(A_and_B=True, new_method=True)[0]
A_upright = A.subs({r: 1, g: 1, m: 1}).subs({q1: 0, q2: 0, q3: 0, u1: 0, u3: 0})
assert A_upright.subs(u2, 1 / sqrt(3)).eigenvals() == {S(0): 6}
s = symbols('s') # coefficient of viscous friction
# Mass and Inertia scalars
m, Ixx, Iyy, Izz, Ixy, Iyz, Ixz = symbols('m Ixx Iyy Izz Ixy Iyz Ixz')
q = dynamicsymbols('q:5') # Generalized coordinates
qd = [qi.diff(t) for qi in q] # Generalized coordinate time derivatives
u = dynamicsymbols('u:3') # Generalized speeds
ud = [ui.diff(t) for ui in u] # Generalized speeds time derivatives
ua = dynamicsymbols('ua:3') # Auxiliary generalized speeds
CF = dynamicsymbols('Rx Ry Rz') # Contact forces
r = dynamicsymbols('r:3') # Coordinates, in R frame, from O to P
rd = [ri.diff(t) for ri in r] # Time derivatives of xi
N = ReferenceFrame('N') # Inertial Reference Frame
Y = N.orientnew('Y', 'Axis', [q[0], N.z]) # Yaw Frame
L = Y.orientnew('L', 'Axis', [q[1], Y.x]) # Lean Frame
R = L.orientnew('R', 'Axis', [q[2], L.y]) # Rattleback body fixed frame
I = inertia(R, Ixx, Iyy, Izz, Ixy, Iyz, Ixz) # Inertia dyadic
# Angular velocity using u's as body fixed measure numbers of angular velocity
R.set_ang_vel(N, u[0]*R.x + u[1]*R.y + u[2]*R.z)
# Rattleback ground contact point
P = Point('P')
P.set_vel(N, ua[0]*Y.x + ua[1]*Y.y + ua[2]*Y.z)
# Rattleback paraboloid -- parameterize coordinates of contact point by the
# roll and pitch angles, and the geometry
# TODO: FIXME!!!
def test_aux_dep():
# This test is about rolling disc dynamics, comparing the results found
# with KanesMethod to those found when deriving the equations "manually"
# with SymPy.
# The terms Fr, Fr*, and Fr*_steady are all compared between the two
# methods. Here, Fr*_steady refers to the generalized inertia forces for an
# equilibrium configuration.
# Note: comparing to the test of test_rolling_disc() in test_kane.py, this
# test also tests auxiliary speeds and configuration and motion constraints
#, seen in the generalized dependent coordinates q[3], and depend speeds
# u[3], u[4] and u[5].
# First, mannual derivation of Fr, Fr_star, Fr_star_steady.
# Symbols for time and constant parameters.
# Symbols for contact forces: Fx, Fy, Fz.
t, r, m, g, I, J = symbols('t r m g I J')
Fx, Fy, Fz = symbols('Fx Fy Fz')
# Configuration variables and their time derivatives:
# q[0] -- yaw
# q[1] -- lean
# q[2] -- spin
# q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in
# A.z direction
# Generalized speeds and their time derivatives:
# u[0] -- disc angular velocity component, disc fixed x direction
# u[1] -- disc angular velocity component, disc fixed y direction
# u[2] -- disc angular velocity component, disc fixed z direction
# u[3] -- disc velocity component, A.x direction
# u[4] -- disc velocity component, A.y direction
# u[5] -- disc velocity component, A.z direction
# Auxiliary generalized speeds:
# ua[0] -- contact point auxiliary generalized speed, A.x direction
# ua[1] -- contact point auxiliary generalized speed, A.y direction
# ua[2] -- contact point auxiliary generalized speed, A.z direction
q = dynamicsymbols('q:4')
qd = [qi.diff(t) for qi in q]
u = dynamicsymbols('u:6')
ud = [ui.diff(t) for ui in u]
#ud_zero = {udi : 0 for udi in ud}
ud_zero = dict(zip(ud, [0.]*len(ud)))
ua = dynamicsymbols('ua:3')
#ua_zero = {uai : 0 for uai in ua}
ua_zero = dict(zip(ua, [0.]*len(ua)))
# Reference frames:
# Yaw intermediate frame: A.
# Lean intermediate frame: B.
# Disc fixed frame: C.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q[0], N.z])
B = A.orientnew('B', 'Axis', [q[1], A.x])
C = B.orientnew('C', 'Axis', [q[2], B.y])
# Angular velocity and angular acceleration of disc fixed frame
# u[0], u[1] and u[2] are generalized independent speeds.
C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z)
C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B)
+ cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Velocity and acceleration of points:
# Disc-ground contact point: P.
# Center of disc: O, defined from point P with depend coordinate: q[3]
# u[3], u[4] and u[5] are generalized dependent speeds.
P = Point('P')
P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z)
O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y)
O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z)
O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N)))
# Kinematic differential equations:
# Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates
# directions of B, for qd0, qd1 and qd2.
# the other is v_o_n_qd = O.vel(N) in A.z direction for qd3.
# Then, solve for dq/dt's in terms of u's: qd_kd.
w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y
v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P))
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_o_n_qd - O.vel(N), A.z)])
qd_kd = solve(kindiffs, qd)
# Values of generalized speeds during a steady turn for later substitution
# into the Fr_star_steady.
steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u)
steady_conditions.update({qd[1] : 0, qd[3] : 0})
# Partial angular velocities and velocities.
partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua]
partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua]
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
# Configuration constraint: f_c, the projection of radius r in A.z direction
# is q[3].
# Velocity constraints: f_v, for u3, u4 and u5.
# Acceleration constraints: f_a.
f_c = Matrix([dot(-r*B.z, A.z) - q[3]])
f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N),
O.pos_from(P))), ai).expand() for ai in A])
v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A])
# Solve for constraint equations in the form of
# u_dependent = A_rs * [u_i; u_aux].
# First, obtain constraint coefficient matrix: M_v * [u; ua] = 0;
# Second, taking u[0], u[1], u[2] as independent,
# taking u[3], u[4], u[5] as dependent,
# rearranging the matrix of M_v to be A_rs for u_dependent.
# Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
M_v = zeros(3, 9)
for i in range(3):
for j, ui in enumerate(u + ua):
M_v[i, j] = f_v[i].diff(ui)
M_v_i = M_v[:, :3]
M_v_d = M_v[:, 3:6]
M_v_aux = M_v[:, 6:]
M_v_i_aux = M_v_i.row_join(M_v_aux)
A_rs = - M_v_d.inv() * M_v_i_aux
u_dep = A_rs[:, :3] * Matrix(u[:3])
u_dep_dict = dict(zip(u[3:], u_dep))
#u_dep_dict = {udi : u_depi[0] for udi, u_depi in zip(u[3:], u_dep.tolist())}
# Active forces: F_O acting on point O; F_P acting on point P.
# Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
F_O = m*g*A.z
F_P = Fx * A.x + Fy * A.y + Fz * A.z
Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in
zip(partial_v_O, partial_v_P)])
# Inertia force: R_star_O.
# Inertia of disc: I_C_O, where J is a inertia component about principal axis.
# Inertia torque: T_star_C.
# Generalized inertia forces (unconstrained): Fr_star_u.
R_star_O = -m*O.acc(N)
I_C_O = inertia(B, I, J, I)
T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
+ cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in
zip(partial_v_O, partial_w_C)])
# Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
# Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
+ A_rs.T * Fr_star_u[3:6, :]
Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
.subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()
# Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.
# Rigid Bodies: disc, with inertia I_C_O.
iner_tuple = (I_C_O, O)
disc = RigidBody('disc', O, C, m, iner_tuple)
bodyList = [disc]
# Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
F_o = (O, F_O)
F_p = (P, F_P)
forceList = [F_o, F_p]
# KanesMethod.
kane = KanesMethod(
N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs,
q_dependent=q[3:], configuration_constraints = f_c,
u_dependent=u[3:], velocity_constraints= f_v,
u_auxiliary=ua
)
# fr, frstar, frstar_steady and kdd(kinematic differential equations).
(fr, frstar)= kane.kanes_equations(forceList, bodyList)
frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
.subs({q[3]: -r*cos(q[1])}).expand()
kdd = kane.kindiffdict()
# Test
# First try Fr_c == fr;
# Second try Fr_star_c == frstar;
# Third try Fr_star_steady == frstar_steady.
# Both signs are checked in case the equations were found with an inverse
# sign.
assert ((Matrix(Fr_c).expand() == fr.expand()) or
(Matrix(Fr_c).expand() == (-fr).expand()))
assert ((Matrix(Fr_star_c).expand() == frstar.expand()) or
(Matrix(Fr_star_c).expand() == (-frstar).expand()))
assert ((Matrix(Fr_star_steady).expand() == frstar_steady.expand()) or
(Matrix(Fr_star_steady).expand() == (-frstar_steady).expand()))
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_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_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],
]
