.. DO NOT EDIT. .. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY. .. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE: .. "examples\plot_rolling_axle_uneven_street.py" .. LINE NUMBERS ARE GIVEN BELOW. .. only:: html .. note:: :class: sphx-glr-download-link-note :ref:`Go to the end ` to download the full example code. .. rst-class:: sphx-glr-example-title .. _sphx_glr_examples_plot_rolling_axle_uneven_street.py: Axle Rolling on an Uneven Surface ================================= Description ----------- Two thin homogeneous wheels of mass :math:`m_L, m_R` and radius :math:`r_L, r_R` are attached to a massless axle of length :math:`l`. They can rotate independently from each other and roll on a smooth surface in the X/Y plane without slipping. The surface is smooth enough that at any point in time the wheels touch the surface at exactly one point. A particle of mass :math:`m_o` is attached on the rim of each wheel. Gravity points in the negative Z direction. Notes ----- - While on a flat surface this system has two degrees of freedom, on a general smooth uneven surface the system has only **one** degree of freedom. - The system is difficult to integrate numerically, it seems to be very stiff. The reaction forces enforcing the no slip condition must be very large: the coordinates of the contact points change (almost) non-differentiably at times (In the plot below, at around 1.5 sec.) States ------ - :math:`q_L, q_R` : the angles of the wheels w.r.t. the axle. - :math:`u_L, u_R` : the speeds of the wheels. - :math:`x_L, z_L, x_R, z_R` : the coordinates of the contact points. - :math:`ux_L, uz_L, ux_R, uz_R` : the speeds of the contact points. - :math:`l_y, l_z, r_y, r_z` : the components of the vectors from the contact points to the centers of mass. - :math:`ul_y, ul_z, ur_y, ur_z` : the speeds of these components. Parameters ---------- - :math:`m_L, m_R, m_o` : the masses of the wheels and the particles. - :math:`g` : the gravitational acceleration. - :math:`r_L, r_R` : the radii of the wheels. - :math:`l` : the length of the axle. .. GENERATED FROM PYTHON SOURCE LINES 50-63 .. code-block:: Python import sympy as sm import sympy.physics.mechanics as me import time import numpy as np import matplotlib.pyplot as plt from scipy.integrate import solve_ivp from scipy.optimize import root, minimize from scipy.interpolate import interp1d from matplotlib.animation import FuncAnimation from matplotlib.patches import Ellipse from matplotlib.transforms import Affine2D .. GENERATED FROM PYTHON SOURCE LINES 64-68 Equations of Motion ------------------- Rotation angles of the wheels and the body, and their speeds. .. GENERATED FROM PYTHON SOURCE LINES 68-71 .. code-block:: Python qL, qR, q2, q3 = me.dynamicsymbols('qL qR q2 q3') uL, uR, u2, u3 = me.dynamicsymbols('uL uR u2 u3') .. GENERATED FROM PYTHON SOURCE LINES 72-73 Coordinates of the contact points of the left / right wheel and their speeds. .. GENERATED FROM PYTHON SOURCE LINES 73-76 .. code-block:: Python xL, yL, xR, yR = me.dynamicsymbols('xL yL xR yR') uxL, uyL, uxR, uyR = me.dynamicsymbols('uxL uyL uxR uyR') .. GENERATED FROM PYTHON SOURCE LINES 77-79 Components of the vectors from the contact points to the centers of mass, in AX and their speeds. .. GENERATED FROM PYTHON SOURCE LINES 79-82 .. code-block:: Python ly, lz, ry, rz = me.dynamicsymbols('ly lz ry rz') uly, ulz, ury, urz = me.dynamicsymbols('uly ulz ury urz') .. GENERATED FROM PYTHON SOURCE LINES 83-85 Parameters of the system: masses, gravity, radii of the wheels, and distance between the wheels. .. GENERATED FROM PYTHON SOURCE LINES 85-88 .. code-block:: Python mL, mR, mo, g, rL, rR, l = sm.symbols( 'mL mR mo g rL rR l') .. GENERATED FROM PYTHON SOURCE LINES 89-90 Parameters for the surface. .. GENERATED FROM PYTHON SOURCE LINES 90-93 .. code-block:: Python amplitude, frequenz, reibung, alpha, beta, gamma = sm.symbols( 'amplitude frequenz reibung alpha beta gamma') .. GENERATED FROM PYTHON SOURCE LINES 94-95 Define the reference frames and points. .. GENERATED FROM PYTHON SOURCE LINES 95-103 .. code-block:: Python N, AX, AL, AR = sm.symbols('N, AX, AL, AR', cls=me.ReferenceFrame) O, CPL, CPR, DmcL, DmcR = sm.symbols('O, CPL, CPR, DmcL, DmcR', cls=me.Point) m_DmcL, m_DmcR = sm.symbols('m_DmcL, m_DmcR', cls=me.Point) O.set_vel(N, 0) t = me.dynamicsymbols._t .. GENERATED FROM PYTHON SOURCE LINES 104-105 The axle does not rotate around itself. .. GENERATED FROM PYTHON SOURCE LINES 105-108 .. code-block:: Python AX.orient_body_fixed(N, [q3, q2, 0], 'ZYX') AX.set_ang_vel(N, u2*AX.y + u3*AX.z) .. GENERATED FROM PYTHON SOURCE LINES 109-111 The left wheel rotates around the axle, that is, around AX.x, similarly for the right wheel. .. GENERATED FROM PYTHON SOURCE LINES 111-116 .. code-block:: Python AL.orient_axis(AX, qL, AX.x) AL.set_ang_vel(AX, uL*AX.x) AR.orient_axis(AX, qR, AX.x) AR.set_ang_vel(AX, uR*AX.x) .. GENERATED FROM PYTHON SOURCE LINES 117-118 Particles attached to the wheels. .. GENERATED FROM PYTHON SOURCE LINES 118-122 .. code-block:: Python m_DmcL.set_pos(DmcL, rL*AL.y) m_DmcR.set_pos(DmcR, rR*AR.y) .. GENERATED FROM PYTHON SOURCE LINES 123-124 The street is modelled. *rumpel* must be an integer. .. GENERATED FROM PYTHON SOURCE LINES 124-142 .. code-block:: Python x_h, y_h = sm.symbols('x_h y_h') rumpel = 2 def gesamt(x, y, amplitude, frequenz, rumpel): strasse = sum([amplitude/j * (sm.sin(j*frequenz*sm.pi * x) + sm.sin(j*frequenz*sm.pi * y)) for j in range(1, rumpel)]) return strasse def gesamt_plot(x_h, y_h, amplitude, frequenz): return sum([amplitude/j * (sm.sin(j*frequenz*sm.pi * x_h) + sm.sin(j*frequenz*sm.pi * y_h)) for j in range(1, rumpel)]) .. GENERATED FROM PYTHON SOURCE LINES 143-145 Create the dictionary to replace :math:`\dfrac{d}{dt}(\textrm{gen. coord})` with the corresponding symbols. .. GENERATED FROM PYTHON SOURCE LINES 145-161 .. code-block:: Python kin_dict = { xL.diff(t): uxL, yL.diff(t): uyL, xR.diff(t): uxR, yR.diff(t): uyR, qL.diff(t): uL, qR.diff(t): uR, ly.diff(t): uly, lz.diff(t): ulz, ry.diff(t): ury, rz.diff(t): urz, q2.diff(t): u2, q3.diff(t): u3, } .. GENERATED FROM PYTHON SOURCE LINES 162-163 CPL is the contact point, where the left wheel touches the street. .. GENERATED FROM PYTHON SOURCE LINES 163-169 .. code-block:: Python CPL.set_pos(O, xL*N.x + yL*N.y + gesamt(xL, yL, amplitude, frequenz, rumpel)*N.z) CPL.set_vel(N, uxL*N.x + uyL*N.y + gesamt(xL, yL, amplitude, frequenz, rumpel).diff(t)*N.z) .. GENERATED FROM PYTHON SOURCE LINES 170-173 Define the vectors pointing from the contact point to the corresponding center of the wheel. :math:`vector_L \perp AX.x` and :math:`vector_R \perp AX.x`, so they have no component in AX.x direction. .. GENERATED FROM PYTHON SOURCE LINES 173-177 .. code-block:: Python vectorL = ly * AX.y + lz * AX.z vectorR = ry * AX.y + rz * AX.z .. GENERATED FROM PYTHON SOURCE LINES 178-180 :math:`vector_L` and :math:`vector_R` must have magnitude equal to the radius of the respective wheel. .. GENERATED FROM PYTHON SOURCE LINES 180-187 .. code-block:: Python constr_length = sm.Matrix([ vectorL.magnitude() - rL, vectorR.magnitude() - rR, ]) constr_length .. rst-class:: sphx-glr-script-out .. code-block:: none Matrix([ [-rL + sqrt(ly(t)**2 + lz(t)**2)], [-rR + sqrt(ry(t)**2 + rz(t)**2)]]) .. GENERATED FROM PYTHON SOURCE LINES 188-189 Set centers of mass of wheels, second contact point CPR. .. GENERATED FROM PYTHON SOURCE LINES 189-193 .. code-block:: Python DmcL.set_pos(CPL, vectorL) DmcR.set_pos(DmcL, l*AX.x) CPR.set_pos(DmcR, -vectorR) .. GENERATED FROM PYTHON SOURCE LINES 194-198 :math:`vector_L` must be in the plane formed by the gradient :math:`n_L` at the point (xL, yL) and by AX.x the direction of the axle, that is :math:`vector_L \circ (n_L \times AX.x) = 0`. Same for :math:`vector_R`. .. GENERATED FROM PYTHON SOURCE LINES 198-215 .. code-block:: Python nL = (-gesamt(xL, yL, amplitude, frequenz, rumpel).diff(xL) * N.x - gesamt(xL, yL, amplitude, frequenz, rumpel).diff(yL) * N.y + N.z).normalize() nR = (-gesamt(xR, yR, amplitude, frequenz, rumpel).diff(xR) * N.x - gesamt(xR, yR, amplitude, frequenz, rumpel).diff(yR) * N.y + N.z).normalize() perpL = nL.cross(AX.x) perpR = nR.cross(AX.x) constrT = sm.Matrix([ perpL.dot(vectorL), perpR.dot(vectorR), ]) .. GENERATED FROM PYTHON SOURCE LINES 216-217 Determine the constraints for :math:`xR, yR, q_2` for the location of CPR. .. GENERATED FROM PYTHON SOURCE LINES 217-226 .. code-block:: Python CPR_pos = xR*N.x + yR*N.y + gesamt(xR, yR, amplitude, frequenz, rumpel)*N.z delta_loc = CPR.pos_from(O) - CPR_pos constr_CPR = sm.Matrix([ delta_loc.dot(N.x), delta_loc.dot(N.y), delta_loc.dot(N.z), ]) .. GENERATED FROM PYTHON SOURCE LINES 227-228 Combine the configuration constraints. .. GENERATED FROM PYTHON SOURCE LINES 228-233 .. code-block:: Python config_constr = constr_length.col_join(constrT).col_join(constr_CPR) print(f"config_constr contains {sm.count_ops(config_constr)} operations " f"and has shape {config_constr.shape}") .. rst-class:: sphx-glr-script-out .. code-block:: none config_constr contains 484 operations and has shape (7, 1) .. GENERATED FROM PYTHON SOURCE LINES 234-235 No slip condition for CPL and for CPR. .. GENERATED FROM PYTHON SOURCE LINES 235-253 .. code-block:: Python DmcL.set_vel(N, AL.ang_vel_in(N).cross(vectorL)) # No slip condition for DmcL DmcR.set_vel(N, AR.ang_vel_in(N).cross(vectorR)) # No slip condition for DmcR vDmcL = DmcL.pos_from(O).diff(t, N) vDmcR = DmcR.pos_from(O).diff(t, N) deltaL_vel = vDmcL - DmcL.vel(N) deltaR_vel = vDmcR - DmcR.vel(N) frame = N constr_no_slip = sm.Matrix([ deltaL_vel.dot(frame.x), deltaL_vel.dot(frame.y), deltaR_vel.dot(frame.x), deltaR_vel.dot(frame.y), ]) .. GENERATED FROM PYTHON SOURCE LINES 254-256 Now the speeds of DmcL, DmcR are definded, so the speeds of m_DmcL, m_DmcR can be defined as well. .. GENERATED FROM PYTHON SOURCE LINES 256-259 .. code-block:: Python m_DmcL.v2pt_theory(DmcL, N, AL) _ = m_DmcR.v2pt_theory(DmcR, N, AR) .. GENERATED FROM PYTHON SOURCE LINES 260-261 Combine the speed constraints. .. GENERATED FROM PYTHON SOURCE LINES 261-266 .. code-block:: Python speed_constr = (constr_no_slip.col_join(config_constr.diff(t))) print(f"speed_constr contains {sm.count_ops(speed_constr):,} operations" f"and have shape {speed_constr.shape}") .. rst-class:: sphx-glr-script-out .. code-block:: none speed_constr contains 2,705 operationsand have shape (11, 1) .. GENERATED FROM PYTHON SOURCE LINES 267-268 Kane's Equations. .. GENERATED FROM PYTHON SOURCE LINES 268-285 .. code-block:: Python iXXL = 0.5 * mL * rL**2 iYYL = 0.25 * mL * rL**2 iZZL = 0.25 * mL * rL**2 iXXR = 0.5 * mR * rR**2 iYYR = 0.25 * mR * rR**2 iZZR = 0.25 * mR * rR**2 IL = me.inertia(AL, iXXL, iYYL, iZZL) IR = me.inertia(AR, iXXR, iYYR, iZZR) BodyL = me.RigidBody('BodyL', DmcL, AL, mL, (IL, DmcL)) BodyR = me.RigidBody('BodyR', DmcR, AR, mR, (IR, DmcR)) partL = me.Particle('partL', m_DmcL, mo) partR = me.Particle('partR', m_DmcR, mo) BODY = [BodyL, BodyR, partL, partR] .. GENERATED FROM PYTHON SOURCE LINES 286-287 Set the external forces acting on the system. .. GENERATED FROM PYTHON SOURCE LINES 287-293 .. code-block:: Python FL1 = [(DmcL, -mL*g*N.z), (DmcR, -mR*g*N.z), (m_DmcL, -mo*g*N.z), (m_DmcR, -mo*g*N.z)] Torque = [(AL, -reibung * uL*AX.x), (AR, -reibung * uR*AX.x)] FL = FL1 + Torque .. GENERATED FROM PYTHON SOURCE LINES 294-295 Kane's method. .. GENERATED FROM PYTHON SOURCE LINES 295-329 .. code-block:: Python kd = sm.Matrix([key - value for key, value in kin_dict.items()]) speed_constraint = speed_constr q_1 = [xL, yL, qL, qR, q3] q_2 = ([q2, xR, yR] + [ly, lz, ry, rz]) q_ind = q_1 q_dep = q_2 u_ind = [uL] u_dep = ([uxL, uyL, uxR, uyR, u2, uR, u3] + [uly, ulz, ury, urz]) kane = me.KanesMethod( N, q_ind=q_ind, q_dependent=q_dep, u_ind=u_ind, u_dependent=u_dep, kd_eqs=kd, velocity_constraints=speed_constraint, configuration_constraints=config_constr, ) fr, frstar = kane.kanes_equations(BODY, FL) MM = kane.mass_matrix_full forcing = kane.forcing_full print(f'The mass matrix MM has {sm.count_ops(MM):,} ' f'operations, shape = {MM.shape}') print(f"The force vector forcing has {sm.count_ops(forcing):,} operations, " f"shape={forcing.shape}") .. rst-class:: sphx-glr-script-out .. code-block:: none The mass matrix MM has 5,991,567 operations, shape = (24, 24) The force vector forcing has 7,436,635 operations, shape=(24, 1) .. GENERATED FROM PYTHON SOURCE LINES 330-331 Compilation. .. GENERATED FROM PYTHON SOURCE LINES 331-338 .. code-block:: Python qL = q_ind + q_dep + u_ind + u_dep pL = [mL, mR, mo, g, rL, rR, l, amplitude, frequenz, reibung] MM_lam = sm.lambdify(qL + pL, MM, cse=True) force_lam = sm.lambdify(qL + pL, forcing, cse=True) .. GENERATED FROM PYTHON SOURCE LINES 339-340 Needed for verification of accuracy. .. GENERATED FROM PYTHON SOURCE LINES 340-342 .. code-block:: Python config_constr_lam = sm.lambdify(qL + pL, config_constr, cse=True) .. GENERATED FROM PYTHON SOURCE LINES 343-344 Needed for roots to solve for q_dep. .. GENERATED FROM PYTHON SOURCE LINES 344-354 .. code-block:: Python config1_constr_lam = sm.lambdify(q_dep + q_ind + pL, config_constr, cse=True) AA, bb = sm.linear_eq_to_matrix(speed_constr.subs(kin_dict), u_dep) AA_lam = sm.lambdify(q_dep + q_ind + u_ind + pL, AA, cse=True) bb_lam = sm.lambdify(q_dep + q_ind + u_ind + pL, bb, cse=True) speed_constr_lam = sm.lambdify(qL + pL, speed_constr.subs(kin_dict), cse=True) .. GENERATED FROM PYTHON SOURCE LINES 355-356 Set Parameters, independent coordinates and the independent speed. .. GENERATED FROM PYTHON SOURCE LINES 356-379 .. code-block:: Python mL1 = 1.0 mL2 = 4 * mL1 mo1 = 0.01 g1 = 9.81 rL1 = 2.0 rR1 = 1.0 l1 = 5.0 amplitude1 = 0.15 frequenz1 = 0.25 reibung1 = 0.0 xL1 = 0.0 yL1 = 0.0 q31 = np.deg2rad(45.0) qL1 = 0.0 qR1 = 0.0 uL1 = 1.0 pL_vals = [mL1, mL2, mo1, g1, rL1, rR1, l1, amplitude1, frequenz1, reibung1] .. GENERATED FROM PYTHON SOURCE LINES 380-382 Find a good initial guess for the dependent coords, then improve the accuracy by solving the nonlinear system of equations. .. GENERATED FROM PYTHON SOURCE LINES 382-395 .. code-block:: Python config_sum = sum([i**2 for i in config_constr]) config_sum_lam = sm.lambdify(q_dep + q_ind + pL, config_sum, cse=True) def func(y0, args): return config_sum_lam(*y0, *args) q_ind_vals = [xL1, yL1, qL1, qR1, q31] y0 = [1.0] * len(q_dep) args = q_ind_vals + pL_vals .. GENERATED FROM PYTHON SOURCE LINES 396-398 Force :math:`l_z, r_z` to be positive, so that the wheels are above the street. .. GENERATED FROM PYTHON SOURCE LINES 398-400 .. code-block:: Python bounds = [(None, None)] * 4 + [(0.0, None)] + [(None, None)] + [(0.0, None)] .. GENERATED FROM PYTHON SOURCE LINES 401-403 Loop around minimize to improve the accuracy of the initial guess for the dependent coordinates. .. GENERATED FROM PYTHON SOURCE LINES 403-416 .. code-block:: Python for _ in range(5): res = minimize(func, y0, args=(args,), method='Nelder-Mead', options={'xatol': 1e-8, 'fatol': 1e-8}, bounds=bounds) y0 = res.x print(res.message) print(f"Minimum of config_sum is {res.fun:.5e}") def func1(y0, args): return config1_constr_lam(*y0, *args).squeeze() .. rst-class:: sphx-glr-script-out .. code-block:: none Maximum number of function evaluations has been exceeded. Minimum of config_sum is 1.64560e-01 Optimization terminated successfully. Minimum of config_sum is 5.37800e-17 Optimization terminated successfully. Minimum of config_sum is 4.73943e-17 Optimization terminated successfully. Minimum of config_sum is 4.72940e-17 Optimization terminated successfully. Minimum of config_sum is 4.72044e-17 .. GENERATED FROM PYTHON SOURCE LINES 417-419 Solve the nonlinear system using the results of the minimizing above as initial guess. .. GENERATED FROM PYTHON SOURCE LINES 419-429 .. code-block:: Python res1 = root(func1, y0, args=(args,), method='hybr') print('\n') print(res1.message) print(f"Difference in norm between res and res1 is " f" {np.linalg.norm(res.x - res1.x):.5e}") q_dep_vals = res1.x for i, j in zip(q_dep, q_dep_vals): print(f"{i} = {j:.5f}") .. rst-class:: sphx-glr-script-out .. code-block:: none The solution converged. Difference in norm between res and res1 is 8.23902e-09 q2(t) = 0.17943 xR(t) = 3.60497 yR(t) = 3.60497 ly(t) = -0.00000 lz(t) = 2.00000 ry(t) = -0.00000 rz(t) = 1.00000 .. GENERATED FROM PYTHON SOURCE LINES 430-431 Solve the *linear* system of equations for the dependent speeds. .. GENERATED FROM PYTHON SOURCE LINES 431-439 .. code-block:: Python u_dep_vals = np.linalg.solve(AA_lam(*q_dep_vals, *q_ind_vals, uL1, *pL_vals), bb_lam(*q_dep_vals, *q_ind_vals, uL1, *pL_vals)) u_dep_vals = list(i[0] for i in u_dep_vals) for i, j in zip(u_dep, u_dep_vals): print(f"{i} = {j:.5f}") .. rst-class:: sphx-glr-script-out .. code-block:: none uxL(t) = 1.51489 uyL(t) = -1.51489 uxR(t) = 0.69650 uyR(t) = -0.69650 u2(t) = 0.00000 uR(t) = 1.00810 u3(t) = 0.20162 uly(t) = 0.07041 ulz(t) = 0.00000 ury(t) = -0.05909 urz(t) = -0.00000 .. GENERATED FROM PYTHON SOURCE LINES 440-442 Find the minimum curvature of the surface and check it is larger than the wheels. (Formula from the internet) .. GENERATED FROM PYTHON SOURCE LINES 442-470 .. code-block:: Python x_h, y_h = sm.symbols('x_h y_h') fx = gesamt_plot(x_h, y_h, amplitude, frequenz).diff(x_h) fy = gesamt_plot(x_h, y_h, amplitude, frequenz).diff(y_h) fxx = gesamt_plot(x_h, y_h, amplitude, frequenz).diff(x_h, 2) fyy = gesamt_plot(x_h, y_h, amplitude, frequenz).diff(y_h, 2) fxy = gesamt_plot(x_h, y_h, amplitude, frequenz).diff(x_h, y_h) E = 1 + fx**2 F = fx * fy G = 1 + fy**2 L = fxx / sm.sqrt(1 + fx**2 + fy**2) M = fxy / sm.sqrt(1 + fx**2 + fy**2) NN = fyy / sm.sqrt(1 + fx**2 + fy**2) I1 = sm.Matrix([[E, F], [F, G]]) II = sm.Matrix([[L, M], [M, NN]]) # Shape operator S = I1.inv() * II # Eigenvalues = principal curvatures k1, k2 = S.eigenvals().keys() .. GENERATED FROM PYTHON SOURCE LINES 471-472 Find the principal curvatures. .. GENERATED FROM PYTHON SOURCE LINES 472-506 .. code-block:: Python k1_lam = sm.lambdify([x_h, y_h, amplitude, frequenz], k1, cse=True) k2_lam = sm.lambdify([x_h, y_h, amplitude, frequenz], k2, cse=True) def func1(x0, args): return np.abs(1.0 / k1_lam(*x0, *args)) def func2(x0, args): return np.abs(1.0 / k2_lam(*x0, *args)) x0 = np.array((5.0, 10.0)) # initial guess args = np.array((amplitude1, frequenz1)) for _ in range(10): minimal1 = minimize(func1, x0, args, tol=1e-6) x0 = minimal1.x for _ in range(10): minimal2 = minimize(func2, x0, args, tol=1e-6) x0 = minimal2.x print("minimal1:", minimal1.message) print("minimal2:", minimal2.message) min_radius = min(minimal1.fun, minimal2.fun) print('maximally admissible radius = {:.4f}'.format(min_radius)) if min_radius < max(rL1, rR1): raise ValueError("The initial conditions are not viable, because " "the radius of the wheels is larger than the maximally " "admissible radius.") .. rst-class:: sphx-glr-script-out .. code-block:: none minimal1: Optimization terminated successfully. minimal2: Optimization terminated successfully. maximally admissible radius = 10.8076 .. GENERATED FROM PYTHON SOURCE LINES 507-511 Numerical integration --------------------- The following lists are used to store the condition number of the mass matrix .. GENERATED FROM PYTHON SOURCE LINES 511-514 .. code-block:: Python time_list = [0.0] cond_list = [0.0] .. GENERATED FROM PYTHON SOURCE LINES 515-516 Numerical integration of the system of differential equations. .. GENERATED FROM PYTHON SOURCE LINES 516-553 .. code-block:: Python def gradient(t, y0, args): MM_eval = MM_lam(*y0, *args) if t > time_list[-1]: cond_list.append(np.linalg.cond(MM_eval)) time_list.append(t) force_eval = force_lam(*y0, *args) dydt = np.linalg.solve(MM_eval, force_eval).squeeze() return dydt y0 = q_ind_vals + q_dep_vals.tolist() + [uL1] + u_dep_vals args = pL_vals interval = 7.0 schritte = 500 rtol = 1e-6 atol = 1e-6 sys_times = np.linspace(0., interval, schritte) start_time = time.time() resultat1 = solve_ivp(gradient, [0, interval], y0, args=(args,), t_eval=sys_times, method='Radau', rtol=rtol, atol=atol, ) print(resultat1.message) print(f"solve_ivp made {resultat1.nfev:,} function evaluations") end_time = time.time() print(f"Elapsed time for integration: {end_time - start_time:.2f} seconds") resultat = resultat1.y.T print(resultat.shape) .. rst-class:: sphx-glr-script-out .. code-block:: none The solver successfully reached the end of the integration interval. solve_ivp made 456,101 function evaluations Elapsed time for integration: 132.17 seconds (500, 24) .. GENERATED FROM PYTHON SOURCE LINES 554-555 Plot the condition number of the mass matrix and its derivative over time. .. GENERATED FROM PYTHON SOURCE LINES 555-571 .. code-block:: Python fig, ax = plt.subplots(2, 1, figsize=(8, 4), constrained_layout=True, sharex=True) ax[0].plot(time_list[1:], cond_list[1:]) ax[0].set_yscale('log') ax[0].set_ylabel('Condition Number') ax[0].set_title('Condition Number of Mass Matrix Over Time') print('maximal condition number = {:.2e}'.format(max(cond_list[1:]))) print('minimal condition number = {:.2e}'.format(min(cond_list[1:]))) ableitung = np.gradient(cond_list[1:], time_list[1:]) ax[1].set_yscale('log') ax[1].plot(time_list[1:], ableitung) ax[1].set_xlabel('Time') ax[1].set_ylabel('Derivative of Condition Number') _ = ax[1].set_title('Derivative of Condition Number Over Time') .. image-sg:: /examples/images/sphx_glr_plot_rolling_axle_uneven_street_001.png :alt: Condition Number of Mass Matrix Over Time, Derivative of Condition Number Over Time :srcset: /examples/images/sphx_glr_plot_rolling_axle_uneven_street_001.png :class: sphx-glr-single-img .. rst-class:: sphx-glr-script-out .. code-block:: none maximal condition number = 9.17e+11 minimal condition number = 9.33e+02 .. GENERATED FROM PYTHON SOURCE LINES 572-573 See how well the constraints are kept. .. GENERATED FROM PYTHON SOURCE LINES 573-590 .. code-block:: Python fig, ax = plt.subplots(2, 1, figsize=(8, 6), layout='constrained', sharex=True) for i in range(7): ax[0].plot(resultat1.t, config_constr_lam(*resultat1.y, *args)[i].T, label=f'$constr_{i}$') ax[0].set_ylabel('Error') ax[1].set_ylabel('Error') ax[1].set_xlabel('Time') ax[0].set_title('Errors in Configuration Constraints with solve_ivp') ax[1].set_title('Errors in Speed Constraints with solve_ivp') ax[0].legend() for i in range(10): ax[1].plot(resultat1.t, speed_constr_lam(*resultat1.y, *args)[i].T, label=f'$constr_{i}$') _ = ax[1].legend() .. image-sg:: /examples/images/sphx_glr_plot_rolling_axle_uneven_street_002.png :alt: Errors in Configuration Constraints with solve_ivp, Errors in Speed Constraints with solve_ivp :srcset: /examples/images/sphx_glr_plot_rolling_axle_uneven_street_002.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 591-597 Plot some coordinates. Note the almost non-differentiable changes in some generalized coordinate at some points in time. (in the plot at around 1.5 sec) This seems to indicate that very strong reaction forces are needed to enforce the no slip condition. .. GENERATED FROM PYTHON SOURCE LINES 597-614 .. code-block:: Python bezeichnung = [str(i) for i in q_ind + q_dep + u_ind + u_dep] fig, ax = plt.subplots(2, 1, figsize=(8, 6.5), constrained_layout=True, sharex=True) for i in range(12, 24): ax[0].plot(sys_times[0: resultat.shape[0]], resultat[:, i], label=bezeichnung[i]) ax[0].set_title('Generalized speeds') ax[0].legend(loc='upper right') for i in range(0, 12): ax[1].plot(sys_times[0: resultat.shape[0]], resultat[:, i], label=bezeichnung[i]) ax[1].set_title('Generalized coordinates') ax[1].set_xlabel('Time [s]') _ = ax[1].legend(loc='upper right') .. image-sg:: /examples/images/sphx_glr_plot_rolling_axle_uneven_street_003.png :alt: Generalized speeds, Generalized coordinates :srcset: /examples/images/sphx_glr_plot_rolling_axle_uneven_street_003.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 615-616 Plot the energy of the system and a few other quantities of interest. .. GENERATED FROM PYTHON SOURCE LINES 616-692 .. code-block:: Python delta_CPL_z = CPL.pos_from(O).dot(N.z) - gesamt(xL, yL, amplitude, frequenz, rumpel) delta_CPL_z = me.msubs(delta_CPL_z, kin_dict) delta_CPL_z_lam = sm.lambdify(q_ind + q_dep + u_ind + u_dep + pL, delta_CPL_z, cse=True) delta_CPR_z = CPR.pos_from(O).dot(N.z) - gesamt(xR, yR, amplitude, frequenz, rumpel) delta_CPR_z = me.msubs(delta_CPR_z, kin_dict) delta_CPR_z_lam = sm.lambdify(q_ind + q_dep + u_ind + u_dep + pL, delta_CPR_z, cse=True) kin_energy = sum([body.kinetic_energy(N) for body in BODY]) kin_energy = me.msubs(kin_energy, kin_dict) Dmc_dist = DmcL.pos_from(DmcR).magnitude() Dmc_dist = me.msubs(Dmc_dist, kin_dict) Dmc_dist_lam = sm.lambdify(q_ind + q_dep + u_ind + u_dep + pL, Dmc_dist, cse=True) pot_energy = sum([body.masscenter.pos_from(O).dot(N.z) * body.mass * g for body in BODY]) pot_energy = me.msubs(pot_energy, kin_dict) kin_lam = sm.lambdify(q_ind + q_dep + u_ind + u_dep + pL, kin_energy, cse=True) pot_lam = sm.lambdify(q_ind + q_dep + u_ind + u_dep + pL, pot_energy, cse=True) kin_np = np.empty(resultat.shape[0]) pot_np = np.empty(resultat.shape[0]) total_np = np.empty(resultat.shape[0]) dist_np = np.empty(resultat.shape[0]) CPL_z_np = np.empty(resultat.shape[0]) CPR_z_np = np.empty(resultat.shape[0]) for i in range(resultat.shape[0]): kin_np[i] = kin_lam(*resultat[i], *pL_vals) pot_np[i] = pot_lam(*resultat[i], *pL_vals) total_np[i] = kin_np[i] + pot_np[i] dist_np[i] = Dmc_dist_lam(*resultat[i], *pL_vals) CPL_z_np[i] = delta_CPL_z_lam(*[resultat[i, j] for j in range(24)], *pL_vals) CPR_z_np[i] = delta_CPR_z_lam(*[resultat[i, j] for j in range(24)], *pL_vals) fig, ax = plt.subplots(2, 1, figsize=(8, 4), constrained_layout=True) ax[0].plot(sys_times[0: resultat.shape[0]], kin_np, label='kinetic energy') ax[0].plot(sys_times[0: resultat.shape[0]], pot_np, label='potential energy') ax[0].plot(sys_times[0: resultat.shape[0]], total_np, label='total energy') ax[0].legend(loc='upper left') ax[1].plot(sys_times[0: resultat.shape[0]], dist_np, label='distance between centers of mass') ax[1].plot(sys_times[0: resultat.shape[0]], CPL_z_np, label='CPL z coordinate') ax[1].plot(sys_times[0: resultat.shape[0]], CPR_z_np, label='CPR z coordinate') ax[1].legend(loc='upper left') ax[0].set_title('Energy of the system') ax[1].set_title('Distance between centers of mass and ' 'z coordinates of CPL and CPR') ax[1].set_xlabel('Time [s]') ax[0].set_ylabel('Energy [J]') ax[1].set_ylabel('Distance [m]') max_dist = np.max(dist_np) min_dist = np.min(dist_np) print("Error in distance between centers of mass from being constant: " f"{(max_dist - min_dist) / min_dist:.4e}") if reibung1 == 0.0: print("Error in the total energy from being constant: " f"{(np.max(total_np) - np.min(total_np)) / np.max(total_np):.4e}") print(F"error in CPL z coordinate from being equal to the road height: " f"{(np.max(CPL_z_np) - np.min(CPL_z_np)):.4e}") print(F"error in CPR z coordinate from being equal to the road height: " f"{(np.max(CPR_z_np) - np.min(CPR_z_np)):.4e}") .. image-sg:: /examples/images/sphx_glr_plot_rolling_axle_uneven_street_004.png :alt: Energy of the system, Distance between centers of mass and z coordinates of CPL and CPR :srcset: /examples/images/sphx_glr_plot_rolling_axle_uneven_street_004.png :class: sphx-glr-single-img .. rst-class:: sphx-glr-script-out .. code-block:: none Error in distance between centers of mass from being constant: 0.0000e+00 Error in the total energy from being constant: 5.7616e-03 error in CPL z coordinate from being equal to the road height: 0.0000e+00 error in CPR z coordinate from being equal to the road height: 2.5479e-07 .. GENERATED FROM PYTHON SOURCE LINES 693-694 How well are the no slip conditions kept? .. GENERATED FROM PYTHON SOURCE LINES 694-722 .. code-block:: Python constr_no_slip_test = sm.Matrix([ deltaL_vel.dot(frame.x), deltaL_vel.dot(frame.y), deltaL_vel.dot(frame.z), deltaR_vel.dot(frame.x), deltaR_vel.dot(frame.y), deltaR_vel.dot(frame.z), ]).subs(kin_dict) constr_no_slip_test = [constr_no_slip_test[i] for i in range(constr_no_slip_test.shape[0])] constr_no_slip_test_lam = sm.lambdify(q_ind + q_dep + u_ind + u_dep + pL, constr_no_slip_test, cse=True) constr_no_slip_test_np = np.empty((resultat.shape[0], 6)) for i in range(resultat.shape[0]): constr_no_slip_test_np[i] = constr_no_slip_test_lam(*resultat[i], *pL_vals) fig, ax = plt.subplots(1, 1, figsize=(8, 2), constrained_layout=True) for i in (0, 1, 2, 3, 4, 5): ax.plot(sys_times[0: resultat.shape[0]], constr_no_slip_test_np[:, i], label=f'constraint {i}') ax.legend(loc='upper right') _ = ax.set_title('Violation of no-slip constraints') .. image-sg:: /examples/images/sphx_glr_plot_rolling_axle_uneven_street_005.png :alt: Violation of no-slip constraints :srcset: /examples/images/sphx_glr_plot_rolling_axle_uneven_street_005.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 723-725 Animation --------- .. GENERATED FROM PYTHON SOURCE LINES 725-814 .. code-block:: Python fps = 20 t_arr = resultat1.t state_sol = interp1d(t_arr, resultat1.y.T, kind='cubic', axis=0) coordinates = DmcL.pos_from(O).to_matrix(N) for point in (DmcR, m_DmcL, m_DmcR): coordinates = coordinates.row_join(point.pos_from(O).to_matrix(N)) coords_lam = sm.lambdify(qL + pL, coordinates, cse=True) max_x = np.max(np.concatenate((resultat1.y.T[:, 0], resultat1.y.T[:, 6]))) max_y = np.max(np.concatenate((resultat1.y.T[:, 1], resultat1.y.T[:, 7]))) min_x = np.min(np.concatenate((resultat1.y.T[:, 0], resultat1.y.T[:, 6]))) min_y = np.min(np.concatenate((resultat1.y.T[:, 1], resultat1.y.T[:, 7]))) gesamt_plot_lam = sm.lambdify([x_h, y_h, amplitude, frequenz], gesamt_plot(x_h, y_h, amplitude, frequenz), cse=True) max_radius = 2.0 * max(rL1, rR1) xx = np.linspace(min_x-max_radius, max_x+max_radius, 100) yy = np.linspace(min_y-max_radius, max_y+max_radius, 100) XX, YY = np.meshgrid(xx, yy) ZZ = gesamt_plot_lam(XX, YY, amplitude1, frequenz1) fig, ax = plt.subplots(figsize=(7, 7)) ax.set_xlim(min_x-max_radius, max_x+max_radius) ax.set_ylim(min_y-max_radius, max_y+max_radius) ax.set_aspect('equal') ax.set_xlabel('x', fontsize=15) ax.set_ylabel('y', fontsize=15) cf = ax.contourf(XX, YY, ZZ, levels=50, cmap='viridis') fig.colorbar(cf, label='z value [m]', shrink=0.8) line1, = ax.plot([], [], lw=1, marker='o', markersize=0, color='red') line4 = ax.scatter([], [], color='black', s=20) line5 = ax.scatter([], [], color='black', s=20) # ellipses defined in local frame AX winkel_q2 = state_sol(0)[5] ellipseL = Ellipse((0, 0), width=2.0*rL1*np.sin(winkel_q2), height=2.0*rL1, fill=True, lw=2, color='red', alpha=0.5) ax.add_patch(ellipseL) ellipseR = Ellipse((0, 0), width=2.0*rR1*np.sin(winkel_q2), height=2.0*rR1, fill=True, lw=2, color='magenta', alpha=0.5) ax.add_patch(ellipseR) def update(t): message = (f'running time {t:.2f} sec \n' f'the left wheel is red with radius {rL1}, the ' f'right wheel is magenta \n with radius {rR1},' f' The black dots are the particles attached \n to the wheels') ax.set_title(message, fontsize=11) coords = coords_lam(*state_sol(t), *pL_vals) line1.set_data([coords[0, 0], coords[0, 1]], [coords[1, 0], coords[1, 1]]) line4.set_offsets([coords[0, 2], coords[1, 2]]) line5.set_offsets([coords[0, 3], coords[1, 3]]) # transform from AX → inertial frame theta = state_sol(t)[4] X = coords[0, 0] Y = coords[1, 0] transform = Affine2D().rotate(theta).translate(X, Y) + ax.transData ellipseL.set_width(2.0*rL1*np.sin(state_sol(t)[5])) ellipseL.set_transform(transform) X = coords[0, 1] Y = coords[1, 1] transform = Affine2D().rotate(theta).translate(X, Y) + ax.transData ellipseR.set_width(2.0*rR1*np.sin(state_sol(t)[5])) ellipseR.set_transform(transform) return line1, line4, line5, ellipseL, ellipseR # Create the animation animation = FuncAnimation(fig, update, frames=np.concatenate([np.arange( 0, resultat1.t[-1], 1.0/fps), [resultat1.t[-1]]]), interval=1000/fps, blit=False) plt.show() .. container:: sphx-glr-animation .. raw:: html
.. rst-class:: sphx-glr-timing **Total running time of the script:** (4 minutes 14.918 seconds) .. _sphx_glr_download_examples_plot_rolling_axle_uneven_street.py: .. only:: html .. container:: sphx-glr-footer sphx-glr-footer-example .. container:: sphx-glr-download sphx-glr-download-jupyter :download:`Download Jupyter notebook: plot_rolling_axle_uneven_street.ipynb ` .. container:: sphx-glr-download sphx-glr-download-python :download:`Download Python source code: plot_rolling_axle_uneven_street.py ` .. container:: sphx-glr-download sphx-glr-download-zip :download:`Download zipped: plot_rolling_axle_uneven_street.zip ` .. only:: html .. rst-class:: sphx-glr-signature `Gallery generated by Sphinx-Gallery `_