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