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)

#!/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()))

# 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))

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))

# 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)

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)
#.........这里部分代码省略.........

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

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

# 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]

# 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))))

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()))

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)

                       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')

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)
)

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])

#.........这里部分代码省略.........
        #    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')

# 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))

# 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]

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])
#.........这里部分代码省略.........