.. DO NOT EDIT. .. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY. .. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE: .. "examples\plot_axle_rolling_controlled.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_axle_rolling_controlled.py: Rolling Axle, Controlled ======================== Description ----------- This is a modification of the ``rolling axle on an uneven street`` example, see https://pydy.org/pst-notebooks/examples/plot_rolling_axle_uneven_street.html There, with the no slip conditions enforced, the system has only one degree of freedom, so controlling it to reach some fixed point is not possible. Now, I want it to move to some final position by applying torques to the wheels. Hence the no slip conditions are relaxed, using opnty's ``eom_bounds`` keyword. Notes ----- - It takes quite long to get an acceptable solution, presumably because the jacobian is not very sparse. - I am not very sure about the 'mechanical meaning' of the relaxed no slip conditions. - The animation is a bit 'jumpy' to save space. It may be improved by using a larger value for ``fps``. **States** - :math:`q_L, q_R` : rotation angles of the wheels - :math:`x_L, y_L` : coordinates of the contact point of the left wheel - :math:`q_2, q_3` : rotation angles of the axle - :math:`x_R, y_R` : coordinates of the contact point of the right wheel - :math:`l_y, l_z` : components of the vector from the contact point to the center of the left wheel - :math:`r_y, r_z` : components of the vector from the contact point to the center of the right wheel - :math:`u_L, u_R` : angular speeds of the wheels - :math:`u_2, u_3` : angular speeds of the axle - :math:`ux_L, uy_L` : speeds of the contact point of the left wheel - :math:`ux_R, uy_R` : speeds of the contact point of the right wheel - :math:`ul_y, ul_z` : speeds of the vector from the contact point to the center of the left wheel - :math:`ur_y, ur_z` : speeds of the vector from the contact point to the center of the right wheel - :math:`T_L, T_R` : torques applied to the wheels. Controls of opty **Parameters** - :math:`m_L, m_R` : masses of the wheels - :math:`m_o` : mass of the particle attached to the wheels - :math:`g` : gravity - :math:`r_L, r_R` : radii of the wheels - :math:`l` : distance between the wheels - :math:`amplitude, frequenz` : parameters of the street - :math:`reibung` : friction between the wheels and the axle **Further symbols** - :math:`N` : inertial frame - :math:`AX` : frame attached to the axle - :math:`AL` : frame attached to the left wheel - :math:`AR` : frame attached to the right wheel - :math:`O` : reference point, fixed in N - :math:`CPL` : contact point of the left wheel - :math:`CPR` : contact point of the right wheel - :math:`Dmc_L` : center of mass of the left wheel - :math:`Dmc_R` : center of mass of the right wheel - :math:`m_{Dmc_L}` : particle attached to the left wheel - :math:`m_{Dmc_R}` : particle attached to the right wheel .. GENERATED FROM PYTHON SOURCE LINES 75-87 .. code-block:: Python import sympy as sm import sympy.physics.mechanics as me import numpy as np import matplotlib.pyplot as plt from scipy.interpolate import interp1d from scipy.optimize import root, minimize from opty import Problem from matplotlib.animation import FuncAnimation from matplotlib.patches import Ellipse from matplotlib.transforms import Affine2D .. GENERATED FROM PYTHON SOURCE LINES 88-89 If True, print some information about the eom. .. GENERATED FROM PYTHON SOURCE LINES 89-91 .. code-block:: Python info = True .. GENERATED FROM PYTHON SOURCE LINES 92-93 Rotation angles of the wheels and the body, and their speeds. .. GENERATED FROM PYTHON SOURCE LINES 93-96 .. 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 97-98 Coordinates of the contact points of the left / right wheel. .. GENERATED FROM PYTHON SOURCE LINES 98-101 .. code-block:: Python xL, yL, xR, yR = me.dynamicsymbols('xL yL xR yR') uxL, uyL, uxR, uyR = me.dynamicsymbols('uxL uyL uxR uyR') # their 'speeds' .. GENERATED FROM PYTHON SOURCE LINES 102-104 Components of the vectors from the contact points to the centers of mass, in N. Their speeds. .. GENERATED FROM PYTHON SOURCE LINES 104-107 .. 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 108-109 Torques on the wheels. Controls for opty. .. GENERATED FROM PYTHON SOURCE LINES 109-111 .. code-block:: Python TL, TR = me.dynamicsymbols('TL TR') .. GENERATED FROM PYTHON SOURCE LINES 112-114 Parameters of the system: masses, gravity, radii of the wheels, and distance between the wheels. .. GENERATED FROM PYTHON SOURCE LINES 114-117 .. code-block:: Python mL, mR, mo, g, rL, rR, l = sm.symbols( 'mL mR mo g rL rR l') .. GENERATED FROM PYTHON SOURCE LINES 118-119 Parameters for the surface. .. GENERATED FROM PYTHON SOURCE LINES 119-121 .. code-block:: Python amplitude, frequenz, reibung = sm.symbols('amplitude frequenz reibung') .. GENERATED FROM PYTHON SOURCE LINES 122-123 Define some frames, points, etc. .. GENERATED FROM PYTHON SOURCE LINES 123-131 .. 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 132-133 The axle does not rotate around itself. .. GENERATED FROM PYTHON SOURCE LINES 133-138 .. code-block:: Python AX.orient_body_fixed(N, [q3, q2, 0], 'ZYX') rot = AX.ang_vel_in(N) AX.set_ang_vel(N, u2*AX.y + u3*AX.z) rot1 = AX.ang_vel_in(N) .. GENERATED FROM PYTHON SOURCE LINES 139-141 The left wheel rotates around the axle, that is, around AX.x, similarly for the right wheel. .. GENERATED FROM PYTHON SOURCE LINES 141-146 .. 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 147-148 Particles attached to the wheels. .. GENERATED FROM PYTHON SOURCE LINES 148-152 .. code-block:: Python m_DmcL.set_pos(DmcL, rL*AL.y) m_DmcR.set_pos(DmcR, rR*AR.y) .. GENERATED FROM PYTHON SOURCE LINES 153-154 Here the street is modelled. *rumpel* must be an integer. .. GENERATED FROM PYTHON SOURCE LINES 154-172 .. 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 173-175 Create the dictionary to replace :math:`\dfrac{d}{dt}(\textrm{gen. coord})` with the corresponding symbols. .. GENERATED FROM PYTHON SOURCE LINES 175-193 .. 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, } kin_dict .. rst-class:: sphx-glr-script-out .. code-block:: none {Derivative(xL(t), t): uxL(t), Derivative(yL(t), t): uyL(t), Derivative(xR(t), t): uxR(t), Derivative(yR(t), t): uyR(t), Derivative(qL(t), t): uL(t), Derivative(qR(t), t): uR(t), Derivative(ly(t), t): uly(t), Derivative(lz(t), t): ulz(t), Derivative(ry(t), t): ury(t), Derivative(rz(t), t): urz(t), Derivative(q2(t), t): u2(t), Derivative(q3(t), t): u3(t)} .. GENERATED FROM PYTHON SOURCE LINES 194-198 Configuration Constraints ------------------------- :math:`CP_L, CP_R` are the contact points where the wheels touch the street. .. GENERATED FROM PYTHON SOURCE LINES 198-204 .. 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 205-209 Define the vectors pointing from the contact point to the corresponding center of the wheel. :math:`\text{vector}_L \perp A.x` and :math:`\text{vector}_R \perp A.x`, so they have no component in A.x direction. .. GENERATED FROM PYTHON SOURCE LINES 209-213 .. code-block:: Python vectorL = ly * AX.y + lz * AX.z vectorR = ry * AX.y + rz * AX.z .. GENERATED FROM PYTHON SOURCE LINES 214-216 :math:`\text{vector}_L` and :math:`\text{vector}_R` must have magnitude equal to the respective wheel :math:`r_L` and :math:`r_R`. .. GENERATED FROM PYTHON SOURCE LINES 216-223 .. 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 224-225 Set centers of mass of wheels, second contact point CPR. .. GENERATED FROM PYTHON SOURCE LINES 225-230 .. code-block:: Python DmcL.set_pos(CPL, vectorL) DmcL.v2pt_theory(CPL, N, AX) DmcR.set_pos(DmcL, l * AX.x) CPR.set_pos(DmcR, -vectorR) .. GENERATED FROM PYTHON SOURCE LINES 231-236 :math:`\text{vector}_L` must be in the plane formed by the gradient :math:`n_L` at the point (xL, yL) on the surface and by :math:`A.x` the direction of the axle, that is :math:`\text{vector}_L \circ (n_L \times A.x) = 0` Same for :math:`\text{vector}_R`. .. GENERATED FROM PYTHON SOURCE LINES 236-253 .. 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 254-255 Determine the constraints for :math:`x_R, y_R, q_2` for the location of CPR. .. GENERATED FROM PYTHON SOURCE LINES 255-265 .. 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 266-267 Combine the configuration constraints. .. GENERATED FROM PYTHON SOURCE LINES 267-274 .. code-block:: Python config_constr = constr_length.col_join(constrT).col_join(constr_CPR) if info: print(f"config_constr contains {sm.count_ops(config_constr)} operations") print("DS", me.find_dynamicsymbols(config_constr)) print("FS", config_constr.free_symbols, "shape = ", config_constr.shape) .. rst-class:: sphx-glr-script-out .. code-block:: none config_constr contains 484 operations DS {ly(t), xL(t), yR(t), q3(t), lz(t), q2(t), ry(t), xR(t), yL(t), rz(t)} FS {l, rR, frequenz, t, rL, amplitude} shape = (7, 1) .. GENERATED FROM PYTHON SOURCE LINES 275-280 No Slip Constraints ------------------- Set the speeds of the centers of the wheels and of the particles attached to it. .. GENERATED FROM PYTHON SOURCE LINES 280-302 .. code-block:: Python DmcL.v2pt_theory(CPL, N, AL) DmcR.v2pt_theory(CPR, N, AR) 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), ]) m_DmcL.v2pt_theory(DmcL, N, AL) _ = m_DmcR.v2pt_theory(DmcR, N, AR) .. GENERATED FROM PYTHON SOURCE LINES 303-306 Kane's Equations ---------------- .. GENERATED FROM PYTHON SOURCE LINES 306-323 .. 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 324-325 Set the external forces acting on the system. .. GENERATED FROM PYTHON SOURCE LINES 325-330 .. 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)] .. GENERATED FROM PYTHON SOURCE LINES 331-332 The control for opty. .. GENERATED FROM PYTHON SOURCE LINES 332-334 .. code-block:: Python TorqueC = [(AL, TL*AL.x), (AR, TR*AR.x)] .. GENERATED FROM PYTHON SOURCE LINES 335-336 Combine the forces and torques. .. GENERATED FROM PYTHON SOURCE LINES 336-338 .. code-block:: Python FL = FL1 + Torque + TorqueC .. GENERATED FROM PYTHON SOURCE LINES 339-340 Kane's method. .. GENERATED FROM PYTHON SOURCE LINES 340-367 .. code-block:: Python speed_constr = config_constr.diff(t) kd = sm.Matrix([key - value for key, value in kin_dict.items()]) q_ind = [qL, qR, xL, yL, q3] q_dep = [xR, yR, q2, ly, lz, ry, rz] u_ind = [uL, uR, uxL, uyL, u3] u_dep = [uxR, uyR, u2, 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_constr, configuration_constraints=config_constr, ) fr, frstar = kane.kanes_equations(BODY, FL) eom1 = kd.col_join(fr + frstar) .. GENERATED FROM PYTHON SOURCE LINES 368-369 Append the configuration constraints. .. GENERATED FROM PYTHON SOURCE LINES 369-371 .. code-block:: Python eom2 = eom1.col_join(config_constr) .. GENERATED FROM PYTHON SOURCE LINES 372-374 Append the non-slip constraints. They will be loosened in the optimization problem. .. GENERATED FROM PYTHON SOURCE LINES 374-384 .. code-block:: Python eom = eom2.col_join(constr_no_slip) # Print some information about the eom. if info: print(f"eom have {sm.count_ops(eom):,} operations, " f"{sm.count_ops(sm.cse(eom)):,} after cse, " f"shape = {eom.shape}, \n") print("eom dynamic symbols", me.find_dynamicsymbols(eom)) print("shapes of eom", eom.shape) .. rst-class:: sphx-glr-script-out .. code-block:: none eom have 2,035,388 operations, 1,868 after cse, shape = (28, 1), eom dynamic symbols {Derivative(uxL(t), t), uyL(t), Derivative(qL(t), t), Derivative(q3(t), t), Derivative(uyL(t), t), Derivative(rz(t), t), Derivative(yR(t), t), qR(t), uL(t), Derivative(ry(t), t), uR(t), rz(t), ulz(t), xL(t), TL(t), Derivative(q2(t), t), q2(t), Derivative(uL(t), t), u3(t), urz(t), yL(t), ry(t), Derivative(xR(t), t), Derivative(uR(t), t), ly(t), yR(t), Derivative(u2(t), t), uxL(t), Derivative(yL(t), t), q3(t), lz(t), TR(t), Derivative(ly(t), t), u2(t), xR(t), Derivative(lz(t), t), uly(t), Derivative(qR(t), t), Derivative(ury(t), t), Derivative(xL(t), t), Derivative(urz(t), t), uyR(t), qL(t), ury(t), uxR(t), Derivative(u3(t), t)} shapes of eom (28, 1) .. GENERATED FROM PYTHON SOURCE LINES 385-386 Set parameters. .. GENERATED FROM PYTHON SOURCE LINES 386-399 .. code-block:: Python par_map = {} par_map[mL] = 1.0 par_map[mR] = 0.25 par_map[mo] = 0.1 par_map[g] = 9.81 par_map[rL] = 2.0 par_map[rR] = 1.0 par_map[l] = 5.0 par_map[amplitude] = 0.15 par_map[frequenz] = 0.25 par_map[reibung] = 1.0 .. GENERATED FROM PYTHON SOURCE LINES 400-401 Set the independent gen. coordinates. .. GENERATED FROM PYTHON SOURCE LINES 401-407 .. code-block:: Python qL1 = 0.0 qR1 = 0.0 xL1 = 1.0 yL1 = 0.0 q31 = 0.0 .. GENERATED FROM PYTHON SOURCE LINES 408-409 Final position of the contact point of the left wheel. .. GENERATED FROM PYTHON SOURCE LINES 409-413 .. code-block:: Python xL_end = 20.0 yL_end = 20.0 .. GENERATED FROM PYTHON SOURCE LINES 414-416 Calculate consistent initial dependent generalized coordinates. All generalized speeds are set to zero initially. .. GENERATED FROM PYTHON SOURCE LINES 416-441 .. code-block:: Python pL = [key for key in par_map.keys()] pL_vals = [par_map[key] for key in pL] config_constr_lam = sm.lambdify(q_dep + q_ind + pL, config_constr, cse=True) def func(y, args): return config_constr_lam(*y, *args).squeeze() y0 = np.zeros(len(q_dep)) args = [qL1, qR1, xL1, yL1, q31] + pL_vals res = root(func, y0, args=args) xR1, yR1, q21, ly1, lz1, ry1, rz1 = res.x if res.x[4] <= 0 or res.x[6] <= 0: raise ValueError(f"use different initial guess, lz = {res.x[4]}, " f"rz = {res.x[6]} meaning at least one wheel is " "below the street") for i, j in zip(q_dep, res.x): print(f"{i} = {j:.3f}") .. rst-class:: sphx-glr-script-out .. code-block:: none xR(t) = 6.090 yR(t) = -0.126 q2(t) = 0.249 ly(t) = -0.246 lz(t) = 1.985 ry(t) = -0.120 rz(t) = 0.993 .. GENERATED FROM PYTHON SOURCE LINES 442-444 Set up Problem -------------- .. GENERATED FROM PYTHON SOURCE LINES 444-493 .. code-block:: Python h = sm.symbols('h') num_nodes = 300 t0, tf = 0.0, h * (num_nodes - 1) interval_value = h state_symbols = q_ind + q_dep + u_ind + u_dep instance_constraints = [ qL.func(t0) - qL1, qR.func(t0) - qR1, xL.func(t0) - xL1, yL.func(t0) - yL1, q3.func(t0) - q31, xR.func(t0) - xR1, yR.func(t0) - yR1, q2.func(t0) - q21, ly.func(t0) - ly1, lz.func(t0) - lz1, ry.func(t0) - ry1, rz.func(t0) - rz1, *[speed.func(t0) - 0.0 for speed in u_ind + u_dep], TL.func(t0) - 0.0, TR.func(t0) - 0.0, xL.func(tf) - xL_end, yL.func(tf) - yL_end, *[speed.func(tf) - 0.0 for speed in u_ind + u_dep], ] def obj(free): # Minimize the duration. return free[-1] def obj_grad(free): grad = np.zeros_like(free) grad[-1] = 1.0 return grad limit = 50.0 bounds = { h: (0.0, 0.1), TL: (-limit, limit), TR: (-limit, limit), lz: (0.0, par_map[rL]), # left hub must remain above the street rz: (0.0, par_map[rR]), # right hub must remain above the street } .. GENERATED FROM PYTHON SOURCE LINES 494-495 Loosen the non slip constraints. .. GENERATED FROM PYTHON SOURCE LINES 495-519 .. code-block:: Python delta_eom = 7.5 # Arbitrary. Not sure about the mechanical meaning. eom_bounds = { 24: (-delta_eom, delta_eom), 25: (-delta_eom, delta_eom), 26: (-delta_eom, delta_eom), 27: (-delta_eom, delta_eom), } prob = Problem( obj, obj_grad, eom, state_symbols, num_nodes, h, known_parameter_map=par_map, instance_constraints=instance_constraints, time_symbol=t, bounds=bounds, eom_bounds=eom_bounds, ) .. GENERATED FROM PYTHON SOURCE LINES 520-522 Solve the Problem ----------------- .. GENERATED FROM PYTHON SOURCE LINES 524-538 .. code-block:: Python initial_guess = np.ones(prob.num_free) initial_guess[-1] = 0.005 for i in range(3): if i == 0: prob.add_option('max_iter', 50) elif i == 1: prob.add_option('max_iter', 1000) else: prob.add_option('max_iter', 25000) solution, info = prob.solve(initial_guess) print(info['status_msg']) initial_guess = solution .. rst-class:: sphx-glr-script-out .. code-block:: none b'Maximum number of iterations exceeded (can be specified by an option).' b'Maximum number of iterations exceeded (can be specified by an option).' b'Algorithm terminated successfully at a locally optimal point, satisfying the convergence tolerances (can be specified by options).' .. GENERATED FROM PYTHON SOURCE LINES 539-540 Plot the constraint violations. .. GENERATED FROM PYTHON SOURCE LINES 540-543 .. code-block:: Python _ = prob.plot_constraint_violations(solution, subplots=True, show_bounds=True) .. image-sg:: /examples/images/sphx_glr_plot_axle_rolling_controlled_001.png :alt: Constraint violations Values of bounded EoMs :srcset: /examples/images/sphx_glr_plot_axle_rolling_controlled_001.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 544-545 Plot the trajectories. .. GENERATED FROM PYTHON SOURCE LINES 545-548 .. code-block:: Python _ = prob.plot_trajectories(solution, show_bounds=True) .. image-sg:: /examples/images/sphx_glr_plot_axle_rolling_controlled_002.png :alt: State Trajectories, Input Trajectories :srcset: /examples/images/sphx_glr_plot_axle_rolling_controlled_002.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 549-550 Plot the objective value over iterations. .. GENERATED FROM PYTHON SOURCE LINES 550-553 .. code-block:: Python _ = prob.plot_objective_value() .. image-sg:: /examples/images/sphx_glr_plot_axle_rolling_controlled_003.png :alt: Objective Value :srcset: /examples/images/sphx_glr_plot_axle_rolling_controlled_003.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 554-555 Plot the sparsity pattern of the Jacobian. .. GENERATED FROM PYTHON SOURCE LINES 555-558 .. code-block:: Python _ = prob.plot_jacobian_sparsity() .. image-sg:: /examples/images/sphx_glr_plot_axle_rolling_controlled_004.png :alt: plot axle rolling controlled :srcset: /examples/images/sphx_glr_plot_axle_rolling_controlled_004.png :class: sphx-glr-single-img .. GENERATED FROM PYTHON SOURCE LINES 559-561 Find the minimum curvature of the surface and check it is larger than the wheels. (Formula from the internet) .. GENERATED FROM PYTHON SOURCE LINES 561-624 .. 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() 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): # just needed to get the arguments matching for minimuze return np.abs(1.0 / k1_lam(*x0, *args)) def func2(x0, args): # just needed to get the arguments matching for minimuze return np.abs(1.0 / k2_lam(*x0, *args)) x0 = np.array((5.0, 10.0)) # initial guess args = np.array((par_map[amplitude], par_map[frequenz])) 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(par_map[rL], par_map[rR]): 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 625-629 Distance between :math:`Dmc_L` and :math:`Dmc_R` should be constant. Distance of :math:`CP_L`, :math:`CP_R` from the surface of the street should be zero. .. GENERATED FROM PYTHON SOURCE LINES 629-684 .. code-block:: Python resultat, *_, h_act = prob.parse_free(solution) resultat = resultat.T sys_times = np.linspace(t0, num_nodes * h_act, num_nodes) 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) 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) 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]): 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) dist_np[i] = Dmc_dist_lam(*[resultat[i, j] for j in range(24)], *pL_vals) fig, ax = plt.subplots(1, 1, figsize=(10, 3)) ax.plot(sys_times[0: resultat.shape[0]], dist_np, label='distance between centers of mass') ax.plot(sys_times[0: resultat.shape[0]], CPL_z_np, label='CPL z coordinate') ax.plot(sys_times[0: resultat.shape[0]], CPR_z_np, label='CPR z coordinate') ax.legend(loc='upper left') ax.set_title('Distance between centers of mass and ' 'z coordinates of CPL and CPR') ax.set_xlabel('Time [s]') ax.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}") 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_axle_rolling_controlled_005.png :alt: Distance between centers of mass and z coordinates of CPL and CPR :srcset: /examples/images/sphx_glr_plot_axle_rolling_controlled_005.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 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.1768e-11 .. GENERATED FROM PYTHON SOURCE LINES 685-687 Animation --------- .. GENERATED FROM PYTHON SOURCE LINES 687-787 .. code-block:: Python fps = 7 rL1 = par_map[rL] rR1 = par_map[rR] amplitude1 = par_map[amplitude] frequenz1 = par_map[frequenz] resultat, *_, h_act = prob.parse_free(solution) resultat = resultat.T print(f"actual time step h = {h_act:.4f}") t_arr = np.linspace(t0, num_nodes*h_act, num_nodes) state_sol = interp1d(t_arr, resultat, 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(state_symbols + pL, coordinates, cse=True) max_x = np.max(np.concatenate((resultat[:, 2], resultat[:, 5]))) max_y = np.max(np.concatenate((resultat[:, 3], resultat[:, 6]))) min_x = np.min(np.concatenate((resultat[:, 2], resultat[:, 5]))) min_y = np.min(np.concatenate((resultat[:, 3], resultat[:, 6]))) 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) # axle line1, = ax.plot([], [], lw=1, marker='o', markersize=0, color='red') # particles attached to the wheels line4 = ax.scatter([], [], color='black', s=20) line5 = ax.scatter([], [], color='black', s=20) # startpoint, endpoint ax.scatter([xL1], [yL1], color='red', s=50, edgecolor='black') ax.scatter([xL_end], [yL_end], color='green', s=50, edgecolor='black') # ellipses defined in local frame AX winkel_q2 = state_sol(0)[7] 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)[7])) 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)[7])) ellipseR.set_transform(transform) return line1, line4, line5, ellipseL, ellipseR # Create the animation animation = FuncAnimation(fig, update, frames=np.concatenate([np.arange( 0, t_arr[-1], 1.0/fps), [t_arr[-1]]]), interval=1000/fps, blit=False) plt.show() .. container:: sphx-glr-animation .. raw:: html
.. rst-class:: sphx-glr-script-out .. code-block:: none actual time step h = 0.0289 .. rst-class:: sphx-glr-timing **Total running time of the script:** (29 minutes 6.670 seconds) .. _sphx_glr_download_examples_plot_axle_rolling_controlled.py: .. only:: html .. container:: sphx-glr-footer sphx-glr-footer-example .. container:: sphx-glr-download sphx-glr-download-jupyter :download:`Download Jupyter notebook: plot_axle_rolling_controlled.ipynb ` .. container:: sphx-glr-download sphx-glr-download-python :download:`Download Python source code: plot_axle_rolling_controlled.py ` .. container:: sphx-glr-download sphx-glr-download-zip :download:`Download zipped: plot_axle_rolling_controlled.zip ` .. only:: html .. rst-class:: sphx-glr-signature `Gallery generated by Sphinx-Gallery `_