def test_pend():
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, l, g = symbols('m l g')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y)
kd = [qd - u]
FL = [(P, m * g * N.x)]
pa = Particle()
pa.mass = m
pa.point = P
BL = [pa]
KM = Kane(N)
KM.coords([q])
KM.speeds([u])
KM.kindiffeq(kd)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
rhs.simplify()
assert expand(rhs[0]) == expand(-g / l * sin(q))
def test_linearize_pendulum_lagrange_minimal():
q1 = dynamicsymbols('q1') # angle of pendulum
q1d = dynamicsymbols('q1', 1) # Angular velocity
L, m, t = symbols('L, m, t')
g = 9.8
# Compose world frame
N = ReferenceFrame('N')
pN = Point('N*')
pN.set_vel(N, 0)
# A.x is along the pendulum
A = N.orientnew('A', 'axis', [q1, N.z])
A.set_ang_vel(N, q1d*N.z)
# Locate point P relative to the origin N*
P = pN.locatenew('P', L*A.x)
P.v2pt_theory(pN, N, A)
pP = Particle('pP', P, m)
# Solve for eom with Lagranges method
Lag = Lagrangian(N, pP)
LM = LagrangesMethod(Lag, [q1], forcelist=[(P, m*g*N.x)], frame=N)
LM.form_lagranges_equations()
# Linearize
A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=True)
assert A == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
assert B == Matrix([])
def test_one_dof():
# This is for a 1 dof spring-mass-damper case.
# It is described in more detail in the KanesMethod docstring.
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, c, k = symbols('m c k')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, u * N.x)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
# The old input format raises a deprecation warning, so catch it here so
# it doesn't cause py.test to fail.
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
assert simplify(KM.rhs() -
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1)
assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1], [-k/m, -c/m]]))
def test_one_dof():
# This is for a 1 dof spring-mass-damper case.
# It is described in more detail in the Kane docstring.
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, c, k = symbols('m c k')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, u * N.x)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle()
pa.mass = m
pa.point = P
BL = [pa]
KM = Kane(N)
KM.coords([q])
KM.speeds([u])
KM.kindiffeq(kd)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
assert KM.linearize() == (Matrix([[0, 1], [k, c]]), Matrix([]))
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in Kane. The inertia of the pendulum is
# defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols('g')
k, ls = symbols('k ls')
a, mA, mC = symbols('a mA mC')
F = dynamicsymbols('F')
Ix, Iy, Iz = symbols('Ix Iy Iz')
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
# Creating reference frames
N = ReferenceFrame('N')
A = ReferenceFrame('A')
A.orient(N, 'Axis', [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point('O')
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew('C', q1 * N.x)
Ao = C.locatenew('Ao', a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle('Cart', C, mC)
Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F)]
km=Kane(N)
km.coords([q1, q2])
km.speeds([u1, u2])
km.kindiffeq(kindiffs)
(fr,frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == -Iz
def test_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of Kane. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 1)
r, m, g = symbols('r m g')
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^
R.ang_vel_in(N)))
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [q1d - u3/cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]
ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody()
BodyD.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
BodyList = [BodyD]
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3, u4, u5])
KM.kindiffeq(kd)
kdd = KM.kindiffdict()
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
KM2 = Kane(N)
KM2.coords([q1, q2, q3])
KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5])
KM2.kindiffeq(kd)
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
assert fr.expand() == fr2.expand()
assert frstar.expand() == frstar2.expand()
def __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()
self.shape2 = Cylinder()
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_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_rigidbody():
m, m2, v1, v2, v3, omega = symbols('m m2 v1 v2 v3 omega')
A = ReferenceFrame('A')
A2 = ReferenceFrame('A2')
P = Point('P')
P2 = Point('P2')
I = Dyadic([])
I2 = Dyadic([])
B = RigidBody('B', P, A, m, (I, P))
assert B.mass == m
assert B.frame == A
assert B.masscenter == P
assert B.inertia == (I, B.masscenter)
B.mass = m2
B.frame = A2
B.masscenter = P2
B.inertia = (I2, B.masscenter)
assert B.mass == m2
assert B.frame == A2
assert B.masscenter == P2
assert B.inertia == (I2, B.masscenter)
assert B.masscenter == P2
assert B.inertia == (I2, B.masscenter)
# Testing linear momentum function assuming A2 is the inertial frame
N = ReferenceFrame('N')
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
assert B.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
def test_linear_momentum():
N = ReferenceFrame('N')
Ac = Point('Ac')
Ac.set_vel(N, 25 * N.y)
I = outer(N.x, N.x)
A = RigidBody('A', Ac, N, 20, (I, Ac))
P = Point('P')
Pa = Particle('Pa', P, 1)
Pa.point.set_vel(N, 10 * N.x)
assert linear_momentum(N, A, Pa) == 10 * N.x + 500 * N.y
def test_linear_momentum():
N = ReferenceFrame("N")
Ac = Point("Ac")
Ac.set_vel(N, 25 * N.y)
I = outer(N.x, N.x)
A = RigidBody("A", Ac, N, 20, (I, Ac))
P = Point("P")
Pa = Particle("Pa", P, 1)
Pa.point.set_vel(N, 10 * N.x)
assert linear_momentum(N, A, Pa) == 10 * N.x + 500 * N.y
def test_rolling_disc():
# Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and 3
# speed variables are need to describe this system, along with the
# disc's mass and radius, and the local gravity.
q1, q2, q3 = dynamicsymbols('q1 q2 q3')
q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1)
r, m, g = symbols('r m g')
# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc's center of mass.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
# Forming the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
# Finally we form the equations of motion, using the same steps we did
# before. Supply the Lagrangian, the generalized speeds.
BodyD.set_potential_energy(- m * g * r * cos(q2))
Lag = Lagrangian(N, BodyD)
q = [q1, q2, q3]
q1 = Function('q1')
q2 = Function('q2')
q3 = Function('q3')
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
RHS = l.rhs()
RHS.simplify()
t = symbols('t')
assert (l.mass_matrix[3:6] == [0, 5*m*r**2/4, 0])
assert RHS[4].simplify() == (-8*g*sin(q2(t)) + 5*r*sin(2*q2(t)
)*Derivative(q1(t), t)**2 + 12*r*cos(q2(t))*Derivative(q1(t), t
)*Derivative(q3(t), t))/(10*r)
assert RHS[5] == (-5*cos(q2(t))*Derivative(q1(t), t) + 6*tan(q2(t)
)*Derivative(q3(t), t) + 4*Derivative(q1(t), t)/cos(q2(t))
)*Derivative(q2(t), t)
def test_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of KanesMethod. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 1)
r, m, g = symbols('r m g')
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
w_R_N_qd = R.ang_vel_in(N)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5],
kd_eqs=kd)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd,
u_auxiliary=[u4, u5])
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar.simplify()
frstar2.simplify()
assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols("g")
k, ls = symbols("k ls")
a, mA, mC = symbols("a mA mC")
F = dynamicsymbols("F")
Ix, Iy, Iz = symbols("Ix Iy Iz")
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols("q1 q2")
q1d, q2d = dynamicsymbols("q1 q2", 1)
u1, u2 = dynamicsymbols("u1 u2")
u1d, u2d = dynamicsymbols("u1 u2", 1)
# Creating reference frames
N = ReferenceFrame("N")
A = ReferenceFrame("A")
A.orient(N, "Axis", [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point("O")
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew("C", q1 * N.x)
Ao = C.locatenew("Ao", a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle("Cart", C, mC)
Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC), (C, -N.x * k * (q1 - ls)), (C, N.x * F)]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
def test_point_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
class RootLink(Link):
"""TODO
"""
def __init__(self, linkage):
super(RootLink, self).__init__(linkage, 'root')
# TODO rename to avoid name conflicts, or inform the user that 'N' is
# taken.
self._frame = ReferenceFrame('N')
self._origin = Point('NO')
self._origin.set_vel(self._frame, 0)
# TODO need to set_acc?
self._origin.set_acc(self._frame, 0)
def 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 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_input_format():
# 1 dof problem from test_one_dof
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, c, k = symbols('m c k')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, u * N.x)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
# test for input format kane.kanes_equations((body1, body2, particle1))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for error raised when a wrong force list (in this case a string) is provided
from sympy.utilities.pytest import raises
raises(ValueError, lambda: KM._form_fr('bad input'))
# 2 dof problem from test_two_dof
q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
N = ReferenceFrame('N')
P1 = Point('P1')
P2 = Point('P2')
P1.set_vel(N, u1 * N.x)
P2.set_vel(N, (u1 + u2) * N.x)
kd = [q1d - u1, q2d - u2]
FL = ((P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
q2 - c2 * u2) * N.x))
pa1 = Particle('pa1', P1, m)
pa2 = Particle('pa2', P2, m)
BL = (pa1, pa2)
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
# test for input format
# kane.kanes_equations((body1, body2), (load1, load2))
KM.kanes_equations(BL, FL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
c2 * u2) / m)
def __init__(self, linkage, name, joint, parent):
"""TODO
"""
super(DynamicLink, self).__init__(linkage, name)
self._joint = joint
# Give the joint access to the link.
self._joint._link = self
self._parent = parent
# Create rigid body.
mass = symbols(name + '_mass')
frame = ReferenceFrame(name + '_frame')
self._origin = Point(name + '_origin')
# TODO explain what this method call does.
joint._orient(name, parent.frame, parent.origin, frame, self._origin)
# All expressed in body frame.
masscenter = self._origin.locatenew(name + '_masscenter',
symbols(name + '_mcx') * frame.x +
symbols(name + '_mcy') * frame.y +
symbols(name + '_mcz') * frame.z)
masscenter.set_vel(frame, 0)
masscenter.v2pt_theory(self._origin, linkage.root.frame, frame)
inertia = functions.inertia(frame,
symbols(name + '_ixx'),
symbols(name + '_iyy'),
symbols(name + '_izz'),
symbols(name + '_ixy'),
symbols(name + '_iyz'),
symbols(name + '_izx'))
# TODO allow specification of non-central inertia
self._rigidbody = RigidBody(name + '_rigidbody',
masscenter, frame, mass, (inertia, masscenter))
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_dub_pen():
# The system considered is the double pendulum. Like in the
# test of the simple pendulum above, we begin by creating the generalized
# coordinates and the simple generalized speeds and accelerations which
# will be used later. Following this we create frames and points necessary
# for the kinematics. The procedure isn't explicitly explained as this is
# similar to the simple pendulum. Also this is documented on the pydy.org
# website.
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
q1dd, q2dd = dynamicsymbols('q1 q2', 2)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
l, m, g = symbols('l m g')
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = N.orientnew('B', 'Axis', [q2, N.z])
A.set_ang_vel(N, q1d * A.z)
B.set_ang_vel(N, q2d * A.z)
O = Point('O')
P = O.locatenew('P', l * A.x)
R = P.locatenew('R', l * B.x)
O.set_vel(N, 0)
P.v2pt_theory(O, N, A)
R.v2pt_theory(P, N, B)
ParP = Particle('ParP', P, m)
ParR = Particle('ParR', R, m)
ParP.potential_energy = - m * g * l * cos(q1)
ParR.potential_energy = - m * g * l * cos(q1) - m * g * l * cos(q2)
L = Lagrangian(N, ParP, ParR)
lm = LagrangesMethod(L, [q1, q2], bodies=[ParP, ParR])
lm.form_lagranges_equations()
assert simplify(l*m*(2*g*sin(q1) + l*sin(q1)*sin(q2)*q2dd
+ l*sin(q1)*cos(q2)*q2d**2 - l*sin(q2)*cos(q1)*q2d**2
+ l*cos(q1)*cos(q2)*q2dd + 2*l*q1dd) - lm.eom[0]) == 0
assert simplify(l*m*(g*sin(q2) + l*sin(q1)*sin(q2)*q1dd
- l*sin(q1)*cos(q2)*q1d**2 + l*sin(q2)*cos(q1)*q1d**2
+ l*cos(q1)*cos(q2)*q1dd + l*q2dd) - lm.eom[1]) == 0
assert lm.bodies == [ParP, ParR]
def test_kinetic_energy():
m, M, l1 = symbols('m M l1')
omega = dynamicsymbols('omega')
N = ReferenceFrame('N')
O = Point('O')
O.set_vel(N, 0 * N.x)
Ac = O.locatenew('Ac', l1 * N.x)
P = Ac.locatenew('P', l1 * N.x)
a = ReferenceFrame('a')
a.set_ang_vel(N, omega * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle('Pa', P, m)
I = outer(N.z, N.z)
A = RigidBody('A', Ac, a, M, (I, Ac))
assert 0 == kinetic_energy(N, Pa, A) - (M*l1**2*omega**2/2
+ 2*l1**2*m*omega**2 + omega**2/2)
def test_angular_momentum_and_linear_momentum():
m, M, l1 = symbols("m M l1")
q1d = dynamicsymbols("q1d")
N = ReferenceFrame("N")
O = Point("O")
O.set_vel(N, 0 * N.x)
Ac = O.locatenew("Ac", l1 * N.x)
P = Ac.locatenew("P", l1 * N.x)
a = ReferenceFrame("a")
a.set_ang_vel(N, q1d * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle("Pa", P, m)
I = outer(N.z, N.z)
A = RigidBody("A", Ac, a, M, (I, Ac))
assert linear_momentum(N, A, Pa) == 2 * m * q1d * l1 * N.y + M * l1 * q1d * N.y
assert angular_momentum(O, N, A, Pa) == 4 * m * q1d * l1 ** 2 * N.z + q1d * N.z
def test_angular_momentum_and_linear_momentum():
m, M, l1 = symbols('m M l1')
q1d = dynamicsymbols('q1d')
N = ReferenceFrame('N')
O = Point('O')
O.set_vel(N, 0 * N.x)
Ac = O.locatenew('Ac', l1 * N.x)
P = Ac.locatenew('P', l1 * N.x)
a = ReferenceFrame('a')
a.set_ang_vel(N, q1d * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle('Pa', P, m)
I = outer(N.z, N.z)
A = RigidBody('A', Ac, a, M, (I, Ac))
assert linear_momentum(N, A, Pa) == 2 * m * q1d* l1 * N.y + M * l1 * q1d * N.y
assert angular_momentum(O, N, A, Pa) == 4 * m * q1d * l1**2 * N.z + q1d * N.z
def test_potential_energy():
m, M, l1, g, h, H = symbols('m M l1 g h H')
omega = dynamicsymbols('omega')
N = ReferenceFrame('N')
O = Point('O')
O.set_vel(N, 0 * N.x)
Ac = O.locatenew('Ac', l1 * N.x)
P = Ac.locatenew('P', l1 * N.x)
a = ReferenceFrame('a')
a.set_ang_vel(N, omega * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle('Pa', P, m)
I = outer(N.z, N.z)
A = RigidBody('A', Ac, a, M, (I, Ac))
Pa.set_potential_energy(m * g * h)
A.set_potential_energy(M * g * H)
assert potential_energy(A, Pa) == m * g * h + M * g * H
def test_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)
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)
fr2, _ = km2.kanes_equations([], forces)
fr1_expected = Matrix([
-R * g * m * sin(q[1]),
-R * (Px * cos(q[0]) + Py * sin(q[0])) * tan(q[1]),
R * (Px * cos(q[0]) + Py * sin(q[0])), Px, Py
])
fr2_expected = Matrix([-R * g * m * sin(q[1]), 0, 0])
assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand()))
assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
def test_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([])
m, alpha, friction, g, t = symbols("m alpha friction g t")
# Projection on X-Y plane
x1 = s * cos(alpha)
y1 = s * sin(alpha)
# Velocity on X-Y plane
vx1 = diff(x1,t)
vy1 = diff(y1,t)
# Setting Reference Frames
N = ReferenceFrame("N")
# Point mass assumption
P = Point("P")
# Velocity of Point mass in X-Y plane
P.set_vel(N,vx1 * N.x + vy1 * N.y)
# Making a particle from point mass
Pa = Particle("P",P,m)
# Potential energy of system
Pa.potential_energy = m * g * y1
# Non-restorative forces
fl = [(P,friction*cos(alpha)*N.x + friction*sin(alpha)*N.y)]
# Setting-up Lagrangian
L = Lagrangian(N,Pa)
q0, q1, q2 = dynamicsymbols('q0 q1 q2')
q0d, q1d, q2d = dynamicsymbols('q0 q1 q2', level=1)
u1, u2, u3 = dynamicsymbols('u1 u2 u3')
LA, LB, LP = symbols('LA LB LP')
p1, p2, p3 = symbols('p1 p2 p3')
g, mA, mB, mC, mD, t = symbols('g mA mB mC mD t')
## --- reference frames ---
E = ReferenceFrame('E')
A = E.orientnew('A', 'Axis', [q0, E.x])
B = A.orientnew('B', 'Axis', [q1, A.y])
C = B.orientnew('C', 'Axis', [0, B.x])
D = C.orientnew('D', 'Axis', [0, C.x])
## --- points and their velocities ---
pO = Point('O')
pA_star = pO.locatenew('A*', LA * A.z)
pP = pO.locatenew('P', LP * A.z)
pB_star = pP.locatenew('B*', LB * B.z)
pC_star = pB_star.locatenew('C*', q2 * B.z)
pD_star = pC_star.locatenew('D*', p1 * B.x + p2 * B.y + p3 * B.z)
pO.set_vel(E, 0) # Point O is fixed in Reference Frame E
pA_star.v2pt_theory(pO, E, A) # Point A* is fixed in Reference Frame A
pP.v2pt_theory(pO, E, A) # Point P is fixed in Reference Frame A
pB_star.v2pt_theory(pP, E, B) # Point B* is fixed in Reference Frame B
# Point C* is moving in Reference Frame B
pC_star.set_vel(B, pC_star.pos_from(pB_star).diff(t, B))
pC_star.v1pt_theory(pB_star, E, B)
pD_star.set_vel(B, pC_star.vel(B)) # Point D* is fixed rel to Point C* in B
pD_star.v1pt_theory(pB_star, E, B) # Point D* is moving in Reference Frame B
from sympy.physics.mechanics import ReferenceFrame, Point, dot, dynamicsymbols
from util import msprint, subs, partial_velocities
from util import generalized_active_forces, potential_energy
g, m1, m2, k, L, omega, t = symbols('g m1 m2 k L ω t')
q1, q2, q3 = dynamicsymbols('q1:4')
qd1, qd2, qd3 = dynamicsymbols('q1:4', level=1)
u1, u2, u3 = dynamicsymbols('u1:4')
## --- Define ReferenceFrames ---
A = ReferenceFrame('A')
B = A.orientnew('B', 'Axis', [omega * t, A.y])
E = B.orientnew('E', 'Axis', [q3, B.z])
## --- Define Points and their velocities ---
pO = Point('O')
pO.set_vel(A, 0)
pP1 = pO.locatenew('P1', q1 * B.x + q2 * B.y)
pD_star = pP1.locatenew('D*', L * E.x)
pP1.set_vel(B, pP1.pos_from(pO).dt(B))
pD_star.v2pt_theory(pP1, B, E)
pP1.v1pt_theory(pO, A, B)
pD_star.v2pt_theory(pP1, A, E)
## --- Expressions for generalized speeds u1, u2, u3 ---
kde = [
u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y),
u3 - dot(E.ang_vel_in(B), E.z)
from sympy import expand, symbols, trigsimp, sin, cos, S
from sympy.physics.mechanics import ReferenceFrame, RigidBody, Point
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint
m, a, b = symbols('m a b')
q1, q2 = dynamicsymbols('q1, q2')
q1d, q2d = dynamicsymbols('q1, q2', level=1)
# reference frames
# N.x parallel to horizontal line, N.y parallel to line AC
N = ReferenceFrame('N')
A = N.orientnew('A', 'axis', [-q1, N.y])
B = A.orientnew('B', 'axis', [-q2, A.x])
# points B*, O
pO = Point('O')
pB_star = pO.locatenew('B*', S(1) / 3 * (2 * a * B.x - b * B.y))
pO.set_vel(N, 0)
pB_star.v2pt_theory(pO, N, B)
# rigidbody B
I_B_Bs = inertia(B, m * b**2 / 18, m * a**2 / 18, m * (a**2 + b**2) / 18)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
# kinetic energy
K = rbB.kinetic_energy(N)
print('K = {0}'.format(msprint(trigsimp(K))))
K_expected = m / 4 * ((a**2 + b**2 * sin(q2)**2 / 3) * q1d**2 +
a * b * cos(q2) * q1d * q2d + b**2 * q2d**2 / 3)
print('diff = {0}'.format(msprint(expand(trigsimp(K - K_expected)))))
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]
alg_con_full = [4]
q1d = dynamicsymbols('q1', level=1)
u1 = dynamicsymbols('u1')
g, m, r, theta = symbols('g m r θ')
omega, t = symbols('ω t')
# reference frames
M = ReferenceFrame('M')
# Plane containing W rotates about a vertical line through center of W.
N = M.orientnew('N', 'Axis', [omega * t, M.z])
C = N.orientnew('C', 'Axis', [q1, N.x])
R = C # R is fixed relative to C
A = C.orientnew('A', 'Axis', [-theta, R.x])
B = C.orientnew('B', 'Axis', [theta, R.x])
# points, velocities
pO = Point('O') # Point O is at the center of the circular wire
pA = pO.locatenew('A', -r * A.z)
pB = pO.locatenew('B', -r * B.z)
pR_star = pO.locatenew('R*', 1 / S(2) * (pA.pos_from(pO) + pB.pos_from(pO)))
pO.set_vel(N, 0)
pO.set_vel(C, 0)
for p in [pA, pB, pR_star]:
p.set_vel(C, 0)
p.v1pt_theory(pO, N, C)
# kinematic differential equations
kde = [u1 - q1d]
kde_map = solve(kde, [q1d])
# contact/distance forces
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, Particle
from sympy.physics.mechanics import Lagrangian, LagrangesMethod
from sympy.physics.mechanics import mprint
t = symbols('t') # 時間
m1, m2, l, g = symbols('m1 m2 l g') # パラメータ
p, theta = dynamicsymbols('p theta') # 一般化座標
F = dynamicsymbols('F') # 外力
q = Matrix([p, theta]) # 座標ベクトル
qd = q.diff(t) # 座標の時間微分
N = ReferenceFrame('N') # 参照座標系
# 質点1（台車）の質点、位置
P1 = Point('P1')
x1 = p # 質点1の位置
y1 = 0
vx1 = diff(x1, t) # x1.diff(t)?でもOK?
vy1 = diff(y1, t)
P1.set_vel(N, vx1 * N.x + vy1 * N.y)
Pa1 = Particle('Pa1', P1, m1)
Pa1.potential_energy = m1 * g * y1
# 質点2（振子）の質点、位置
P2 = Point('P2')
x2 = p - l * sin(theta)
y2 = cos(theta)
vx2 = diff(x2, t)
vy2 = diff(y2, t)
def test_bicycle():
if ON_TRAVIS:
skip("Too slow for travis.")
# 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.
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
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
eps = 1.e-12
for i in range(6):
error = Res.subs(v, i) - A.subs(v, i)
assert all(abs(x) < eps for x in error)
x2 = X
y2 = 0
# Velocity on X-Y plane
vx1 = diff(x1, t)
vy1 = diff(y1, t)
vx2 = diff(x2, t)
vy2 = diff(y2, t)
# Setting Reference Frames
N = ReferenceFrame("N")
# Lumped mass abstraction
P_1 = Point("P_1")
P_2 = Point("P_2")
# Velocity of Point mass in X-Y plane
P_1.set_vel(N, vx1 * N.x + vy1 * N.y)
P_2.set_vel(N, vx2 * N.x + vy2 * N.y)
# Making a particle from point mass
Pa_1 = Particle("P_1", P_1, m_theta)
Pa_2 = Particle("P_2", P_2, m_x)
# Potential energy of system
Pa_1.potential_energy = m_theta * g * y1
Pa_2.potential_energy = 0
# Non-restorative forces
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
])
fr, fr_star = km.kanes_equations(forces, bodies)
assert (fr.expand() == fr_expected.expand())
assert (trigsimp(fr_star).expand() == fr_star_expected.expand())
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 = [dot(p.vel(F), b) for b in A for p in [pB_hat, pC_hat]]
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]
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
])
assert (trigsimp(fr_star.subs(vc_map).subs(
u3, 0)).doit().expand() == fr_star_expected.expand())
# 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)))
fr2, fr_star2 = km.kanes_equations(forces, bodies2)
assert (trigsimp(fr_star2.subs(vc_map).subs(
u3, 0)).doit().expand() == fr_star_expected.expand())
u1, u2, u3, u4, u5, u6, u7 = u = dynamicsymbols('q1:8', level=1)
r, theta, b = symbols('r θ b', real=True, positive=True)
# define reference frames
R = ReferenceFrame('R') # fixed race rf, let R.z point upwards
A = R.orientnew('A', 'axis', [q7, R.z]) # rf that rotates with S* about R.z
# B.x, B.z are parallel with face of cone, B.y is perpendicular
B = A.orientnew('B', 'axis', [-theta, A.x])
S = ReferenceFrame('S')
S.set_ang_vel(A, u1 * A.x + u2 * A.y + u3 * A.z)
C = ReferenceFrame('C')
C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z)
# define points
pO = Point('O')
pS_star = pO.locatenew('S*', b * A.y)
pS_hat = pS_star.locatenew('S^', -r * B.y) # S^ touches the cone
pS1 = pS_star.locatenew('S1',
-r * A.z) # S1 touches horizontal wall of the race
pS2 = pS_star.locatenew('S2', r * A.y) # S2 touches vertical wall of the race
pO.set_vel(R, 0)
pS_star.v2pt_theory(pO, R, A)
pS1.v2pt_theory(pS_star, R, S)
pS2.v2pt_theory(pS_star, R, S)
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0.
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
pO.set_vel(C, 0)
from sympy.physics.vector import ReferenceFrame
from sympy.physics.mechanics import msprint
from sympy.physics.mechanics import Point, Particle
from sympy.physics.mechanics import Lagrangian, LagrangesMethod
from sympy import symbols, sin, cos
s, theta, h = dynamicsymbols('s θ h') # s, theta, h
sd, thetad, hd = dynamicsymbols('s θ h', 1)
m, g, l = symbols('m g l')
N = ReferenceFrame('N')
# part a
r1 = s * N.x
r2 = (s + l * cos(theta)) * N.x + l * sin(theta) * N.y
O = Point('O')
p1 = O.locatenew('p1', r1)
p2 = O.locatenew('p2', r2)
O.set_vel(N, 0)
p1.set_vel(N, p1.pos_from(O).dt(N))
p2.set_vel(N, p2.pos_from(O).dt(N))
P1 = Particle('P1', p1, 2 * m)
P2 = Particle('P2', p2, m)
P1.set_potential_energy(0)
P2.set_potential_energy(P2.mass * g * (p2.pos_from(O) & N.y))
L1 = Lagrangian(N, P1, P2)
print('{} = {}'.format('L1', msprint(L1)))
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,
)
with warns_deprecated_sympy():
(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).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}
#Reference frames - rotation only
#orient lower leg frame with respect to inertial (base) frame
lower_leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
#command below displays direction cosine matrix (DCM)
#pretty_print(lower_leg_frame.dcm(inertial_frame))
#orient upper leg frame with respect to lower leg frame
upper_leg_frame.orient(lower_leg_frame, 'Axis',(theta2,lower_leg_frame.z))
#pretty_print(simplify(upper_leg_frame.dcm(inertial_frame)))
#orient torso
torso_frame.orient(upper_leg_frame, 'Axis', (theta3, upper_leg_frame.z))
#pretty_print(simplify(torso_frame.dcm(inertial_frame)))
#Init joints- points = translation from existing points
ankle = Point('A')
lower_leg_length = symbols('l_L')
knee = Point('K')
knee.set_pos(ankle, lower_leg_length * lower_leg_frame.y)
#pretty_print(knee.pos_from(ankle))
#pretty_print(knee.pos_from(ankle).express(inertial_frame).simplify())
upper_leg_length = symbols('l_U')
hip = Point('H')
hip.set_pos(knee, upper_leg_length * upper_leg_frame.y)
#COM Locations
lower_leg_com_length, upper_leg_com_length, torso_com_length = symbols('d_L, d_U, d_T')
lower_leg_mass_center = Point('L_o')
lower_leg_mass_center.set_pos(ankle, lower_leg_com_length * lower_leg_frame.y)
lower_leg_mass_center.pos_from(ankle)
upper_leg_mass_center = Point('U_o')
from sympy.physics.mechanics import inertia, msprint
from sympy.physics.mechanics import Point, RigidBody
from sympy import pi, solve, symbols, simplify
from sympy import acos, sin, cos
# 2a
q1 = dynamicsymbols('q1')
px, py, pz = symbols('px py pz', real=True, positive=True)
sx, sy, sz = symbols('sx sy sz', real=True, sositive=True)
m, g, l0, k = symbols('m g l0 k', real=True, positive=True)
Ixx, Iyy, Izz = symbols('Ixx Iyy Izz', real=True, positive=True)
N = ReferenceFrame('N')
B = N.orientnew('B', 'axis', [q1, N.z])
pA = Point('A')
pA.set_vel(N, 0)
pP = pA.locatenew('P', l0 * N.y - 2 * l0 * N.z)
pS = pP.locatenew('S', -px * B.x - pz * B.z)
I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb = RigidBody('plane', pS, B, m, (I, pS))
H = rb.angular_momentum(pS, N)
print('H about S in frame N = {}'.format(msprint(H)))
print('dH/dt = {}'.format(msprint(H.dt(N))))
print('a_S_N = {}'.format(pS.acc(N)))
# Reference frames
N = ReferenceFrame("N")
A = N.orientnew("A", "Body", [pi, 0, q1], "123")
B = A.orientnew("B", "Body", [pi / 2, 0, q2], "123")
C = B.orientnew("C", "Body", [-pi / 2, 0, q3], "123")
D = C.orientnew("D", "Body", [pi / 2, 0, q4], "123")
E = D.orientnew("E", "Body", [-pi / 2, 0, q5], "123")
F = E.orientnew("F", "Body", [pi / 2, 0, q6], "123")
G = F.orientnew("G", "Body", [-pi / 2, 0, q7], "123")
# The interface module's reference frame
H = G.orientnew("H", "Body", [pi, 0, 0], "123")
# Unit for distance: meter
P0 = Point("O")
P1 = P0.locatenew("P1", 0.1564 * N.z)
P2 = P1.locatenew("P2", 0.0054 * A.y - 0.1284 * A.z)
P3 = P2.locatenew("P3", -0.2104 * B.y - 0.0064 * B.z)
P4 = P3.locatenew("P4", 0.0064 * C.y - 0.2104 * C.z)
P5 = P4.locatenew("P5", -0.2084 * D.y - 0.0064 * D.z)
P6 = P5.locatenew("P6", -0.1059 * E.z)
P7 = P6.locatenew("P7", -0.1059 * F.y)
P8 = P7.locatenew("P8", -0.0615 * G.z)
# Mid-point of each link
Ao_half = P1.locatenew("P1_half", 0.0054 / 2 * A.y - 0.1284 / 2 * A.z)
Bo_half = P2.locatenew("P2_half", -0.2014 / 2 * B.y - 0.0064 / 2 * B.z)
Co_half = P3.locatenew("P3_half", 0.0064 / 2 * C.y - 0.2104 / 2 * C.z)
Do_half = P4.locatenew("P4_half", -0.2084 / 2 * D.y - 0.0064 / 2 * D.z)
Eo_half = P5.locatenew("P5_half", -0.1059 / 2 * E.z)
from util import generalized_active_forces, generalized_inertia_forces
from util import partial_velocities, subs
g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t')
q1, q2, q3, q4, q5 = q = dynamicsymbols('q1:6')
qd = dynamicsymbols('q1:6', level=1)
u1, u2, u3, u4, u5 = u = dynamicsymbols('u1:6')
# reference frames
A = ReferenceFrame('A')
B_prime = A.orientnew('B_prime', 'Axis', [q1, A.z])
B = B_prime.orientnew('B', 'Axis', [pi / 2 - q2, B_prime.x])
C = B.orientnew('C', 'Axis', [q3, B.z])
# points, 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', q4 * A.x + q5 * 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)
# C* is the point at the center of disk C.
pC_star = pC_hat.locatenew('C*', R * B.y)
pC_star.set_vel(C, 0)
pC_star.set_vel(B, 0)
def test_particle():
m, m2, v1, v2, v3, r, g, h = symbols('m m2 v1 v2 v3 r g h')
P = Point('P')
P2 = Point('P2')
p = Particle('pa', P, m)
assert p.__str__() == 'pa'
assert p.mass == m
assert p.point == P
# Test the mass setter
p.mass = m2
assert p.mass == m2
# Test the point setter
p.point = P2
assert p.point == P2
# Test the linear momentum function
N = ReferenceFrame('N')
O = Point('O')
P2.set_pos(O, r * N.y)
P2.set_vel(N, v1 * N.x)
raises(TypeError, lambda: Particle(P, P, m))
raises(TypeError, lambda: Particle('pa', m, m))
assert p.linear_momentum(N) == m2 * v1 * N.x
assert p.angular_momentum(O, N) == -m2 * r * v1 * N.z
P2.set_vel(N, v2 * N.y)
assert p.linear_momentum(N) == m2 * v2 * N.y
assert p.angular_momentum(O, N) == 0
P2.set_vel(N, v3 * N.z)
assert p.linear_momentum(N) == m2 * v3 * N.z
assert p.angular_momentum(O, N) == m2 * r * v3 * N.x
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
p.potential_energy = m * g * h
assert p.potential_energy == m * g * h
# TODO make the result not be system-dependent
assert p.kinetic_energy(N) in [
m2 * (v1**2 + v2**2 + v3**2) / 2,
m2 * v1**2 / 2 + m2 * v2**2 / 2 + m2 * v3**2 / 2
]
def test_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', 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* 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()
def __init__(self, name, nq, directions=None):
YAMSBody.__init__(self, name)
self.name = name
self.L = symbols('L_' + name)
self.q = [] # DOF
self.qd = [] # DOF velocity as "anonymous" variables
self.qdot = [] # DOF velocities
self.qddot = [] # DOF accelerations
t = dynamicsymbols._t
for i in np.arange(nq):
self.q.append(dynamicsymbols('q_{}{}'.format(name, i + 1)))
self.qd.append(dynamicsymbols('qd_{}{}'.format(name, i + 1)))
self.qdot.append(diff(self.q[i], t))
self.qddot.append(diff(self.qdot[i], t))
# --- Mass matrix related
self.mass = symbols('M_{}'.format(name))
self.J = Taylor(self.name, 'J', 3, 3, nq=nq, rname='xyz', cname='xyz')
self.Ct = Taylor(self.name,
'C_t',
nq,
3,
nq=nq,
rname=None,
cname='xyz')
self.Cr = Taylor(self.name,
'C_r',
nq,
3,
nq=nq,
rname=None,
cname=['x', 'y', 'z'])
self.Me = Taylor(self.name,
'M_e',
nq,
nq,
nq=nq,
rname=None,
cname=None)
self.mdCM = Taylor(self.name,
'M_d',
3,
1,
nq=nq,
rname='xyz',
cname=[''])
# --- h-omega related terms
self.Gr = Taylor(self.name,
'G_r',
3,
3,
nq=nq,
rname='xyz',
cname='xyz')
self.Ge = Taylor(self.name,
'G_e',
nq,
3,
nq=nq,
rname=None,
cname='xyz')
self.Oe = Taylor(self.name,
'O_e',
nq,
6,
nq=nq,
rname=None,
cname=['xx', 'yy', 'zz', 'xy', 'yz', 'xz'])
# --- Stiffness and damping
self.Ke = Taylor(self.name,
'K_e',
nq,
nq,
nq=nq,
rname=None,
cname=None,
order=1)
self.De = Taylor(self.name,
'D_e',
nq,
nq,
nq=nq,
rname=None,
cname=None,
order=1)
self.defineExtremity(directions)
self.origin = Point('O_' + self.name)
# Properties from Rigid body
# inertia
# NOTE: masscenter put at origin for flexible bodies for now
self.masscenter.set_pos(self.origin, 0 * self.frame.x)
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')
F3 = symbols('F3')
# 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)
B = A.orientnew('B', 'axis', [q2, A.z])
C = A.orientnew('C', 'axis', [q3, A.z])
# 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.set_vel(A, 0)
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]:
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()
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)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
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()
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, 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}
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 test_input_format():
# 1 dof problem from test_one_dof
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, c, k = symbols('m c k')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, u * N.x)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
# test for input format kane.kanes_equations((body1, body2, particle1))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
# test for input format kane.kanes_equations(bodies=(body1, body 2))
assert KM.kanes_equations(BL)[0] == Matrix([0])
# test for error raised when a wrong force list (in this case a string) is provided
from sympy.utilities.pytest import raises
raises(ValueError, lambda: KM._form_fr('bad input'))
# 2 dof problem from test_two_dof
q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
N = ReferenceFrame('N')
P1 = Point('P1')
P2 = Point('P2')
P1.set_vel(N, u1 * N.x)
P2.set_vel(N, (u1 + u2) * N.x)
kd = [q1d - u1, q2d - u2]
FL = ((P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
q2 - c2 * u2) * N.x))
pa1 = Particle('pa1', P1, m)
pa2 = Particle('pa2', P2, m)
BL = (pa1, pa2)
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
# test for input format
# kane.kanes_equations((body1, body2), (load1, load2))
KM.kanes_equations(BL, FL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
c2 * u2) / m)
def 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()
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()))
#from kane import KanesMethod
from util import msprint, subs, partial_velocities, generalized_active_forces
g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t')
q = dynamicsymbols('q1:6')
qd = dynamicsymbols('q1:6', level=1)
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).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)
def test_linearize_pendulum_lagrange_nonminimal():
q1, q2 = dynamicsymbols('q1:3')
q1d, q2d = dynamicsymbols('q1:3', level=1)
L, m, t = symbols('L, m, t')
g = 9.8
# Compose World Frame
N = ReferenceFrame('N')
pN = Point('N*')
pN.set_vel(N, 0)
# A.x is along the pendulum
theta1 = atan(q2/q1)
A = N.orientnew('A', 'axis', [theta1, N.z])
# Create point P, the pendulum mass
P = pN.locatenew('P1', q1*N.x + q2*N.y)
P.set_vel(N, P.pos_from(pN).dt(N))
pP = Particle('pP', P, m)
# Constraint Equations
f_c = Matrix([q1**2 + q2**2 - L**2])
# Calculate the lagrangian, and form the equations of motion
Lag = Lagrangian(N, pP)
LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c, forcelist=[(P, m*g*N.x)], frame=N)
LM.form_lagranges_equations()
# Compose operating point
op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0}
# Solve for multiplier operating point
lam_op = LM.solve_multipliers(op_point=op_point)
op_point.update(lam_op)
# Perform the Linearization
A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d],
op_point=op_point, A_and_B=True)
assert A == Matrix([[0, 1], [-9.8/L, 0]])
assert B == Matrix([])
def test_one_dof():
# This is for a 1 dof spring-mass-damper case.
# It is described in more detail in the KanesMethod docstring.
q, u = dynamicsymbols('q u')
qd, ud = dynamicsymbols('q u', 1)
m, c, k = symbols('m c k')
N = ReferenceFrame('N')
P = Point('P')
P.set_vel(N, u * N.x)
kd = [qd - u]
FL = [(P, (-k * q - c * u) * N.x)]
pa = Particle('pa', P, m)
BL = [pa]
KM = KanesMethod(N, [q], [u], kd)
KM.kanes_equations(FL, BL)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
assert (KM.linearize(A_and_B=True, new_method=True)[0] ==
Matrix([[0, 1], [-k/m, -c/m]]))
# Ensure that the old linearizer still works and that the new linearizer
# gives the same results. The old linearizer is deprecated and should be
# removed in >= 0.7.7.
M_old = KM.mass_matrix_full
# The old linearizer raises a deprecation warning, so catch it here so
# it doesn't cause py.test to fail.
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
F_A_old, F_B_old, r_old = KM.linearize()
M_new, F_A_new, F_B_new, r_new = KM.linearize(new_method=True)
assert simplify(M_new.inv() * F_A_new - M_old.inv() * F_A_old) == zeros(2)
def test_nonminimal_pendulum():
q1, q2 = dynamicsymbols('q1:3')
q1d, q2d = dynamicsymbols('q1:3', level=1)
L, m, t = symbols('L, m, t')
g = 9.8
# Compose World Frame
N = ReferenceFrame('N')
pN = Point('N*')
pN.set_vel(N, 0)
# Create point P, the pendulum mass
P = pN.locatenew('P1', q1*N.x + q2*N.y)
P.set_vel(N, P.pos_from(pN).dt(N))
pP = Particle('pP', P, m)
# Constraint Equations
f_c = Matrix([q1**2 + q2**2 - L**2])
# Calculate the lagrangian, and form the equations of motion
Lag = Lagrangian(N, pP)
LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c,
forcelist=[(P, m*g*N.x)], frame=N)
LM.form_lagranges_equations()
# Check solution
lam1 = LM.lam_vec[0, 0]
eom_sol = Matrix([[m*Derivative(q1, t, t) - 9.8*m + 2*lam1*q1],
[m*Derivative(q2, t, t) + 2*lam1*q2]])
assert LM.eom == eom_sol
# Check multiplier solution
lam_sol = Matrix([(19.6*q1 + 2*q1d**2 + 2*q2d**2)/(4*q1**2/m + 4*q2**2/m)])
assert LM.solve_multipliers(sol_type='Matrix') == lam_sol
def test_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],
]
