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_point_funcs():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, 5 * B.y)
O = Point('O')
P = O.locatenew('P', q * B.x)
assert P.pos_from(O) == q * B.x
P.set_vel(B, qd * B.x + q2d * B.y)
assert P.vel(B) == qd * B.x + q2d * B.y
O.set_vel(N, 0)
assert O.vel(N) == 0
assert P.a1pt_theory(O, N, B) == ((-25 * q + qdd) * B.x + (q2dd) * B.y +
(-10 * qd) * B.z)
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 10 * B.x)
O.set_vel(N, 5 * N.x)
assert O.vel(N) == 5 * N.x
assert P.a2pt_theory(O, N, B) == (-10 * qd**2) * B.x + (10 * qdd) * B.y
B.set_ang_vel(N, 5 * B.y)
O = Point('O')
P = O.locatenew('P', q * B.x)
P.set_vel(B, qd * B.x + q2d * B.y)
O.set_vel(N, 0)
assert P.v1pt_theory(O, N, B) == qd * B.x + q2d * B.y - 5 * q * B.z
def test_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_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols("g")
k, ls = symbols("k ls")
a, mA, mC = symbols("a mA mC")
F = dynamicsymbols("F")
Ix, Iy, Iz = symbols("Ix Iy Iz")
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols("q1 q2")
q1d, q2d = dynamicsymbols("q1 q2", 1)
u1, u2 = dynamicsymbols("u1 u2")
u1d, u2d = dynamicsymbols("u1 u2", 1)
# Creating reference frames
N = ReferenceFrame("N")
A = ReferenceFrame("A")
A.orient(N, "Axis", [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point("O")
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew("C", q1 * N.x)
Ao = C.locatenew("Ao", a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle("Cart", C, mC)
Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [
(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F),
]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warns_deprecated_sympy():
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols('g')
k, ls = symbols('k ls')
a, mA, mC = symbols('a mA mC')
F = dynamicsymbols('F')
Ix, Iy, Iz = symbols('Ix Iy Iz')
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
# Creating reference frames
N = ReferenceFrame('N')
A = ReferenceFrame('A')
A.orient(N, 'Axis', [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point('O')
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew('C', q1 * N.x)
Ao = C.locatenew('Ao', a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle('Cart', C, mC)
Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F)]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
def test_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_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 test_point_v1pt_theorys():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, qd * B.z)
O = Point('O')
P = O.locatenew('P', B.x)
P.set_vel(B, 0)
O.set_vel(N, 0)
assert P.v1pt_theory(O, N, B) == qd * B.y
O.set_vel(N, N.x)
assert P.v1pt_theory(O, N, B) == N.x + qd * B.y
P.set_vel(B, B.z)
assert P.v1pt_theory(O, N, B) == B.z + N.x + qd * B.y
def test_point_v1pt_theorys():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, qd * B.z)
O = Point('O')
P = O.locatenew('P', B.x)
P.set_vel(B, 0)
O.set_vel(N, 0)
assert P.v1pt_theory(O, N, B) == qd * B.y
O.set_vel(N, N.x)
assert P.v1pt_theory(O, N, B) == N.x + qd * B.y
P.set_vel(B, B.z)
assert P.v1pt_theory(O, N, B) == B.z + N.x + qd * B.y
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_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_point_a1pt_theorys():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, qd * B.z)
O = Point('O')
P = O.locatenew('P', B.x)
P.set_vel(B, 0)
O.set_vel(N, 0)
assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y
P.set_vel(B, q2d * B.z)
assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y + q2dd * B.z
O.set_vel(N, q2d * B.x)
assert P.a1pt_theory(O, N, B) == ((q2dd - qd**2) * B.x + (q2d * qd + qdd) * B.y +
q2dd * B.z)
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_point_a1pt_theorys():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, qd * B.z)
O = Point('O')
P = O.locatenew('P', B.x)
P.set_vel(B, 0)
O.set_vel(N, 0)
assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y
P.set_vel(B, q2d * B.z)
assert P.a1pt_theory(O, N, B) == -(qd**2) * B.x + qdd * B.y + q2dd * B.z
O.set_vel(N, q2d * B.x)
assert P.a1pt_theory(O, N, B) == ((q2dd - qd**2) * B.x +
(q2d * qd + qdd) * B.y + q2dd * B.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_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_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_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
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_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 expand(2 * B.kinetic_energy(N)) == omega**2 + M * v**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_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_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_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_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))
raises(TypeError, lambda: kinetic_energy(Pa, Pa, A))
raises(TypeError, lambda: kinetic_energy(N, N, A))
assert (0 == (kinetic_energy(N, Pa, A) -
(M * l1**2 * omega**2 / 2 + 2 * l1**2 * m * omega**2 +
omega**2 / 2)).expand())
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) == Vector(0)
expected = (I + M * l ** 2 + 4 * m * l ** 2) * omega * N.z
assert (angular_momentum(O, N, A, Pa) - expected).simplify() == Vector(0)
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
expected = (I + M * l**2 + 4 * m * l**2) * omega * N.z
assert angular_momentum(O, N, A, Pa) == expected
#lower_leg_right_mass_center.pos_from(origin).express(inertial_frame).simplify()
#%% Kinematical Differential Equations
omega0, omega1, omega2, omega3, psi = dynamicsymbols('omega0, omega1, omega2, omega3, psi')
kinematical_differential_equations = [omega0 - theta0.diff(),
omega1 - theta1.diff(),
omega2 - theta2.diff(),
omega3 - theta3.diff(),
psi - phi.diff(),
]
kinematical_differential_equations
#%% Angular Velocities
print("Defining angular velocities")
lower_leg_left_frame.set_ang_vel(inertial_frame, 0 * inertial_frame.z)
#lower_leg_left_frame.ang_vel_in(inertial_frame)
upper_leg_left_frame.set_ang_vel(upper_leg_left_frame, omega0 * inertial_frame.z)
#upper_leg_left_frame.ang_vel_in(inertial_frame)
hip_frame.set_ang_vel(hip_frame, omega1 * inertial_frame.z)
#hip_frame.ang_vel_in(inertial_frame)
upper_leg_right_frame.set_ang_vel(upper_leg_right_frame, omega2 * inertial_frame.z)
#upper_leg_right_frame.ang_vel_in(inertial_frame)
lower_leg_right_frame.set_ang_vel(lower_leg_right_frame, omega3 * inertial_frame.z)
#lower_leg_right_frame.ang_vel_in(inertial_frame)
#%% Linear Velocities
print("Defining linear velocities")
one = Point('1')
one_length = symbols('l_1')
two = Point('2')
two.set_pos(one, one_length*one_frame.y)
three = Point('3')
two_length = symbols('l_2')
three.set_pos(two, two_length*two_frame.y)
#Sets up the angular velocities
omega1, omega2 = dynamicsymbols('omega1, omega2')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega1 - theta1.diff(),
omega2 - theta2.diff()]
#Sets up the rotational axes of the angular velocities
one_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z)
one_frame.ang_vel_in(inertial_frame)
two_frame.set_ang_vel(one_frame, omega2*inertial_frame.z)
two_frame.ang_vel_in(inertial_frame)
#Sets up the linear velocities of the points on the linkages
#one.set_vel(inertial_frame, 0)
two.v2pt_theory(one, inertial_frame, one_frame)
two.vel(inertial_frame)
three.v2pt_theory(two, inertial_frame, two_frame)
three.vel(inertial_frame)
#Sets up the masses of the linkages
one_mass, two_mass = symbols('m_1, m_2')
#Defines the linkages as particles
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.
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)
# Define kinematical differential equations
# =========================================
omega_a, omega_b, omega_c, omega_go, omega_bco = dynamicsymbols('omega_a, omega_b, omega_c, omega_go, omega_bco')
time = symbols('t')
#omega_go = theta_go.diff(time)
#omega_bco = theta_bco.diff(time)
kinematical_differential_equations = [omega_a - theta_a.diff(),
omega_b - theta_b.diff(),
omega_c - theta_c.diff(),
omega_go - theta_]
# Angular Velocities
# ==================
a_frame.set_ang_vel(inertial_frame, omega_a*inertial_frame.z)
b_frame.set_ang_vel(a_frame, omega_b*inertial_frame.z)
c_frame.set_ang_vel(b_frame, omega_c*inertial_frame.z)
go_frame.set_ang_vel(inertial_frame, omega_go*inertial_frame.z)
bco_frame.set_ang_vel(b_frame, omega_bco*inertial_frame.z)
# Linear Velocities
# =================
A.set_vel(inertial_frame, 0)
B.v2pt_theory(A, inertial_frame, a_frame)
C.v2pt_theory(B, inertial_frame, b_frame)
D.v2pt_theory(C, inertial_frame, c_frame)
com_global.v2pt_theory(A, inertial_frame, go_frame)
com_bc.v2pt_theory(B, inertial_frame, bco_frame)
D.set_pos(C, c_length*c_frame.y)
E = Point('E')
d_length = symbols('l_d')
E.set_pos(D, d_length*d_frame.y)
#Sets up the angular velocities
omega_a, omega_b, omega_c, omega_d = dynamicsymbols('omega_a, omega_b, omega_c, omega_d')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega_a - theta_a.diff(),
omega_b - theta_b.diff(),
omega_c - theta_c.diff(),
omega_d - theta_d.diff()]
#Sets up the rotational axes of the angular velocities
a_frame.set_ang_vel(inertial_frame, omega_a*inertial_frame.z)
a_frame.ang_vel_in(inertial_frame)
b_frame.set_ang_vel(a_frame, omega_b*inertial_frame.z)
b_frame.ang_vel_in(inertial_frame)
c_frame.set_ang_vel(c_frame, omega_c*inertial_frame.z)
c_frame.ang_vel_in(inertial_frame)
d_frame.set_ang_vel(d_frame, omega_d*inertial_frame.z)
d_frame.ang_vel_in(inertial_frame)
#Sets up the linear velocities of the points on the linkages
A.set_vel(inertial_frame, 0)
B.v2pt_theory(A, inertial_frame, a_frame)
B.vel(inertial_frame)
C.v2pt_theory(B, inertial_frame, b_frame)
C.vel(inertial_frame)
D.v2pt_theory(C, inertial_frame, c_frame)
from util import msprint, partial_velocities, generalized_active_forces
## --- Declare symbols ---
u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
TB, TE = symbols('TB TE')
# --- ReferenceFrames ---
A = ReferenceFrame('A')
B = ReferenceFrame('B')
CD = ReferenceFrame('CD')
CD_prime = ReferenceFrame('CD_prime')
E = ReferenceFrame('E')
F = ReferenceFrame('F')
# --- Define angular velocities of reference frames ---
B.set_ang_vel(A, u1 * A.x)
B.set_ang_vel(F, u2 * A.x)
CD.set_ang_vel(F, u3 * F.y)
CD_prime.set_ang_vel(F, u4 * -F.y)
E.set_ang_vel(F, u5 * -A.x)
# --- define velocity constraints ---
teeth = dict([('A', 60), ('B', 30), ('C', 30), ('D', 61), ('E', 20)])
vc = [
u2 * teeth['B'] - u3 * teeth['C'], # w_B_F * r_B = w_CD_F * r_C
u2 * teeth['B'] - u4 * teeth['C'], # w_B_F * r_B = w_CD'_F * r_C
u5 * teeth['E'] - u3 * teeth['D'], # w_E_F * r_E = w_CD_F * r_D
u5 * teeth['E'] - u4 * teeth['D'], # w_E_F * r_E = w_CD'_F * r_D;
(-u1 + u2) * teeth['A'] - u3 * teeth['D'], # w_A_F * r_A = w_CD_F * r_D;
(-u1 + u2) * teeth['A'] - u4 * teeth['D']
] # w_A_F * r_A = w_CD'_F * r_D;
from __future__ import division
from sympy import simplify, symbols
from sympy import sin, cos, pi, integrate, Matrix
from sympy.physics.mechanics import ReferenceFrame, Point, dynamicsymbols
from util import msprint, partial_velocities, generalized_active_forces
## --- Declare symbols ---
u1, u2, u3, u4, u5, u6, u7, u8, u9 = dynamicsymbols('u1:10')
c, R = symbols('c R')
x, y, z, r, phi, theta = symbols('x y z r phi theta')
# --- Reference Frames ---
A = ReferenceFrame('A')
B = ReferenceFrame('B')
C = ReferenceFrame('C')
B.set_ang_vel(A, u1 * B.x + u2 * B.y + u3 * B.z)
C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z)
C.set_ang_vel(B, C.ang_vel_in(A) - B.ang_vel_in(A))
pC_star = Point('C*')
pC_star.set_vel(C, 0)
# since radius of cavity is very small, assume C* has zero velocity in B
pC_star.set_vel(B, 0)
pC_star.set_vel(A, u7 * B.x + u8 * B.y + u9 * B.z)
## --- define points P, P' ---
# point on C
pP = pC_star.locatenew('P', x * B.x + y * B.y + z * B.z)
pP.set_vel(C, 0)
pP.v2pt_theory(pC_star, B, C)
pP.v2pt_theory(pC_star, A, C)
omega1, omega2, omega3, omega4 = dynamicsymbols("omega1, omega2, omega3, omega4")
time = symbols("t")
kinematical_differential_equations = [
omega1 - theta1.diff(time),
omega2 - theta2.diff(time),
omega3 - theta3.diff(time),
omega4 - theta4.diff(time),
]
# Angular Velocities
# ==================
l_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)
crotch_frame.set_ang_vel(l_leg_frame, omega2 * l_leg_frame.z)
body_frame.set_ang_vel(crotch_frame, omega4 * crotch_frame.z)
r_leg_frame.set_ang_vel(crotch_frame, omega3 * crotch_frame.z)
# Linear Velocities
# =================
l_ankle.set_vel(inertial_frame, 0)
l_leg_mass_center.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)
l_hip.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)
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 xrange(6):
error = Res.subs(v, i) - A.subs(v, i)
assert all(abs(x) < eps for x in error)
j0_mass_center = Point('j0_o')
j0_mass_center.set_pos(joint0, j0_com_length * j0_frame.y)
j1_mass_center = Point('j1_o')
j1_mass_center.set_pos(joint1, j1_com_length * j1_frame.y)
j2_mass_center = Point('j2_o')
j2_mass_center.set_pos(joint2, j2_com_length * j2_frame.y)
EE.set_pos(joint2, j2_com_length * 2 * j2_frame.y)
#kinematic differential equations
#init var for generalized speeds (aka angular velocities)
omega0, omega1, omega2 = dynamicsymbols('omega0, omega1, omega2')
kinematical_differential_equations = [
omega0 - theta0.diff(), omega1 - theta1.diff(), omega2 - theta2.diff()
]
pretty_print(kinematical_differential_equations)
j0_frame.set_ang_vel(inertial_frame, omega0 * j0_frame.y)
j1_frame.set_ang_vel(j0_frame, omega1 * j0_frame.z)
j2_frame.set_ang_vel(j1_frame, omega2 * j1_frame.x)
#set base link fixed to ground plane
joint0.set_vel(inertial_frame, 0)
#get linear velocities of each point
j0_mass_center.v2pt_theory(joint0, inertial_frame, j0_frame)
joint1.v2pt_theory(joint0, inertial_frame, j0_frame)
j1_mass_center.v2pt_theory(joint1, inertial_frame, j1_frame)
joint2.v2pt_theory(joint1, inertial_frame, j1_frame)
j2_mass_center.v2pt_theory(joint2, inertial_frame, j2_frame)
EE.v2pt_theory(joint2, inertial_frame, j2_frame)
print("finished Kinematics")
# Define kinematical differential equations
# =========================================
omega1, omega2 = dynamicsymbols('omega1, omega2')
time = symbols('t')
kinematical_differential_equations = [omega1 - theta1.diff(time),
omega2 - theta2.diff(time)]
# Angular Velocities
# ==================
lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)
upper_leg_frame.set_ang_vel(lower_leg_frame, omega2 * lower_leg_frame.z)
# Linear Velocities
# =================
ankle.set_vel(inertial_frame, 0)
lower_leg_mass_center.v2pt_theory(ankle, inertial_frame, lower_leg_frame)
knee.v2pt_theory(ankle, inertial_frame, lower_leg_frame)
upper_leg_mass_center.v2pt_theory(knee, inertial_frame, upper_leg_frame)
real=True,
positive=True)
omega, t = symbols('ω t')
theta = 30 * pi / 180 # radians
b = r * (1 + sin(theta)) / (cos(theta) - sin(theta))
# Note: using b as found in Ex3.10. Pure rolling between spheres and race R
# is likely a typo and should be between spheres and cone C.
# 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)
#C.set_ang_vel(A, u4*A.x + u5*A.y + u6*A.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
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0.
pO.set_vel(R, 0)
pS_star.v2pt_theory(pO, R, A)
from __future__ import print_function, division
from sympy import symbols, simplify
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point
from sympy.physics.vector import init_vprinting
init_vprinting(use_latex='mathjax', pretty_print=False)
inertial_frame = ReferenceFrame('I')
lower_leg_frame = ReferenceFrame('L')
theta1, theta2, theta3 = dynamicsymbols('theta1, theta2, theta3')
lower_leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
lower_leg_frame.dcm(inertial_frame)
omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3')
lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)
print(lower_leg_frame.ang_vel_in(inertial_frame))
#and the body frame to to the leg frame by angle theta2
s_frame.orient(inertial_frame, 'Axis', (theta_s, inertial_frame.z))
#Sets up points for the joints and places them relative to each other
S = Point('S')
s_length = symbols('l_s')
T = Point('T')
T.set_pos(S, s_length*s_frame.y)
#Sets up the angular velocities
omega_s = dynamicsymbols('omega_s')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega_s - theta_s.diff()]
#Sets up the rotational axes of the angular velocities
s_frame.set_ang_vel(inertial_frame, omega_s*inertial_frame.z)
#Sets up the linear velocities of the points on the linkages
S.set_vel(inertial_frame, 0)
T.v2pt_theory(S, inertial_frame, s_frame)
#Sets up the masses of the linkages
s_mass = symbols('m_s')
#Defines the linkages as particles
tP = Particle('tP', T, s_mass)
#Sets up gravity information and assigns gravity to act on mass centers
g = symbols('g')
t_grav_force_vector = -1*s_mass*g*inertial_frame.y
t_grav_force = (T, t_grav_force_vector)
# Define kinematical differential equations
# =========================================
omega1_dp1, omega2_dp1, omega1_dp2, omega2_dp2 = dynamicsymbols("omega_1dp1, omega_2dp1, omega_1dp2, omega_2dp2")
time = symbols("t")
kinematical_differential_equations_dp1 = [omega1_dp1 - theta1_dp1.diff(time), omega2_dp1 - theta2_dp2.diff(time)]
kinematical_differential_equations_dp2 = [omega1_dp2 - theta1_dp2.diff(time), omega2_dp2 - theta2_dp2.diff(time)]
# Angular Velocities
# ==================
one_frame_dp1.set_ang_vel(inertial_frame_dp1, omega1_dp1 * inertial_frame_dp1.z)
one_frame_dp1.ang_vel_in(inertial_frame_dp1)
one_frame_dp2.set_ang_vel(inertial_frame_dp2, omega1_dp2 * inertial_frame_dp2.z)
one_frame_dp2.ang_vel_in(inertial_frame_dp2)
two_frame_dp1.set_ang_vel(one_frame_dp1, omega2_dp1 * one_frame_dp1.z - omega1_dp2 * one_frame_dp1.z)
two_frame_dp1.ang_vel_in(inertial_frame_dp1)
two_frame_dp2.set_ang_vel(one_frame_dp2, omega2_dp2 * one_frame_dp2.z)
two_frame_dp2.ang_vel_in(inertial_frame_dp2)
# Linear Velocities
# =================
one_dp1.set_vel(inertial_frame_dp1, 0)
one_dp2.set_vel(inertial_frame_dp2, two_dp1.vel)
from sympy import simplify, symbols
from sympy import sin, cos, pi, integrate, Matrix
from sympy.physics.mechanics import ReferenceFrame, Point, dynamicsymbols
from util import msprint, partial_velocities, generalized_active_forces
## --- Declare symbols ---
u1, u2, u3, u4, u5, u6, u7, u8, u9 = dynamicsymbols('u1:10')
c, R = symbols('c R')
x, y, z, r, phi, theta = symbols('x y z r phi theta')
# --- Reference Frames ---
A = ReferenceFrame('A')
B = ReferenceFrame('B')
C = ReferenceFrame('C')
B.set_ang_vel(A, u1 * B.x + u2 * B.y + u3 * B.z)
C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z)
C.set_ang_vel(B, C.ang_vel_in(A) - B.ang_vel_in(A))
pC_star = Point('C*')
pC_star.set_vel(C, 0)
# since radius of cavity is very small, assume C* has zero velocity in B
pC_star.set_vel(B, 0)
pC_star.set_vel(A, u7 * B.x + u8*B.y + u9*B.z)
## --- define points P, P' ---
# point on C
pP = pC_star.locatenew('P', x * B.x + y * B.y + z * B.z)
pP.set_vel(C, 0)
pP.v2pt_theory(pC_star, B, C)
pP.v2pt_theory(pC_star, A, C)
# Define kinematical differential equations
# =========================================
omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3')
time = symbols('t')
kinematical_differential_equations = [
omega1 - theta1.diff(time), omega2 - theta2.diff(time),
omega3 - theta3.diff(time)
]
# Angular Velocities
# ==================
lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)
upper_leg_frame.set_ang_vel(lower_leg_frame, omega2 * lower_leg_frame.z)
torso_frame.set_ang_vel(upper_leg_frame, omega3 * upper_leg_frame.z)
# Linear Velocities
# =================
ankle.set_vel(inertial_frame, 0)
lower_leg_mass_center.v2pt_theory(ankle, inertial_frame, lower_leg_frame)
knee.v2pt_theory(ankle, inertial_frame, lower_leg_frame)
upper_leg_mass_center.v2pt_theory(knee, inertial_frame, upper_leg_frame)
wLLBx - thLLBx.diff(),wLLBy - thLLBy.diff(),wLLBz - thLLBz.diff(),
wRLBx - thRLBx.diff(),wRLBy - thRLBy.diff(),wRLBz - thRLBz.diff(),
wJbarx - thJbarx.diff(),wJbary - thJbary.diff(),wJbarz - thJbarz.diff(),
wLFTy - thLFTy.diff(),wRFTy - thRFTy.diff(),wLRTy - thLRTy.diff(),wRRTy - thRRTy.diff(),
veltransRPy - transRPy.diff()]
# ______________________________________________________________________________________________________________________________
# Angular Velocities
# Now we can use the generalized speeds to define the angular velocities of the reference frames. Due to our definitions of rotations these are simply ωnk̂ ωnk^ .
# Hint: Remember how we located the joint centers and center of mass locations. The syntax is very similar here.
#lower_leg_frame.set_ang_vel(inertial_frame,omega1*inertial_frame.z) # Sets the angular velocity relative to a reference frame
#lower_leg_frame.ang_vel_in(inertial_frame) # Describe the frame's angular velocity in terms of another reference frame
Chassis_frame.set_ang_vel(inertial_frame,(wRoll * Chassis_frame.x + wPitch * Chassis_frame.y + wYaw * Chassis_frame.z))
#RP_frame.set_ang_vel(Chassis_frame,[wRPx,wRPy,wRPz])#Not needed because RP does not have an angular velocity
LUCA_frame.set_ang_vel(Chassis_frame,(wLUCAx * LUCA_frame.x))
RUCA_frame.set_ang_vel(Chassis_frame,(wRUCAx * RUCA_frame.x))
LLCA_frame.set_ang_vel(Chassis_frame,(wLLCAx * LLCA_frame.x))
RLCA_frame.set_ang_vel(Chassis_frame,(wLUCAx * RLCA_frame.x))
LSpdl_frame.set_ang_vel(Chassis_frame,(wLSpdlx * LSpdl_frame.x + wLSpdly * LSpdl_frame.y + wLSpdlz * LSpdl_frame.z))
RSpdl_frame.set_ang_vel(Chassis_frame,(wRSpdlx * RSpdl_frame.x + wRSpdly * RSpdl_frame.y + wRSpdlz * RSpdl_frame.z))
LTR_frame.set_ang_vel(Chassis_frame,(wLTRx * LTR_frame.x + wLTRy * LTR_frame.y + wLTRz * LTR_frame.z))
RTR_frame.set_ang_vel(Chassis_frame,(wRTRx * RTR_frame.x + wRTRy * RTR_frame.y + wRTRz * RTR_frame.z))
Hsg_frame.set_ang_vel(inertial_frame,(wHsgx * Hsg_frame.x + wHsgy * Hsg_frame.y + wHsgz * Hsg_frame.z))
LBC_frame.set_ang_vel(Hsg_frame,(wLBCy * LBC_frame.y))
RBC_frame.set_ang_vel(Hsg_frame,(wRBCy * RBC_frame.y))
LUB_frame.set_ang_vel(Chassis_frame,(wLUBx * LUB_frame.x + wLUBy * LUB_frame.y + wLUBz * LUB_frame.z))
RUB_frame.set_ang_vel(Chassis_frame,(wRUBx * RUB_frame.x + wRUBy * RUB_frame.y + wRUBz * RUB_frame.z))
LLB_frame.set_ang_vel(Chassis_frame,(wLLBx * LLB_frame.x + wLLBy * LLB_frame.y + wLLBz * LLB_frame.z))
r_leg_mass_center = Point("RL_o")
r_leg_mass_center.set_pos(r_hip, -1 * r_leg_com_length * body_frame.y)
# Define kinematical differential equations
# =========================================
omega1, omega2 = dynamicsymbols("omega1, omega2")
time = symbols("t")
kinematical_differential_equations = [omega1 - theta1.diff(time), omega2 - theta2.diff(time)]
# Angular Velocities
# ==================
l_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)
body_frame.set_ang_vel(l_leg_frame, omega2 * l_leg_frame.z)
# Linear Velocities
# =================
l_ankle.set_vel(inertial_frame, 0)
l_leg_mass_center.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)
l_hip.v2pt_theory(l_ankle, inertial_frame, l_leg_frame)
r_hip.v2pt_theory(l_hip, inertial_frame, body_frame)
body_mass_center.v2pt_theory(l_hip, inertial_frame, body_frame)
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import dot, dynamicsymbols, msprint
q1, q2, q3, q4, q5, q6, q7 = q = dynamicsymbols('q1:8')
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)
## --- Declare symbols ---
u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
TB, TE = symbols('TB TE')
# --- ReferenceFrames ---
A = ReferenceFrame('A')
B = ReferenceFrame('B')
CD = ReferenceFrame('CD')
CD_prime = ReferenceFrame('CD_prime')
E = ReferenceFrame('E')
F = ReferenceFrame('F')
# --- Define angular velocities of reference frames ---
B.set_ang_vel(A, u1 * A.x)
B.set_ang_vel(F, u2 * A.x)
CD.set_ang_vel(F, u3 * F.y)
CD_prime.set_ang_vel(F, u4 * -F.y)
E.set_ang_vel(F, u5 * -A.x)
# --- define velocity constraints ---
teeth = dict([('A', 60), ('B', 30), ('C', 30),
('D', 61), ('E', 20)])
vc = [u2*teeth['B'] - u3*teeth['C'], # w_B_F * r_B = w_CD_F * r_C
u2*teeth['B'] - u4*teeth['C'], # w_B_F * r_B = w_CD'_F * r_C
u5*teeth['E'] - u3*teeth['D'], # w_E_F * r_E = w_CD_F * r_D
u5*teeth['E'] - u4*teeth['D'], # w_E_F * r_E = w_CD'_F * r_D;
(-u1 + u2)*teeth['A'] - u3*teeth['D'], # w_A_F * r_A = w_CD_F * r_D;
(-u1 + u2)*teeth['A'] - u4*teeth['D']] # w_A_F * r_A = w_CD'_F * r_D;
vc_map = solve(vc, [u2, u3, u4, u5])
a_frame.orient(inertial_frame, 'Axis', (theta_a, inertial_frame.z))
base_frame.orient(inertial_frame, 'Axis', (theta_base, inertial_frame.z))
#Sets up points for the joints and places them relative to each other
A = Point('A')
a_length = symbols('l_a')
B = Point('B')
B.set_pos(A, a_length*a_frame.y)
#Sets up the angular velocities
omega_base, omega_a = dynamicsymbols('omega_b, omega_a')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega_a - theta_a.diff()]
#Sets up the rotational axes of the angular velocities
a_frame.set_ang_vel(inertial_frame, omega_base*inertial_frame.z + omega_a*inertial_frame.z)
#Sets up the linear velocities of the points on the linkages
base_vel_x, base_vel_y = dynamicsymbols('v_bx, v_by')
A.set_vel(inertial_frame, base_vel_x*base_frame.x + base_vel_y*base_frame.y)
B.v2pt_theory(A, inertial_frame, a_frame)
#Sets up the masses of the linkages
a_mass = symbols('m_a')
#Defines the linkages as particles
bP = Particle('bP', B, a_mass)
#Sets up gravity information and assigns gravity to act on mass centers
g = symbols('g')
b_grav_force_vector = -1*a_mass*g*inertial_frame.y
#%% Kinematical Differential Equations
omega0, omega1, omega2, omega3, psi = dynamicsymbols(
'omega0, omega1, omega2, omega3, psi')
kinematical_differential_equations = [
omega0 - theta0.diff(),
omega1 - theta1.diff(),
omega2 - theta2.diff(),
omega3 - theta3.diff(),
psi - phi.diff(),
]
kinematical_differential_equations
#%% Angular Velocities
print("Defining angular velocities")
lower_leg_left_frame.set_ang_vel(inertial_frame, 0 * inertial_frame.z)
#lower_leg_left_frame.ang_vel_in(inertial_frame)
upper_leg_left_frame.set_ang_vel(upper_leg_left_frame,
omega0 * inertial_frame.z)
#upper_leg_left_frame.ang_vel_in(inertial_frame)
hip_frame.set_ang_vel(hip_frame, omega1 * inertial_frame.z)
#hip_frame.ang_vel_in(inertial_frame)
upper_leg_right_frame.set_ang_vel(upper_leg_right_frame,
omega2 * inertial_frame.z)
#upper_leg_right_frame.ang_vel_in(inertial_frame)
lower_leg_right_frame.set_ang_vel(lower_leg_right_frame,
omega3 * inertial_frame.z)
def test_point_funcs():
q, q2 = dynamicsymbols('q q2')
qd, q2d = dynamicsymbols('q q2', 1)
qdd, q2dd = dynamicsymbols('q q2', 2)
N = ReferenceFrame('N')
B = ReferenceFrame('B')
B.set_ang_vel(N, 5 * B.y)
O = Point('O')
P = O.locatenew('P', q * B.x)
assert P.pos_from(O) == q * B.x
P.set_vel(B, qd * B.x + q2d * B.y)
assert P.vel(B) == qd * B.x + q2d * B.y
O.set_vel(N, 0)
assert O.vel(N) == 0
assert P.a1pt_theory(O, N, B) == ((-25 * q + qdd) * B.x + (q2dd) * B.y +
(-10 * qd) * B.z)
B = N.orientnew('B', 'Axis', [q, N.z])
O = Point('O')
P = O.locatenew('P', 10 * B.x)
O.set_vel(N, 5 * N.x)
assert O.vel(N) == 5 * N.x
assert P.a2pt_theory(O, N, B) == (-10 * qd**2) * B.x + (10 * qdd) * B.y
B.set_ang_vel(N, 5 * B.y)
O = Point('O')
P = O.locatenew('P', q * B.x)
P.set_vel(B, qd * B.x + q2d * B.y)
O.set_vel(N, 0)
assert P.v1pt_theory(O, N, B) == qd * B.x + q2d * B.y - 5 * q * B.z
