def coupled_speeds(ic_matrix, partials):
ulist = partials.ulist
print('dynamically coupled speeds:')
found = False
for i, r in enumerate(ulist):
for j, s in enumerate(ulist[i + 1:], i + 1):
if trigsimp(ic_matrix[i, j]) != 0:
found = True
print('{0} and {1}'.format(msprint(r), msprint(s)))
if not found:
print('None')
# define euler angle symbols and reference frames
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
theta, r, R = symbols('θ r R', real=True, positive=True)
theta_val = pi/3
N = ReferenceFrame('N')
#B = N.orientnew('B', 'body', [q1, theta, q2], 'zxz')
F1 = N.orientnew('F1', 'axis', [q1, N.z])
F2 = F1.orientnew('F2', 'axis', [theta, F1.x])
B = F2.orientnew('B', 'axis', [q2, F2.z])
print('rotation matrix from frame N to frame B:')
C = B.dcm(N).subs(theta, theta_val)
print(msprint(C))
# velocity of the disk at the point of contact with the ground is not moving
# since the disk rolls without slipping.
pA = Point('pA') # ball bearing A
pB = pA.locatenew('pB', -R*F1.y) # ball bearing B
pC = pB.locatenew('pC', -r*F2.y) # disk ground contact point
##pO.set_vel(N, 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)
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()))
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())))
a*u1*sin(q1): -u3 - a*u1*sin(q2)*cos(q1)/cos(q2),
(b - b_star)*u2*sin(q2): u3 + a*u1*sin(q1) - b_star*u2*sin(q2)}
for p in [pB_star, pC_star, pP_prime]:
v = p.vel(N).express(N).subs(kde_map).subs(eq_gen_speed_map)
partials[p] = dict(zip(u, map(lambda x: v.diff(x, N), u)))
u1d, u2d, u3d = ud = [x.diff(t) for x in u]
for k, v in vc_map.items():
vc_map[k.diff(t)] = v.diff(t).subs(kde_map).subs(vc_map)
# generalized active/inertia forces
Fr, _ = generalized_active_forces(partials, forces + torques)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map)
print('Fr')
for i, fr in enumerate(Fr, 1):
print('{0}: {1}'.format(i, msprint(fr)))
print('Fr_star')
for i, fr in enumerate(Fr_star, 1):
fr = trigsimp(expand(expand_trig(fr)), deep=True, recursive=True)
print('{0}: {1}'.format(i, msprint(fr)))
# The dynamical equations would be of the form Fr = Fr* (r = 1, 2, 3).
Fr_expected = [
alpha2 + a*(R1*cos(q1) - R3*sin(q1)),
b*(R1*cos(q2) + R3*sin(q2)),
Q3 - R3]
Fr_star_expected = [
-mA*(a_star**2 + kA**2)*u1d,
-mB*((b_star**2 + kB**2)*u2d - b_star*sin(q2)*u3d),
-1*((mB + mC)*u3d - mB*b_star*sin(q2)*u2d + mB*b_star*cos(q2)*u2**2)]
for x, y in zip(Fr, Fr_expected):
# kinematic differential equations
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# use nonholonomic partial velocities to find the nonholonomic
# generalized active forces
vc = [dot(pDs.vel(B), E.y).subs(kde_map)]
vc_map = solve(vc, [u3])
partials = partial_velocities(points, [u1, u2], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, system, kde_map, vc_map)
# dynamical equations
dyn_eq = [x + y for x, y in zip(Fr, Fr_star)]
u1d, u2d = ud = [x.diff(t) for x in [u1, u2]]
dyn_eq_map = solve(dyn_eq, ud)
for x in ud:
print('{0} = {1}'.format(msprint(x),
msprint(trigsimp(dyn_eq_map[x]))))
u1d_expected = (-g*sin(q3) + omega**2*q1*cos(q3) +
(m2*L*omega**2*cos(q3)**2 - m1*u2**2/L)/(m1 + m2))
u2d_expected = -g*cos(q3) - omega**2*q1*sin(q3) + u1*u2/L
assert expand(trigsimp(dyn_eq_map[u1d] - u1d_expected)) == 0
assert expand(trigsimp(dyn_eq_map[u2d] - u2d_expected)) == 0
N = ReferenceFrame('N')
F1 = N.orientnew('F1', 'axis', [q1, N.z])
F2 = F1.orientnew('F2', 'axis', [theta, F1.x])
B = F2.orientnew('B', 'axis', [q2, F2.z])
# 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')
# 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")
for i, x in enumerate(Fr, 1):
print("F{0} = {1}".format(i, msprint(x)))
L = K - V
le = lagrange_equations(L, q, u, kde_map)
(b - b_star) * u2 * sin(q2): u3 + a * u1 * sin(q1) - b_star * u2 * sin(q2)
}
for p in [pB_star, pC_star, pP_prime]:
v = p.vel(N).express(N).subs(kde_map).subs(eq_gen_speed_map)
partials[p] = dict(zip(u, map(lambda x: v.diff(x, N), u)))
u1d, u2d, u3d = ud = [x.diff(t) for x in u]
for k, v in vc_map.items():
vc_map[k.diff(t)] = v.diff(t).subs(kde_map).subs(vc_map)
# generalized active/inertia forces
Fr, _ = generalized_active_forces(partials, forces + torques)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map)
print('Fr')
for i, fr in enumerate(Fr, 1):
print('{0}: {1}'.format(i, msprint(fr)))
print('Fr_star')
for i, fr in enumerate(Fr_star, 1):
fr = trigsimp(expand(expand_trig(fr)), deep=True, recursive=True)
print('{0}: {1}'.format(i, msprint(fr)))
# The dynamical equations would be of the form Fr = Fr* (r = 1, 2, 3).
Fr_expected = [
alpha2 + a * (R1 * cos(q1) - R3 * sin(q1)),
b * (R1 * cos(q2) + R3 * sin(q2)), Q3 - R3
]
Fr_star_expected = [
-mA * (a_star**2 + kA**2) * u1d,
-mB * ((b_star**2 + kB**2) * u2d - b_star * sin(q2) * u3d),
-1 * ((mB + mC) * u3d - mB * b_star * sin(q2) * u2d +
mB * b_star * cos(q2) * u2**2)
pS_star = pO.locatenew('S*', b * A.y)
pS_hat = pS_star.locatenew('S^', -r * B.y) # S^ touches the cone
pS1 = pS_star.locatenew('S1',
-r * A.z) # S1 touches horizontal wall of the race
pS2 = pS_star.locatenew('S2', r * A.y) # S2 touches vertical wall of the race
pO.set_vel(R, 0)
pS_star.v2pt_theory(pO, R, A)
pS1.v2pt_theory(pS_star, R, S)
pS2.v2pt_theory(pS_star, R, S)
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0.
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
# Since S is rolling against C, v_S^_C = 0.
# Cone has only angular velocity in R.z direction.
vc += [dot(pS_hat.vel(C), basis).subs(vc_map) for basis in A]
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc_map = solve(vc, u)
# Pure rolling between S and C, dot(ω_C_S, B.y) = 0.
b_val = solve([dot(C.ang_vel_in(S), B.y).subs(vc_map).simplify()], b)[0][0]
print('b = {0}'.format(msprint(collect(cancel(expand_trig(b_val)), r))))
b_expected = r * (1 + sin(theta)) / (cos(theta) - sin(theta))
assert trigsimp(b_val - b_expected) == 0
# 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)))))
O = Point('O')
p1 = O.locatenew('p1', r1)
p2 = O.locatenew('p2', r2)
O.set_vel(N, 0)
p1.set_vel(N, p1.pos_from(O).dt(N))
p2.set_vel(N, p2.pos_from(O).dt(N))
P1 = Particle('P1', p1, 2 * m)
P2 = Particle('P2', p2, m)
P1.set_potential_energy(0)
P2.set_potential_energy(P2.mass * g * (p2.pos_from(O) & N.y))
L1 = Lagrangian(N, P1, P2)
print('{} = {}'.format('L1', msprint(L1)))
lm1 = LagrangesMethod(L1, [s, theta])
lm1.form_lagranges_equations()
rhs = lm1.rhs()
t = symbols('t')
print('{} = {}'.format(msprint(sd.diff(t)), msprint(rhs[2].simplify())))
print('{} = {}\n'.format(msprint(thetad.diff(t)), msprint(rhs[3].simplify())))
# part b
r1 = s * N.x + h * N.y
r2 = (s + l * cos(theta)) * N.x + (h + l * sin(theta)) * N.y
p1 = O.locatenew('p1', r1)
p2 = O.locatenew('p2', r2)
p1.set_vel(N, p1.pos_from(O).dt(N))
# forces, torques
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
torques = []
# collect all significant points/frames of the system
system = [y for x in bodies for y in [x.masscenter, x.frame]]
system += [x[0] for x in forces + torques]
# partial velocities
partials = partial_velocities(system, [u1, u2, u3],
F,
kde_map,
express_frame=A)
# Fr, Fr*
Fr, _ = generalized_active_forces(partials, forces + torques, uaux=[u3])
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map, uaux=[u3])
friction = -u_prime * Q1 * (pQ.vel(F).normalize().express(A)).subs(u3, 0)
Q_map = dict(zip([Q2, Q3], [dot(friction, x) for x in [A.y, A.z]]))
Q_map[Q1] = trigsimp(solve(F3 - Fr[-1].subs(Q_map), Q1)[0])
#F3 + F3* = 0
Q_map[Q1] = Q_map[Q1].subs(F3, -Fr_star[2])
print('Q1 = {0}'.format(msprint(Q_map[Q1])))
Q1_expected = e * M * g * cos(theta) / (
f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2))
assert expand(radsimp(Q_map[Q1] - Q1_expected)) == 0
from sympy.physics.mechanics import inertia, msprint
from sympy.physics.mechanics import Point, RigidBody
from sympy import pi, solve, symbols, simplify
from sympy import acos, sin, cos
# 2a
q1 = dynamicsymbols('q1')
px, py, pz = symbols('px py pz', real=True, positive=True)
sx, sy, sz = symbols('sx sy sz', real=True, sositive=True)
m, g, l0, k = symbols('m g l0 k', real=True, positive=True)
Ixx, Iyy, Izz = symbols('Ixx Iyy Izz', real=True, positive=True)
N = ReferenceFrame('N')
B = N.orientnew('B', 'axis', [q1, N.z])
pA = Point('A')
pA.set_vel(N, 0)
pP = pA.locatenew('P', l0 * N.y - 2 * l0 * N.z)
pS = pP.locatenew('S', -px * B.x - pz * B.z)
I = inertia(B, Ixx, Iyy, Izz, 0, 0, 0)
rb = RigidBody('plane', pS, B, m, (I, pS))
H = rb.angular_momentum(pS, N)
print('H about S in frame N = {}'.format(msprint(H)))
print('dH/dt = {}'.format(msprint(H.dt(N))))
print('a_S_N = {}'.format(pS.acc(N)))
pRh = pRs.locatenew('R^', cR * B.x)
# orthogonal projection of front wheel center on steer axis
pFh = pRh.locatenew('S^', ls * B.z)
# front wheel center point
pFs = pFh.locatenew('S*', cF * H.x)
# front wheel/ground contact point
pQ = pFs.locatenew('Q', rF * F_z)
# N.z component of vector to pQ from pP
# this is our configuration constraint
f = simplify(pQ.pos_from(pP) & N.z)
print("f = {}\n".format(msprint(f)))
# calculate the derivative of f for use with Newton-Raphson
df = f.diff(theta)
print("df/dθ = {}\n".format(msprint(df)))
# constraint function for zero steer/lean configuration and
# using the benchmark parameters
f0 = lambdify(theta, f.subs({phi: 0, delta: 0}).subs(benchmark_parameters))
df0 = lambdify(theta, df.subs({phi: 0, delta: 0}).subs(benchmark_parameters))
print("verifying constraint equations are correct")
print("for zero steer/lean, pitch should be pi/10")
findroot(f0, 0.3, solver="newton", tol=1e-8, verbose=True, df=df0)
# convert to moore parameters
# define points
pO = Point('O')
pO.set_vel(A, 0)
pP1 = pO.locatenew('P1', q1 * B.x + q2 * B.y)
pP2 = pP1.locatenew('P2', L * R.x)
pP1.set_vel(B, pP1.pos_from(pO).dt(B))
pP2.v2pt_theory(pP1, B, R)
pP1.v1pt_theory(pO, A, B)
pP2.v2pt_theory(pP1, A, R)
# define particles
paP1 = Particle('P1', pP1, m1)
paP2 = Particle('P2', pP2, m2)
# kinetic energy
K_S = lambda rf: sum(pa.kinetic_energy(rf) for pa in [paP1, paP2])
K_S_A = trigsimp(K_S(A))
K_S_B = trigsimp(K_S(B))
print('K_S_A = {0}'.format(msprint(K_S_A)))
print('K_S_B = {0}'.format(msprint(K_S_B)))
K_S_B_expected = ((m1 + m2) * (q1d**2 + q2d**2) / 2 - m2 * L *
(q1d * sin(q3) - q2d * cos(q3) - L * q3d / 2) * q3d)
K_S_A_expected = K_S_B + omega**2 / 2 * (m1 * q1**2 + m2 *
(q1 + L * cos(q3))**2)
assert expand(K_S_A - K_S_A_expected) == 0
assert expand(K_S_B - K_S_B_expected) == 0
from sympy.physics.mechanics import dynamicsymbols, inertia, msprint
m, a, b = symbols('m a b')
q1, q2 = dynamicsymbols('q1, q2')
q1d, q2d = dynamicsymbols('q1, q2', level=1)
# reference frames
# N.x parallel to horizontal line, N.y parallel to line AC
N = ReferenceFrame('N')
A = N.orientnew('A', 'axis', [-q1, N.y])
B = A.orientnew('B', 'axis', [-q2, A.x])
# points B*, O
pO = Point('O')
pB_star = pO.locatenew('B*', S(1) / 3 * (2 * a * B.x - b * B.y))
pO.set_vel(N, 0)
pB_star.v2pt_theory(pO, N, B)
# rigidbody B
I_B_Bs = inertia(B, m * b**2 / 18, m * a**2 / 18, m * (a**2 + b**2) / 18)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_Bs, pB_star))
# kinetic energy
K = rbB.kinetic_energy(N)
print('K = {0}'.format(msprint(trigsimp(K))))
K_expected = m / 4 * ((a**2 + b**2 * sin(q2)**2 / 3) * q1d**2 +
a * b * cos(q2) * q1d * q2d + b**2 * q2d**2 / 3)
print('diff = {0}'.format(msprint(expand(trigsimp(K - K_expected)))))
assert expand(trigsimp(K - K_expected)) == 0
N = ReferenceFrame('N')
F1 = N.orientnew('F1', 'axis', [q1, N.z])
F2 = F1.orientnew('F2', 'axis', [theta, F1.x])
B = F2.orientnew('B', 'axis', [q2, F2.z])
# 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')
pDs.set_vel(E, 0)
pDs.v2pt_theory(pP1, B, E)
pDs.v2pt_theory(pP1, A, E)
# define generalized speeds and constraints
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
vc = [dot(pDs.vel(B), E.y)]
vc_map = solve(subs(vc, kde_map), [u3])
# define system of particles
system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
# calculate kinetic energy, generalized inertia forces
K = sum(map(lambda x: x.kinetic_energy(A), system))
Fr_tilde_star = generalized_inertia_forces_K(K, q, [u1, u2], kde_map, vc_map)
for i, f in enumerate(Fr_tilde_star, 1):
print("F{0}* = {1}".format(i, msprint(simplify(f))))
Fr_tilde_star_expected = [((m1 + m2)*(omega**2*q1*cos(q3) - u1.diff(t)) -
m1*u2**2/L + m2*L*omega**2*cos(q3)**2),
(-m1*(u2.diff(t) + omega**2*q1*sin(q3) -
u1*u2/L))]
for x, y in zip(Fr_tilde_star, Fr_tilde_star_expected):
assert simplify(x - y) == 0
pDs.v2pt_theory(pP1, B, E)
pDs.v2pt_theory(pP1, A, E)
# define generalized speeds and constraints
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
vc = [dot(pDs.vel(B), E.y)]
vc_map = solve(subs(vc, kde_map), [u3])
# define system of particles
system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
# calculate kinetic energy, generalized inertia forces
K = sum(map(lambda x: x.kinetic_energy(A), system))
Fr_tilde_star = generalized_inertia_forces_K(K, q, [u1, u2], kde_map, vc_map)
for i, f in enumerate(Fr_tilde_star, 1):
print("F{0}* = {1}".format(i, msprint(simplify(f))))
Fr_tilde_star_expected = [
((m1 + m2) * (omega**2 * q1 * cos(q3) - u1.diff(t)) - m1 * u2**2 / L +
m2 * L * omega**2 * cos(q3)**2),
(-m1 * (u2.diff(t) + omega**2 * q1 * sin(q3) - u1 * u2 / L))
]
for x, y in zip(Fr_tilde_star, Fr_tilde_star_expected):
assert simplify(x - y) == 0
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)))
# velocity of the disk at the point of contact with the ground is not moving
# 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)))))
# inertia[0] is defined to be the central inertia for each rigid body
rbA = RigidBody("rbA", pA_star, A, mA, (IA, pA_star))
rbB = RigidBody("rbB", pB_star, B, mB, (IB, pB_star))
rbC = RigidBody("rbC", pC_star, C, mC, (IC, pC_star))
rbD = RigidBody("rbD", pD_star, D, mD, (ID, pD_star))
bodies = [rbA, rbB, rbC, rbD]
## --- generalized speeds ---
kde = [u1 - dot(A.ang_vel_in(E), A.x), u2 - dot(B.ang_vel_in(A), B.y), u3 - dot(pC_star.vel(B), B.z)]
kde_map = solve(kde, [q0d, q1d, q2d])
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# kinetic energy of robot arm E
K = sum(rb.kinetic_energy(E) for rb in bodies).subs(kde_map)
print("K = {0}".format(msprint(K)))
# find potential energy contribution of the set of gravitational forces
forces = [(pA_star, -mA * g * E.x), (pB_star, -mB * g * E.x), (pC_star, -mC * g * E.x), (pD_star, -mD * g * E.x)]
## --- define partial velocities ---
partials = partial_velocities([f[0] for f in forces], [u1, u2, u3], E, kde_map)
## -- calculate generalized active forces ---
Fr, _ = generalized_active_forces(partials, forces)
V = potential_energy(Fr, [q0, q1, q2], [u1, u2, u3], kde_map)
# print('V = {0}'.format(msprint(V)))
print("\nSetting C = g*mD*p1, α1, α2, α3 = 0")
V = V.subs(dict(zip(symbols("C α1 α2 α3"), [g * mD * p1, 0, 0, 0])))
print("V = {0}".format(msprint(V)))
rbC = RigidBody("rbC", pC_star, C, m, (I_C, pC_star))
# forces
R_C_hat = Px * A.x + Py * A.y + Pz * A.z
R_C_star = -m * g * A.z
forces = [(pC_hat, R_C_hat), (pC_star, R_C_star)]
# partial velocities
bodies = [rbC]
system = [i.masscenter for i in bodies] + [i.frame for i in bodies] + list(zip(*forces)[0])
partials = partial_velocities(system, [u1, u2, u3], A, kde_map, vc_map)
# generalized active forces
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map, vc_map)
# dynamical equations
dyn_eq = subs([x + y for x, y in zip(Fr, Fr_star)], kde_map)
u1d, u2d, u3d = ud = [x.diff(t) for x in [u1, u2, u3]]
dyn_eq_map = solve(dyn_eq, ud)
for x in ud:
print("{0} = {1}".format(msprint(x), msprint(trigsimp(dyn_eq_map[x]))))
u1d_expected = (u2 ** 2 * tan(q2) - 6 * u2 * u3 - 4 * g * sin(q2) / R) / 5
u2d_expected = 2 * u3 * u1 - u1 * u2 * tan(q2)
u3d_expected = 2 * u1 * u2 / 3
assert trigsimp(expand(dyn_eq_map[u1d] - u1d_expected)) == 0
assert trigsimp(expand(dyn_eq_map[u2d] - u2d_expected)) == 0
assert trigsimp(expand(dyn_eq_map[u3d] - u3d_expected)) == 0
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()))
# define euler angle symbols and reference frames
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
theta, r, R = symbols('θ r R', real=True, positive=True)
theta_val = pi / 3
N = ReferenceFrame('N')
#B = N.orientnew('B', 'body', [q1, theta, q2], 'zxz')
F1 = N.orientnew('F1', 'axis', [q1, N.z])
F2 = F1.orientnew('F2', 'axis', [theta, F1.x])
B = F2.orientnew('B', 'axis', [q2, F2.z])
print('rotation matrix from frame N to frame B:')
C = B.dcm(N).subs(theta, theta_val)
print(msprint(C))
# velocity of the disk at the point of contact with the ground is not moving
# since the disk rolls without slipping.
pA = Point('pA') # ball bearing A
pB = pA.locatenew('pB', -R * F1.y) # ball bearing B
pC = pB.locatenew('pC', -r * F2.y) # disk ground contact point
##pO.set_vel(N, 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_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)))))
print('T_B = {}'.format(msprint(simplify(rbB.kinetic_energy(N).subs(qd_val)))))
T = expand(
simplify(
rbA.kinetic_energy(N).subs(qd_val) +
rbB.kinetic_energy(N).subs(qd_val) + rbC.kinetic_energy(N)))
print('T = {}'.format(msprint(T)))
#T = rb_disc.kinetic_energy(N).subs({theta: theta_val, q2d: q2d_val})
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
s = B.ang_vel_in(F) & F.x
Mx = Ia*(wx + s).diff(t)
My = It*(wy).diff(t) - (It - Ia)*wz*wx + Ia*s*wz
Mz = It*(wz).diff(t) - (It - Ia)*wx*wy + Ia*s*wy
M = Mx*F.x + My*F.y + Mz*F.z
print('M = {}'.format(M.subs(q2dd, 0)))
Fr, a = symbols('Fr a')
f = Fr*F.x
soln = solve([(((2*a*N.z)^f) - M) & F.y], Fr)
print('\nFr = {}'.format(msprint(soln[Fr])))
rbA = RigidBody("rbA", pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody("rbB", pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody("rbC", pC_star, C, mB, (inertia_C, pC_star))
bodies = [rbA, rbB, rbC]
# forces, torques
forces = [(pS_star, -M * g * F.x), (pQ, Q1 * A.x + Q2 * A.y + Q3 * A.z)]
torques = []
# collect all significant points/frames of the system
system = [y for x in bodies for y in [x.masscenter, x.frame]]
system += [x[0] for x in forces + torques]
# partial velocities
partials = partial_velocities(system, [u1, u2, u3], F, kde_map, express_frame=A)
# Fr, Fr*
Fr, _ = generalized_active_forces(partials, forces + torques, uaux=[u3])
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map, uaux=[u3])
friction = -u_prime * Q1 * (pQ.vel(F).normalize().express(A)).subs(u3, 0)
Q_map = dict(zip([Q2, Q3], [dot(friction, x) for x in [A.y, A.z]]))
Q_map[Q1] = trigsimp(solve(F3 - Fr[-1].subs(Q_map), Q1)[0])
# F3 + F3* = 0
Q_map[Q1] = Q_map[Q1].subs(F3, -Fr_star[2])
print("Q1 = {0}".format(msprint(Q_map[Q1])))
Q1_expected = e * M * g * cos(theta) / (f - u_prime * R * u2 / sqrt(u2 ** 2 + f ** 2 * u1 ** 2))
assert expand(radsimp(Q_map[Q1] - Q1_expected)) == 0
A = ReferenceFrame('A')
A_1 = A.orientnew('A_1', 'axis', [ea[0], A.x])
A_2 = A_1.orientnew('A_2', 'axis', [ea[1], A_1.y])
B = A_2.orientnew('B', 'axis', [ea[2], A_2.z])
# display the rotation matrix C from frame A to frame B
print('Rotation matrix C:')
mprint(B.dcm(A))
# define angular velocity vector
w = dynamicsymbols('ω_x ω_y ω_z')
omega_A = sum((a*uv for a, uv in zip(w, A)), Vector(0))
omega_B = sum((a*uv for a, uv in zip(w, B)), Vector(0))
# display angular velocity vector omega
print('\nangular velocity:')
omega = B.ang_vel_in(A)
print('ω = {}'.format(msprint(omega)))
print('ω_A = {}'.format(msprint(omega.express(A))))
print('ω_B = {}'.format(msprint(omega.express(B))))
# solve for alpha, beta, gamma in terms of angular velocity components
dea = [a.diff() for a in ea]
for omega_fr, frame in [(omega_A, A), (omega_B, B)]:
eqs = [dot(omega - omega_fr, uv) for uv in frame]
soln = solve(eqs, dea)
print('\nif angular velocity components are taken to be in frame '
'{}'.format(frame))
for a in dea:
print('{} = {}'.format(msprint(a), msprint(soln[a])))
rbC = RigidBody('rbC', pC_star, C, mC, (IC, pC_star))
rbD = RigidBody('rbD', pD_star, D, mD, (ID, pD_star))
bodies = [rbA, rbB, rbC, rbD]
## --- generalized speeds ---
kde = [
u1 - dot(A.ang_vel_in(E), A.x), u2 - dot(B.ang_vel_in(A), B.y),
u3 - dot(pC_star.vel(B), B.z)
]
kde_map = solve(kde, [q0d, q1d, q2d])
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# kinetic energy of robot arm E
K = sum(rb.kinetic_energy(E) for rb in bodies).subs(kde_map)
print('K = {0}'.format(msprint(K)))
# find potential energy contribution of the set of gravitational forces
forces = [(pA_star, -mA * g * E.x), (pB_star, -mB * g * E.x),
(pC_star, -mC * g * E.x), (pD_star, -mD * g * E.x)]
## --- define partial velocities ---
partials = partial_velocities([f[0] for f in forces], [u1, u2, u3], E, kde_map)
## -- calculate generalized active forces ---
Fr, _ = generalized_active_forces(partials, forces)
V = potential_energy(Fr, [q0, q1, q2], [u1, u2, u3], kde_map)
#print('V = {0}'.format(msprint(V)))
print('\nSetting C = g*mD*p1, α1, α2, α3 = 0')
V = V.subs(dict(zip(symbols('C α1 α2 α3'), [g * mD * p1, 0, 0, 0])))
print('V = {0}'.format(msprint(V)))
kde_map = solve(kde, [q1d, q2d, q3d, q4d, q5d])
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] + [dot(pD.vel(F), A.z)]
vc_map = solve(subs(vc, kde_map), [u3, u4, u5])
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
K = mB*R**2/4
J = mB*R**2/2
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
bodies = [rbA, rbB, rbC]
system = [i.masscenter for i in bodies] + [i.frame for i in bodies]
partials = partial_velocities(system, [u1, u2], F, kde_map, vc_map)
M = trigsimp(inertia_coefficient_matrix(bodies, partials))
print('inertia_coefficient_matrix = {0}'.format(msprint(M)))
M_expected = Matrix([[IA + mA*a**2 + mB*(R**2/2 + 3*b**2), 0],
[0, mA + 3*mB]])
assert expand(M - M_expected) == Matrix.zeros(2)
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)))))
print('T_B = {}'.format(msprint(simplify(rbB.kinetic_energy(N).subs(qd_val)))))
T = expand(simplify(
rbA.kinetic_energy(N).subs(qd_val) +
rbB.kinetic_energy(N).subs(qd_val) +
rbC.kinetic_energy(N)))
print('T = {}'.format(msprint(T)))
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'))
print('dT/dt = {} = 0'.format(msprint(simplify(dT))))
def print_fr_star(fr_star):
for i, f in enumerate(fr_star, 1):
print("F{0}* = {1}".format(i, msprint(trigsimp(together(f)))))
kde = [u1 - dot(A.ang_vel_in(F), A.x), u2 - dot(pD.vel(F), A.y), u3 - q3d, u4 - q4d, u5 - q5d]
kde_map = solve(kde, [q1d, q2d, q3d, q4d, q5d])
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]] + [dot(pD.vel(F), A.z)]
vc_map = solve(subs(vc, kde_map), [u3, u4, u5])
# inertias of bodies A, B, C
# IA22, IA23, IA33 are not specified in the problem statement, but are
# necessary to define an inertia object. Although the values of
# IA22, IA23, IA33 are not known in terms of the variables given in the
# problem statement, they do not appear in the general inertia terms.
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
K = mB * R ** 2 / 4
J = mB * R ** 2 / 2
inertia_B = inertia(B, K, K, J)
inertia_C = inertia(C, K, K, J)
# define the rigid bodies A, B, C
rbA = RigidBody("rbA", pA_star, A, mA, (inertia_A, pA_star))
rbB = RigidBody("rbB", pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody("rbC", pC_star, C, mB, (inertia_C, pC_star))
bodies = [rbA, rbB, rbC]
system = [i.masscenter for i in bodies] + [i.frame for i in bodies]
partials = partial_velocities(system, [u1, u2], F, kde_map, vc_map)
M = trigsimp(inertia_coefficient_matrix(bodies, partials))
print("inertia_coefficient_matrix = {0}".format(msprint(M)))
M_expected = Matrix([[IA + mA * a ** 2 + mB * (R ** 2 / 2 + 3 * b ** 2), 0], [0, mA + 3 * mB]])
assert expand(M - M_expected) == Matrix.zeros(2)
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
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
# 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)))
print('\nI11/I33, I22/I33 =< 1, since I33 >= I11, I22, so K_ω_min <= K_ω')
print('Similarly, I22/I11, I33/I11 >= 1, '
'since I11 <= I22, I33, so K_ω_max >= K_ω')
O = Point('O')
p1 = O.locatenew('p1', r1)
p2 = O.locatenew('p2', r2)
O.set_vel(N, 0)
p1.set_vel(N, p1.pos_from(O).dt(N))
p2.set_vel(N, p2.pos_from(O).dt(N))
P1 = Particle('P1', p1, 2*m)
P2 = Particle('P2', p2, m)
P1.set_potential_energy(0)
P2.set_potential_energy(P2.mass * g * (p2.pos_from(O) & N.y))
L1 = Lagrangian(N, P1, P2)
print('{} = {}'.format('L1', msprint(L1)))
lm1 = LagrangesMethod(L1, [s, theta])
lm1.form_lagranges_equations()
rhs = lm1.rhs()
t = symbols('t')
print('{} = {}'.format(msprint(sd.diff(t)), msprint(rhs[2].simplify())))
print('{} = {}\n'.format(msprint(thetad.diff(t)), msprint(rhs[3].simplify())))
# part b
r1 = s*N.x + h*N.y
r2 = (s + l*cos(theta))*N.x + (h + l*sin(theta))*N.y
p1 = O.locatenew('p1', r1)
p2 = O.locatenew('p2', r2)
p1.set_vel(N, p1.pos_from(O).dt(N))
A = ReferenceFrame('A')
A_1 = A.orientnew('A_1', 'axis', [ea[0], A.x])
A_2 = A_1.orientnew('A_2', 'axis', [ea[1], A_1.y])
B = A_2.orientnew('B', 'axis', [ea[2], A_2.z])
# display the rotation matrix C from frame A to frame B
print('Rotation matrix C:')
mprint(B.dcm(A))
# define angular velocity vector
w = dynamicsymbols('ω_x ω_y ω_z')
omega_A = sum((a * uv for a, uv in zip(w, A)), Vector(0))
omega_B = sum((a * uv for a, uv in zip(w, B)), Vector(0))
# display angular velocity vector omega
print('\nangular velocity:')
omega = B.ang_vel_in(A)
print('ω = {}'.format(msprint(omega)))
print('ω_A = {}'.format(msprint(omega.express(A))))
print('ω_B = {}'.format(msprint(omega.express(B))))
# solve for alpha, beta, gamma in terms of angular velocity components
dea = [a.diff() for a in ea]
for omega_fr, frame in [(omega_A, A), (omega_B, B)]:
eqs = [dot(omega - omega_fr, uv) for uv in frame]
soln = solve(eqs, dea)
print('\nif angular velocity components are taken to be in frame '
'{}'.format(frame))
for a in dea:
print('{} = {}'.format(msprint(a), msprint(soln[a])))
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'))
print('dT/dt = {} = 0'.format(msprint(simplify(dT))))
#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')
for i, x in enumerate(Fr, 1):
print('F{0} = {1}'.format(i, msprint(x)))
L = K - V
le = lagrange_equations(L, q, u, kde_map)
pRh = pRs.locatenew('R^', cR*B.x)
# orthogonal projection of front wheel center on steer axis
pFh = pRh.locatenew('S^', ls*B.z)
# front wheel center point
pFs = pFh.locatenew('S*', cF*H.x)
# front wheel/ground contact point
pQ = pFs.locatenew('Q', rF*F_z)
# N.z component of vector to pQ from pP
# this is our configuration constraint
f = simplify(pQ.pos_from(pP) & N.z)
print("f = {}\n".format(msprint(f)))
# calculate the derivative of f for use with Newton-Raphson
df = f.diff(theta)
print("df/dθ = {}\n".format(msprint(df)))
# constraint function for zero steer/lean configuration and
# using the benchmark parameters
f0 = lambdify(theta, f.subs({phi: 0, delta: 0}).subs(benchmark_parameters))
df0 = lambdify(theta, df.subs({phi: 0, delta: 0}).subs(benchmark_parameters))
print("verifying constraint equations are correct")
print("for zero steer/lean, pitch should be pi/10")
findroot(f0, 0.3, solver="newton", tol=1e-8, verbose=True, df=df0)
# convert to moore parameters
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
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
# 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)))
print('\nI11/I33, I22/I33 =< 1, since I33 >= I11, I22, so K_ω_min <= K_ω')
print('Similarly, I22/I11, I33/I11 >= 1, '
'since I11 <= I22, I33, so K_ω_max >= K_ω')
pO = Point('O')
pS_star = pO.locatenew('S*', b*A.y)
pS_hat = pS_star.locatenew('S^', -r*B.y) # S^ touches the cone
pS1 = pS_star.locatenew('S1', -r*A.z) # S1 touches horizontal wall of the race
pS2 = pS_star.locatenew('S2', r*A.y) # S2 touches vertical wall of the race
pO.set_vel(R, 0)
pS_star.v2pt_theory(pO, R, A)
pS1.v2pt_theory(pS_star, R, S)
pS2.v2pt_theory(pS_star, R, S)
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0.
vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R]
pO.set_vel(C, 0)
pS_star.v2pt_theory(pO, C, A)
pS_hat.v2pt_theory(pS_star, C, S)
# Since S is rolling against C, v_S^_C = 0.
# Cone has only angular velocity in R.z direction.
vc += [dot(pS_hat.vel(C), basis).subs(vc_map) for basis in A]
vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]]
vc_map = solve(vc, u)
# Pure rolling between S and C, dot(ω_C_S, B.y) = 0.
b_val = solve([dot(C.ang_vel_in(S), B.y).subs(vc_map).simplify()], b)[0][0]
print('b = {0}'.format(msprint(collect(cancel(expand_trig(b_val)), r))))
b_expected = r*(1 + sin(theta))/(cos(theta) - sin(theta))
assert trigsimp(b_val - b_expected) == 0
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
bodies = [rbA, rbB, rbC]
# forces, torques
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
torques = []
# collect all significant points/frames of the system
system = [y for x in bodies for y in [x.masscenter, x.frame]]
system += [x[0] for x in forces + torques]
# partial velocities
partials = partial_velocities(system, [u1, u2, u3], F,
kde_map, express_frame=A)
# Fr, Fr*
Fr, _ = generalized_active_forces(partials, forces + torques, uaux=[u3])
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map, uaux=[u3])
friction = -u_prime*Q1*(pQ.vel(F).normalize().express(A)).subs(u3, 0)
Q_map = dict(zip([Q2, Q3], [dot(friction, x) for x in [A.y, A.z]]))
Q_map[Q1] = trigsimp(solve(F3 - Fr[-1].subs(Q_map), Q1)[0])
#F3 + F3* = 0
Q_map[Q1] = Q_map[Q1].subs(F3, -Fr_star[2])
print('Q1 = {0}'.format(msprint(Q_map[Q1])))
Q1_expected = e*M*g*cos(theta)/(f - u_prime*R*u2/sqrt(u2**2 + f**2*u1**2))
assert expand(radsimp(Q_map[Q1] - Q1_expected)) == 0
def print_fr_star(fr_star):
for i, f in enumerate(fr_star, 1):
print("F{0}* = {1}".format(i, msprint(trigsimp(together(f)))))
system = [Particle('P1', pP1, m1), Particle('P2', pDs, m2)]
# kinematic differential equations
kde = [u1 - dot(pP1.vel(A), E.x), u2 - dot(pP1.vel(A), E.y), u3 - q3d]
kde_map = solve(kde, qd)
# include second derivatives in kde map
for k, v in kde_map.items():
kde_map[k.diff(t)] = v.diff(t)
# use nonholonomic partial velocities to find the nonholonomic
# generalized active forces
vc = [dot(pDs.vel(B), E.y).subs(kde_map)]
vc_map = solve(vc, [u3])
partials = partial_velocities(points, [u1, u2], A, kde_map, vc_map)
Fr, _ = generalized_active_forces(partials, forces)
Fr_star, _ = generalized_inertia_forces(partials, system, kde_map, vc_map)
# dynamical equations
dyn_eq = [x + y for x, y in zip(Fr, Fr_star)]
u1d, u2d = ud = [x.diff(t) for x in [u1, u2]]
dyn_eq_map = solve(dyn_eq, ud)
for x in ud:
print('{0} = {1}'.format(msprint(x), msprint(trigsimp(dyn_eq_map[x]))))
u1d_expected = (-g * sin(q3) + omega**2 * q1 * cos(q3) +
(m2 * L * omega**2 * cos(q3)**2 - m1 * u2**2 / L) / (m1 + m2))
u2d_expected = -g * cos(q3) - omega**2 * q1 * sin(q3) + u1 * u2 / L
assert expand(trigsimp(dyn_eq_map[u1d] - u1d_expected)) == 0
assert expand(trigsimp(dyn_eq_map[u2d] - u2d_expected)) == 0
from sympy.physics.mechanics import dot, dynamicsymbols, inertia, msprint
m, B11, B22, B33, B12, B23, B31 = symbols('m B11 B22 B33 B12 B23 B31')
q1, q2, q3, q4, q5, q6 = dynamicsymbols('q1:7')
# reference frames
A = ReferenceFrame('A')
B = A.orientnew('B', 'body', [q4, q5, q6], 'xyz')
omega = B.ang_vel_in(A)
# points B*, O
pB_star = Point('B*')
pO = pB_star.locatenew('O', q1*B.x + q2*B.y + q3*B.z)
pO.set_vel(A, 0)
pO.set_vel(B, 0)
pB_star.v2pt_theory(pO, A, B)
# rigidbody B
I_B_O = inertia(B, B11, B22, B33, B12, B23, B31)
rbB = RigidBody('rbB', pB_star, B, m, (I_B_O, pO))
# kinetic energy
K = rbB.kinetic_energy(A)
print('K = {0}'.format(msprint(trigsimp(K))))
K_expected = dot(dot(omega, I_B_O), omega)/2
print('K_expected = 1/2*omega*I*omega = {0}'.format(
msprint(trigsimp(K_expected))))
assert expand(expand_trig(K - K_expected)) == 0
