def test_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of Kane. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 1)
r, m, g = symbols('r m g')
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^
R.ang_vel_in(N)))
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [q1d - u3/cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]
ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody()
BodyD.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
BodyList = [BodyD]
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3, u4, u5])
KM.kindiffeq(kd)
kdd = KM.kindiffdict()
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
KM2 = Kane(N)
KM2.coords([q1, q2, q3])
KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5])
KM2.kindiffeq(kd)
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5:0})
assert fr.expand() == fr2.expand()
assert frstar.expand() == frstar2.expand()
def test_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_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_deprecated_set_potential_energy():
m, g, h = symbols('m g h')
A = ReferenceFrame('A')
P = Point('P')
I = Dyadic(0)
B = RigidBody('B', P, A, m, (I, P))
with warns_deprecated_sympy():
B.set_potential_energy(m * g * h)
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.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().as_mutable()
RHS.simplify()
t = symbols('t')
assert tuple(l.mass_matrix[3:6]) == (0, 5 * m * r**2 / 4, 0)
assert RHS[4].simplify() == (
(-8 * g * sin(q2(t)) + r *
(5 * sin(2 * q2(t)) * Derivative(q1(t), t) +
12 * cos(q2(t)) * Derivative(q3(t), t)) * Derivative(q1(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_parallel_axis():
N = ReferenceFrame('N')
m, Ix, Iy, Iz, a, b = symbols('m, I_x, I_y, I_z, a, b')
Io = inertia(N, Ix, Iy, Iz)
o = Point('o')
p = o.locatenew('p', a * N.x + b * N.y)
R = RigidBody('R', o, N, m, (Io, o))
Ip = R.parallel_axis(p)
Ip_expected = inertia(N, Ix + m * b**2, Iy + m * a**2,
Iz + m * (a**2 + b**2), ixy=-m * a * b)
assert Ip == Ip_expected
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_parallel_axis():
N = ReferenceFrame("N")
m, Ix, Iy, Iz, a, b = symbols("m, I_x, I_y, I_z, a, b")
Io = inertia(N, Ix, Iy, Iz)
o = Point("o")
p = o.locatenew("p", a * N.x + b * N.y)
R = RigidBody("R", o, N, m, (Io, o))
Ip = R.parallel_axis(p)
Ip_expected = inertia(N,
Ix + m * b**2,
Iy + m * a**2,
Iz + m * (a**2 + b**2),
ixy=-m * a * b)
assert Ip == Ip_expected
def _convert_bodies(self):
# Convert `Body` to `Particle` and `RigidBody`
bodylist = []
for body in self.bodies:
if body.is_rigidbody:
rb = RigidBody(body.name, body.masscenter, body.frame, body.mass,
(body.central_inertia, body.masscenter))
rb.potential_energy = body.potential_energy
bodylist.append(rb)
else:
part = Particle(body.name, body.masscenter, body.mass)
part.potential_energy = body.potential_energy
bodylist.append(part)
return bodylist
def test_rigidbody2():
M, v, r, omega = dynamicsymbols('M v r omega')
N = ReferenceFrame('N')
b = ReferenceFrame('b')
b.set_ang_vel(N, omega * b.x)
P = Point('P')
I = outer (b.x, b.x)
Inertia_tuple = (I, P)
B = RigidBody('B', P, b, M, Inertia_tuple)
P.set_vel(N, v * b.x)
assert B.angularmomentum(P, N) == omega * b.x
O = Point('O')
O.set_vel(N, v * b.x)
P.set_pos(O, r * b.y)
assert B.angularmomentum(O, N) == omega * b.x - M*v*r*b.z
def __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_angular_momentum_and_linear_momentum():
"""A rod with length 2l, centroidal inertia I, and mass M along with a
particle of mass m fixed to the end of the rod rotate with an angular rate
of omega about point O which is fixed to the non-particle end of the rod.
The rod's reference frame is A and the inertial frame is N."""
m, M, l, I = symbols('m, M, l, I')
omega = dynamicsymbols('omega')
N = ReferenceFrame('N')
a = ReferenceFrame('a')
O = Point('O')
Ac = O.locatenew('Ac', l * N.x)
P = Ac.locatenew('P', l * N.x)
O.set_vel(N, 0 * N.x)
a.set_ang_vel(N, omega * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle('Pa', P, m)
A = RigidBody('A', Ac, a, M, (I * outer(N.z, N.z), Ac))
expected = 2 * m * omega * l * N.y + M * l * omega * N.y
assert linear_momentum(N, A, Pa) == expected
raises(TypeError, lambda: angular_momentum(N, N, A, Pa))
raises(TypeError, lambda: angular_momentum(O, O, A, Pa))
raises(TypeError, lambda: angular_momentum(O, N, O, Pa))
expected = (I + M * l**2 + 4 * m * l**2) * omega * N.z
assert angular_momentum(O, N, A, Pa) == expected
def test_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_linearize_rolling_disc_lagrange():
q1, q2, q3 = q = dynamicsymbols("q1 q2 q3")
q1d, q2d, q3d = qd = dynamicsymbols("q1 q2 q3", 1)
r, m, g = symbols("r m g")
N = ReferenceFrame("N")
Y = N.orientnew("Y", "Axis", [q1, N.z])
L = Y.orientnew("L", "Axis", [q2, Y.x])
R = L.orientnew("R", "Axis", [q3, L.y])
C = Point("C")
C.set_vel(N, 0)
Dmc = C.locatenew("Dmc", r * L.z)
Dmc.v2pt_theory(C, N, R)
I = inertia(L, m / 4 * r ** 2, m / 2 * r ** 2, m / 4 * r ** 2)
BodyD = RigidBody("BodyD", Dmc, R, m, (I, Dmc))
BodyD.potential_energy = -m * g * r * cos(q2)
Lag = Lagrangian(N, BodyD)
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
# Linearize about steady-state upright rolling
op_point = {
q1: 0,
q2: 0,
q3: 0,
q1d: 0,
q2d: 0,
q1d.diff(): 0,
q2d.diff(): 0,
q3d.diff(): 0,
}
A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0]
sol = Matrix(
[
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, -6 * q3d, 0],
[0, -4 * g / (5 * r), 0, 6 * q3d / 5, 0, 0],
[0, 0, 0, 0, 0, 0],
]
)
assert A == sol
def __init__(self):
#We define some quantities required for tests here..
self.p = dynamicsymbols('p:3')
self.q = dynamicsymbols('q:3')
self.dynamic = list(self.p) + list(self.q)
self.states = [radians(45) for x in self.p] + \
[radians(30) for x in self.q]
self.I = ReferenceFrame('I')
self.A = self.I.orientnew('A', 'space', self.p, 'XYZ')
self.B = self.A.orientnew('B', 'space', self.q, 'XYZ')
self.O = Point('O')
self.P1 = self.O.locatenew('P1', 10 * self.I.x + \
10 * self.I.y + 10 * self.I.z)
self.P2 = self.P1.locatenew('P2', 10 * self.I.x + \
10 * self.I.y + 10 * self.I.z)
self.point_list1 = [[2, 3, 1], [4, 6, 2], [5, 3, 1], [5, 3, 6]]
self.point_list2 = [[3, 1, 4], [3, 8, 2], [2, 1, 6], [2, 1, 1]]
self.shape1 = Cylinder(1.0, 1.0)
self.shape2 = Cylinder(1.0, 1.0)
self.Ixx, self.Iyy, self.Izz = symbols('Ixx Iyy Izz')
self.mass = symbols('mass')
self.parameters = [self.Ixx, self.Iyy, self.Izz, self.mass]
self.param_vals = [0, 0, 0, 0]
self.inertia = inertia(self.A, self.Ixx, self.Iyy, self.Izz)
self.rigid_body = RigidBody('rigid_body1', self.P1, self.A, \
self.mass, (self.inertia, self.P1))
self.global_frame1 = VisualizationFrame('global_frame1', \
self.A, self.P1, self.shape1)
self.global_frame2 = VisualizationFrame('global_frame2', \
self.B, self.P2, self.shape2)
self.scene1 = Scene(self.I, self.O, \
(self.global_frame1, self.global_frame2), \
name='scene')
self.particle = Particle('particle1', self.P1, self.mass)
#To make it more readable
p = self.p
q = self.q
#Here is the dragon ..
self.transformation_matrix = \
[[cos(p[1])*cos(p[2]), sin(p[2])*cos(p[1]), -sin(p[1]), 0], \
[sin(p[0])*sin(p[1])*cos(p[2]) - sin(p[2])*cos(p[0]), \
sin(p[0])*sin(p[1])*sin(p[2]) + cos(p[0])*cos(p[2]), \
sin(p[0])*cos(p[1]), 0], \
[sin(p[0])*sin(p[2]) + sin(p[1])*cos(p[0])*cos(p[2]), \
-sin(p[0])*cos(p[2]) + sin(p[1])*sin(p[2])*cos(p[0]), \
cos(p[0])*cos(p[1]), 0], \
[10, 10, 10, 1]]
def test_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_disc_on_an_incline_plane():
# Disc rolling on an inclined plane
# First the generalized coordinates are created. The mass center of the
# disc is located from top vertex of the inclined plane by the generalized
# coordinate 'y'. The orientation of the disc is defined by the angle
# 'theta'. The mass of the disc is 'm' and its radius is 'R'. The length of
# the inclined path is 'l', the angle of inclination is 'alpha'. 'g' is the
# gravitational constant.
y, theta = dynamicsymbols('y theta')
yd, thetad = dynamicsymbols('y theta', 1)
m, g, R, l, alpha = symbols('m g R l alpha')
# Next, we create the inertial reference frame 'N'. A reference frame 'A'
# is attached to the inclined plane. Finally a frame is created which is attached to the disk.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [pi / 2 - alpha, N.z])
B = A.orientnew('B', 'Axis', [-theta, A.z])
# Creating the disc 'D'; we create the point that represents the mass
# center of the disc and set its velocity. The inertia dyadic of the disc
# is created. Finally, we create the disc.
Do = Point('Do')
Do.set_vel(N, yd * A.x)
I = m * R**2 / 2 * B.z | B.z
D = RigidBody('D', Do, B, m, (I, Do))
# To construct the Lagrangian, 'L', of the disc, we determine its kinetic
# and potential energies, T and U, respectively. L is defined as the
# difference between T and U.
D.potential_energy = m * g * (l - y) * sin(alpha)
L = Lagrangian(N, D)
# We then create the list of generalized coordinates and constraint
# equations. The constraint arises due to the disc rolling without slip on
# on the inclined path. We then invoke the 'LagrangesMethod' class and
# supply it the necessary arguments and generate the equations of motion.
# The'rhs' method solves for the q_double_dots (i.e. the second derivative
# with respect to time of the generalized coordinates and the lagrange
# multipliers.
q = [y, theta]
hol_coneqs = [y - R * theta]
m = LagrangesMethod(L, q, hol_coneqs=hol_coneqs)
m.form_lagranges_equations()
rhs = m.rhs()
rhs.simplify()
assert rhs[2] == 2 * g * sin(alpha) / 3
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols("g")
k, ls = symbols("k ls")
a, mA, mC = symbols("a mA mC")
F = dynamicsymbols("F")
Ix, Iy, Iz = symbols("Ix Iy Iz")
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols("q1 q2")
q1d, q2d = dynamicsymbols("q1 q2", 1)
u1, u2 = dynamicsymbols("u1 u2")
u1d, u2d = dynamicsymbols("u1 u2", 1)
# Creating reference frames
N = ReferenceFrame("N")
A = ReferenceFrame("A")
A.orient(N, "Axis", [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point("O")
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew("C", q1 * N.x)
Ao = C.locatenew("Ao", a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle("Cart", C, mC)
Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [
(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F),
]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warns_deprecated_sympy():
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
def test_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_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(0)
I2 = Dyadic(0)
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)
raises(TypeError, lambda: RigidBody(P, P, A, m, (I, P)))
raises(TypeError, lambda: RigidBody('B', P, P, m, (I, P)))
raises(TypeError, lambda: RigidBody('B', P, A, m, (P, P)))
raises(TypeError, lambda: RigidBody('B', P, A, m, (I, I)))
assert B.__str__() == 'B'
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_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_rigidbody2():
M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
N = ReferenceFrame('N')
b = ReferenceFrame('b')
b.set_ang_vel(N, omega * b.x)
P = Point('P')
I = outer(b.x, b.x)
Inertia_tuple = (I, P)
B = RigidBody('B', P, b, M, Inertia_tuple)
P.set_vel(N, v * b.x)
assert B.angular_momentum(P, N) == omega * b.x
O = Point('O')
O.set_vel(N, v * b.x)
P.set_pos(O, r * b.y)
assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z
B.potential_energy = M * g * h
assert B.potential_energy == M * g * h
assert B.kinetic_energy(N) == (omega**2 + M * v**2) / 2
def test_potential_energy():
m, M, l1, g, h, H = symbols("m M l1 g h H")
omega = dynamicsymbols("omega")
N = ReferenceFrame("N")
O = Point("O")
O.set_vel(N, 0 * N.x)
Ac = O.locatenew("Ac", l1 * N.x)
P = Ac.locatenew("P", l1 * N.x)
a = ReferenceFrame("a")
a.set_ang_vel(N, omega * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle("Pa", P, m)
I = outer(N.z, N.z)
A = RigidBody("A", Ac, a, M, (I, Ac))
Pa.potential_energy = m * g * h
A.potential_energy = M * g * H
assert potential_energy(A, Pa) == m * g * h + M * g * H
def test_Lagrangian():
M, m, g, h = symbols("M m g h")
N = ReferenceFrame("N")
O = Point("O")
O.set_vel(N, 0 * N.x)
P = O.locatenew("P", 1 * N.x)
P.set_vel(N, 10 * N.x)
Pa = Particle("Pa", P, 1)
Ac = O.locatenew("Ac", 2 * N.y)
Ac.set_vel(N, 5 * N.y)
a = ReferenceFrame("a")
a.set_ang_vel(N, 10 * N.z)
I = outer(N.z, N.z)
A = RigidBody("A", Ac, a, 20, (I, Ac))
Pa.potential_energy = m * g * h
A.potential_energy = M * g * h
raises(TypeError, lambda: Lagrangian(A, A, Pa))
raises(TypeError, lambda: Lagrangian(N, N, Pa))
def test_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_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_potential_energy():
m, M, l1, g, h, H = symbols("m M l1 g h H")
omega = dynamicsymbols("omega")
N = ReferenceFrame("N")
O = Point("O")
O.set_vel(N, 0 * N.x)
Ac = O.locatenew("Ac", l1 * N.x)
P = Ac.locatenew("P", l1 * N.x)
a = ReferenceFrame("a")
a.set_ang_vel(N, omega * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle("Pa", P, m)
I = outer(N.z, N.z)
A = RigidBody("A", Ac, a, M, (I, Ac))
Pa.potential_energy = m * g * h
A.potential_energy = M * g * H
assert potential_energy(A, Pa) == m * g * h + M * g * H
def test_Lagrangian():
M, m, g, h = symbols('M m g h')
N = ReferenceFrame('N')
O = Point('O')
O.set_vel(N, 0 * N.x)
P = O.locatenew('P', 1 * N.x)
P.set_vel(N, 10 * N.x)
Pa = Particle('Pa', P, 1)
Ac = O.locatenew('Ac', 2 * N.y)
Ac.set_vel(N, 5 * N.y)
a = ReferenceFrame('a')
a.set_ang_vel(N, 10 * N.z)
I = outer(N.z, N.z)
A = RigidBody('A', Ac, a, 20, (I, Ac))
Pa.potential_energy = m * g * h
A.potential_energy = M * g * h
raises(TypeError, lambda: Lagrangian(A, A, Pa))
raises(TypeError, lambda: Lagrangian(N, N, Pa))
def __init__(self,
name,
masscenter=None,
mass=None,
frame=None,
central_inertia=None):
self.name = name
self.loads = []
if frame is None:
frame = ReferenceFrame(name + '_frame')
if masscenter is None:
masscenter = Point(name + '_masscenter')
if central_inertia is None and mass is None:
ixx = Symbol(name + '_ixx')
iyy = Symbol(name + '_iyy')
izz = Symbol(name + '_izz')
izx = Symbol(name + '_izx')
ixy = Symbol(name + '_ixy')
iyz = Symbol(name + '_iyz')
_inertia = (inertia(frame, ixx, iyy, izz, ixy, iyz,
izx), masscenter)
else:
_inertia = (central_inertia, masscenter)
if mass is None:
_mass = Symbol(name + '_mass')
else:
_mass = mass
masscenter.set_vel(frame, 0)
# If user passes masscenter and mass then a particle is created
# otherwise a rigidbody. As a result a body may or may not have inertia.
if central_inertia is None and mass is not None:
self.frame = frame
self.masscenter = masscenter
Particle.__init__(self, name, masscenter, _mass)
else:
RigidBody.__init__(self, name, masscenter, frame, _mass, _inertia)
def test_rigidbody():
m = Symbol('m')
A = ReferenceFrame('A')
P = Point('P')
I = Dyadic([])
B = RigidBody()
assert B.mass == None
assert B.mc == None
assert B.inertia == (None, None)
assert B.frame == None
B.mass = m
B.frame = A
B.cm = P
B.inertia = (I, B.cm)
assert B.mass == m
assert B.frame == A
assert B.cm == P
assert B.inertia == (I, B.cm)
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_rigidbody3():
q1, q2, q3, q4 = dynamicsymbols('q1:5')
p1, p2, p3 = symbols('p1:4')
m = symbols('m')
A = ReferenceFrame('A')
B = A.orientnew('B', 'axis', [q1, A.x])
O = Point('O')
O.set_vel(A, q2*A.x + q3*A.y + q4*A.z)
P = O.locatenew('P', p1*B.x + p2*B.y + p3*B.z)
I = outer(B.x, B.x)
rb1 = RigidBody('rb1', P, B, m, (I, P))
# I_S/O = I_S/S* + I_S*/O
rb2 = RigidBody('rb2', P, B, m,
(I + inertia_of_point_mass(m, P.pos_from(O), B), O))
assert rb1.central_inertia == rb2.central_inertia
assert rb1.angular_momentum(O, A) == rb2.angular_momentum(O, A)
def test_linear_momentum():
N = ReferenceFrame("N")
Ac = Point("Ac")
Ac.set_vel(N, 25 * N.y)
I = outer(N.x, N.x)
A = RigidBody("A", Ac, N, 20, (I, Ac))
P = Point("P")
Pa = Particle("Pa", P, 1)
Pa.point.set_vel(N, 10 * N.x)
raises(TypeError, lambda: linear_momentum(A, A, Pa))
raises(TypeError, lambda: linear_momentum(N, N, Pa))
assert linear_momentum(N, A, Pa) == 10 * N.x + 500 * N.y
def test_aux():
# Same as above, except we have 2 auxiliary speeds for the ground contact
# point, which is known to be zero. In one case, we go through then
# substitute the aux. speeds in at the end (they are zero, as well as their
# derivative), in the other case, we use the built-in auxiliary speed part
# of Kane. The equations from each should be the same.
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
u4d, u5d = dynamicsymbols('u4, u5', 1)
r, m, g = symbols('r m g')
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
R.set_ang_acc(N,
R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^ R.ang_vel_in(N)))
C = Point('C')
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
kd = [q1d - u3 / cos(q2), q2d - u1, q3d - u2 + u3 * tan(q2)]
ForceList = [(Dmc, -m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
BodyList = [BodyD]
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3, u4, u5])
KM.kindiffeq(kd)
(fr, frstar) = KM.kanes_equations(ForceList, BodyList)
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
KM2 = Kane(N)
KM2.coords([q1, q2, q3])
KM2.speeds([u1, u2, u3], u_auxiliary=[u4, u5])
KM2.kindiffeq(kd)
(fr2, frstar2) = KM2.kanes_equations(ForceList, BodyList)
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
assert fr.expand() == fr2.expand()
assert frstar.expand() == frstar2.expand()
def __init__(self, name, masscenter=None, mass=None, frame=None,
central_inertia=None):
self.name = name
self.loads = []
if frame is None:
frame = ReferenceFrame(name + '_frame')
if masscenter is None:
masscenter = Point(name + '_masscenter')
if central_inertia is None and mass is None:
ixx = Symbol(name + '_ixx')
iyy = Symbol(name + '_iyy')
izz = Symbol(name + '_izz')
izx = Symbol(name + '_izx')
ixy = Symbol(name + '_ixy')
iyz = Symbol(name + '_iyz')
_inertia = (inertia(frame, ixx, iyy, izz, ixy, iyz, izx),
masscenter)
else:
_inertia = (central_inertia, masscenter)
if mass is None:
_mass = Symbol(name + '_mass')
else:
_mass = mass
masscenter.set_vel(frame, 0)
# If user passes masscenter and mass then a particle is created
# otherwise a rigidbody. As a result a body may or may not have inertia.
if central_inertia is None and mass is not None:
self.frame = frame
self.masscenter = masscenter
Particle.__init__(self, name, masscenter, _mass)
else:
RigidBody.__init__(self, name, masscenter, frame, _mass, _inertia)
def test_rigidbody():
m, m2 = symbols('m m2')
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.mc == P
assert B.inertia == (I, B.mc)
B.mass = m2
B.frame = A2
B.mc = P2
B.inertia = (I2, B.mc)
assert B.mass == m2
assert B.frame == A2
assert B.mc == P2
assert B.inertia == (I2, B.mc)
def test_linearize_rolling_disc_lagrange():
q1, q2, q3 = q = dynamicsymbols("q1 q2 q3")
q1d, q2d, q3d = qd = dynamicsymbols("q1 q2 q3", 1)
r, m, g = symbols("r m g")
N = ReferenceFrame("N")
Y = N.orientnew("Y", "Axis", [q1, N.z])
L = Y.orientnew("L", "Axis", [q2, Y.x])
R = L.orientnew("R", "Axis", [q3, L.y])
C = Point("C")
C.set_vel(N, 0)
Dmc = C.locatenew("Dmc", r * L.z)
Dmc.v2pt_theory(C, N, R)
I = inertia(L, m / 4 * r ** 2, m / 2 * r ** 2, m / 4 * r ** 2)
BodyD = RigidBody("BodyD", Dmc, R, m, (I, Dmc))
BodyD.potential_energy = -m * g * r * cos(q2)
Lag = Lagrangian(N, BodyD)
l = LagrangesMethod(Lag, q)
l.form_lagranges_equations()
# Linearize about steady-state upright rolling
op_point = {q1: 0, q2: 0, q3: 0, q1d: 0, q2d: 0, q1d.diff(): 0, q2d.diff(): 0, q3d.diff(): 0}
A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0]
sol = Matrix(
[
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, -6 * q3d, 0],
[0, -4 * g / (5 * r), 0, 6 * q3d / 5, 0, 0],
[0, 0, 0, 0, 0, 0],
]
)
assert A == sol
def test_gravity():
N = ReferenceFrame('N')
m, M, g = symbols('m M g')
F1, F2 = dynamicsymbols('F1 F2')
po = Point('po')
pa = Particle('pa', po, m)
A = ReferenceFrame('A')
P = Point('P')
I = outer(A.x, A.x)
B = RigidBody('B', P, A, M, (I, P))
forceList = [(po, F1), (P, F2)]
forceList.extend(gravity(g*N.y, pa, B))
l = [(po, F1), (P, F2), (po, g*m*N.y), (P, g*M*N.y)]
for i in range(len(l)):
for j in range(len(l[i])):
assert forceList[i][j] == l[i][j]
def test_gravity():
N = ReferenceFrame("N")
m, M, g = symbols("m M g")
F1, F2 = dynamicsymbols("F1 F2")
po = Point("po")
pa = Particle("pa", po, m)
A = ReferenceFrame("A")
P = Point("P")
I = outer(A.x, A.x)
B = RigidBody("B", P, A, M, (I, P))
forceList = [(po, F1), (P, F2)]
forceList.extend(gravity(g * N.y, pa, B))
l = [(po, F1), (P, F2), (po, g * m * N.y), (P, g * M * N.y)]
for i in range(len(l)):
for j in range(len(l[i])):
assert forceList[i][j] == l[i][j]
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 kinetic_energy(self, frame):
"""Kinetic energy of the body.
Parameters
==========
frame : ReferenceFrame or Body
The Body's angular velocity and the velocity of it's mass
center are typically defined with respect to an inertial frame but
any relevant frame in which the velocities are known can be supplied.
Examples
========
>>> from sympy.physics.mechanics import Body, ReferenceFrame, Point
>>> from sympy import symbols
>>> m, v, r, omega = symbols('m v r omega')
>>> N = ReferenceFrame('N')
>>> O = Point('O')
>>> P = Body('P', masscenter=O, mass=m)
>>> P.masscenter.set_vel(N, v * N.y)
>>> P.kinetic_energy(N)
m*v**2/2
>>> N = ReferenceFrame('N')
>>> b = ReferenceFrame('b')
>>> b.set_ang_vel(N, omega * b.x)
>>> P = Point('P')
>>> P.set_vel(N, v * N.x)
>>> B = Body('B', masscenter=P, frame=b)
>>> B.kinetic_energy(N)
B_ixx*omega**2/2 + B_mass*v**2/2
See Also
========
sympy.physics.mechanics : Particle, RigidBody
"""
if isinstance(frame, Body):
frame = Body.frame
if self.is_rigidbody:
return RigidBody(self.name, self.masscenter, self.frame, self.mass,
(self.central_inertia, self.masscenter)).kinetic_energy(frame)
return Particle(self.name, self.masscenter, self.mass).kinetic_energy(frame)
def test_center_of_mass():
a = ReferenceFrame('a')
m = symbols('m', real=True)
p1 = Particle('p1', Point('p1_pt'), S.One)
p2 = Particle('p2', Point('p2_pt'), S(2))
p3 = Particle('p3', Point('p3_pt'), S(3))
p4 = Particle('p4', Point('p4_pt'), m)
b_f = ReferenceFrame('b_f')
b_cm = Point('b_cm')
mb = symbols('mb')
b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
p2.point.set_pos(p1.point, a.x)
p3.point.set_pos(p1.point, a.x + a.y)
p4.point.set_pos(p1.point, a.y)
b.masscenter.set_pos(p1.point, a.y + a.z)
point_o=Point('o')
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
assert point_o.pos_from(p1.point)-expr == 0
def test_rigidbody2():
M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
N = ReferenceFrame('N')
b = ReferenceFrame('b')
b.set_ang_vel(N, omega * b.x)
P = Point('P')
I = outer(b.x, b.x)
Inertia_tuple = (I, P)
B = RigidBody('B', P, b, M, Inertia_tuple)
P.set_vel(N, v * b.x)
assert B.angular_momentum(P, N) == omega * b.x
O = Point('O')
O.set_vel(N, v * b.x)
P.set_pos(O, r * b.y)
assert B.angular_momentum(O, N) == omega * b.x - M * v * r * b.z
B.set_potential_energy(M * g * h)
assert B.potential_energy == M * g * h
assert B.kinetic_energy(N) == (omega**2 + M * v**2) / 2
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)))
pC_star = Point('pC*') # center of mass of rod
pA_star = pC_star.locatenew('pA*', L/2*C.x) # center of disk A
pB_star = pC_star.locatenew('pB*', -L/2*C.x) # center of disk A
pA_hat = pA_star.locatenew('pA^', -r*C.z) # contact point of disk A and ground
pB_hat = pB_star.locatenew('pB^', -r*C.z) # contact point of disk A and ground
pC_star.set_vel(N, v*C.y)
pA_star.v2pt_theory(pC_star, N, C) # pA* and pC* are both fixed in frame C
pB_star.v2pt_theory(pC_star, N, C) # pB* and pC* are both fixed in frame C
pA_hat.v2pt_theory(pA_star, N, A) # pA* and pA^ are both fixed in frame A
pB_hat.v2pt_theory(pB_star, N, B) # pB* and pB^ are both fixed in frame B
I_rod = inertia(C, 0, m0*L**2/12, m0*L**2/12, 0, 0, 0)
rbC = RigidBody('rod_C', pC_star, C, m0, (I_rod, pC_star))
I_discA = inertia(A, m*r**2/2, m*r**2/4, m*r**2/4, 0, 0, 0)
rbA = RigidBody('disc_A', pA_star, A, m, (I_discA, pA_star))
I_discB = inertia(B, m*r**2/2, m*r**2/4, m*r**2/4, 0, 0, 0)
rbB = RigidBody('disc_B', pB_star, B, m, (I_discB, pB_star))
print('omega_A_N = {}'.format(msprint(A.ang_vel_in(N).express(C))))
print('v_pA*_N = {}'.format(msprint(pA_hat.vel(N))))
qd_val = solve([dot(pA_hat.vel(N), C.y), dot(pB_hat.vel(N), C.y)],
[q2d, q3d])
print(msprint(qd_val))
print('T_A = {}'.format(msprint(simplify(rbA.kinetic_energy(N).subs(qd_val)))))
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)
# calculate velocities in A
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)
## --- Expressions for generalized speeds u1, u2, u3, u4, u5 ---
u_expr = map(lambda x: dot(C.ang_vel_in(A), x), B)
u_expr += qd[3:]
kde = [u_i - u_ex for u_i, u_ex in zip(u, u_expr)]
kde_map = solve(kde, qd)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])
# define disc rigidbody
I_C = inertia(C, m*R**2/4, m*R**2/4, m*R**2/2)
rbC = RigidBody('rbC', pC_star, C, m, (I_C, pC_star))
# kinetic energy
K = collect(trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map)),
m*R**2/8)
print('K = {0}'.format(msprint(K)))
K_expected = (m*R**2/8) * (5*u1**2 + u2**2 + 6*u3**2)
assert expand(K - K_expected) == 0
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
# Since S is rolling against C, v_S^_C = 0.
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
vc += [dot(pS_hat.vel(C), basis) for basis in A]
# Cone has only angular velocity ω in R.z direction.
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc += [omega - dot(C.ang_vel_in(R), R.z)]
vc_map = solve(vc, u)
# cone rigidbody
I_C = inertia(A, I11, I22, J)
rbC = RigidBody('rbC', pO, C, M, (I_C, pO))
# sphere rigidbody
I_S = inertia(A, 2*m*r**2/5, 2*m*r**2/5, 2*m*r**2/5)
rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star))
# kinetic energy
K = radsimp(expand((rbC.kinetic_energy(R) +
4*rbS.kinetic_energy(R)).subs(vc_map)))
print('K = {0}'.format(msprint(collect(K, omega**2/2))))
K_expected = (J + 18*m*r**2*(2 + sqrt(3))/5) * omega**2/2
#print('K_expected = {0}'.format(msprint(collect(expand(K_expected),
# omega**2/2))))
assert expand(K - K_expected) == 0
pA.set_vel(N, 0)
pA.set_vel(F1, 0)
pB.set_vel(F1, 0)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
#pC.v2pt_theory(pB, N, B)
#print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ')
#print(pC.vel(N).express(N).subs(q2d, q2d_val))
Ixx = m*r**2/4
Iyy = m*r**2/4
Izz = m*r**2/2
I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB))
T = rb_disc.kinetic_energy(N).subs({theta: theta_val, q2d: q2d_val})
print('T = {}'.format(msprint(simplify(T))))
from sympy import S
#T2 = q1d**2*(m*r**2/4*(S(5)/8 - (2*R + r)/(2*r) + (2*R + r)**2/(2*r)**2) + m*R**2/2)
T2 = m*q1d**2*R**2/2 + m*r**2/2 * (
(2*R + r)**2/(2*r)**2 * q1d**2 / 2 -
(2*R + r)**2/(2*r) * q1d**2 / 2 +
5*q1d**2/16)
from sympy import expand
print('T - T2 = {}'.format(msprint(expand(T - T2).simplify())))
t = symbols('t')
dT = T.diff(symbols('t'))
# bearing A
pA = Point('A')
pA.set_vel(N, 0)
pA.set_vel(F1, 0)
# bearing B, center of mass of disc
pB = pA.locatenew('pB', -R*F1.y)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
print('v_B_N = {}'.format(msprint(pB.vel(N))))
Ixx = m*r**2/4
Iyy = m*r**2/4
Izz = m*r**2/2
I_disc = inertia(F2, Ixx, Iyy, Izz, 0, 0, 0)
rb_disc = RigidBody('disc', pB, B, m, (I_disc, pB))
H = rb_disc.angular_momentum(pB, N).subs(vals).express(F2).simplify()
print("H about B in frame N = {}".format(msprint(H)))
#2b
# disc/ground contact point
pC = pB.locatenew('pC', -r*F2.y)
fAx, fAy, fAz, fBx, fBy, fBz = symbols('fAx fAy fAz fBx fBy fBz')
fCx, fCy, fCz = symbols('fCx fCy fCz')
mAx, mAy, mAz, mBx, mBy, mBz = symbols('mAx mAy mAz mBx mBy mBz')
# forces on rod, disc
fA = fAx*F1.x + fAy*F1.y + fAz*F1.z # force exerted on rod at point A
fB = fBx*F2.x + fBy*F2.y + fBz*F2.z # force exerted on disc by rod at point B
fC = fCx*F2.x + fCy*F2.y + fCz*F2.z # force exerted on ground by disc at point C
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
q1dd, q2dd = dynamicsymbols('q1 q2', 2)
m, Ia, It = symbols('m Ia It', real=True, positive=True)
N = ReferenceFrame('N')
F = N.orientnew('F', 'axis', [q1, N.z]) # gimbal frame
B = F.orientnew('B', 'axis', [q2, F.x]) # flywheel frame
P = Point('P')
P.set_vel(N, 0)
P.set_vel(F, 0)
P.set_vel(B, 0)
I = inertia(F, Ia, It, It, 0, 0, 0)
rb = RigidBody('flywheel', P, B, m, (I, P))
H = rb.angular_momentum(P, N)
print('H_P_N = {}\n = {}'.format(H.express(N), H.express(F)))
dH = H.dt(N)
print('d^N(H_P_N)/dt = {}'.format(dH.express(F).subs(q2dd, 0)))
print('\ndH/dt = M')
print('M = {}'.format(dH.express(F).subs(q2dd, 0)))
print('\ncalculation using euler angles')
t = symbols('t')
omega = F.ang_vel_in(N)
wx = omega & F.x
wy = omega & F.y
wz = omega & F.z
def test_rolling_disc():
# Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and three
# speed variables are need to describe this system, along with the disc's
# mass and radius, and the local graivty (note that mass will drop out).
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
r, m, g = symbols('r m g')
# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
L = Y.orientnew('L', 'Axis', [q2, Y.x])
R = L.orientnew('R', 'Axis', [q3, L.y])
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^ R.ang_vel_in(N)))
# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc mass center.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.locatenew('Dmc', r * L.z)
Dmc.v2pt_theory(C, N, R)
Dmc.a2pt_theory(C, N, R)
# This is a simple way to form the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
# Kinematic differential equations; how the generalized coordinate time
# derivatives relate to generalized speeds.
kd = [q1d - u3/cos(q3), q2d - u1, q3d - u2 + u3 * tan(q2)]
# Creation of the force list; it is the gravitational force at the mass
# center of the disc. Then we create the disc by assigning a Point to the
# mass center attribute, a ReferenceFrame to the frame attribute, and mass
# and inertia. Then we form the body list.
ForceList = [(Dmc, - m * g * Y.z)]
BodyD = RigidBody()
BodyD.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
BodyList = [BodyD]
# Finally we form the equations of motion, using the same steps we did
# before. Specify inertial frame, supply generalized speeds, supply
# kinematic differential equation dictionary, compute Fr from the force
# list and Fr* fromt the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = Kane(N)
KM.coords([q1, q2, q3])
KM.speeds([u1, u2, u3])
KM.kindiffeq(kd)
KM.kanes_equations(ForceList, BodyList)
MM = KM.mass_matrix
forcing = KM.forcing
rhs = MM.inv() * forcing
kdd = KM.kindiffdict()
rhs = rhs.subs(kdd)
assert rhs.expand() == Matrix([(10*u2*u3*r - 5*u3**2*r*tan(q2) +
4*g*sin(q2))/(5*r), -2*u1*u3/3, u1*(-2*u2 + u3*tan(q2))]).expand()
w: 1,
f: 2,
v0: 20}
N = ReferenceFrame('N')
B = N.orientnew('B', 'axis', [q3, N.z])
O = Point('O')
S = O.locatenew('S', q1*N.x + q2*N.y)
S.set_vel(N, S.pos_from(O).dt(N))
#Is = m/12*(l**2 + w**2)
Is = symbols('Is')
I = inertia(B, 0, 0, Is, 0, 0, 0)
rb = RigidBody('rb', S, B, m, (I, S))
rb.set_potential_energy(0)
L = Lagrangian(N, rb)
lm = LagrangesMethod(
L, q, nonhol_coneqs = [q1d*sin(q3) - q2d*cos(q3) + l/2*q3d])
lm.form_lagranges_equations()
rhs = lm.rhs()
print('{} = {}'.format(msprint(q1d.diff(t)),
msprint(rhs[3].simplify())))
print('{} = {}'.format(msprint(q2d.diff(t)),
msprint(rhs[4].simplify())))
print('{} = {}'.format(msprint(q3d.diff(t)),
msprint(rhs[5].simplify())))
print('{} = {}'.format('λ', msprint(rhs[6].simplify())))
l_leg_inertia_dyadic = inertia(l_leg_frame, 0, 0, l_leg_inertia)
l_leg_central_inertia = (l_leg_inertia_dyadic, l_leg_mass_center)
body_inertia_dyadic = inertia(body_frame, 0, 0, body_inertia)
body_central_inertia = (body_inertia_dyadic, body_mass_center)
r_leg_inertia_dyadic = inertia(r_leg_frame, 0, 0, r_leg_inertia)
r_leg_central_inertia = (r_leg_inertia_dyadic, r_leg_mass_center)
# Rigid Bodies
# ============
l_leg = RigidBody('Lower Leg', l_leg_mass_center, l_leg_frame,
l_leg_mass, l_leg_central_inertia)
body = RigidBody('Upper Leg', body_mass_center, body_frame,
body_mass, body_central_inertia)
r_leg = RigidBody('R_Leg', r_leg_mass_center, r_leg_frame,
r_leg_mass, r_leg_central_inertia)
# Gravity
# =======
g = symbols('g')
l_leg_grav_force = (l_leg_mass_center,
-l_leg_mass * g * inertial_frame.y)
body_grav_force = (body_mass_center,
# calculate velocities in A
pC_star.v2pt_theory(pR, A, B)
pC_hat.v2pt_theory(pC_star, A, C)
# kinematic differential equations
# kde = [dot(C.ang_vel_in(A), x) - y for x, y in zip(B, u[:3])]
# kde += [x - y for x, y in zip(qd[3:], u[3:])]
# kde_map = solve(kde, qd)
kde = [x - y for x, y in zip(u, qd)]
kde_map = solve(kde, qd)
vc = map(lambda x: dot(pC_hat.vel(A), x), [A.x, A.y])
vc_map = solve(subs(vc, kde_map), [u4, u5])
# define disc rigidbody
IC = inertia(C, m * R ** 2 / 4, m * R ** 2 / 4, m * R ** 2 / 2)
rbC = RigidBody("rbC", pC_star, C, m, (IC, pC_star))
rbC.set_potential_energy(m * g * dot(pC_star.pos_from(pR), A.z))
# potential energy
V = rbC.potential_energy
print("V = {0}".format(msprint(V)))
# kinetic energy
K = trigsimp(rbC.kinetic_energy(A).subs(kde_map).subs(vc_map))
print("K = {0}".format(msprint(K)))
u_indep = [u1, u2, u3]
Fr = generalized_active_forces_K(K, q, u_indep, kde_map, vc_map)
# Fr + Fr* = 0 but the dynamical equations cannot be formulated by only
# kinetic energy as Fr = -Fr* for r = 1, ..., p
print("\ngeneralized active forces, Fr")
q1, q2, q3 = dynamicsymbols('q1, q2 q3')
#omega1, omega2, omega3 = dynamicsymbols('ω1 ω2 ω3')
q1d, q2d = dynamicsymbols('q1, q2', level=1)
m, I11, I22, I33 = symbols('m I11 I22 I33', real=True, positive=True)
# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q1, q2, q3], 'xyz')
# points B*, O
pB_star = Point('B*')
pB_star.set_vel(A, 0)
# rigidbody B
I_B_Bs = inertia(B, I11, I22, I33)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
# kinetic energy
K = rbB.kinetic_energy(A) # velocity of point B* is zero
print('K_ω = {0}'.format(msprint(K)))
print('\nSince I11, I22, I33 are the central principal moments of inertia')
print('let I_min = I11, I_max = I33')
I_min = I11
I_max = I33
H = rbB.angular_momentum(pB_star, A)
K_min = dot(H, H) / I_max / 2
K_max = dot(H, H) / I_min / 2
print('K_ω_min = {0}'.format(msprint(K_min)))
print('K_ω_max = {0}'.format(msprint(K_max)))
# since the disk rolls without slipping.
pA = Point('pA') # ball bearing A
pB = pA.locatenew('pB', -R*F1.y) # ball bearing B
pA.set_vel(N, 0)
pA.set_vel(F1, 0)
pB.set_vel(F1, 0)
pB.set_vel(B, 0)
pB.v2pt_theory(pA, N, F1)
#pC.v2pt_theory(pB, N, B)
#print('\nvelocity of point C in N, v_C_N, at q1 = 0 = ')
#print(pC.vel(N).express(N).subs(q2d, q2d_val))
Ixx = m*r**2/4
Iyy = m*r**2/4
Izz = m*r**2/2
I_disc = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb_disc = RigidBody('Disc', pB, B, m, (I_disc, pB))
#T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val}).subs({theta: theta_val})
T = rb_disc.kinetic_energy(N).subs({q2d: q2d_val})
print('T = {}'.format(msprint(simplify(T))))
values = {R: 1, r: 1, m: 0.5, theta: theta_val}
q1d_val = solve([-1 - q2d_val], q1d)[q1d]
#print(msprint(q1d_val))
print('T = {}'.format(msprint(simplify(T.subs(q1d, q1d_val).subs(values)))))
from sympy.physics.vector import ReferenceFrame
from sympy.physics.mechanics import inertia, msprint
from sympy.physics.mechanics import Point, RigidBody
from sympy.physics.mechanics import Lagrangian, LagrangesMethod
from sympy import symbols
q = q1, q2, q3, q4, q5, q6 = dynamicsymbols("q1:7")
m, g, k, px, Ip = symbols("m g k px Ip")
N = ReferenceFrame("N")
B = N.orientnew("B", "body", [q4, q5, q6], "zyx")
A = Point("A")
S = A.locatenew("S", q1 * N.x + q2 * N.y + q3 * N.z)
P = S.locatenew("P", px * B.x)
A.set_vel(N, 0)
S.set_vel(N, S.pos_from(A).dt(N))
P.v2pt_theory(S, N, B)
Ixx = Ip / 2
Iyy = Ip / 2
Izz = Ip
I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb = RigidBody("rb", S, B, m, (I, S))
rb.set_potential_energy(-m * g * (rb.masscenter.pos_from(A) & N.z) + k / 2 * (P.pos_from(A)).magnitude() ** 2)
L = Lagrangian(N, rb)
print("{} = {}\n".format("L", msprint(L)))
lm = LagrangesMethod(L, q)
print(msprint(lm.form_lagranges_equations()))
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)))))
assert expand(trigsimp(K - K_expected)) == 0
