def test_coordinate_vars():
"""Tests the coordinate variables functionality"""
assert CoordinateSym('Ax', A, 0) == A[0]
assert CoordinateSym('Ax', A, 1) == A[1]
assert CoordinateSym('Ax', A, 2) == A[2]
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
assert isinstance(A[0], CoordinateSym) and \
isinstance(A[0], CoordinateSym) and \
isinstance(A[0], CoordinateSym)
assert A.variable_map(A) == {A[0]: A[0], A[1]: A[1], A[2]: A[2]}
assert A[0].frame == A
B = A.orientnew('B', 'Axis', [q, A.z])
assert B.variable_map(A) == {
B[2]: A[2],
B[1]: -A[0] * sin(q) + A[1] * cos(q),
B[0]: A[0] * cos(q) + A[1] * sin(q)
}
assert A.variable_map(B) == {
A[0]: B[0] * cos(q) - B[1] * sin(q),
A[1]: B[0] * sin(q) + B[1] * cos(q),
A[2]: B[2]
}
assert A.dt(B[0]) == -A[0] * sin(q) * qd + A[1] * cos(q) * qd
assert A.dt(B[1]) == -A[0] * cos(q) * qd - A[1] * sin(q) * qd
assert A.dt(B[2]) == 0
assert express(B[0], A) == A[0] * cos(q) + A[1] * sin(q)
assert express(B[1], A) == -A[0] * sin(q) + A[1] * cos(q)
assert express(B[2], A) == A[2]
assert B.dt(A[0] * A.x + A[1] * A.y +
A[2] * A.z) == A[1] * qd * A.x - A[0] * qd * A.y
assert A.dt(B[0] * B.x + B[1] * B.y +
B[2] * B.z) == -B[1] * qd * B.x + B[0] * qd * B.y
assert A.express(B[0]*B[1]*B[2]) == \
A[2]*(-A[0]*sin(q) + A[1]*cos(q))*(A[0]*cos(q) + A[1]*sin(q))
assert (A.dt(B[0] * B[1] * B[2]) -
(A[2] * (-A[0]**2 * cos(2 * q) - 2 * A[0] * A[1] * sin(2 * q) +
A[1]**2 * cos(2 * q)) * qd)).trigsimp() == 0
assert A.express(B[0]*B.x + B[1]*B.y + B[2]*B.z) == \
(B[0]*cos(q) - B[1]*sin(q))*A.x + (B[0]*sin(q) + \
B[1]*cos(q))*A.y + B[2]*A.z
assert A.express(B[0]*B.x + B[1]*B.y + B[2]*B.z, variables=True) == \
A[0]*A.x + A[1]*A.y + A[2]*A.z
assert B.express(A[0]*A.x + A[1]*A.y + A[2]*A.z) == \
(A[0]*cos(q) + A[1]*sin(q))*B.x + \
(-A[0]*sin(q) + A[1]*cos(q))*B.y + A[2]*B.z
assert B.express(A[0]*A.x + A[1]*A.y + A[2]*A.z, variables=True) == \
B[0]*B.x + B[1]*B.y + B[2]*B.z
N = B.orientnew('N', 'Axis', [-q, B.z])
assert N.variable_map(A) == {N[0]: A[0], N[2]: A[2], N[1]: A[1]}
C = A.orientnew('C', 'Axis', [q, A.x + A.y + A.z])
mapping = A.variable_map(C)
assert mapping[A[0]] == 2*C[0]*cos(q)/3 + C[0]/3 - 2*C[1]*sin(q + pi/6)/3 +\
C[1]/3 - 2*C[2]*cos(q + pi/3)/3 + C[2]/3
assert mapping[A[1]] == -2*C[0]*cos(q + pi/3)/3 + \
C[0]/3 + 2*C[1]*cos(q)/3 + C[1]/3 - 2*C[2]*sin(q + pi/6)/3 + C[2]/3
assert mapping[A[2]] == -2*C[0]*sin(q + pi/6)/3 + C[0]/3 - \
2*C[1]*cos(q + pi/3)/3 + C[1]/3 + 2*C[2]*cos(q)/3 + C[2]/3
def test_coordinate_vars():
"""Tests the coordinate variables functionality"""
assert CoordinateSym('Ax', A, 0) == A[0]
assert CoordinateSym('Ax', A, 1) == A[1]
assert CoordinateSym('Ax', A, 2) == A[2]
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
assert isinstance(A[0], CoordinateSym) and \
isinstance(A[0], CoordinateSym) and \
isinstance(A[0], CoordinateSym)
assert A.variable_map(A) == {A[0]:A[0], A[1]:A[1], A[2]:A[2]}
assert A[0].frame == A
B = A.orientnew('B', 'Axis', [q, A.z])
assert B.variable_map(A) == {B[2]: A[2], B[1]: -A[0]*sin(q) + A[1]*cos(q),
B[0]: A[0]*cos(q) + A[1]*sin(q)}
assert A.variable_map(B) == {A[0]: B[0]*cos(q) - B[1]*sin(q),
A[1]: B[0]*sin(q) + B[1]*cos(q), A[2]: B[2]}
assert time_derivative(B[0], A) == -A[0]*sin(q)*qd + A[1]*cos(q)*qd
assert time_derivative(B[1], A) == -A[0]*cos(q)*qd - A[1]*sin(q)*qd
assert time_derivative(B[2], A) == 0
assert express(B[0], A, variables=True) == A[0]*cos(q) + A[1]*sin(q)
assert express(B[1], A, variables=True) == -A[0]*sin(q) + A[1]*cos(q)
assert express(B[2], A, variables=True) == A[2]
assert time_derivative(A[0]*A.x + A[1]*A.y + A[2]*A.z, B) == A[1]*qd*A.x - A[0]*qd*A.y
assert time_derivative(B[0]*B.x + B[1]*B.y + B[2]*B.z, A) == - B[1]*qd*B.x + B[0]*qd*B.y
assert express(B[0]*B[1]*B[2], A, variables=True) == \
A[2]*(-A[0]*sin(q) + A[1]*cos(q))*(A[0]*cos(q) + A[1]*sin(q))
assert (time_derivative(B[0]*B[1]*B[2], A) -
(A[2]*(-A[0]**2*cos(2*q) -
2*A[0]*A[1]*sin(2*q) +
A[1]**2*cos(2*q))*qd)).trigsimp() == 0
assert express(B[0]*B.x + B[1]*B.y + B[2]*B.z, A) == \
(B[0]*cos(q) - B[1]*sin(q))*A.x + (B[0]*sin(q) + \
B[1]*cos(q))*A.y + B[2]*A.z
assert express(B[0]*B.x + B[1]*B.y + B[2]*B.z, A, variables=True) == \
A[0]*A.x + A[1]*A.y + A[2]*A.z
assert express(A[0]*A.x + A[1]*A.y + A[2]*A.z, B) == \
(A[0]*cos(q) + A[1]*sin(q))*B.x + \
(-A[0]*sin(q) + A[1]*cos(q))*B.y + A[2]*B.z
assert express(A[0]*A.x + A[1]*A.y + A[2]*A.z, B, variables=True) == \
B[0]*B.x + B[1]*B.y + B[2]*B.z
N = B.orientnew('N', 'Axis', [-q, B.z])
assert N.variable_map(A) == {N[0]: A[0], N[2]: A[2], N[1]: A[1]}
C = A.orientnew('C', 'Axis', [q, A.x + A.y + A.z])
mapping = A.variable_map(C)
assert mapping[A[0]] == 2*C[0]*cos(q)/3 + C[0]/3 - 2*C[1]*sin(q + pi/6)/3 +\
C[1]/3 - 2*C[2]*cos(q + pi/3)/3 + C[2]/3
assert mapping[A[1]] == -2*C[0]*cos(q + pi/3)/3 + \
C[0]/3 + 2*C[1]*cos(q)/3 + C[1]/3 - 2*C[2]*sin(q + pi/6)/3 + C[2]/3
assert mapping[A[2]] == -2*C[0]*sin(q + pi/6)/3 + C[0]/3 - \
2*C[1]*cos(q + pi/3)/3 + C[1]/3 + 2*C[2]*cos(q)/3 + C[2]/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"])
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")
def test_express():
assert express(A.x, C) == cos(q3)*C.x + sin(q3)*C.z
assert express(A.y, C) == sin(q2)*sin(q3)*C.x + cos(q2)*C.y - \
sin(q2)*cos(q3)*C.z
assert express(A.z, C) == -sin(q3)*cos(q2)*C.x + sin(q2)*C.y + \
cos(q2)*cos(q3)*C.z
assert express(A.x, N) == cos(q1)*N.x + sin(q1)*N.y
assert express(A.y, N) == -sin(q1)*N.x + cos(q1)*N.y
assert express(A.z, N) == N.z
assert express(A.x, A) == A.x
assert express(A.y, A) == A.y
assert express(A.z, A) == A.z
assert express(A.x, B) == B.x
assert express(A.y, B) == cos(q2)*B.y - sin(q2)*B.z
assert express(A.z, B) == sin(q2)*B.y + cos(q2)*B.z
assert express(A.x, C) == cos(q3)*C.x + sin(q3)*C.z
assert express(A.y, C) == sin(q2)*sin(q3)*C.x + cos(q2)*C.y - \
sin(q2)*cos(q3)*C.z
assert express(A.z, C) == -sin(q3)*cos(q2)*C.x + sin(q2)*C.y + \
cos(q2)*cos(q3)*C.z
# Check to make sure UnitVectors get converted properly
assert express(N.x, N) == N.x
assert express(N.y, N) == N.y
assert express(N.z, N) == N.z
assert express(N.x, A) == (cos(q1)*A.x - sin(q1)*A.y)
assert express(N.y, A) == (sin(q1)*A.x + cos(q1)*A.y)
assert express(N.z, A) == A.z
assert express(N.x, B) == (cos(q1)*B.x - sin(q1)*cos(q2)*B.y + \
sin(q1)*sin(q2)*B.z)
assert express(N.y, B) == (sin(q1)*B.x + cos(q1)*cos(q2)*B.y - \
sin(q2)*cos(q1)*B.z)
assert express(N.z, B) == (sin(q2)*B.y + cos(q2)*B.z)
assert express(N.x, C) == (
(cos(q1)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*C.x -
sin(q1)*cos(q2)*C.y +
(sin(q3)*cos(q1)+sin(q1)*sin(q2)*cos(q3))*C.z)
assert express(N.y, C) == (
(sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1))*C.x +
cos(q1)*cos(q2)*C.y +
(sin(q1)*sin(q3) - sin(q2)*cos(q1)*cos(q3))*C.z)
assert express(N.z, C) == (-sin(q3)*cos(q2)*C.x + sin(q2)*C.y +
cos(q2)*cos(q3)*C.z)
assert express(A.x, N) == (cos(q1)*N.x + sin(q1)*N.y)
assert express(A.y, N) == (-sin(q1)*N.x + cos(q1)*N.y)
assert express(A.z, N) == N.z
assert express(A.x, A) == A.x
assert express(A.y, A) == A.y
assert express(A.z, A) == A.z
assert express(A.x, B) == B.x
assert express(A.y, B) == (cos(q2)*B.y - sin(q2)*B.z)
assert express(A.z, B) == (sin(q2)*B.y + cos(q2)*B.z)
assert express(A.x, C) == (cos(q3)*C.x + sin(q3)*C.z)
assert express(A.y, C) == (sin(q2)*sin(q3)*C.x + cos(q2)*C.y -
sin(q2)*cos(q3)*C.z)
assert express(A.z, C) == (-sin(q3)*cos(q2)*C.x + sin(q2)*C.y +
cos(q2)*cos(q3)*C.z)
assert express(B.x, N) == (cos(q1)*N.x + sin(q1)*N.y)
assert express(B.y, N) == (-sin(q1)*cos(q2)*N.x +
cos(q1)*cos(q2)*N.y + sin(q2)*N.z)
assert express(B.z, N) == (sin(q1)*sin(q2)*N.x -
sin(q2)*cos(q1)*N.y + cos(q2)*N.z)
assert express(B.x, A) == A.x
assert express(B.y, A) == (cos(q2)*A.y + sin(q2)*A.z)
assert express(B.z, A) == (-sin(q2)*A.y + cos(q2)*A.z)
assert express(B.x, B) == B.x
assert express(B.y, B) == B.y
assert express(B.z, B) == B.z
assert express(B.x, C) == (cos(q3)*C.x + sin(q3)*C.z)
assert express(B.y, C) == C.y
assert express(B.z, C) == (-sin(q3)*C.x + cos(q3)*C.z)
assert express(C.x, N) == (
(cos(q1)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*N.x +
(sin(q1)*cos(q3)+sin(q2)*sin(q3)*cos(q1))*N.y -
sin(q3)*cos(q2)*N.z)
assert express(C.y, N) == (
-sin(q1)*cos(q2)*N.x + cos(q1)*cos(q2)*N.y + sin(q2)*N.z)
assert express(C.z, N) == (
(sin(q3)*cos(q1)+sin(q1)*sin(q2)*cos(q3))*N.x +
(sin(q1)*sin(q3)-sin(q2)*cos(q1)*cos(q3))*N.y +
cos(q2)*cos(q3)*N.z)
assert express(C.x, A) == (cos(q3)*A.x + sin(q2)*sin(q3)*A.y -
sin(q3)*cos(q2)*A.z)
assert express(C.y, A) == (cos(q2)*A.y + sin(q2)*A.z)
assert express(C.z, A) == (sin(q3)*A.x - sin(q2)*cos(q3)*A.y +
cos(q2)*cos(q3)*A.z)
assert express(C.x, B) == (cos(q3)*B.x - sin(q3)*B.z)
assert express(C.y, B) == B.y
assert express(C.z, B) == (sin(q3)*B.x + cos(q3)*B.z)
assert express(C.x, C) == C.x
assert express(C.y, C) == C.y
assert express(C.z, C) == C.z == (C.z)
# Check to make sure Vectors get converted back to UnitVectors
assert N.x == express((cos(q1)*A.x - sin(q1)*A.y), N)
assert N.y == express((sin(q1)*A.x + cos(q1)*A.y), N)
assert N.x == express((cos(q1)*B.x - sin(q1)*cos(q2)*B.y +
sin(q1)*sin(q2)*B.z), N)
assert N.y == express((sin(q1)*B.x + cos(q1)*cos(q2)*B.y -
sin(q2)*cos(q1)*B.z), N)
assert N.z == express((sin(q2)*B.y + cos(q2)*B.z), N)
"""
These don't really test our code, they instead test the auto simplification
(or lack thereof) of SymPy.
assert N.x == express((
(cos(q1)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*C.x -
sin(q1)*cos(q2)*C.y +
(sin(q3)*cos(q1)+sin(q1)*sin(q2)*cos(q3))*C.z), N)
assert N.y == express((
(sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1))*C.x +
cos(q1)*cos(q2)*C.y +
(sin(q1)*sin(q3) - sin(q2)*cos(q1)*cos(q3))*C.z), N)
assert N.z == express((-sin(q3)*cos(q2)*C.x + sin(q2)*C.y +
cos(q2)*cos(q3)*C.z), N)
"""
assert A.x == express((cos(q1)*N.x + sin(q1)*N.y), A)
assert A.y == express((-sin(q1)*N.x + cos(q1)*N.y), A)
assert A.y == express((cos(q2)*B.y - sin(q2)*B.z), A)
assert A.z == express((sin(q2)*B.y + cos(q2)*B.z), A)
assert A.x == express((cos(q3)*C.x + sin(q3)*C.z), A)
# Tripsimp messes up here too.
#print express((sin(q2)*sin(q3)*C.x + cos(q2)*C.y -
# sin(q2)*cos(q3)*C.z), A)
assert A.y == express((sin(q2)*sin(q3)*C.x + cos(q2)*C.y -
sin(q2)*cos(q3)*C.z), A)
assert A.z == express((-sin(q3)*cos(q2)*C.x + sin(q2)*C.y +
cos(q2)*cos(q3)*C.z), A)
assert B.x == express((cos(q1)*N.x + sin(q1)*N.y), B)
assert B.y == express((-sin(q1)*cos(q2)*N.x +
cos(q1)*cos(q2)*N.y + sin(q2)*N.z), B)
assert B.z == express((sin(q1)*sin(q2)*N.x -
sin(q2)*cos(q1)*N.y + cos(q2)*N.z), B)
assert B.y == express((cos(q2)*A.y + sin(q2)*A.z), B)
assert B.z == express((-sin(q2)*A.y + cos(q2)*A.z), B)
assert B.x == express((cos(q3)*C.x + sin(q3)*C.z), B)
assert B.z == express((-sin(q3)*C.x + cos(q3)*C.z), B)
"""
assert C.x == express((
(cos(q1)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*N.x +
(sin(q1)*cos(q3)+sin(q2)*sin(q3)*cos(q1))*N.y -
sin(q3)*cos(q2)*N.z), C)
assert C.y == express((
-sin(q1)*cos(q2)*N.x + cos(q1)*cos(q2)*N.y + sin(q2)*N.z), C)
assert C.z == express((
(sin(q3)*cos(q1)+sin(q1)*sin(q2)*cos(q3))*N.x +
(sin(q1)*sin(q3)-sin(q2)*cos(q1)*cos(q3))*N.y +
cos(q2)*cos(q3)*N.z), C)
"""
assert C.x == express((cos(q3)*A.x + sin(q2)*sin(q3)*A.y -
sin(q3)*cos(q2)*A.z), C)
assert C.y == express((cos(q2)*A.y + sin(q2)*A.z), C)
assert C.z == express((sin(q3)*A.x - sin(q2)*cos(q3)*A.y +
cos(q2)*cos(q3)*A.z), C)
assert C.x == express((cos(q3)*B.x - sin(q3)*B.z), C)
assert C.z == express((sin(q3)*B.x + cos(q3)*B.z), C)
def test_express():
assert express(A.x, C) == cos(q3) * C.x + sin(q3) * C.z
assert express(A.y, C) == sin(q2) * sin(q3) * C.x + cos(q2) * C.y - sin(q2) * cos(q3) * C.z
assert express(A.z, C) == -sin(q3) * cos(q2) * C.x + sin(q2) * C.y + cos(q2) * cos(q3) * C.z
assert express(A.x, N) == cos(q1) * N.x + sin(q1) * N.y
assert express(A.y, N) == -sin(q1) * N.x + cos(q1) * N.y
assert express(A.z, N) == N.z
assert express(A.x, A) == A.x
assert express(A.y, A) == A.y
assert express(A.z, A) == A.z
assert express(A.x, B) == B.x
assert express(A.y, B) == cos(q2) * B.y - sin(q2) * B.z
assert express(A.z, B) == sin(q2) * B.y + cos(q2) * B.z
assert express(A.x, C) == cos(q3) * C.x + sin(q3) * C.z
assert express(A.y, C) == sin(q2) * sin(q3) * C.x + cos(q2) * C.y - sin(q2) * cos(q3) * C.z
assert express(A.z, C) == -sin(q3) * cos(q2) * C.x + sin(q2) * C.y + cos(q2) * cos(q3) * C.z
# Check to make sure UnitVectors get converted properly
assert express(N.x, N) == N.x
assert express(N.y, N) == N.y
assert express(N.z, N) == N.z
assert express(N.x, A) == (cos(q1) * A.x - sin(q1) * A.y)
assert express(N.y, A) == (sin(q1) * A.x + cos(q1) * A.y)
assert express(N.z, A) == A.z
assert express(N.x, B) == (cos(q1) * B.x - sin(q1) * cos(q2) * B.y + sin(q1) * sin(q2) * B.z)
assert express(N.y, B) == (sin(q1) * B.x + cos(q1) * cos(q2) * B.y - sin(q2) * cos(q1) * B.z)
assert express(N.z, B) == (sin(q2) * B.y + cos(q2) * B.z)
assert express(N.x, C) == (
(cos(q1) * cos(q3) - sin(q1) * sin(q2) * sin(q3)) * C.x
- sin(q1) * cos(q2) * C.y
+ (sin(q3) * cos(q1) + sin(q1) * sin(q2) * cos(q3)) * C.z
)
assert express(N.y, C) == (
(sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1)) * C.x
+ cos(q1) * cos(q2) * C.y
+ (sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)) * C.z
)
assert express(N.z, C) == (-sin(q3) * cos(q2) * C.x + sin(q2) * C.y + cos(q2) * cos(q3) * C.z)
assert express(A.x, N) == (cos(q1) * N.x + sin(q1) * N.y)
assert express(A.y, N) == (-sin(q1) * N.x + cos(q1) * N.y)
assert express(A.z, N) == N.z
assert express(A.x, A) == A.x
assert express(A.y, A) == A.y
assert express(A.z, A) == A.z
assert express(A.x, B) == B.x
assert express(A.y, B) == (cos(q2) * B.y - sin(q2) * B.z)
assert express(A.z, B) == (sin(q2) * B.y + cos(q2) * B.z)
assert express(A.x, C) == (cos(q3) * C.x + sin(q3) * C.z)
assert express(A.y, C) == (sin(q2) * sin(q3) * C.x + cos(q2) * C.y - sin(q2) * cos(q3) * C.z)
assert express(A.z, C) == (-sin(q3) * cos(q2) * C.x + sin(q2) * C.y + cos(q2) * cos(q3) * C.z)
assert express(B.x, N) == (cos(q1) * N.x + sin(q1) * N.y)
assert express(B.y, N) == (-sin(q1) * cos(q2) * N.x + cos(q1) * cos(q2) * N.y + sin(q2) * N.z)
assert express(B.z, N) == (sin(q1) * sin(q2) * N.x - sin(q2) * cos(q1) * N.y + cos(q2) * N.z)
assert express(B.x, A) == A.x
assert express(B.y, A) == (cos(q2) * A.y + sin(q2) * A.z)
assert express(B.z, A) == (-sin(q2) * A.y + cos(q2) * A.z)
assert express(B.x, B) == B.x
assert express(B.y, B) == B.y
assert express(B.z, B) == B.z
assert express(B.x, C) == (cos(q3) * C.x + sin(q3) * C.z)
assert express(B.y, C) == C.y
assert express(B.z, C) == (-sin(q3) * C.x + cos(q3) * C.z)
assert express(C.x, N) == (
(cos(q1) * cos(q3) - sin(q1) * sin(q2) * sin(q3)) * N.x
+ (sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1)) * N.y
- sin(q3) * cos(q2) * N.z
)
assert express(C.y, N) == (-sin(q1) * cos(q2) * N.x + cos(q1) * cos(q2) * N.y + sin(q2) * N.z)
assert express(C.z, N) == (
(sin(q3) * cos(q1) + sin(q1) * sin(q2) * cos(q3)) * N.x
+ (sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)) * N.y
+ cos(q2) * cos(q3) * N.z
)
assert express(C.x, A) == (cos(q3) * A.x + sin(q2) * sin(q3) * A.y - sin(q3) * cos(q2) * A.z)
assert express(C.y, A) == (cos(q2) * A.y + sin(q2) * A.z)
assert express(C.z, A) == (sin(q3) * A.x - sin(q2) * cos(q3) * A.y + cos(q2) * cos(q3) * A.z)
assert express(C.x, B) == (cos(q3) * B.x - sin(q3) * B.z)
assert express(C.y, B) == B.y
assert express(C.z, B) == (sin(q3) * B.x + cos(q3) * B.z)
assert express(C.x, C) == C.x
assert express(C.y, C) == C.y
assert express(C.z, C) == C.z == (C.z)
# Check to make sure Vectors get converted back to UnitVectors
assert N.x == express((cos(q1) * A.x - sin(q1) * A.y), N)
assert N.y == express((sin(q1) * A.x + cos(q1) * A.y), N)
assert N.x == express((cos(q1) * B.x - sin(q1) * cos(q2) * B.y + sin(q1) * sin(q2) * B.z), N)
assert N.y == express((sin(q1) * B.x + cos(q1) * cos(q2) * B.y - sin(q2) * cos(q1) * B.z), N)
assert N.z == express((sin(q2) * B.y + cos(q2) * B.z), N)
"""
These don't really test our code, they instead test the auto simplification
(or lack thereof) of SymPy.
assert N.x == express((
(cos(q1)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*C.x -
sin(q1)*cos(q2)*C.y +
(sin(q3)*cos(q1)+sin(q1)*sin(q2)*cos(q3))*C.z), N)
assert N.y == express((
(sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1))*C.x +
cos(q1)*cos(q2)*C.y +
(sin(q1)*sin(q3) - sin(q2)*cos(q1)*cos(q3))*C.z), N)
assert N.z == express((-sin(q3)*cos(q2)*C.x + sin(q2)*C.y +
cos(q2)*cos(q3)*C.z), N)
"""
assert A.x == express((cos(q1) * N.x + sin(q1) * N.y), A)
assert A.y == express((-sin(q1) * N.x + cos(q1) * N.y), A)
assert A.y == express((cos(q2) * B.y - sin(q2) * B.z), A)
assert A.z == express((sin(q2) * B.y + cos(q2) * B.z), A)
assert A.x == express((cos(q3) * C.x + sin(q3) * C.z), A)
# Tripsimp messes up here too.
# print express((sin(q2)*sin(q3)*C.x + cos(q2)*C.y -
# sin(q2)*cos(q3)*C.z), A)
assert A.y == express((sin(q2) * sin(q3) * C.x + cos(q2) * C.y - sin(q2) * cos(q3) * C.z), A)
assert A.z == express((-sin(q3) * cos(q2) * C.x + sin(q2) * C.y + cos(q2) * cos(q3) * C.z), A)
assert B.x == express((cos(q1) * N.x + sin(q1) * N.y), B)
assert B.y == express((-sin(q1) * cos(q2) * N.x + cos(q1) * cos(q2) * N.y + sin(q2) * N.z), B)
assert B.z == express((sin(q1) * sin(q2) * N.x - sin(q2) * cos(q1) * N.y + cos(q2) * N.z), B)
assert B.y == express((cos(q2) * A.y + sin(q2) * A.z), B)
assert B.z == express((-sin(q2) * A.y + cos(q2) * A.z), B)
assert B.x == express((cos(q3) * C.x + sin(q3) * C.z), B)
assert B.z == express((-sin(q3) * C.x + cos(q3) * C.z), B)
"""
assert C.x == express((
(cos(q1)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*N.x +
(sin(q1)*cos(q3)+sin(q2)*sin(q3)*cos(q1))*N.y -
sin(q3)*cos(q2)*N.z), C)
assert C.y == express((
-sin(q1)*cos(q2)*N.x + cos(q1)*cos(q2)*N.y + sin(q2)*N.z), C)
assert C.z == express((
(sin(q3)*cos(q1)+sin(q1)*sin(q2)*cos(q3))*N.x +
(sin(q1)*sin(q3)-sin(q2)*cos(q1)*cos(q3))*N.y +
cos(q2)*cos(q3)*N.z), C)
"""
assert C.x == express((cos(q3) * A.x + sin(q2) * sin(q3) * A.y - sin(q3) * cos(q2) * A.z), C)
assert C.y == express((cos(q2) * A.y + sin(q2) * A.z), C)
assert C.z == express((sin(q3) * A.x - sin(q2) * cos(q3) * A.y + cos(q2) * cos(q3) * A.z), C)
assert C.x == express((cos(q3) * B.x - sin(q3) * B.z), C)
assert C.z == express((sin(q3) * B.x + cos(q3) * B.z), C)
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'])
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')
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]]]
)
