def VP_RR(coord1, coord2, coord3):
"""
Parameters
----------
coord1 : coord del vector 1 VP
coord2 : coord del vector 2 VP
coord3 : coord del vector 3 VP
Returns
-------
vectores primitivos de la red RR
para 3D
sirve para 2D: agregar + z en a1 y a2
para coord3 = [0,0,1], ignorar b3
"""
dim1, dim2, dim3 = len(coord1), len(coord2), len(coord3)
if dim1 == dim2 == dim3:
dim = dim1
else:
raise TypeError('Error:Dimension de coordenadas diferentes')
a1 = Vector(coord1, dim)
a2 = Vector(coord2, dim)
a3 = Vector(coord3, dim)
vol = Volumen_CP(a1, a2, a3)
b1 = 2 * pi * cross(a2, a3) / vol
b2 = 2 * pi * cross(a3, a1) / vol
b3 = 2 * pi * cross(a1, a2) / vol
return b1, b2, b3
def test_cross():
assert cross(A.x, A.x) == 0
assert cross(A.x, A.y) == A.z
assert cross(A.x, A.z) == -A.y
assert cross(A.y, A.x) == -A.z
assert cross(A.y, A.y) == 0
assert cross(A.y, A.z) == A.x
assert cross(A.z, A.x) == A.y
assert cross(A.z, A.y) == -A.x
assert cross(A.z, A.z) == 0
def test_cross():
assert cross(A.x, A.x) == 0
assert cross(A.x, A.y) == A.z
assert cross(A.x, A.z) == -A.y
assert cross(A.y, A.x) == -A.z
assert cross(A.y, A.y) == 0
assert cross(A.y, A.z) == A.x
assert cross(A.z, A.x) == A.y
assert cross(A.z, A.y) == -A.x
assert cross(A.z, A.z) == 0
def inertia_torque(rb, frame, kde_map=None, constraint_map=None, uaux=None):
# get central inertia
# I_S/O = I_S/S* + I_S*/O
I = rb.inertia[0] - inertia_of_point_mass(
rb.mass, rb.masscenter.pos_from(rb.inertia[1]), rb.frame)
alpha = rb.frame.ang_acc_in(frame)
omega = rb.frame.ang_vel_in(frame)
if not isinstance(alpha, Vector) and alpha == 0:
alpha = Vector([])
if not isinstance(omega, Vector) and omega == 0:
omega = Vector([])
if uaux is not None:
# auxilliary speeds do not change alpha, omega
# use doit() to evaluate terms such as
# Derivative(0, t) to 0.
uaux_zero = dict(zip(uaux, [0] * len(uaux)))
alpha = subs(alpha, uaux_zero)
omega = subs(omega, uaux_zero)
if kde_map is not None:
alpha = subs(alpha, kde_map)
omega = subs(omega, kde_map)
if constraint_map is not None:
alpha = subs(alpha, constraint_map)
omega = subs(omega, constraint_map)
return -dot(alpha, I) - dot(cross(omega, I), omega)
def inertia_torque(rb, frame, kde_map=None, constraint_map=None, uaux=None):
# get central inertia
# I_S/O = I_S/S* + I_S*/O
I = rb.inertia[0] - inertia_of_point_mass(rb.mass,
rb.masscenter.pos_from(rb.inertia[1]), rb.frame)
alpha = rb.frame.ang_acc_in(frame)
omega = rb.frame.ang_vel_in(frame)
if not isinstance(alpha, Vector) and alpha == 0:
alpha = Vector([])
if not isinstance(omega, Vector) and omega == 0:
omega = Vector([])
if uaux is not None:
# auxilliary speeds do not change alpha, omega
# use doit() to evaluate terms such as
# Derivative(0, t) to 0.
uaux_zero = dict(zip(uaux, [0] * len(uaux)))
alpha = subs(alpha, uaux_zero)
omega = subs(omega, uaux_zero)
if kde_map is not None:
alpha = subs(alpha, kde_map)
omega = subs(omega, kde_map)
if constraint_map is not None:
alpha = subs(alpha, constraint_map)
omega = subs(omega, constraint_map)
return -dot(alpha, I) - dot(cross(omega, I), omega)
def Volumen_CP(a1, a2, a3):
"""
Parameters
----------
a1 : 1 vector de la base VP
a2 : 2 vector de la base VP
a3 : 3 vector de la base VP
Returns
-------
volumen de 1 CP
"""
return dot(a1, cross(a2, a3))
from sympy import acos, pi, symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import cross, dot
# vectors A, B have equal magnitude 10 N
alpha = 10 # [N]
N = ReferenceFrame('N')
pO = Point('O')
pS = pO.locatenew('S', 10*N.x + 5*N.z)
pR = pO.locatenew('R', 10*N.x + 12*N.y)
pQ = pO.locatenew('Q', 12*N.y + 10*N.z)
pP = pO.locatenew('P', 4*N.x + 7*N.z)
A = alpha * pQ.pos_from(pP).normalize()
B = alpha * pS.pos_from(pR).normalize()
R = A + B
M = cross(pP.pos_from(pO), A) + cross(pR.pos_from(pO), B)
Ms = dot(R, M) * R / dot(R, R)
ps = cross(R, M) / dot(R, R)
# Replace set S (vectors A, B) with a wrench consisting of the resultant of S,
# placed on the central axis of S and a couple whose torque is equal to S's
# moment of minimum magnitude.
r = R
C = Ms
print("|C| = {0}, {1}".format(C.magnitude(), C.magnitude().n()))
# Since the bound vector r is placed on the central axis of S, |p*| gives the
# distance from point O to r.
print("|p*| = {0}, {1}".format(ps.magnitude(), ps.magnitude().n()))
# Determinant of the Jacobian of the mapping from a, b, c to x, y, z
# See Wikipedia for a lucid explanation of why we must comput this Jacobian:
# http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant#Further_examples
J = Matrix([dot(p, uv) for uv in A]).transpose().jacobian([phi, theta, s])
dv = J.det().trigsimp() # Need to ensure this is positive
print("dx*dy*dz = {0}*dphi*dtheta*ds".format(dv))
# We want to compute the inertia scalars of the torus relative to it's mass
# center, for the following six unit vector pairs
unit_vector_pairs = [(A.x, A.x), (A.y, A.y), (A.z, A.z), (A.x, A.y),
(A.y, A.z), (A.x, A.z)]
# Calculate the six unique inertia scalars using equation 3.3.9 of Kane &
# Levinson, 1985.
inertia_scalars = []
for n_a, n_b in unit_vector_pairs:
# Integrand of Equation 3.3.9
integrand = rho * dot(cross(p, n_a), cross(p, n_b)) * dv
# Compute the integral by integrating over the whole volume of the tours
I_ab = integrate(
integrate(integrate(integrand, (phi, 0, 2 * pi)), (theta, 0, 2 * pi)),
(s, 0, r))
inertia_scalars.append(I_ab)
# Create an inertia dyad from the list of inertia scalars
I_A_O = inertia(A, *inertia_scalars)
print("Inertia of torus about mass center = {0}".format(I_A_O))
k, n = symbols('k n', real=True, positive=True)
N = ReferenceFrame('N')
# calculate right triangle density
V = b * c / 2
rho = m / V
# Kane 1985 lists scalars of inertia I_11, I_12, I_22 for a right triangular
# plate, but not I_3x.
pO = Point('O')
pCs = pO.locatenew('C*', b / 3 * N.x + c / 3 * N.y)
pP = pO.locatenew('P', x * N.x + y * N.y)
p = pP.pos_from(pCs)
I_3 = rho * integrate_v(
integrate_v(cross(p, cross(N.z, p)), N, (x, 0, b * (1 - y / c))), N,
(y, 0, c))
print("I_3 = {0}".format(I_3))
# inertia for a right triangle given ReferenceFrame, height b, base c, mass
inertia_rt = lambda rf, b_, c_, m_: inertia(
rf, m_ * c_**2 / 18, m_ * b_**2 / 18, dot(I_3, N.z), m_ * b_ * c_ / 36,
dot(I_3, N.y), dot(I_3, N.x)).subs({
m: m_,
b: b_,
c: c_
})
theta = (30 + 90) * pi / 180
N2 = N.orientnew('N2', 'Axis', [theta, N.x])
# Rattleback ellipsoid center location, see:
# "Realistic mathematical modeling of the rattleback", Kane, Thomas R. and
# David A. Levinson, 1982, International Journal of Non-Linear Mechanics
#mu = [dot(rk, Y.z) for rk in R]
#eps = sqrt((a*mu[0])**2 + (b*mu[1])**2 + (c*mu[2])**2)
O = P.locatenew('O', -rx * R.x - rx * R.y - rx * R.z)
RO = O.locatenew('RO', d * R.x + e * R.y + f * R.z)
w_r_n = wx * R.x + wy * R.y + wz * R.z
omega_dict = {
wx: dot(qd[0] * Y.z, R.x),
wy: dot(qd[0] * Y.z, R.y),
wz: dot(qd[0] * Y.z, R.z)
}
v_ro_n = cross(w_r_n, RO.pos_from(P))
a_ro_n = cross(w_r_n, v_ro_n)
mu_dict = {mu_x: dot(R.x, Y.z), mu_y: dot(R.y, Y.z), mu_z: dot(R.z, Y.z)}
#F_RO = m*g*Y.z + Fx*Y.x + Fy*Y.y + Fz*Y.z
#F_RO = Fx*R.x + Fy*R.y + Fz*R.z + m*g*Y.z
F_RO = Fx * R.x + Fy * R.y + Fz * R.z + m * g * (mu_x * R.x + mu_y * R.y +
mu_z * R.z)
newton_eqn = F_RO - m * a_ro_n
force_scalars = solve([dot(newton_eqn, uv).expand() for uv in R], [Fx, Fy, Fz])
#print("v_ro_n =", v_ro_n)
#print("a_ro_n =", a_ro_n)
#print("Force scalars =", force_scalars)
euler_eqn = cross(P.pos_from(RO), F_RO) - cross(w_r_n, dot(I, w_r_n))
#print(euler_eqn)
# Angular velocity using u's as body fixed measure numbers of angular velocity
R.set_ang_vel(N, u[0]*R.x + u[1]*R.y + u[2]*R.z)
# Rattleback ground contact point
P = Point('P')
P.set_vel(N, ua[0]*Y.x + ua[1]*Y.y + ua[2]*Y.z)
# Rattleback ellipsoid center location, see:
# "Realistic mathematical modeling of the rattleback", Kane, Thomas R. and
# David A. Levinson, 1982, International Journal of Non-Linear Mechanics
mu = [dot(rk, Y.z) for rk in R]
eps = sqrt((a*mu[0])**2 + (b*mu[1])**2 + (c*mu[2])**2)
O = P.locatenew('O', -a*a*mu[0]/eps*R.x
-b*b*mu[1]/eps*R.y
-c*c*mu[2]/eps*R.z)
O.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), O.pos_from(P)))
# Mass center position and velocity
RO = O.locatenew('RO', d*R.x + e*R.y + f*R.z)
RO.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), RO.pos_from(P)))
qd_rhs = [(u[2]*cos(q[2]) - u[0]*sin(q[2]))/cos(q[1]),
u[0]*cos(q[2]) + u[2]*sin(q[2]),
u[1] + tan(q[1])*(u[0]*sin(q[2]) - u[2]*cos(q[2])),
dot(P.pos_from(O).diff(t, R), N.x),
dot(P.pos_from(O).diff(t, R), N.y)]
# Partial angular velocities and partial velocities
partial_w = [R.ang_vel_in(N).diff(ui, N) for ui in u + ua]
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
partial_v_RO = [RO.vel(N).diff(ui, N) for ui in u + ua]
def test_operator_match():
"""Test that the output of dot, cross, outer functions match
operator behavior.
"""
A = ReferenceFrame("A")
v = A.x + A.y
d = v | v
zerov = Vector(0)
zerod = Dyadic(0)
# dot products
assert d & d == dot(d, d)
assert d & zerod == dot(d, zerod)
assert zerod & d == dot(zerod, d)
assert d & v == dot(d, v)
assert v & d == dot(v, d)
assert d & zerov == dot(d, zerov)
assert zerov & d == dot(zerov, d)
raises(TypeError, lambda: dot(d, S(0)))
raises(TypeError, lambda: dot(S(0), d))
raises(TypeError, lambda: dot(d, 0))
raises(TypeError, lambda: dot(0, d))
assert v & v == dot(v, v)
assert v & zerov == dot(v, zerov)
assert zerov & v == dot(zerov, v)
raises(TypeError, lambda: dot(v, S(0)))
raises(TypeError, lambda: dot(S(0), v))
raises(TypeError, lambda: dot(v, 0))
raises(TypeError, lambda: dot(0, v))
# cross products
raises(TypeError, lambda: cross(d, d))
raises(TypeError, lambda: cross(d, zerod))
raises(TypeError, lambda: cross(zerod, d))
assert d ^ v == cross(d, v)
assert v ^ d == cross(v, d)
assert d ^ zerov == cross(d, zerov)
assert zerov ^ d == cross(zerov, d)
assert zerov ^ d == cross(zerov, d)
raises(TypeError, lambda: cross(d, S(0)))
raises(TypeError, lambda: cross(S(0), d))
raises(TypeError, lambda: cross(d, 0))
raises(TypeError, lambda: cross(0, d))
assert v ^ v == cross(v, v)
assert v ^ zerov == cross(v, zerov)
assert zerov ^ v == cross(zerov, v)
raises(TypeError, lambda: cross(v, S(0)))
raises(TypeError, lambda: cross(S(0), v))
raises(TypeError, lambda: cross(v, 0))
raises(TypeError, lambda: cross(0, v))
# outer products
raises(TypeError, lambda: outer(d, d))
raises(TypeError, lambda: outer(d, zerod))
raises(TypeError, lambda: outer(zerod, d))
raises(TypeError, lambda: outer(d, v))
raises(TypeError, lambda: outer(v, d))
raises(TypeError, lambda: outer(d, zerov))
raises(TypeError, lambda: outer(zerov, d))
raises(TypeError, lambda: outer(zerov, d))
raises(TypeError, lambda: outer(d, S(0)))
raises(TypeError, lambda: outer(S(0), d))
raises(TypeError, lambda: outer(d, 0))
raises(TypeError, lambda: outer(0, d))
assert v | v == outer(v, v)
assert v | zerov == outer(v, zerov)
assert zerov | v == outer(zerov, v)
raises(TypeError, lambda: outer(v, S(0)))
raises(TypeError, lambda: outer(S(0), v))
raises(TypeError, lambda: outer(v, 0))
raises(TypeError, lambda: outer(0, v))
def test_linearize_rolling_disc_kane():
# Symbols for time and constant parameters
t, r, m, g, v = symbols('t r m g v')
# Configuration variables and their time derivatives
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
# Generalized speeds and their time derivatives
u = dynamicsymbols('u:6')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
# Reference frames
N = ReferenceFrame('N') # Inertial frame
NO = Point('NO') # Inertial origin
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center
# Disc angular velocity in N expressed using time derivatives of coordinates
w_c_n_qd = C.ang_vel_in(N)
w_b_n_qd = B.ang_vel_in(N)
# Inertial angular velocity and angular acceleration of disc fixed frame
C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z)
# Disc center velocity in N expressed using time derivatives of coordinates
v_co_n_qd = CO.pos_from(NO).dt(N)
# Disc center velocity in N expressed using generalized speeds
CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z)
# Disc Ground Contact Point
P = CO.locatenew('P', r*B.z)
P.v2pt_theory(CO, N, C)
# Configuration constraint
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
# Velocity level constraints
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
# Set angular velocity of remaining frames
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Active forces
F_CO = m*g*A.z
# Create inertia dyadic of disc C about point CO
I = (m * r**2) / 4
J = (m * r**2) / 2
I_C_CO = inertia(C, I, J, I)
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
BL = [Disc]
FL = [(CO, F_CO)]
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs,
q_dependent=[q6], configuration_constraints=f_c,
u_dependent=[u4, u5, u6], velocity_constraints=f_v)
(fr, fr_star) = KM.kanes_equations(FL, BL)
# Test generalized form equations
linearizer = KM.to_linearizer()
assert linearizer.f_c == f_c
assert linearizer.f_v == f_v
assert linearizer.f_a == f_v.diff(t)
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
for qi in qd:
assert sol[qi] == qdots[qi]
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0])
# Perform the linearization
# Precomputed operating point
q_op = {q6: -r*cos(q2)}
u_op = {u1: 0,
u2: sin(q2)*q1d + q3d,
u3: cos(q2)*q1d,
u4: -r*(sin(q2)*q1d + q3d)*cos(q3),
u5: 0,
u6: -r*(sin(q2)*q1d + q3d)*sin(q3)}
qd_op = {q2d: 0,
q4d: -r*(sin(q2)*q1d + q3d)*cos(q1),
q5d: -r*(sin(q2)*q1d + q3d)*sin(q1),
q6d: 0}
ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5,
u2d: 0,
u3d: 0,
u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2),
u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5),
u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)}
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True)
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
# Precomputed solution
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
[-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0],
[0, 4/5, 0, 0, 0, 0, 0, 6*q3d/5],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, -2*q3d, 0, 0]])
B_sol = Matrix([])
# Check that linearization is correct
assert A.subs(upright_nominal) == A_sol
assert B.subs(upright_nominal) == B_sol
# Check eigenvalues at critical speed are all zero:
assert A.subs(upright_nominal).subs(q3d, 1/sqrt(3)).eigenvals() == {0: 8}
# 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)
print('\nEx8.20')
# inertia torque for a rigid body:
# T* = -dot(alpha, I) - dot(cross(omega, I), omega)
T_star = lambda rb, F: (-dot(rb.frame.ang_acc_in(F), rb.inertia[0]) -
dot(cross(rb.frame.ang_vel_in(F), rb.inertia[0]),
rb.frame.ang_vel_in(F)))
for rb in bodies:
print('\nT* ({0}) = {1}'.format(rb, msprint(T_star(rb, E).subs(kde_map))))
print('\nEx8.21')
system = [getattr(b, i) for b in bodies for i in ['frame', 'masscenter']]
partials = partial_velocities(system, [u1, u2, u3], E, kde_map)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map)
for i, f in enumerate(Fr_star, 1):
print("\nF*{0} = {1}".format(i, msprint(simplify(f))))
R, V = symbols('R, V', positive=True)
r1 = R * (C.x + C.y + C.z)
v1 = V * (C.x - 2 * C.z)
# Consola: 'r1'
# Consola: 'v1'
from sympy.physics.mechanics import dot, cross
r1.dot(v1)
dot(r1, v1)
# Consola: 'r1 & v1' -> -RV
r1.cross(v1)
cross(r1, v1)
# Consola: 'r1 ^ v1'
# Obtener la norma de los vectores con 'magnitude' y normalizalos con 'normalize'
# Consola: '(r1 ^ v1).magnitude()'
# Consola: '(r1 ^ v1).normalize()'
# MOVIMIENTO RELATIVO
A1 = IJKReferenceFrame("1")
A0 = IJKReferenceFrame("0")
phi = symbols('phi')
A0.orient(
A1, 'Axis', [phi, A1.z]
def calc_inertia_vec(rho, p, n_a, N, iv):
integrand = rho * cross(p, cross(n_a, p))
return sum(simplify(integrate(dot(integrand, n), iv)) * n
for n in N)
# 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)
print('\nEx8.20')
# inertia torque for a rigid body:
# T* = -dot(alpha, I) - dot(cross(omega, I), omega)
T_star = lambda rb, F: (-dot(rb.frame.ang_acc_in(F), rb.inertia[0]) - dot(
cross(rb.frame.ang_vel_in(F), rb.inertia[0]), rb.frame.ang_vel_in(F)))
for rb in bodies:
print('\nT* ({0}) = {1}'.format(rb, msprint(T_star(rb, E).subs(kde_map))))
print('\nEx8.21')
system = [getattr(b, i) for b in bodies for i in ['frame', 'masscenter']]
partials = partial_velocities(system, [u1, u2, u3], E, kde_map)
Fr_star, _ = generalized_inertia_forces(partials, bodies, kde_map)
for i, f in enumerate(Fr_star, 1):
print("\nF*{0} = {1}".format(i, msprint(simplify(f))))
q2d - u2,
q3d - u3,
q4d - u4])
qd_kd = sym.solve(kindiffs, qd)
# Values of generalized speeds during a steady turning
steady_conditions = sym.solve(kindiffs.subs({q2d:0, q3d:0, q4d:0}), [u1, u2, u3, u4])
steady_conditions.update({q2d:0, q3d:0, q4d:0})
# Special unit vectors:
# g_3: direction along front wheel radius.
# long_v: longitudinal direction of front wheel.
# lateral_v: lateral direction of front wheel.
g_3 = (mec.express(A['3'], E) - mec.dot(E['2'], A['3'])*E['2']).normalize()
# OR g_3 = E['2'].cross(A['3']).cross(E['2']).normalize()
long_v = mec.cross (E['2'], A['3']).normalize()
lateral_v = mec.cross (A['3'], long_v)
# Points and velocities:
# dn: rear wheel contact point.
# do: rear wheel center.
# rtr: rear tire radius.
# fn: front wheel contact point.
# fo: front wheel center.
# ftr: front tire radius
# co: rear frame center.
# eo: front frame center.
# ce: steer axis point.
# SAF: steer axis foot.
## Rear
dn = mec.Point('dn')
def test_operator_match():
"""Test that the output of dot, cross, outer functions match
operator behavior.
"""
A = ReferenceFrame('A')
v = A.x + A.y
d = v | v
zerov = Vector(0)
zerod = Dyadic(0)
# dot products
assert d & d == dot(d, d)
assert d & zerod == dot(d, zerod)
assert zerod & d == dot(zerod, d)
assert d & v == dot(d, v)
assert v & d == dot(v, d)
assert d & zerov == dot(d, zerov)
assert zerov & d == dot(zerov, d)
raises(TypeError, lambda: dot(d, S(0)))
raises(TypeError, lambda: dot(S(0), d))
raises(TypeError, lambda: dot(d, 0))
raises(TypeError, lambda: dot(0, d))
assert v & v == dot(v, v)
assert v & zerov == dot(v, zerov)
assert zerov & v == dot(zerov, v)
raises(TypeError, lambda: dot(v, S(0)))
raises(TypeError, lambda: dot(S(0), v))
raises(TypeError, lambda: dot(v, 0))
raises(TypeError, lambda: dot(0, v))
# cross products
raises(TypeError, lambda: cross(d, d))
raises(TypeError, lambda: cross(d, zerod))
raises(TypeError, lambda: cross(zerod, d))
assert d ^ v == cross(d, v)
assert v ^ d == cross(v, d)
assert d ^ zerov == cross(d, zerov)
assert zerov ^ d == cross(zerov, d)
assert zerov ^ d == cross(zerov, d)
raises(TypeError, lambda: cross(d, S(0)))
raises(TypeError, lambda: cross(S(0), d))
raises(TypeError, lambda: cross(d, 0))
raises(TypeError, lambda: cross(0, d))
assert v ^ v == cross(v, v)
assert v ^ zerov == cross(v, zerov)
assert zerov ^ v == cross(zerov, v)
raises(TypeError, lambda: cross(v, S(0)))
raises(TypeError, lambda: cross(S(0), v))
raises(TypeError, lambda: cross(v, 0))
raises(TypeError, lambda: cross(0, v))
# outer products
raises(TypeError, lambda: outer(d, d))
raises(TypeError, lambda: outer(d, zerod))
raises(TypeError, lambda: outer(zerod, d))
raises(TypeError, lambda: outer(d, v))
raises(TypeError, lambda: outer(v, d))
raises(TypeError, lambda: outer(d, zerov))
raises(TypeError, lambda: outer(zerov, d))
raises(TypeError, lambda: outer(zerov, d))
raises(TypeError, lambda: outer(d, S(0)))
raises(TypeError, lambda: outer(S(0), d))
raises(TypeError, lambda: outer(d, 0))
raises(TypeError, lambda: outer(0, d))
assert v | v == outer(v, v)
assert v | zerov == outer(v, zerov)
assert zerov | v == outer(zerov, v)
raises(TypeError, lambda: outer(v, S(0)))
raises(TypeError, lambda: outer(S(0), v))
raises(TypeError, lambda: outer(v, 0))
raises(TypeError, lambda: outer(0, v))
x, y, r = symbols('x y r', real=True)
k, n = symbols('k n', real=True, positive=True)
N = ReferenceFrame('N')
# calculate right triangle density
V = b * c / 2
rho = m / V
# Kane 1985 lists scalars of inertia I_11, I_12, I_22 for a right triangular
# plate, but not I_3x.
pO = Point('O')
pCs = pO.locatenew('C*', b/3*N.x + c/3*N.y)
pP = pO.locatenew('P', x*N.x + y*N.y)
p = pP.pos_from(pCs)
I_3 = rho * integrate_v(integrate_v(cross(p, cross(N.z, p)),
N, (x, 0, b*(1 - y/c))),
N, (y, 0, c))
print("I_3 = {0}".format(I_3))
# inertia for a right triangle given ReferenceFrame, height b, base c, mass
inertia_rt = lambda rf, b_, c_, m_: inertia(rf,
m_*c_**2/18,
m_*b_**2/18,
dot(I_3, N.z),
m_*b_*c_/36,
dot(I_3, N.y),
dot(I_3, N.x)).subs({m:m_, b:b_, c:c_})
theta = (30 + 90) * pi / 180
N2 = N.orientnew('N2', 'Axis', [theta, N.x])
return curl(a, R)
if __name__ == '__main__':
# базис пространства
R = ReferenceFrame('R')
# (y^2*z,-xy,z^3) – вектор, функции в координатах
a = R[1]**2 * R[2] * R.x - R[0] * R[1] * R.y + R[2]**3 * R.z
# (x^2*z,-xy,z)
b = R[0]**2 * R[2] * R.x - R[0] * R[1] * R.y + R[2] * R.z
# (y*z,-xy,xy)
c = R[1] * R[2] * R.x - R[0] * R[1] * R.y + R[0] * R[1] * R.z
# [a, b] x rot(C)
res1 = cross(a, b).dot(rot(c))
res1 = res1.expand().simplify()
# b x (div A) * c - a x (div B) * c
res2 = b.dot(div(a) * c) - a.dot(div(b) * c)
res2 = res2.expand().simplify()
# выражения, которые получились
print(res1.evalf(subs={
"R_x": 1,
"R_y": 1,
"R_z": 1
}), res2.evalf(subs={
"R_x": 1,
"R_y": 1,
"R_z": 1
def test_cross_different_frames():
assert cross(N.x, A.x) == sin(q1)*A.z
assert cross(N.x, A.y) == cos(q1)*A.z
assert cross(N.x, A.z) == -sin(q1)*A.x-cos(q1)*A.y
assert cross(N.y, A.x) == -cos(q1)*A.z
assert cross(N.y, A.y) == sin(q1)*A.z
assert cross(N.y, A.z) == cos(q1)*A.x-sin(q1)*A.y
assert cross(N.z, A.x) == A.y
assert cross(N.z, A.y) == -A.x
assert cross(N.z, A.z) == 0
assert cross(N.x, A.x) == sin(q1)*A.z
assert cross(N.x, A.y) == cos(q1)*A.z
assert cross(N.x, A.x + A.y) == sin(q1)*A.z + cos(q1)*A.z
assert cross(A.x + A.y, N.x) == -sin(q1)*A.z - cos(q1)*A.z
assert cross(A.x, C.x) == sin(q3)*C.y
assert cross(A.x, C.y) == -sin(q3)*C.x + cos(q3)*C.z
assert cross(A.x, C.z) == -cos(q3)*C.y
assert cross(C.x, A.x) == -sin(q3)*C.y
assert cross(C.y, A.x) == sin(q3)*C.x - cos(q3)*C.z
assert cross(C.z, A.x) == cos(q3)*C.y
def vector_mul(a, b):
return cross(a, b)
x, y = me.dynamicsymbols('x y')
xd, yd = me.dynamicsymbols('x y', 1)
e = sm.cos(x)+sm.sin(x)+sm.tan(x)+sm.cosh(x)+sm.sinh(x)+sm.tanh(x)+sm.acos(x)+sm.asin(x)+sm.atan(x)+sm.log(x)+sm.exp(x)+sm.sqrt(x)+sm.factorial(x)+sm.ceiling(x)+sm.floor(x)+sm.sign(x)
e = (x)**2+sm.log(x, 10)
a = sm.Abs(-1*1)+int(1.5)+round(1.9)
e1 = 2*x+3*y
e2 = x+y
am = sm.Matrix([e1.expand().coeff(x), e1.expand().coeff(y), e2.expand().coeff(x), e2.expand().coeff(y)]).reshape(2, 2)
b = (e1).expand().coeff(x)
c = (e2).expand().coeff(y)
d1 = (e1).collect(x).coeff(x,0)
d2 = (e1).collect(x).coeff(x,1)
fm = sm.Matrix([i.collect(x)for i in sm.Matrix([e1,e2]).reshape(1, 2)]).reshape((sm.Matrix([e1,e2]).reshape(1, 2)).shape[0], (sm.Matrix([e1,e2]).reshape(1, 2)).shape[1])
f = (e1).collect(y)
g = (e1).subs({x:2*x})
gm = sm.Matrix([i.subs({x:3}) for i in sm.Matrix([e1,e2]).reshape(2, 1)]).reshape((sm.Matrix([e1,e2]).reshape(2, 1)).shape[0], (sm.Matrix([e1,e2]).reshape(2, 1)).shape[1])
frame_a=me.ReferenceFrame('a')
frame_b=me.ReferenceFrame('b')
theta = me.dynamicsymbols('theta')
frame_b.orient(frame_a, 'Axis', [theta, frame_a.z])
v1=2*frame_a.x-3*frame_a.y+frame_a.z
v2=frame_b.x+frame_b.y+frame_b.z
a = me.dot(v1, v2)
bm = sm.Matrix([me.dot(v1, v2),me.dot(v1, 2*v2)]).reshape(2, 1)
c=me.cross(v1, v2)
d = 2*v1.magnitude()+3*v1.magnitude()
dyadic=me.outer(3*frame_a.x, frame_a.x)+me.outer(frame_a.y, frame_a.y)+me.outer(2*frame_a.z, frame_a.z)
am = (dyadic).to_matrix(frame_b)
m = sm.Matrix([1,2,3]).reshape(3, 1)
v=m[0]*frame_a.x +m[1]*frame_a.y +m[2]*frame_a.z
d1 = (e1).collect(x).coeff(x, 0)
d2 = (e1).collect(x).coeff(x, 1)
fm = sm.Matrix([i.collect(x) for i in sm.Matrix([e1, e2]).reshape(1, 2)]).reshape(
(sm.Matrix([e1, e2]).reshape(1, 2)).shape[0],
(sm.Matrix([e1, e2]).reshape(1, 2)).shape[1],
)
f = (e1).collect(y)
g = (e1).subs({x: 2 * x})
gm = sm.Matrix([i.subs({x: 3}) for i in sm.Matrix([e1, e2]).reshape(2, 1)]).reshape(
(sm.Matrix([e1, e2]).reshape(2, 1)).shape[0],
(sm.Matrix([e1, e2]).reshape(2, 1)).shape[1],
)
frame_a = me.ReferenceFrame("a")
frame_b = me.ReferenceFrame("b")
theta = me.dynamicsymbols("theta")
frame_b.orient(frame_a, "Axis", [theta, frame_a.z])
v1 = 2 * frame_a.x - 3 * frame_a.y + frame_a.z
v2 = frame_b.x + frame_b.y + frame_b.z
a = me.dot(v1, v2)
bm = sm.Matrix([me.dot(v1, v2), me.dot(v1, 2 * v2)]).reshape(2, 1)
c = me.cross(v1, v2)
d = 2 * v1.magnitude() + 3 * v1.magnitude()
dyadic = (
me.outer(3 * frame_a.x, frame_a.x)
+ me.outer(frame_a.y, frame_a.y)
+ me.outer(2 * frame_a.z, frame_a.z)
)
am = (dyadic).to_matrix(frame_b)
m = sm.Matrix([1, 2, 3]).reshape(3, 1)
v = m[0] * frame_a.x + m[1] * frame_a.y + m[2] * frame_a.z
# '*' indicates scaling multiplication
A=ReferenceFrame('A')
u=3*A.x+4*A.z
print u
# 3*A.x + 4*A.z
# vectors can be from a combination of reference frames
B=ReferenceFrame('B')
v=A.z-A.x+4*B.x
print v
# - A.x + A.z + 4*B.x
# The dot product can be called in multiple fashions
v = A.z - A.x + A.y
print dot(u,v)
print u.dot(v)
print u & v
# 1
# 1
# 1
# Similarily the cross product
print cross(u,v)
print u.cross(v)
print u ^ v
# - 4*A.x - 7*A.y + 3*A.z
# - 4*A.x - 7*A.y + 3*A.z
# - 4*A.x - 7*A.y + 3*A.z
# The magnitude of the vector
print u.magnitude()
# 5
# Normalizing the vector will give a unit vector in the same direction
print u.normalize()
# 3/5*A.x + 4/5*A.z
# Rattleback ground contact point
P = Point('P')
# Rattleback ellipsoid center location, see:
# "Realistic mathematical modeling of the rattleback", Kane, Thomas R. and
# David A. Levinson, 1982, International Journal of Non-Linear Mechanics
#mu = [dot(rk, Y.z) for rk in R]
#eps = sqrt((a*mu[0])**2 + (b*mu[1])**2 + (c*mu[2])**2)
O = P.locatenew('O', -rx*R.x - rx*R.y - rx*R.z)
RO = O.locatenew('RO', d*R.x + e*R.y + f*R.z)
w_r_n = wx*R.x + wy*R.y + wz*R.z
omega_dict = {wx: dot(qd[0]*Y.z, R.x),
wy: dot(qd[0]*Y.z, R.y),
wz: dot(qd[0]*Y.z, R.z)}
v_ro_n = cross(w_r_n, RO.pos_from(P))
a_ro_n = cross(w_r_n, v_ro_n)
mu_dict = {mu_x: dot(R.x, Y.z), mu_y: dot(R.y, Y.z), mu_z: dot(R.z, Y.z)}
#F_RO = m*g*Y.z + Fx*Y.x + Fy*Y.y + Fz*Y.z
#F_RO = Fx*R.x + Fy*R.y + Fz*R.z + m*g*Y.z
F_RO = Fx*R.x + Fy*R.y + Fz*R.z + m*g*(mu_x*R.x + mu_y*R.y + mu_z*R.z)
newton_eqn = F_RO - m*a_ro_n
force_scalars = solve([dot(newton_eqn, uv).expand() for uv in R], [Fx, Fy, Fz])
#print("v_ro_n =", v_ro_n)
#print("a_ro_n =", a_ro_n)
#print("Force scalars =", force_scalars)
euler_eqn = cross(P.pos_from(RO), F_RO) - cross(w_r_n, dot(I, w_r_n))
#print(euler_eqn)
from sympy import symbols
from sympy.physics.mechanics import ReferenceFrame
from sympy.physics.mechanics import cross, dot, dynamicsymbols, inertia
from util import msprint
print("\n part a")
Ia, Ib, Ic, Iab, Ibc, Ica, t = symbols('Ia Ib Ic Iab Ibc Ica t')
omega = dynamicsymbols('omega')
N = ReferenceFrame('N')
# I = (I11 * N.x + I12 * N.y + I13 * N.z) N.x +
# (I21 * N.x + I22 * N.y + I23 * N.z) N.y +
# (I31 * N.x + I32 * N.y + I33 * N.z) N.z
# definition of T* is:
# T* = -dot(alpha, I) - dot(cross(omega, I), omega)
ang_vel = omega * N.x
I = inertia(N, Ia, Ib, Ic, Iab, Ibc, Ica)
T_star = -dot(ang_vel.diff(t, N), I) - dot(cross(ang_vel, I), ang_vel)
print(msprint(T_star))
print("\n part b")
I11, I22, I33, I12, I23, I31 = symbols('I11 I22 I33 I12 I23 I31')
omega1, omega2, omega3 = dynamicsymbols('omega1:4')
B = ReferenceFrame('B')
ang_vel = omega1 * B.x + omega2 * B.y + omega3 * B.z
I = inertia(B, I11, I22, I33, I12, I23, I31)
T_star = -dot(ang_vel.diff(t, B), I) - dot(cross(ang_vel, I), ang_vel)
print(msprint(T_star))
body_r_cm,
body_r_f,
sm.symbols("m"),
(me.outer(body_r_f.x, body_r_f.x), body_r_cm),
)
point_po2.set_pos(particle_p1.point, c1 * frame_a.x)
v = 2 * point_po2.pos_from(particle_p1.point) + c2 * frame_a.y
frame_a.set_ang_vel(frame_n, c3 * frame_a.z)
v = 2 * frame_a.ang_vel_in(frame_n) + c2 * frame_a.y
body_r_f.set_ang_vel(frame_n, c3 * frame_a.z)
v = 2 * body_r_f.ang_vel_in(frame_n) + c2 * frame_a.y
frame_a.set_ang_acc(frame_n, (frame_a.ang_vel_in(frame_n)).dt(frame_a))
v = 2 * frame_a.ang_acc_in(frame_n) + c2 * frame_a.y
particle_p1.point.set_vel(frame_a, c1 * frame_a.x + c3 * frame_a.y)
body_r_cm.set_acc(frame_n, c2 * frame_a.y)
v_a = me.cross(body_r_cm.acc(frame_n), particle_p1.point.vel(frame_a))
x_b_c = v_a
x_b_d = 2 * x_b_c
a_b_c_d_e = x_b_d * 2
a_b_c = 2 * c1 * c2 * c3
a_b_c += 2 * c1
a_b_c = 3 * c1
q1, q2, u1, u2 = me.dynamicsymbols("q1 q2 u1 u2")
q1d, q2d, u1d, u2d = me.dynamicsymbols("q1 q2 u1 u2", 1)
x, y = me.dynamicsymbols("x y")
xd, yd = me.dynamicsymbols("x y", 1)
xd2, yd2 = me.dynamicsymbols("x y", 2)
yy = me.dynamicsymbols("yy")
yy = x * xd ** 2 + 1
m = sm.Matrix([[0]])
m[0] = 2 * x
# Determinant of the Jacobian of the mapping from a, b, c to x, y, z
# See Wikipedia for a lucid explanation of why we must comput this Jacobian:
# http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant#Further_examples
J = Matrix([dot(p, uv) for uv in A]).transpose().jacobian([phi, theta, s])
dv = J.det().trigsimp() # Need to ensure this is positive
print("dx*dy*dz = {0}*dphi*dtheta*ds".format(dv))
# We want to compute the inertia scalars of the torus relative to it's mass
# center, for the following six unit vector pairs
unit_vector_pairs = [(A.x, A.x), (A.y, A.y), (A.z, A.z),
(A.x, A.y), (A.y, A.z), (A.x, A.z)]
# Calculate the six unique inertia scalars using equation 3.3.9 of Kane &
# Levinson, 1985.
inertia_scalars = []
for n_a, n_b in unit_vector_pairs:
# Integrand of Equation 3.3.9
integrand = rho * dot(cross(p, n_a), cross(p, n_b)) * dv
# Compute the integral by integrating over the whole volume of the tours
I_ab = integrate(integrate(integrate(integrand,
(phi, 0, 2*pi)), (theta, 0, 2*pi)), (s, 0, r))
inertia_scalars.append(I_ab)
# Create an inertia dyad from the list of inertia scalars
I_A_O = inertia(A, *inertia_scalars)
print("Inertia of torus about mass center = {0}".format(I_A_O))
I = inertia(B, I1, I2, I3) # central inertia dyadic of B
# forces, torques due to set of gravitational forces γ
C11, C12, C13, C21, C22, C23, C31, C32, C33 = [dot(x, y) for x in A for y in B]
f = (
3
/ M
/ q4 ** 2
* (
(I1 * (1 - 3 * C11 ** 2) + I2 * (1 - 3 * C12 ** 2) + I3 * (1 - 3 * C13 ** 2)) / 2 * A.x
+ (I1 * C21 * C11 + I2 * C22 * C12 + I3 * C23 * C13) * A.y
+ (I1 * C31 * C11 + I2 * C32 * C12 + I3 * C33 * C13) * A.z
)
)
forces = [(pB_star, -G * m * M / q4 ** 2 * (A.x + f))]
torques = [(B, cross(3 * G * m / q4 ** 3 * A.x, dot(I, A.x)))]
partials = partial_velocities(zip(*forces + torques)[0], [u1, u2, u3, u4], A, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
V_gamma = potential_energy(Fr, [q1, q2, q3, q4], [u1, u2, u3, u4], kde_map)
print("V_γ = {0}".format(msprint(V_gamma.subs(q4, R))))
print("Setting C = 0, α1, α2, α3 = 0, α4 = oo")
V_gamma = V_gamma.subs(dict(zip(symbols("C α1 α2 α3 α4"), [0] * 4 + [oo])))
print("V_γ= {0}".format(msprint(V_gamma.subs(q4, R))))
V_gamma_expected = (
-3 * G * m / 2 / R ** 3 * ((I1 - I3) * sin(q2) ** 2 + (I1 - I2) * cos(q2) ** 2 * sin(q3) ** 2)
+ G * m * M / R
+ G * m / 2 / R ** 3 * (2 * I1 - I2 + I3)
)
# kinematic differential equations
kde = [x - y for x, y in zip([u1, u2, u3], map(B.ang_vel_in(A).dot, B))]
kde += [u4 - q4d]
kde_map = solve(kde, [q1d, q2d, q3d, q4d])
I = inertia(B, I1, I2, I3) # central inertia dyadic of B
# forces, torques due to set of gravitational forces γ
C11, C12, C13, C21, C22, C23, C31, C32, C33 = [dot(x, y) for x in A for y in B]
f = 3 / M / q4**2 * ((I1 * (1 - 3 * C11**2) + I2 * (1 - 3 * C12**2) + I3 *
(1 - 3 * C13**2)) / 2 * A.x +
(I1 * C21 * C11 + I2 * C22 * C12 + I3 * C23 * C13) * A.y +
(I1 * C31 * C11 + I2 * C32 * C12 + I3 * C33 * C13) * A.z)
forces = [(pB_star, -G * m * M / q4**2 * (A.x + f))]
torques = [(B, cross(3 * G * m / q4**3 * A.x, dot(I, A.x)))]
partials = partial_velocities(
zip(*forces + torques)[0], [u1, u2, u3, u4], A, kde_map)
Fr, _ = generalized_active_forces(partials, forces + torques)
V_gamma = potential_energy(Fr, [q1, q2, q3, q4], [u1, u2, u3, u4], kde_map)
print('V_γ = {0}'.format(msprint(V_gamma.subs(q4, R))))
print('Setting C = 0, α1, α2, α3 = 0, α4 = oo')
V_gamma = V_gamma.subs(dict(zip(symbols('C α1 α2 α3 α4'), [0] * 4 + [oo])))
print('V_γ= {0}'.format(msprint(V_gamma.subs(q4, R))))
V_gamma_expected = (-3 * G * m / 2 / R**3 *
((I1 - I3) * sin(q2)**2 +
(I1 - I2) * cos(q2)**2 * sin(q3)**2) + G * m * M / R +
G * m / 2 / R**3 * (2 * I1 - I2 + I3))
def calc_inertia_vec(rho, p, n_a, N, iv):
integrand = rho * cross(p, cross(n_a, p))
return sum(simplify(integrate(dot(integrand, n), iv)) * n for n in N)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""Exercise 7.2 from Kane 1985."""
from __future__ import division
from sympy import solve, symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import cross, dot
c1, c2, c3 = symbols('c1 c2 c3', real=True)
alpha = 10 # [N]
N = ReferenceFrame('N')
pO = Point('O')
pS = pO.locatenew('S', 10*N.x + 5*N.z) # [m]
pR = pO.locatenew('R', 10*N.x + 12*N.y) # [m]
pQ = pO.locatenew('Q', 12*N.y + 10*N.z) # [m]
pP = pO.locatenew('P', 4*N.x + 7*N.z) # [m]
A = alpha * pQ.pos_from(pP).normalize()
B = alpha * pS.pos_from(pR).normalize()
C_ = c1*N.x + c2*N.y + c3*N.z
eqs = [dot(A + B + C_, b) for b in N]
soln = solve(eqs, [c1, c2, c3])
C = sum(soln[ci] * b
for ci, b in zip(sorted(soln, cmp=lambda x, y: x.compare(y)), N))
print("C = {0}\nA + B + C = {1}".format(C, A + B + C))
M = cross(pP.pos_from(pO), A) + cross(pR.pos_from(pO), B)
print("|M| = {0} N-m".format(M.magnitude().n()))
def test_cross_different_frames():
assert cross(N.x, A.x) == sin(q1) * A.z
assert cross(N.x, A.y) == cos(q1) * A.z
assert cross(N.x, A.z) == -sin(q1) * A.x - cos(q1) * A.y
assert cross(N.y, A.x) == -cos(q1) * A.z
assert cross(N.y, A.y) == sin(q1) * A.z
assert cross(N.y, A.z) == cos(q1) * A.x - sin(q1) * A.y
assert cross(N.z, A.x) == A.y
assert cross(N.z, A.y) == -A.x
assert cross(N.z, A.z) == 0
assert cross(N.x, A.x) == sin(q1) * A.z
assert cross(N.x, A.y) == cos(q1) * A.z
assert cross(N.x, A.x + A.y) == sin(q1) * A.z + cos(q1) * A.z
assert cross(A.x + A.y, N.x) == -sin(q1) * A.z - cos(q1) * A.z
assert cross(A.x, C.x) == sin(q3) * C.y
assert cross(A.x, C.y) == -sin(q3) * C.x + cos(q3) * C.z
assert cross(A.x, C.z) == -cos(q3) * C.y
assert cross(C.x, A.x) == -sin(q3) * C.y
assert cross(C.y, A.x) == sin(q3) * C.x - cos(q3) * C.z
assert cross(C.z, A.x) == cos(q3) * C.y
def test_aux_dep():
# This test is about rolling disc dynamics, comparing the results found
# with KanesMethod to those found when deriving the equations "manually"
# with SymPy.
# The terms Fr, Fr*, and Fr*_steady are all compared between the two
# methods. Here, Fr*_steady refers to the generalized inertia forces for an
# equilibrium configuration.
# Note: comparing to the test of test_rolling_disc() in test_kane.py, this
# test also tests auxiliary speeds and configuration and motion constraints
#, seen in the generalized dependent coordinates q[3], and depend speeds
# u[3], u[4] and u[5].
# First, mannual derivation of Fr, Fr_star, Fr_star_steady.
# Symbols for time and constant parameters.
# Symbols for contact forces: Fx, Fy, Fz.
t, r, m, g, I, J = symbols('t r m g I J')
Fx, Fy, Fz = symbols('Fx Fy Fz')
# Configuration variables and their time derivatives:
# q[0] -- yaw
# q[1] -- lean
# q[2] -- spin
# q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in
# A.z direction
# Generalized speeds and their time derivatives:
# u[0] -- disc angular velocity component, disc fixed x direction
# u[1] -- disc angular velocity component, disc fixed y direction
# u[2] -- disc angular velocity component, disc fixed z direction
# u[3] -- disc velocity component, A.x direction
# u[4] -- disc velocity component, A.y direction
# u[5] -- disc velocity component, A.z direction
# Auxiliary generalized speeds:
# ua[0] -- contact point auxiliary generalized speed, A.x direction
# ua[1] -- contact point auxiliary generalized speed, A.y direction
# ua[2] -- contact point auxiliary generalized speed, A.z direction
q = dynamicsymbols('q:4')
qd = [qi.diff(t) for qi in q]
u = dynamicsymbols('u:6')
ud = [ui.diff(t) for ui in u]
#ud_zero = {udi : 0 for udi in ud}
ud_zero = dict(zip(ud, [0.]*len(ud)))
ua = dynamicsymbols('ua:3')
#ua_zero = {uai : 0 for uai in ua}
ua_zero = dict(zip(ua, [0.]*len(ua)))
# Reference frames:
# Yaw intermediate frame: A.
# Lean intermediate frame: B.
# Disc fixed frame: C.
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q[0], N.z])
B = A.orientnew('B', 'Axis', [q[1], A.x])
C = B.orientnew('C', 'Axis', [q[2], B.y])
# Angular velocity and angular acceleration of disc fixed frame
# u[0], u[1] and u[2] are generalized independent speeds.
C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z)
C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B)
+ cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Velocity and acceleration of points:
# Disc-ground contact point: P.
# Center of disc: O, defined from point P with depend coordinate: q[3]
# u[3], u[4] and u[5] are generalized dependent speeds.
P = Point('P')
P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z)
O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y)
O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z)
O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N)))
# Kinematic differential equations:
# Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates
# directions of B, for qd0, qd1 and qd2.
# the other is v_o_n_qd = O.vel(N) in A.z direction for qd3.
# Then, solve for dq/dt's in terms of u's: qd_kd.
w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y
v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P))
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_o_n_qd - O.vel(N), A.z)])
qd_kd = solve(kindiffs, qd)
# Values of generalized speeds during a steady turn for later substitution
# into the Fr_star_steady.
steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u)
steady_conditions.update({qd[1] : 0, qd[3] : 0})
# Partial angular velocities and velocities.
partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua]
partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua]
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
# Configuration constraint: f_c, the projection of radius r in A.z direction
# is q[3].
# Velocity constraints: f_v, for u3, u4 and u5.
# Acceleration constraints: f_a.
f_c = Matrix([dot(-r*B.z, A.z) - q[3]])
f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N),
O.pos_from(P))), ai).expand() for ai in A])
v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A])
# Solve for constraint equations in the form of
# u_dependent = A_rs * [u_i; u_aux].
# First, obtain constraint coefficient matrix: M_v * [u; ua] = 0;
# Second, taking u[0], u[1], u[2] as independent,
# taking u[3], u[4], u[5] as dependent,
# rearranging the matrix of M_v to be A_rs for u_dependent.
# Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
M_v = zeros(3, 9)
for i in range(3):
for j, ui in enumerate(u + ua):
M_v[i, j] = f_v[i].diff(ui)
M_v_i = M_v[:, :3]
M_v_d = M_v[:, 3:6]
M_v_aux = M_v[:, 6:]
M_v_i_aux = M_v_i.row_join(M_v_aux)
A_rs = - M_v_d.inv() * M_v_i_aux
u_dep = A_rs[:, :3] * Matrix(u[:3])
u_dep_dict = dict(zip(u[3:], u_dep))
#u_dep_dict = {udi : u_depi[0] for udi, u_depi in zip(u[3:], u_dep.tolist())}
# Active forces: F_O acting on point O; F_P acting on point P.
# Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
F_O = m*g*A.z
F_P = Fx * A.x + Fy * A.y + Fz * A.z
Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in
zip(partial_v_O, partial_v_P)])
# Inertia force: R_star_O.
# Inertia of disc: I_C_O, where J is a inertia component about principal axis.
# Inertia torque: T_star_C.
# Generalized inertia forces (unconstrained): Fr_star_u.
R_star_O = -m*O.acc(N)
I_C_O = inertia(B, I, J, I)
T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
+ cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in
zip(partial_v_O, partial_w_C)])
# Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
# Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
+ A_rs.T * Fr_star_u[3:6, :]
Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
.subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()
# Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.
# Rigid Bodies: disc, with inertia I_C_O.
iner_tuple = (I_C_O, O)
disc = RigidBody('disc', O, C, m, iner_tuple)
bodyList = [disc]
# Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
F_o = (O, F_O)
F_p = (P, F_P)
forceList = [F_o, F_p]
# KanesMethod.
kane = KanesMethod(
N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs,
q_dependent=q[3:], configuration_constraints = f_c,
u_dependent=u[3:], velocity_constraints= f_v,
u_auxiliary=ua
)
# fr, frstar, frstar_steady and kdd(kinematic differential equations).
(fr, frstar)= kane.kanes_equations(forceList, bodyList)
frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
.subs({q[3]: -r*cos(q[1])}).expand()
kdd = kane.kindiffdict()
# Test
# First try Fr_c == fr;
# Second try Fr_star_c == frstar;
# Third try Fr_star_steady == frstar_steady.
# Both signs are checked in case the equations were found with an inverse
# sign.
assert ((Matrix(Fr_c).expand() == fr.expand()) or
(Matrix(Fr_c).expand() == (-fr).expand()))
assert ((Matrix(Fr_star_c).expand() == frstar.expand()) or
(Matrix(Fr_star_c).expand() == (-frstar).expand()))
assert ((Matrix(Fr_star_steady).expand() == frstar_steady.expand()) or
(Matrix(Fr_star_steady).expand() == (-frstar_steady).expand()))
beta = symbols('beta', real=True)
b1, b2, b3 = symbols('b1 b2 b3', real=True)
p1, p2, p3 = symbols('p1 p2 p3', real=True)
N = ReferenceFrame('N')
pO = Point('O')
pS = pO.locatenew('S', 10*N.x + 5*N.z)
pR = pO.locatenew('R', 10*N.x + 12*N.y)
pQ = pO.locatenew('Q', 12*N.y + 10*N.z)
pP = pO.locatenew('P', 4*N.x + 7*N.z)
A = alpha * pQ.pos_from(pP).normalize()
B = alpha * pS.pos_from(pR).normalize()
R = A + B
M = cross(pP.pos_from(pO), A) + cross(pR.pos_from(pO), B)
Ms = dot(R, M) * R / dot(R, R)
ps = cross(R, M) / dot(R, R)
A_prime = beta * pP.pos_from(pO).normalize()
B_prime = b1*N.x + b2*N.y + b3*N.z
pB_prime = pO.locatenew("B'", p1*N.x + p2*N.y + p3*N.z)
M_prime = cross(pB_prime.pos_from(pO), B_prime)
eqs = [dot(R - A_prime - B_prime, n) for n in N]
eqs += [dot(M - M_prime, n) for n in N]
# choose pB_prime to be perpendicular to B_prime
# then pB_prime.magnitude() gives the distance d from O
# to the line of action of B'
eqs.append(dot(pB_prime.pos_from(pO), B_prime))
# Angular velocity using u's as body fixed measure numbers of angular velocity
R.set_ang_vel(N, qd[0]*Y.z + qd[1]*Y.x + qd[2]*L.y)
# Rattleback ground contact point
P = Point('P')
# Rattleback ellipsoid center location, see:
# "Realistic mathematical modeling of the rattleback", Kane, Thomas R. and
# David A. Levinson, 1982, International Journal of Non-Linear Mechanics
mu = [dot(rk, Y.z) for rk in R]
eps = sqrt((a*mu[0])**2 + (b*mu[1])**2 + (c*mu[2])**2)
O = P.locatenew('O', -a*a*mu[0]/eps*R.x
-b*b*mu[1]/eps*R.y
-c*c*mu[2]/eps*R.z)
O.set_vel(N, cross(R.ang_vel_in(N), O.pos_from(P)))
# Mass center position and velocity
RO = O.locatenew('RO', d*R.x + e*R.y + f*R.z)
RO.set_vel(N, O.vel(N) + cross(R.ang_vel_in(N), RO.pos_from(O)))
# Partial angular velocities and partial velocities
partial_w = [R.ang_vel_in(N).diff(qdi, N) for qdi in qd]
partial_v_RO = [RO.vel(N).diff(qdi, N) for qdi in qd]
steady_dict = {qd[1]: 0, qd[2]: 0,
qd[0].diff(t): 0, qd[1].diff(t): 0, qd[2].diff(t): 0}
# Set auxiliary generalized speeds to zero in velocity vectors
O.set_vel(N, O.vel(N).subs(steady_dict))
RO.set_vel(N, RO.vel(N).subs(steady_dict))
def __init__(self):
# Generalized Coordinates and Speeds:
# q1,u1: frame yaw
# q2,u2: frame roll
# q3,u3: frame pitch
# q4,u4: steering rotation
# u5: rear wheel ang. vel.
# u6: front wheel ang. vel.
q1, q2, q3, q4 = mec.dynamicsymbols('q1 q2 q3 q4')
q1d, q2d, q3d, q4d = mec.dynamicsymbols('q1 q2 q3 q4', 1)
u1, u2, u3, u4 = mec.dynamicsymbols('u1 u2 u3 u4')
u5, u6 = mec.dynamicsymbols('u5 u6')
u1d, u2d, u3d, u4d = mec.dynamicsymbols('u1 u2 u3 u4', 1)
u5d, u6d = mec.dynamicsymbols('u5 u6', 1)
self._coordinatesInde = [q1, q2, q4]
self._coordinatesDe = [q3]
self._coordinates = [q1, q2, q3, q4]
self._speedsInde = [u2, u4, u5]
self._speedsDe = [u1, u3, u6]
self._speeds = [u1, u2, u3, u4, u5, u6]
self._speedsDerivative = [u1d, u2d, u3d, u4d, u5d, u6d]
# Axiliary speeds at contact points:
# Rear wheel: ua1, ua2
# Front wheel: ua4, ua5
ua1, ua2, ua3 = mec.dynamicsymbols ('ua1 ua2 ua3')
ua4, ua5, ua6 = mec.dynamicsymbols ('ua4 ua5 ua6')
ua1d, ua2d, ua3d = mec.dynamicsymbols ('ua1 ua2 ua3',1)
ua4d, ua5d, ua6d = mec.dynamicsymbols ('ua4 ua5 ua6',1)
self._auxiliarySpeeds = [ua1, ua2, ua3, ua4, ua5, ua6]
self._auxiliarySpeedsDerivative = [ua1d, ua2d, ua3d, ua4d, ua5d, ua6d]
ua_zero = {uai: 0 for uai in self._auxiliarySpeeds}
# Reference Frames:
# Newtonian Frame: N
# Yaw Frame: A
# Roll Frame: B
# Pitch & Bicycle Frame: C
# Steer & Fork/Handlebar Frame: E
# Rear Wheel Frame: D
# Front Wheel Frame: F
N = mec.ReferenceFrame('N', indices=('1', '2', '3'))
A = mec.ReferenceFrame('A', indices=('1', '2', '3'))
B = mec.ReferenceFrame('B', indices=('1', '2', '3'))
C = mec.ReferenceFrame('C', indices=('1', '2', '3'))
E = mec.ReferenceFrame('E', indices=('1', '2', '3'))
D = mec.ReferenceFrame('D', indices=('1', '2', '3'))
F = mec.ReferenceFrame('F', indices=('1', '2', '3'))
# Orientation of Reference Frames:
# bicycle frame yaw: N->A
# bicycle frame roll: A->B
# pitch to rear frame: B->C
# fork/handlebar steer: C->E
A.orient(N, 'Axis', [q1, N['3']])
B.orient(A, 'Axis', [q2, A['1']])
C.orient(B, 'Axis', [q3, B['2']])
E.orient(C, 'Axis', [q4, C['3']])
# Angular Velocities and define the generalized speeds:
A.set_ang_vel(N, u1 * N['3'])
B.set_ang_vel(A, u2 * A['1'])
C.set_ang_vel(B, u3 * B['2'])
E.set_ang_vel(C, u4 * C['3'])
D.set_ang_vel(C, u5 * C['2'])
F.set_ang_vel(E, u6 * E['2'])
# Parameters:
# Geometry:
# rf: radius of front wheel
# rr: radius of rear wheel
# d1: the perpendicular distance from the steer axis to the center
# of the rear wheel (rear offset)
# d2: the distance between wheels along the steer axis
# d3: the perpendicular distance from the steer axis to the center
# of the front wheel (fork offset)
# l1: the distance in the c1> direction from the center of the rear
# wheel to the frame center of mass
# l2: the distance in the c3> direction from the center of the rear
# wheel to the frame center of mass
# l3: the distance in the e1> direction from the front wheel center to
# the center of mass of the fork
# l4: the distance in the e3> direction from the front wheel center to
# the center of mass of the fork
# Mass:
# mc, md, me, mf: mass of rearframe, rearwheel, frontframe, frontwheel
# Inertia:
# ic11, ic22, ic33, ic31: rear frame
# id11, id22: rear wheel
# ie11, ie22, ie33, ie31: front frame
# if11, if22: front wheel
rf, rr = sym.symbols('rf rr')
d1, d2, d3 = sym.symbols('d1 d2 d3')
l1, l2, l3, l4 = sym.symbols('l1 l2 l3 l4')
g = sym.symbols('g')
t = sym.symbols('t')
mc, md, me, mf = sym.symbols('mc md me mf')
self._massSym = [mc, md, me, mf]
ic11, ic22, ic33, ic31 = sym.symbols('ic11 ic22 ic33 ic31') #rear frame
id11, id22 = sym.symbols('id11 id22') #rear wheel
ie11, ie22, ie33, ie31 = sym.symbols('ie11 ie22 ie33 ie31') #front frame
if11, if22 = sym.symbols('if11 if22') #front wheel
# Special unit vectors:
# g_3: direction along front wheel radius.
# long_v: longitudinal direction of front wheel.
# lateral_v: lateral direction of front wheel.
g_3 = (mec.express(A['3'], E) - mec.dot(E['2'], A['3'])*E['2']).normalize()
# Or g_3 = E['2'].cross(A['3']).cross(E['2']).normalize()
long_v = mec.cross(E['2'], g_3).normalize()
# E.y ^ g_3 # ^ -> cross, & -> dot
lateral_v = mec.cross(A['3'], long_v)
# Points and velocities:
# dn: rear wheel contact point.
# do: rear wheel center.
# rtr: rear tire radius.
# fn: front wheel contact point.
# fo: front wheel center.
# ftr: front tire radius
# co: rear frame center.
# eo: front frame center.
# ce: steer axis point.
# SAF: steer axis foot.
# Rear
dn = mec.Point('dn')
dn.set_vel(N, ua1 * A['1'] + ua2 * A['2'] + ua3 * A['3'])
do = dn.locatenew('do', -rr * B['3'])
do.v2pt_theory(dn, N, D)
do.set_acc(N, do.vel(N).subs(ua_zero).diff(t, C) +
mec.cross(C.ang_vel_in(N), do.vel(N).subs(ua_zero)))
co = mec.Point('co')
co.set_pos(do, l1 * C['1'] + l2 * C['3'])
co.v2pt_theory(do, N, C)
co.set_acc(N, co.vel(N).subs(ua_zero).diff(t, C) +
mec.cross(C.ang_vel_in(N), co.vel(N).subs(ua_zero)))
# Front
fn = mec.Point('fn')
fn.set_vel(N, ua4 * long_v + ua5 * lateral_v + ua6 * A['3'])
fo = fn.locatenew('fo', -rf * g_3)
# OR fo = fn.locatenew('fo', -ftr * A['3'] - rf * g_3)
fo.v2pt_theory(fn, N, F)
fo.set_acc(N, fo.vel(N).subs(ua_zero).diff(t, E) +
mec.cross(E.ang_vel_in(N), fo.vel(N).subs(ua_zero)))
eo = mec.Point('eo')
eo.set_pos(fo, l3 * E['1'] + l4 * E['3'])
eo.v2pt_theory(fo, N, E)
eo.set_acc(N, eo.vel(N).subs(ua_zero).diff(t, E) +
mec.cross(E.ang_vel_in(N), eo.vel(N).subs(ua_zero)))
SAF = do.locatenew('SAF', d1 * C['1'] + d2 * E['3'])
SAF.set_pos(fo, -d3 * E['1'])
# Velociy of SAF in two ways:
# OR v_SAF_1 = v2pt_theory(do, N, C)
# OR v_SAF_2 = v2pt_theory(fo, N, E)
v_SAF_1 = do.vel(N) + mec.cross(C.ang_vel_in(N), SAF.pos_from(do))
v_SAF_2 = fo.vel(N) + mec.cross(E.ang_vel_in(N), SAF.pos_from(fo))
# Holo and nonholo Constraints:
self._holonomic = [fn.pos_from(dn).dot(A['3'])]
self._nonholonomic = [(v_SAF_1-v_SAF_2).dot(uv) for uv in E]
# Rigid Bodies:
# Inertia: Ic, Id, Ie, If
# Bodies: rearFrame, rearWheel, frontFrame, frontWheel
Ic = mec.inertia(C, ic11, ic22, ic33, 0.0, 0.0, ic31)
Id = mec.inertia(C, id11, id22, id11, 0.0, 0.0, 0.0)
Ie = mec.inertia(E, ie11, ie22, ie33, 0.0, 0.0, ie31)
If = mec.inertia(E, if11, if22, if11, 0.0, 0.0, 0.0)
rearFrame_inertia = (Ic, co)
rearFrame=mec.RigidBody('rearFrame',co,C,mc,rearFrame_inertia)
rearWheel_inertia = (Id, do)
rearWheel=mec.RigidBody('rearWheel',do,D,md,rearWheel_inertia)
frontFrame_inertia = (Ie, eo)
frontFrame=mec.RigidBody('frontFrame',eo,E,me,frontFrame_inertia)
frontWheel_inertia = (If, fo)
frontWheel=mec.RigidBody('frontWheel',fo,F,mf,frontWheel_inertia)
bodyList = [rearFrame, rearWheel, frontFrame, frontWheel]
# Generalized Active Forces:
# T4: steer torque.
# Fx_r, Fy_r: rear wheel contact forces.
# Fx_f, Fy_f: front wheel contact forces.
# Fco, Fdo, Feo, Ffo: gravity of four rigid bodies of bicycle.
T4 = mec.dynamicsymbols('T4')
Fx_r, Fy_r, Fz_r, Fx_f, Fy_f, Fz_f= mec.dynamicsymbols('Fx_r Fy_r Fz_r Fx_f Fy_f Fz_f')
Tc = (C, - T4 * C['3']) #back steer torque to rear frame
Te = (E, T4 * C['3']) #steer torque to front frame
F_r = (dn, Fx_r * A['1'] + Fy_r * A['2'] + Fz_r * A['3'])
F_f = (fn, Fx_f * long_v + Fy_f * lateral_v + Fz_f * A['3'])
# OR F_r = (dn, Fx_r * N['1'] + Fy_r * N['2'])
# OR F_f = (fn, Fx_f * N['1'] + Fy_f * N['2'])
Fco = (co, mc * g * A['3'])
Fdo = (do, md * g * A['3'])
Feo = (eo, me * g * A['3'])
Ffo = (fo, mf * g * A['3'])
forceList = [Fco, Fdo, Feo, Ffo, F_r, F_f, Tc, Te]
self._inputForces = [T4]
self._auxiliaryForces = [Fx_r, Fy_r, Fz_r, Fx_f, Fy_f, Fz_f]
# Kinematical Differential Equations:
kinematical = [q1d - u1,
q2d - u2,
q3d - u3,
q4d - u4]
# Kanes Method:
self._kane = mec.KanesMethod(
N, q_ind=self._coordinatesInde, u_ind=self._speedsInde, kd_eqs=kinematical,
q_dependent=self._coordinatesDe, configuration_constraints=self._holonomic,
u_dependent=self._speedsDe, velocity_constraints=self._nonholonomic,
u_auxiliary=self._auxiliarySpeeds
)
(fr, frstar)= self._kane.kanes_equations(forceList, bodyList)
self._Fr = fr
self._Fr_star = frstar
self._kdd = self._kane.kindiffdict()
# Setting another contact points for calculating turning radius:
# Turning radius: rear wheel: Rr, front wheel Rf;
# Contact points: rear contact: P; front contact: Q; Turning center: TC;
# Relative position: P_Q_A1, P_Q_A2.
Rr, Rf = sym.symbols('Rr Rf')
P = mec.Point('P')
TC = P.locatenew('TC', Rr*A['2'])
Q = TC.locatenew('Q', -Rf*lateral_v)
self._turningRadiusSym = [Rr, Rf]
P_Q_A1 = Q.pos_from(P).dot(A['1'])
P_Q_A2 = Q.pos_from(P).dot(A['2'])
self._contact_posi_pq = [P_Q_A1, P_Q_A2]
fn_dn_A1 = fn.pos_from(dn).dot(A['1'])
fn_dn_A2 = fn.pos_from(dn).dot(A['2'])
self._contact_posi_dfn = [fn_dn_A1, fn_dn_A2]
# Total center of mass:
# individual center of mass of four rigid bodies
co_dn_A = [mec.dot(co.pos_from(dn), A_x) for A_x in A]
do_dn_A = [mec.dot(do.pos_from(dn), A_x) for A_x in A]
fo_dn_A = [mec.dot(fo.pos_from(dn), A_x) for A_x in A]
eo_dn_A = [mec.dot(eo.pos_from(dn), A_x) for A_x in A]
self._bodies_dn_A = array(
[[co_dn_A[0], do_dn_A[0], eo_dn_A[0], fo_dn_A[0]],
[co_dn_A[1], do_dn_A[1], eo_dn_A[1], fo_dn_A[1]],
[co_dn_A[2], do_dn_A[2], eo_dn_A[2], fo_dn_A[2]]]
)
def test_linearize_rolling_disc_kane():
# Symbols for time and constant parameters
t, r, m, g, v = symbols('t r m g v')
# Configuration variables and their time derivatives
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
# Generalized speeds and their time derivatives
u = dynamicsymbols('u:6')
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
# Reference frames
N = ReferenceFrame('N') # Inertial frame
NO = Point('NO') # Inertial origin
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center
# Disc angular velocity in N expressed using time derivatives of coordinates
w_c_n_qd = C.ang_vel_in(N)
w_b_n_qd = B.ang_vel_in(N)
# Inertial angular velocity and angular acceleration of disc fixed frame
C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z)
# Disc center velocity in N expressed using time derivatives of coordinates
v_co_n_qd = CO.pos_from(NO).dt(N)
# Disc center velocity in N expressed using generalized speeds
CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z)
# Disc Ground Contact Point
P = CO.locatenew('P', r*B.z)
P.v2pt_theory(CO, N, C)
# Configuration constraint
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
# Velocity level constraints
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
# Set angular velocity of remaining frames
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
# Active forces
F_CO = m*g*A.z
# Create inertia dyadic of disc C about point CO
I = (m * r**2) / 4
J = (m * r**2) / 2
I_C_CO = inertia(C, I, J, I)
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
BL = [Disc]
FL = [(CO, F_CO)]
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs,
q_dependent=[q6], configuration_constraints=f_c,
u_dependent=[u4, u5, u6], velocity_constraints=f_v)
with warns_deprecated_sympy():
(fr, fr_star) = KM.kanes_equations(FL, BL)
# Test generalized form equations
linearizer = KM.to_linearizer()
assert linearizer.f_c == f_c
assert linearizer.f_v == f_v
assert linearizer.f_a == f_v.diff(t)
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
for qi in qd:
assert sol[qi] == qdots[qi]
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0])
# Perform the linearization
# Precomputed operating point
q_op = {q6: -r*cos(q2)}
u_op = {u1: 0,
u2: sin(q2)*q1d + q3d,
u3: cos(q2)*q1d,
u4: -r*(sin(q2)*q1d + q3d)*cos(q3),
u5: 0,
u6: -r*(sin(q2)*q1d + q3d)*sin(q3)}
qd_op = {q2d: 0,
q4d: -r*(sin(q2)*q1d + q3d)*cos(q1),
q5d: -r*(sin(q2)*q1d + q3d)*sin(q1),
q6d: 0}
ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5,
u2d: 0,
u3d: 0,
u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2),
u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5),
u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)}
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True)
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
# Precomputed solution
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
[-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0],
[0, Rational(4, 5), 0, 0, 0, 0, 0, 6*q3d/5],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, -2*q3d, 0, 0]])
B_sol = Matrix([])
# Check that linearization is correct
assert A.subs(upright_nominal) == A_sol
assert B.subs(upright_nominal) == B_sol
# Check eigenvalues at critical speed are all zero:
assert A.subs(upright_nominal).subs(q3d, 1/sqrt(3)).eigenvals() == {0: 8}
beta = symbols('beta', real=True)
b1, b2, b3 = symbols('b1 b2 b3', real=True)
p1, p2, p3 = symbols('p1 p2 p3', real=True)
N = ReferenceFrame('N')
pO = Point('O')
pS = pO.locatenew('S', 10 * N.x + 5 * N.z)
pR = pO.locatenew('R', 10 * N.x + 12 * N.y)
pQ = pO.locatenew('Q', 12 * N.y + 10 * N.z)
pP = pO.locatenew('P', 4 * N.x + 7 * N.z)
A = alpha * pQ.pos_from(pP).normalize()
B = alpha * pS.pos_from(pR).normalize()
R = A + B
M = cross(pP.pos_from(pO), A) + cross(pR.pos_from(pO), B)
Ms = dot(R, M) * R / dot(R, R)
ps = cross(R, M) / dot(R, R)
A_prime = beta * pP.pos_from(pO).normalize()
B_prime = b1 * N.x + b2 * N.y + b3 * N.z
pB_prime = pO.locatenew("B'", p1 * N.x + p2 * N.y + p3 * N.z)
M_prime = cross(pB_prime.pos_from(pO), B_prime)
eqs = [dot(R - A_prime - B_prime, n) for n in N]
eqs += [dot(M - M_prime, n) for n in N]
# choose pB_prime to be perpendicular to B_prime
# then pB_prime.magnitude() gives the distance d from O
# to the line of action of B'
eqs.append(dot(pB_prime.pos_from(pO), B_prime))
# Rattleback ground contact point
P = Point('P')
P.set_vel(N, ua[0]*Y.x + ua[1]*Y.y + ua[2]*Y.z)
# Rattleback paraboloid -- parameterize coordinates of contact point by the
# roll and pitch angles, and the geometry
# TODO: FIXME!!!
# f(x,y,z) = a*x**2 + b*y**2 + z - c
mu = [dot(rk, Y.z) for rk in R]
rx = mu[0]/mu[2]/2/a
ry = mu[1]/mu[2]/2/b
rz = 1 - (mu[0]**2/a + mu[1]**2/b)*(1/2/mu[2])**2
# Locate origin of parabaloid coordinate system relative to contact point
O = P.locatenew('O', -rx*R.x - ry*R.y - rz*R.z)
O.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), O.pos_from(P)))
# Mass center position and velocity
RO = O.locatenew('RO', d*R.x + e*R.y)
RO.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), RO.pos_from(P)))
qd_rhs = [(u[2]*cos(q[2]) - u[0]*sin(q[2]))/cos(q[1]),
u[0]*cos(q[2]) + u[2]*sin(q[2]),
u[1] + tan(q[1])*(u[0]*sin(q[2]) - u[2]*cos(q[2])),
dot(P.pos_from(O).diff(t, R), N.x),
dot(P.pos_from(O).diff(t, R), N.y)]
# Partial angular velocities and partial velocities
partial_w = [R.ang_vel_in(N).diff(ui, N) for ui in u + ua]
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
partial_v_RO = [RO.vel(N).diff(ui, N) for ui in u + ua]
# Angular velocity using u's as body fixed measure numbers of angular velocity
R.set_ang_vel(N, qd[0] * Y.z + qd[1] * Y.x + qd[2] * L.y)
# Rattleback ground contact point
P = Point('P')
# Rattleback ellipsoid center location, see:
# "Realistic mathematical modeling of the rattleback", Kane, Thomas R. and
# David A. Levinson, 1982, International Journal of Non-Linear Mechanics
mu = [dot(rk, Y.z) for rk in R]
eps = sqrt((a * mu[0])**2 + (b * mu[1])**2 + (c * mu[2])**2)
O = P.locatenew(
'O', -a * a * mu[0] / eps * R.x - b * b * mu[1] / eps * R.y -
c * c * mu[2] / eps * R.z)
O.set_vel(N, cross(R.ang_vel_in(N), O.pos_from(P)))
# Mass center position and velocity
RO = O.locatenew('RO', d * R.x + e * R.y + f * R.z)
RO.set_vel(N, O.vel(N) + cross(R.ang_vel_in(N), RO.pos_from(O)))
# Partial angular velocities and partial velocities
partial_w = [R.ang_vel_in(N).diff(qdi, N) for qdi in qd]
partial_v_RO = [RO.vel(N).diff(qdi, N) for qdi in qd]
steady_dict = {
qd[1]: 0,
qd[2]: 0,
qd[0].diff(t): 0,
qd[1].diff(t): 0,
qd[2].diff(t): 0
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""Exercise 7.2 from Kane 1985."""
from __future__ import division
from sympy import acos, pi, solve, symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import cross, dot
c1, c2, c3 = symbols('c1 c2 c3', real=True)
alpha = 10 # [N]
N = ReferenceFrame('N')
pO = Point('O')
pS = pO.locatenew('S', 10 * N.x + 5 * N.z) # [m]
pR = pO.locatenew('R', 10 * N.x + 12 * N.y) # [m]
pQ = pO.locatenew('Q', 12 * N.y + 10 * N.z) # [m]
pP = pO.locatenew('P', 4 * N.x + 7 * N.z) # [m]
A = alpha * pQ.pos_from(pP).normalize()
B = alpha * pS.pos_from(pR).normalize()
C_ = c1 * N.x + c2 * N.y + c3 * N.z
eqs = [dot(A + B + C_, b) for b in N]
soln = solve(eqs, [c1, c2, c3])
C = sum(soln[ci] * b
for ci, b in zip(sorted(soln, cmp=lambda x, y: x.compare(y)), N))
print("C = {0}\nA + B + C = {1}".format(C, A + B + C))
M = cross(pP.pos_from(pO), A) + cross(pR.pos_from(pO), B)
print("|M| = {0} N-m".format(M.magnitude().n()))
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""Exercise 7.7 from Kane 1985."""
from __future__ import division
from sympy import symbols
from sympy.physics.mechanics import ReferenceFrame, Point
from sympy.physics.mechanics import cross, dot
# vectors A, B have equal magnitude alpha
alpha = symbols('alpha', real=True, positive=True)
N = ReferenceFrame('N')
pO = Point('O')
pS = pO.locatenew('S', 10 * N.x + 5 * N.z)
pR = pO.locatenew('R', 10 * N.x + 12 * N.y)
pQ = pO.locatenew('Q', 12 * N.y + 10 * N.z)
pP = pO.locatenew('P', 4 * N.x + 7 * N.z)
A = alpha * pQ.pos_from(pP).normalize()
B = alpha * pS.pos_from(pR).normalize()
R = A + B
M = cross(pP.pos_from(pO), A) + cross(pR.pos_from(pO), B)
Ms = dot(R, M) * R / dot(R, R)
ps = cross(R, M) / dot(R, R)
#print("M* = {0}".format(Ms))
print("|M*|/|R| = {0}".format(Ms.magnitude() / R.magnitude()))
print("|p*| = {0}, {1}".format(ps.magnitude(), ps.magnitude().n()))
x, y = _me.dynamicsymbols('x y')
x_d, y_d = _me.dynamicsymbols('x_ y_', 1)
e = _sm.cos(x)+_sm.sin(x)+_sm.tan(x)+_sm.cosh(x)+_sm.sinh(x)+_sm.tanh(x)+_sm.acos(x)+_sm.asin(x)+_sm.atan(x)+_sm.log(x)+_sm.exp(x)+_sm.sqrt(x)+_sm.factorial(x)+_sm.ceiling(x)+_sm.floor(x)+_sm.sign(x)
e = (x)**2+_sm.log(x, 10)
a = _sm.Abs(-1*1)+int(1.5)+round(1.9)
e1 = 2*x+3*y
e2 = x+y
am = _sm.Matrix([e1.expand().coeff(x), e1.expand().coeff(y), e2.expand().coeff(x), e2.expand().coeff(y)]).reshape(2, 2)
b = (e1).expand().coeff(x)
c = (e2).expand().coeff(y)
d1 = (e1).collect(x).coeff(x,0)
d2 = (e1).collect(x).coeff(x,1)
fm = _sm.Matrix([i.collect(x)for i in _sm.Matrix([e1,e2]).reshape(1, 2)]).reshape((_sm.Matrix([e1,e2]).reshape(1, 2)).shape[0], (_sm.Matrix([e1,e2]).reshape(1, 2)).shape[1])
f = (e1).collect(y)
g = (e1).subs({x:2*x})
gm = _sm.Matrix([i.subs({x:3}) for i in _sm.Matrix([e1,e2]).reshape(2, 1)]).reshape((_sm.Matrix([e1,e2]).reshape(2, 1)).shape[0], (_sm.Matrix([e1,e2]).reshape(2, 1)).shape[1])
frame_a = _me.ReferenceFrame('a')
frame_b = _me.ReferenceFrame('b')
theta = _me.dynamicsymbols('theta')
frame_b.orient(frame_a, 'Axis', [theta, frame_a.z])
v1 = 2*frame_a.x-3*frame_a.y+frame_a.z
v2 = frame_b.x+frame_b.y+frame_b.z
a = _me.dot(v1, v2)
bm = _sm.Matrix([_me.dot(v1, v2),_me.dot(v1, 2*v2)]).reshape(2, 1)
c = _me.cross(v1, v2)
d = 2*v1.magnitude()+3*v1.magnitude()
dyadic = _me.outer(3*frame_a.x, frame_a.x)+_me.outer(frame_a.y, frame_a.y)+_me.outer(2*frame_a.z, frame_a.z)
am = (dyadic).to_matrix(frame_b)
m = _sm.Matrix([1,2,3]).reshape(3, 1)
v = m[0]*frame_a.x +m[1]*frame_a.y +m[2]*frame_a.z
# Configuration constraint and its Jacobian w.r.t. q (Table 1)
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
f_c_dq = f_c.jacobian(q)
# Velocity level constraints (Table 1)
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
f_v_dq = f_v.jacobian(q)
f_v_du = f_v.jacobian(u)
# Kinematic differential equations
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
qdots = solve(kindiffs, qd)
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N),
C.ang_vel_in(N)))
# f_0 and f_1 (Table 1)
f_0 = kindiffs.subs(u_zero)
f_1 = kindiffs.subs(qd_zero)
# Acceleration level constraints (Table 1)
f_a = f_v.diff(t)
# Alternatively, f_a can be formed as
#v_co_n = cross(C.ang_vel_in(N), CO.pos_from(P))
#a_co_n = v_co_n.dt(B) + cross(B.ang_vel_in(N), v_co_n)
#f_a = Matrix([((a_co_n - CO.acc(N)) & uv).expand() for uv in B])
# Kane's dynamic equations via elbow grease
# Partial angular velocities and velocities
partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u]
print("ready for special unit vectors; and points and velocities")
######################
# special unit vectors
######################
# pitch back for the unit vector g_3 along front wheel radius;
g_3 = (mec.express(A["3"], E) - mec.dot(E["2"], A["3"]) * E["2"]).normalize()
# another way: g_3 = E['2'].cross(A['3']).cross(E['2']).normalize()
# roll back for longitudinal and lateral unit vector of front wheel
long_v = mec.cross(E["2"], A["3"]).normalize()
lateral_v = mec.cross(A["3"], long_v).normalize()
#########################
# points and velocities
#########################
####################rear wheel contact point, dn
dn = mec.Point("dn")
dn.set_vel(N, 0.0)
# rear wheel center, do
do = dn.locatenew("do", -rR * B["3"])
# do = dn.locatenew('do', -rtr * A['3'] - rR * B['3']) # rear wheel center with rear tire radius rtr
from sympy.physics.mechanics import cross,dot,ReferenceFrame
# define a reference frame
A=ReferenceFrame('A')
# show different directions
print A.x,A.y,A.z
# should each equal 0
print dot(A.x,A.y),dot(A.y,A.z),dot(A.z,A.x)
# should each equal a new vector
print cross(A.x,A.y),cross(A.y,A.z),cross(A.z,A.x)
# should equal negative of last answer
print cross(A.y,A.x),cross(A.z,A.y),cross(A.x,A.z)
# should equal 1 and 0 respectively
print dot(A.x,A.x),cross(A.x,A.x)
