From ac2a5be141388d59b716943fef412c76f851250b Mon Sep 17 00:00:00 2001 From: Peter230655 Date: Sat, 29 Jun 2024 13:05:45 +0200 Subject: [PATCH 1/4] upright a two link pendulum using opty with variable h --- .../two-pendulum-cart-variable-gallery.py | 311 ++++++++++++++++++ 1 file changed, 311 insertions(+) create mode 100644 examples-gallery/two-pendulum-cart-variable-gallery.py diff --git a/examples-gallery/two-pendulum-cart-variable-gallery.py b/examples-gallery/two-pendulum-cart-variable-gallery.py new file mode 100644 index 0000000..0831bbc --- /dev/null +++ b/examples-gallery/two-pendulum-cart-variable-gallery.py @@ -0,0 +1,311 @@ +""" +Upright a double pendulum. +========================== + +A double pendulum is rotably attached to a cart which can move +along the horizontal X axis. The goal is to get the double +pendulum to an upright position in the shortest time possible, +given an upper limit on the absolute value of the force that +can be applied to the cart. +Gravity points in the negative Y direction. + +**Constants** + +m1: mass of the cart [kg] +m2: mass of the first pendulum [kg] +m3: mass of the second pendulum [kg] +lx: length of the first pendulum rods [m] +iZZ1: moment of inertia of the first pendulum about its end point [kg*m^2] +iZZ2: moment of inertia of the second pendulum about its end point [kg*m^2] +g: acceleration due to gravity [m/s^2] + +**States** + +q1: position of the cart along the X axis [m] +q2: angle of the first pendulum with respect to the vertical [rad] +q3: angle of the second pendulum with respect to the first [rad] +u1: speed of the cart along the X axis [m/s] +u2: angular speed of the first pendulum [rad/s] +u3: angular speed of the second pendulum [rad/s] + +**Specifieds** + +f: force applied to the cart [N] + +""" +import sympy.physics.mechanics as me +from collections import OrderedDict +import numpy as np +import sympy as sm +from opty.direct_collocation import Problem + +import matplotlib.pyplot as plt +import matplotlib +import matplotlib.animation as animation +matplotlib.rcParams['animation.embed_limit'] = 2**128 +from matplotlib import patches + +# %% +# Generate the nonholonomic equations of motion of the system. +N, A1, A2 = sm.symbols('N A1, A2', cls = me.ReferenceFrame) +t = me.dynamicsymbols._t +O, P1, P2, P3 = sm.symbols('O P1 P2 P3', cls = me.Point) +O.set_vel(N, 0) +q1, q2, q3, u1, u2, u3, F = me.dynamicsymbols('q1 q2 q3 u1 u2 u3 F') +lx, m1, m2, m3, g, iZZ1, iZZ2 = sm.symbols('lx, m1, m2, m3 g, iZZ1, iZZ2') + +A1.orient_axis(N, q2, N.z) +A1.set_ang_vel(N, u2 * N.z) +A2.orient_axis(N, q3, N.z) +A2.set_ang_vel(N, u3 * N.z) + +P1.set_pos(O, q1 * N.x) +P2.set_pos(P1, lx * A1.x) +P2.v2pt_theory(P1, N, A1) + +P3.set_pos(P2, lx * A2.x) +P3.v2pt_theory(P2, N, A2) + +P1a = me.Particle('P1a', P1, m1) + +I = me.inertia(A1, 0, 0, iZZ1) +P2a = me.RigidBody('P2a', P2, A1, m2, (I, P2)) + +I = me.inertia(A2, 0, 0, iZZ2) +P3a = me.RigidBody('P3a', P3, A2, m3, (I, P3)) +BODY = [P1a, P2a, P3a] + +FL = [(P1, F * N.x - m1*g*N.y), (P2, -m2*g*N.y), (P3, -m3*g*N.y)] +kd = sm.Matrix([q1.diff(t) - u1, q2.diff(t) - u2, q3.diff(t) - u3]) + +q_ind = [q1, q2, q3] +u_ind = [u1, u2, u3] + +KM = me.KanesMethod(N, q_ind=q_ind, u_ind=u_ind, kd_eqs=kd) +(fr, frstar) = KM.kanes_equations(BODY, FL) +EOM = kd.col_join(fr + frstar) +sm.pprint(EOM) + +# %% +# Define various objects to be use in the optimization problem. +h = sm.symbols('h') # the variable time element + +state_symbols = tuple((*q_ind, *u_ind)) +constant_symbols = (lx, m1, m2, m3, g, iZZ1, iZZ2) +specified_symbols = (F,) + +target_angle = np.pi /2.0 +num_nodes = 250 + +duration = (num_nodes - 1) * h +interval_value = h + +# Specify the known system parameters. +par_map = OrderedDict() +par_map[lx] = 2.0 +par_map[m1] = 1.0 +par_map[m2] = 1.0 +par_map[m3] = 1.0 +par_map[g] = 9.81 +par_map[iZZ1] = 2.0 +par_map[iZZ2] = 2.0 + +def obj(free): + """Minimize h, the time interval between nodes.""" + return free[-1] + +def obj_grad(free): + grad = np.zeros_like(free) + grad[-1] = 1. + return grad + +# %% +# set the initial and final states to form the instance constraints. +# initially, the pendulum should be hanging straight down, without mvoing +# and finally, the pendulum should be upright, without moving. +initial_state_constraints = { + q1: 0., + q2: -np.pi/2., + q3: -np.pi/2., + u1: 0., + u2: 0., + u3: 0. + } + +final_state_constraints = { + q2: target_angle, + q3: target_angle, + u1: 0., + u2: 0., + u3: 0. + } + +instance_constraints = ( +) + tuple( + xi.subs({t: 0}) - xi_val for xi, xi_val in initial_state_constraints.items() +) + tuple( + xi.subs({t: duration}) - xi_val for xi, xi_val in final_state_constraints.items() +) +# bounding h > 0 herlps avoid 'solutions' with h < 0. +bounds = {F: (-150., 150.), q1: (-5, 5), h: (1.e-5, 1.0)} + +# %% +# Create an optimization problem and solve it. +prob = Problem(obj, + obj_grad, + EOM, + state_symbols, + num_nodes, + interval_value, + known_parameter_map=par_map, + instance_constraints=instance_constraints, + time_symbol=t, + bounds=bounds) + +# Initial guess. +initial_guess = np.zeros(prob.num_free) +initial_guess[-1] = 0.01 + +# allows to change the number of iterations. +# standard is 3000 +prob.add_option('max_iter', 6000) + +# Find the optimal solution. +# iterating and using the solution of iteration i +# as initial guess for iteration i+1 sometimes helps. +for i in range(2): + solution, info = prob.solve(initial_guess) + initial_guess = solution + + print(f'{i + 1}st iteration: value of objective function is {obj(solution):.2f}') + prob.plot_objective_value() + print('message from optimizer:', info['status_msg']) + print('Iterations needed',len(prob.obj_value)) + print(f"objective value {info['obj_val']:.3e}") + print('h value:', solution[-1]) + print('\n') + +# %% +# Plot the accuracy of the solution, and the state trajectories. +fehler = info['g'] +anzahl = len(state_symbols) +fig, ax = plt.subplots(2*anzahl + 1, 1, figsize=(10, 5*(anzahl+1)), sharex=True) +duration = (num_nodes - 1) * solution[-1] +times = np.linspace(0.0, duration, num=num_nodes) + +for i, j in enumerate(state_symbols): + if j in final_state_constraints.keys(): + farbe1 = 'red' + else: + farbe1 = 'black' + + if i < len(state_symbols): + farbe = 'b' + else: + farbe = 'r' + ax[i].plot(times, fehler[i * num_nodes:(i + 1) * num_nodes], color = farbe, lw=0.5) + ax[i].grid() + ax[i].set_ylabel(f'${str(j)}$', color=farbe1) +ax[-1].set_xlabel('time') +ax[0]. set_title(f'Errors in the generalized coordinates and speeds \n coordinates/speeds in the final_state_constraints are red', color = 'b'); +print(f'error in the final state constraint q2: {fehler[-5]:.3e}') +print(f'error in the final state constraint q3: {fehler[-4]:.3e}') +print(f'error in the final state constraint u1: {fehler[-3]:.3e}') +print(f'error in the final state constraint u2: {fehler[-2]:.3e}') +print(f'error in the final state constraint u3: {fehler[-1]:.3e}') + +#fig, ax = plt.subplots(7, 1, figsize=(10, 15), sharex=True) +#times = np.linspace(0.0, duration, num=num_nodes) + +for i, j in enumerate(state_symbols + specified_symbols): + k = anzahl + i + ax[k].plot(times, solution[i * num_nodes:(i + 1) * num_nodes]) + ax[k].grid() + ax[k].set_ylabel(str(j)) +ax[-1].set_xlabel('time') +ax[anzahl]. set_title('Generalized coordinates and speeds') +ax[-1]. set_title('Control force'); + +# %% +# animate the solution. +P1_x = np.empty(num_nodes) +P1_y = np.empty(num_nodes) +P2_x = np.empty(num_nodes) +P2_y = np.empty(num_nodes) +P3_x = np.empty(num_nodes) +P3_y = np.empty(num_nodes) + +P1_loc = [me.dot(P1.pos_from(O), uv) for uv in [N.x, N.y]] +P2_loc = [me.dot(P2.pos_from(O), uv) for uv in [N.x, N.y]] +P3_loc = [me.dot(P3.pos_from(O), uv) for uv in [N.x, N.y]] + +qL = q_ind + u_ind +pL_vals = list(constant_symbols) +P1_loc_lam = sm.lambdify(qL + pL_vals, P1_loc, cse=True) +P2_loc_lam = sm.lambdify(qL + pL_vals, P2_loc, cse=True) +P3_loc_lam = sm.lambdify(qL + pL_vals, P3_loc, cse=True) + +for i in range(num_nodes): + q_1 = solution[i] + q_2 = solution[i + num_nodes] + q_3 = solution[i + 2 * num_nodes] + u_1 = solution[i + 3 * num_nodes] + u_2 = solution[i + 4 * num_nodes] + u_3 = solution[i + 5 * num_nodes] + P1_x[i], P1_y[i] = P1_loc_lam(q_1, q_2, q_3, u_1, u_2, u_3, *list(par_map.values())) + P2_x[i], P2_y[i] = P2_loc_lam(q_1, q_2, q_3, u_1, u_2, u_3, *list(par_map.values())) + P3_x[i], P3_y[i] = P3_loc_lam(q_1, q_2, q_3, u_1, u_2, u_3, *list(par_map.values())) + + +# needed to give the picture the right size. +xmin = min(np.min(P1_x),np.min(P2_x), np.min(P3_x)) +xmax = max(np.max(P1_x), np.max(P2_x), np.max(P3_x)) +ymin = min(np.min(P1_y), np.min(P2_y), np.min(P3_y)) +ymax = max(np.max(P1_y), np.max(P2_y), np.max(P3_y)) + +width, height = par_map[lx]/3., par_map[lx]/3. + +def animate_pendulum(time, P1_x, P1_y, P2_x, P2_y): + + fig, ax = plt.subplots(figsize=(8, 8), subplot_kw={'aspect': 'equal'}) + + ax.axis('on') + ax.set_xlim(xmin - 1., xmax + 1.) + ax.set_ylim(ymin - 1., ymax + 1.) + + ax.set_xlabel('X direction', fontsize=15) + ax.set_ylabel('Y direction', fontsize=15) + ax.axhline(0, color='black', lw=2) + + line1, = ax.plot([], [], 'o-', lw=0.5, color='blue') + line2, = ax.plot([], [], 'o-', lw=0.5, color='green') + + recht = patches.Rectangle((P1_x[0] - width/2, P1_y[0] - height/2), width=width, height=height, fill=True, color='red', ec='black') + ax.add_patch(recht) + return fig, ax, line1, line2, recht + +fig, ax, line1, line2, recht = animate_pendulum(times, P1_x, P1_y, P2_x, P2_y) + +def animate(i): + message = (f'running time {times[i]:.2f} sec') + ax.set_title(message, fontsize=15) + recht.set_xy((P1_x[i] - width/2., P1_y[i] - height/2.)) + + wert_x = [P1_x[i], P2_x[i]] + wert_y = [P1_y[i], P2_y[i]] + line1.set_data(wert_x, wert_y) + + wert_x = [P2_x[i], P3_x[i]] + wert_y = [P2_y[i], P3_y[i]] + line2.set_data(wert_x, wert_y) + return line1, line2, + +anim = animation.FuncAnimation(fig, animate, frames=num_nodes, + interval=2000*np.max(times) / num_nodes, + blit=False) +# %% +# A frame from the animation. + +# sphinx_gallery_thumbnail_number = 7 +animate(150) +plt.show() \ No newline at end of file From 630011b7bb7668a6c8b52eaee6494d73c44824cd Mon Sep 17 00:00:00 2001 From: Peter230655 Date: Sun, 30 Jun 2024 07:17:51 +0200 Subject: [PATCH 2/4] is this a pull request? --- ...py => plot_two_link_pendulum_on_a_cart.py} | 162 +++++++----------- 1 file changed, 63 insertions(+), 99 deletions(-) rename examples-gallery/{two-pendulum-cart-variable-gallery.py => plot_two_link_pendulum_on_a_cart.py} (60%) diff --git a/examples-gallery/two-pendulum-cart-variable-gallery.py b/examples-gallery/plot_two_link_pendulum_on_a_cart.py similarity index 60% rename from examples-gallery/two-pendulum-cart-variable-gallery.py rename to examples-gallery/plot_two_link_pendulum_on_a_cart.py index 0831bbc..f3d8992 100644 --- a/examples-gallery/two-pendulum-cart-variable-gallery.py +++ b/examples-gallery/plot_two_link_pendulum_on_a_cart.py @@ -1,4 +1,6 @@ +# %% """ +========================= Upright a double pendulum. ========================== @@ -11,26 +13,26 @@ **Constants** -m1: mass of the cart [kg] -m2: mass of the first pendulum [kg] -m3: mass of the second pendulum [kg] -lx: length of the first pendulum rods [m] -iZZ1: moment of inertia of the first pendulum about its end point [kg*m^2] -iZZ2: moment of inertia of the second pendulum about its end point [kg*m^2] -g: acceleration due to gravity [m/s^2] +- m1: mass of the cart [kg] +- m2: mass of the first pendulum [kg] +- m3: mass of the second pendulum [kg] +- lx: length of the first pendulum rods [m] +- iZZ1: moment of inertia of the first pendulum about its end point [kg*m^2] +- iZZ2: moment of inertia of the second pendulum about its end point [kg*m^2] +- g: acceleration due to gravity [m/s^2] **States** -q1: position of the cart along the X axis [m] -q2: angle of the first pendulum with respect to the vertical [rad] -q3: angle of the second pendulum with respect to the first [rad] -u1: speed of the cart along the X axis [m/s] -u2: angular speed of the first pendulum [rad/s] -u3: angular speed of the second pendulum [rad/s] +- q1: position of the cart along the X axis [m] +- q2: angle of the first pendulum with respect to the vertical [rad] +- q3: angle of the second pendulum with respect to the first [rad] +- u1: speed of the cart along the X axis [m/s] +- u2: angular speed of the first pendulum [rad/s] +- u3: angular speed of the second pendulum [rad/s] **Specifieds** -f: force applied to the cart [N] +- f: force applied to the cart [N] """ import sympy.physics.mechanics as me @@ -40,15 +42,13 @@ from opty.direct_collocation import Problem import matplotlib.pyplot as plt -import matplotlib import matplotlib.animation as animation -matplotlib.rcParams['animation.embed_limit'] = 2**128 from matplotlib import patches # %% # Generate the nonholonomic equations of motion of the system. -N, A1, A2 = sm.symbols('N A1, A2', cls = me.ReferenceFrame) -t = me.dynamicsymbols._t +N, A1, A2 = sm.symbols('N A1, A2', cls = me.ReferenceFrame) +t = me.dynamicsymbols._t O, P1, P2, P3 = sm.symbols('O P1 P2 P3', cls = me.Point) O.set_vel(N, 0) q1, q2, q3, u1, u2, u3, F = me.dynamicsymbols('q1 q2 q3 u1 u2 u3 F') @@ -88,7 +88,7 @@ # %% # Define various objects to be use in the optimization problem. -h = sm.symbols('h') # the variable time element +h = sm.symbols('h') state_symbols = tuple((*q_ind, *u_ind)) constant_symbols = (lx, m1, m2, m3, g, iZZ1, iZZ2) @@ -100,9 +100,10 @@ duration = (num_nodes - 1) * h interval_value = h +# %% # Specify the known system parameters. par_map = OrderedDict() -par_map[lx] = 2.0 +par_map[lx] = 2.0 par_map[m1] = 1.0 par_map[m2] = 1.0 par_map[m3] = 1.0 @@ -121,24 +122,22 @@ def obj_grad(free): # %% # set the initial and final states to form the instance constraints. -# initially, the pendulum should be hanging straight down, without mvoing -# and finally, the pendulum should be upright, without moving. initial_state_constraints = { - q1: 0., - q2: -np.pi/2., - q3: -np.pi/2., - u1: 0., - u2: 0., - u3: 0. - } + q1: 0., + q2: -np.pi/2., + q3: -np.pi/2., + u1: 0., + u2: 0., + u3: 0. + } final_state_constraints = { - q2: target_angle, - q3: target_angle, - u1: 0., - u2: 0., - u3: 0. - } + q2: target_angle, + q3: target_angle, + u1: 0., + u2: 0., + u3: 0. + } instance_constraints = ( ) + tuple( @@ -146,21 +145,22 @@ def obj_grad(free): ) + tuple( xi.subs({t: duration}) - xi_val for xi, xi_val in final_state_constraints.items() ) -# bounding h > 0 herlps avoid 'solutions' with h < 0. +# %% +# bounding h > 0 helps avoid 'solutions' with h < 0. bounds = {F: (-150., 150.), q1: (-5, 5), h: (1.e-5, 1.0)} # %% # Create an optimization problem and solve it. prob = Problem(obj, - obj_grad, - EOM, - state_symbols, - num_nodes, - interval_value, - known_parameter_map=par_map, - instance_constraints=instance_constraints, - time_symbol=t, - bounds=bounds) + obj_grad, + EOM, + state_symbols, + num_nodes, + interval_value, + known_parameter_map=par_map, + instance_constraints=instance_constraints, + time_symbol=t, + bounds=bounds) # Initial guess. initial_guess = np.zeros(prob.num_free) @@ -168,63 +168,22 @@ def obj_grad(free): # allows to change the number of iterations. # standard is 3000 -prob.add_option('max_iter', 6000) +prob.add_option('max_iter', 3000) # Find the optimal solution. -# iterating and using the solution of iteration i -# as initial guess for iteration i+1 sometimes helps. -for i in range(2): - solution, info = prob.solve(initial_guess) - initial_guess = solution +solution, info = prob.solve(initial_guess) - print(f'{i + 1}st iteration: value of objective function is {obj(solution):.2f}') - prob.plot_objective_value() - print('message from optimizer:', info['status_msg']) - print('Iterations needed',len(prob.obj_value)) - print(f"objective value {info['obj_val']:.3e}") - print('h value:', solution[-1]) - print('\n') +prob.plot_objective_value() +print('message from optimizer:', info['status_msg']) +print('Iterations needed',len(prob.obj_value)) +print(f"objective value {info['obj_val']:.3e}") +print('h value:', solution[-1]) +print('\n') # %% # Plot the accuracy of the solution, and the state trajectories. -fehler = info['g'] -anzahl = len(state_symbols) -fig, ax = plt.subplots(2*anzahl + 1, 1, figsize=(10, 5*(anzahl+1)), sharex=True) -duration = (num_nodes - 1) * solution[-1] -times = np.linspace(0.0, duration, num=num_nodes) - -for i, j in enumerate(state_symbols): - if j in final_state_constraints.keys(): - farbe1 = 'red' - else: - farbe1 = 'black' - - if i < len(state_symbols): - farbe = 'b' - else: - farbe = 'r' - ax[i].plot(times, fehler[i * num_nodes:(i + 1) * num_nodes], color = farbe, lw=0.5) - ax[i].grid() - ax[i].set_ylabel(f'${str(j)}$', color=farbe1) -ax[-1].set_xlabel('time') -ax[0]. set_title(f'Errors in the generalized coordinates and speeds \n coordinates/speeds in the final_state_constraints are red', color = 'b'); -print(f'error in the final state constraint q2: {fehler[-5]:.3e}') -print(f'error in the final state constraint q3: {fehler[-4]:.3e}') -print(f'error in the final state constraint u1: {fehler[-3]:.3e}') -print(f'error in the final state constraint u2: {fehler[-2]:.3e}') -print(f'error in the final state constraint u3: {fehler[-1]:.3e}') - -#fig, ax = plt.subplots(7, 1, figsize=(10, 15), sharex=True) -#times = np.linspace(0.0, duration, num=num_nodes) - -for i, j in enumerate(state_symbols + specified_symbols): - k = anzahl + i - ax[k].plot(times, solution[i * num_nodes:(i + 1) * num_nodes]) - ax[k].grid() - ax[k].set_ylabel(str(j)) -ax[-1].set_xlabel('time') -ax[anzahl]. set_title('Generalized coordinates and speeds') -ax[-1]. set_title('Control force'); +prob.plot_trajectories(solution) +prob.plot_constraint_violations(solution) # %% # animate the solution. @@ -280,10 +239,13 @@ def animate_pendulum(time, P1_x, P1_y, P2_x, P2_y): line1, = ax.plot([], [], 'o-', lw=0.5, color='blue') line2, = ax.plot([], [], 'o-', lw=0.5, color='green') - recht = patches.Rectangle((P1_x[0] - width/2, P1_y[0] - height/2), width=width, height=height, fill=True, color='red', ec='black') + recht = patches.Rectangle((P1_x[0] - width/2, P1_y[0] - height/2), + width=width, height=height, fill=True, color='red', ec='black') ax.add_patch(recht) return fig, ax, line1, line2, recht +duration = (num_nodes - 1) * solution[-1] +times = np.linspace(0.0, duration, num=num_nodes) fig, ax, line1, line2, recht = animate_pendulum(times, P1_x, P1_y, P2_x, P2_y) def animate(i): @@ -303,9 +265,11 @@ def animate(i): anim = animation.FuncAnimation(fig, animate, frames=num_nodes, interval=2000*np.max(times) / num_nodes, blit=False) -# %% + +# %% # A frame from the animation. -# sphinx_gallery_thumbnail_number = 7 +# sphinx_gallery_thumbnail_number = 5 animate(150) + plt.show() \ No newline at end of file From 5cb1839df68c02b9e9192e9acc3af645fb9b2ae9 Mon Sep 17 00:00:00 2001 From: Peter230655 Date: Sun, 30 Jun 2024 10:10:44 +0200 Subject: [PATCH 3/4] found better initial conditions, but cannot get the plots right --- .../plot_two_link_pendulum_on_a_cart.py | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/examples-gallery/plot_two_link_pendulum_on_a_cart.py b/examples-gallery/plot_two_link_pendulum_on_a_cart.py index f3d8992..51c219b 100644 --- a/examples-gallery/plot_two_link_pendulum_on_a_cart.py +++ b/examples-gallery/plot_two_link_pendulum_on_a_cart.py @@ -1,7 +1,7 @@ # %% """ ========================= -Upright a double pendulum. +Upright a double pendulum ========================== A double pendulum is rotably attached to a cart which can move @@ -40,7 +40,7 @@ import numpy as np import sympy as sm from opty.direct_collocation import Problem - +import time import matplotlib.pyplot as plt import matplotlib.animation as animation from matplotlib import patches @@ -84,7 +84,7 @@ KM = me.KanesMethod(N, q_ind=q_ind, u_ind=u_ind, kd_eqs=kd) (fr, frstar) = KM.kanes_equations(BODY, FL) EOM = kd.col_join(fr + frstar) -sm.pprint(EOM) +sm.pprint(sm.trigsimp(EOM)) # %% # Define various objects to be use in the optimization problem. @@ -163,16 +163,15 @@ def obj_grad(free): bounds=bounds) # Initial guess. -initial_guess = np.zeros(prob.num_free) -initial_guess[-1] = 0.01 - +WERT = (0., 0., 0., 0.0, 0.0, 0.0, 80.0) +initial_guess = np.array([wert for wert in WERT for _ in range(num_nodes)] + [0.01]) # allows to change the number of iterations. # standard is 3000 prob.add_option('max_iter', 3000) # Find the optimal solution. solution, info = prob.solve(initial_guess) - + prob.plot_objective_value() print('message from optimizer:', info['status_msg']) print('Iterations needed',len(prob.obj_value)) @@ -183,6 +182,7 @@ def obj_grad(free): # %% # Plot the accuracy of the solution, and the state trajectories. prob.plot_trajectories(solution) +# %% prob.plot_constraint_violations(solution) # %% @@ -263,11 +263,12 @@ def animate(i): return line1, line2, anim = animation.FuncAnimation(fig, animate, frames=num_nodes, - interval=2000*np.max(times) / num_nodes, + interval=40, blit=False) -# %% +## %% # A frame from the animation. +animate_pendulum(times, P1_x, P1_y, P2_x, P2_y) # sphinx_gallery_thumbnail_number = 5 animate(150) From 24133805adbc28e0495d7033608c32247d98f283 Mon Sep 17 00:00:00 2001 From: Peter230655 Date: Sun, 30 Jun 2024 17:10:41 +0200 Subject: [PATCH 4/4] Made the changes you just told me to do, except the linear initial guess --- examples-gallery/plot_two_link_pendulum_on_a_cart.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/examples-gallery/plot_two_link_pendulum_on_a_cart.py b/examples-gallery/plot_two_link_pendulum_on_a_cart.py index 51c219b..b398127 100644 --- a/examples-gallery/plot_two_link_pendulum_on_a_cart.py +++ b/examples-gallery/plot_two_link_pendulum_on_a_cart.py @@ -1,6 +1,5 @@ # %% """ -========================= Upright a double pendulum ========================== @@ -95,7 +94,7 @@ specified_symbols = (F,) target_angle = np.pi /2.0 -num_nodes = 250 +num_nodes = 400 duration = (num_nodes - 1) * h interval_value = h @@ -163,7 +162,7 @@ def obj_grad(free): bounds=bounds) # Initial guess. -WERT = (0., 0., 0., 0.0, 0.0, 0.0, 80.0) +WERT = (0., 0., 0., 0.0, 0.0, 0.0, 0.0) initial_guess = np.array([wert for wert in WERT for _ in range(num_nodes)] + [0.01]) # allows to change the number of iterations. # standard is 3000 @@ -263,14 +262,14 @@ def animate(i): return line1, line2, anim = animation.FuncAnimation(fig, animate, frames=num_nodes, - interval=40, + interval=solution[-1]*1000.0, blit=False) ## %% # A frame from the animation. -animate_pendulum(times, P1_x, P1_y, P2_x, P2_y) +fig, ax, line1, line2, recht = animate_pendulum(times, P1_x, P1_y, P2_x, P2_y) # sphinx_gallery_thumbnail_number = 5 -animate(150) +animate(100) plt.show() \ No newline at end of file