From 5b95d18e67d28e7ccdc488854277d167dcbd90a2 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 31 May 2025 15:37:16 +0200 Subject: [PATCH 1/5] Attempt at solving a simple problem with friction using a slack variable. --- .../beginner/plot_friction_slack.py | 129 ++++++++++++++++++ 1 file changed, 129 insertions(+) create mode 100644 examples-gallery/beginner/plot_friction_slack.py diff --git a/examples-gallery/beginner/plot_friction_slack.py b/examples-gallery/beginner/plot_friction_slack.py new file mode 100644 index 00000000..1ae19429 --- /dev/null +++ b/examples-gallery/beginner/plot_friction_slack.py @@ -0,0 +1,129 @@ +""" +Multiphase Collision +==================== + +A block is sliding on a surface with Coulomb friction. Push it with a (limited) +rightward force until it hits a wall 1m on the right. When it hits the wall +enforce a Newtonian collision with a coefficient of restitution e so that the +block bounces off the wall and slides backwards to its original location. Apply +this force such that the block reaches its original location in the minimal +amount of time and that each phase has the same duration. + +""" + +import numpy as np +import sympy as sm +from opty import Problem +import matplotlib.pyplot as plt + +# %% +# Start with defining the fixed duration and number of nodes. +num_nodes = 201 + +# %% +# Symbolic equations of motion, note that we make two sets: one before the # +# collision and one after. +m, mu, g, t, h = sm.symbols('m, mu, g, t, h', real=True) +x, v, psi, Fp, Fn, F = sm.symbols('x, v, psi, Fp, Fn, F', cls=sm.Function) + +state_symbols = (x(t), v(t)) +constant_symbols = (m, mu, g) +specified_symbols = (F(t), Fn(t), Fp(t), psi(t)) + +eom = sm.Matrix([ + x(t).diff(t) - v(t), + m*v(t).diff(t) - Fp(t) + Fn(t) - F(t), + # following two lines ensure: psi >= abs(v) + psi(t) + v(t), # >= 0 + psi(t) - v(t), # >= 0 + # mu*m*g >= Fp + Fn + mu*m*g - Fp(t) - Fn(t), # >= 0 + # mu*m*g*psi = (Fp + Fn)*psi -> mu*m*g = Fn v > 0 & mu*m*g = Fp if v < 0 + (mu*m*g - Fp(t) - Fn(t))*psi(t), + # Fp*psi = -Fp*v -> Fp is zero if v > 0 + Fp(t)*(psi(t) + v(t)), + # Fn*psi = Fn*v -> Fn is zero if v < 0 + Fn(t)*(psi(t) - v(t)), +]) +sm.pprint(eom) + +# %% +# Specify the known system parameters. +par_map = { + m: 10.0, + mu: 0.6, + g: 9.81, +} + + +# %% +# Specify the objective function and it's gradient. +def obj(free): + """Return h (always the last element in the free variables).""" + return free[-1] + + +def obj_grad(free): + """Return the gradient of the objective.""" + grad = np.zeros_like(free) + grad[-1] = 1.0 + return grad + + +# %% +# Specify the symbolic instance constraints, i.e. initial and end conditions +# using node numbers 0 to N - 1 +t0, tf = 0*h, (num_nodes - 1)*h +instance_constraints = ( + x(t0) - 0.0, + v(t0) - 0.0, + #x(tf/2) - 10.0, + #v(tf/2) - 0.0, + x(tf) - 10.0, + v(tf) - 0.0, +) + +bounds = { + F(t): (-400.0, 400.0), + h: (0.0, 1.0), +} + +eom_bounds = { + 2: (0.0, np.inf), + 3: (0.0, np.inf), + 4: (0.0, np.inf), +} + +# %% +# Create an optimization problem. +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, + backend='numpy') + +# %% +# Use a zero as an initial guess. +initial_guess = np.random.random(prob.num_free) +initial_guess[-1] = 0.01 + +# %% +# Find the optimal solution. +solution, info = prob.solve(initial_guess) +print(info['status_msg']) +print(info['obj_val']) + +# %% +# Plot the optimal state and input trajectories. +prob.plot_trajectories(solution) + +# %% +# Plot the constraint violations. +prob.plot_constraint_violations(solution) + +# %% +# Plot the objective function as a function of optimizer iteration. +prob.plot_objective_value() + +plt.show() From cab90220e26a783c163fa1bdf2e25b39af7e06b9 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 31 May 2025 15:51:59 +0200 Subject: [PATCH 2/5] Seems to solve. --- examples-gallery/beginner/plot_friction_slack.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples-gallery/beginner/plot_friction_slack.py b/examples-gallery/beginner/plot_friction_slack.py index 1ae19429..7ce52ff6 100644 --- a/examples-gallery/beginner/plot_friction_slack.py +++ b/examples-gallery/beginner/plot_friction_slack.py @@ -18,7 +18,7 @@ # %% # Start with defining the fixed duration and number of nodes. -num_nodes = 201 +num_nodes = 501 # %% # Symbolic equations of motion, note that we make two sets: one before the # @@ -50,7 +50,7 @@ # %% # Specify the known system parameters. par_map = { - m: 10.0, + m: 1.0, mu: 0.6, g: 9.81, } @@ -84,7 +84,7 @@ def obj_grad(free): ) bounds = { - F(t): (-400.0, 400.0), + F(t): (0.0, 200.0), h: (0.0, 1.0), } @@ -105,7 +105,7 @@ def obj_grad(free): # %% # Use a zero as an initial guess. -initial_guess = np.random.random(prob.num_free) +initial_guess = 0.02*np.ones(prob.num_free) initial_guess[-1] = 0.01 # %% From 4f9e963d513ebb76cca74e6aff8049c45959890c Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 31 May 2025 18:01:29 +0200 Subject: [PATCH 3/5] Solves easily with good guess. --- examples-gallery/beginner/plot_friction_slack.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/examples-gallery/beginner/plot_friction_slack.py b/examples-gallery/beginner/plot_friction_slack.py index 7ce52ff6..aab38cf5 100644 --- a/examples-gallery/beginner/plot_friction_slack.py +++ b/examples-gallery/beginner/plot_friction_slack.py @@ -84,7 +84,7 @@ def obj_grad(free): ) bounds = { - F(t): (0.0, 200.0), + F(t): (0.0, 400.0), h: (0.0, 1.0), } @@ -105,7 +105,13 @@ def obj_grad(free): # %% # Use a zero as an initial guess. -initial_guess = 0.02*np.ones(prob.num_free) +initial_guess = np.zeros(prob.num_free) +initial_guess[0*num_nodes:1*num_nodes] = np.linspace(0.0, 10.0, num=num_nodes) +initial_guess[1*num_nodes:2*num_nodes] = 10.0*np.ones(num_nodes) +initial_guess[2*num_nodes:3*num_nodes] = 10.0*np.ones(num_nodes) # F +initial_guess[3*num_nodes:4*num_nodes] = 0.0*np.ones(num_nodes) # Fn +initial_guess[4*num_nodes:5*num_nodes] = 0.6*np.ones(num_nodes) # Fp +initial_guess[5*num_nodes:6*num_nodes] = 10.0*np.ones(num_nodes) # psi initial_guess[-1] = 0.01 # %% From ec947ca3a285ca86f1d5ac4af8a8e29d139e4bf6 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 1 Jun 2025 09:14:23 +0200 Subject: [PATCH 4/5] Solves with forwards and backwards motion (but not perfectly). --- .../beginner/plot_friction_slack.py | 69 ++++++++++++------- 1 file changed, 46 insertions(+), 23 deletions(-) diff --git a/examples-gallery/beginner/plot_friction_slack.py b/examples-gallery/beginner/plot_friction_slack.py index aab38cf5..57568f80 100644 --- a/examples-gallery/beginner/plot_friction_slack.py +++ b/examples-gallery/beginner/plot_friction_slack.py @@ -1,6 +1,6 @@ """ -Multiphase Collision -==================== +Coulomb Friction with Slack Variables +===================================== A block is sliding on a surface with Coulomb friction. Push it with a (limited) rightward force until it hits a wall 1m on the right. When it hits the wall @@ -14,12 +14,9 @@ import numpy as np import sympy as sm from opty import Problem +from opty.utils import MathJaxRepr import matplotlib.pyplot as plt -# %% -# Start with defining the fixed duration and number of nodes. -num_nodes = 501 - # %% # Symbolic equations of motion, note that we make two sets: one before the # # collision and one after. @@ -46,6 +43,7 @@ Fn(t)*(psi(t) - v(t)), ]) sm.pprint(eom) +MathJaxRepr(eom) # %% # Specify the known system parameters. @@ -73,18 +71,22 @@ def obj_grad(free): # %% # Specify the symbolic instance constraints, i.e. initial and end conditions # using node numbers 0 to N - 1 -t0, tf = 0*h, (num_nodes - 1)*h +# %% +# Start with defining the fixed duration and number of nodes. +N = 400 + +t0, tm, tf = 0*h, (N//2)*h, (N - 1)*h instance_constraints = ( x(t0) - 0.0, v(t0) - 0.0, - #x(tf/2) - 10.0, - #v(tf/2) - 0.0, - x(tf) - 10.0, + x(tm) - 10.0, + v(tm) - 0.0, + x(tf) + 0.0, v(tf) - 0.0, ) bounds = { - F(t): (0.0, 400.0), + F(t): (-400.0, 400.0), h: (0.0, 1.0), } @@ -96,24 +98,45 @@ def obj_grad(free): # %% # Create an optimization problem. -prob = Problem(obj, obj_grad, eom, state_symbols, num_nodes, h, +prob = Problem(obj, obj_grad, eom, state_symbols, N, h, known_parameter_map=par_map, instance_constraints=instance_constraints, time_symbol=t, bounds=bounds, eom_bounds=eom_bounds, - backend='numpy') + backend='numpy', + ) + +prob.add_option('max_iter', 10000) # %% # Use a zero as an initial guess. +half = N//2 initial_guess = np.zeros(prob.num_free) -initial_guess[0*num_nodes:1*num_nodes] = np.linspace(0.0, 10.0, num=num_nodes) -initial_guess[1*num_nodes:2*num_nodes] = 10.0*np.ones(num_nodes) -initial_guess[2*num_nodes:3*num_nodes] = 10.0*np.ones(num_nodes) # F -initial_guess[3*num_nodes:4*num_nodes] = 0.0*np.ones(num_nodes) # Fn -initial_guess[4*num_nodes:5*num_nodes] = 0.6*np.ones(num_nodes) # Fp -initial_guess[5*num_nodes:6*num_nodes] = 10.0*np.ones(num_nodes) # psi + +initial_guess[0*N:1*N - half] = np.linspace(0.0, 10.0, num=half) # x +initial_guess[1*N - half:1*N] = np.linspace(10.0, 0.0, num=half) # x + +initial_guess[1*N:2*N - half] = 10.0 # v +initial_guess[2*N - half:2*N] = -10.0 # v + +initial_guess[2*N:3*N - half] = 5.0 # F +initial_guess[3*N - half:3*N] = -5.0 # F + +initial_guess[3*N:4*N - half] = 0.6 # Fn +initial_guess[4*N - half:4*N] = 0.0 # Fn + +initial_guess[4*N:5*N - half] = 0.0 # Fp +initial_guess[5*N - half:5*N] = 0.6 # Fp + +initial_guess[5*N:6*N - half] = 10.0 # psi +initial_guess[6*N - half:6*N] = 10.0 # psi + initial_guess[-1] = 0.01 +# %% +# Plot the optimal state and input trajectories. +prob.plot_trajectories(initial_guess) + # %% # Find the optimal solution. solution, info = prob.solve(initial_guess) @@ -121,15 +144,15 @@ def obj_grad(free): print(info['obj_val']) # %% -# Plot the optimal state and input trajectories. -prob.plot_trajectories(solution) +# Plot the objective function as a function of optimizer iteration. +prob.plot_objective_value() # %% # Plot the constraint violations. prob.plot_constraint_violations(solution) # %% -# Plot the objective function as a function of optimizer iteration. -prob.plot_objective_value() +# Plot the optimal state and input trajectories. +prob.plot_trajectories(solution) plt.show() From 918a6abadcd84a3f75553e0d499d44dbea9fcc39 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 1 Jun 2025 09:42:58 +0200 Subject: [PATCH 5/5] Reduce to 100 nodes and add plot of friction force. --- .../beginner/plot_friction_slack.py | 76 +++++++++++-------- 1 file changed, 44 insertions(+), 32 deletions(-) diff --git a/examples-gallery/beginner/plot_friction_slack.py b/examples-gallery/beginner/plot_friction_slack.py index 57568f80..52ed631c 100644 --- a/examples-gallery/beginner/plot_friction_slack.py +++ b/examples-gallery/beginner/plot_friction_slack.py @@ -2,12 +2,16 @@ Coulomb Friction with Slack Variables ===================================== -A block is sliding on a surface with Coulomb friction. Push it with a (limited) -rightward force until it hits a wall 1m on the right. When it hits the wall -enforce a Newtonian collision with a coefficient of restitution e so that the -block bounces off the wall and slides backwards to its original location. Apply -this force such that the block reaches its original location in the minimal -amount of time and that each phase has the same duration. +A block of mass :math:`m` is being pushed with force :math:`F(t)` along on a +surface. Coulomb friction acts between the block and the surface. Find a +minimal time solution to push the block 10 meters and then back to the original +position. + +Objectives +---------- + +- Demonstrate how slack variables and inequality constraints can be used to + manage discontinuties in the equations of motion. """ @@ -18,29 +22,29 @@ import matplotlib.pyplot as plt # %% -# Symbolic equations of motion, note that we make two sets: one before the # -# collision and one after. +# Symbolic equations of motion. m, mu, g, t, h = sm.symbols('m, mu, g, t, h', real=True) -x, v, psi, Fp, Fn, F = sm.symbols('x, v, psi, Fp, Fn, F', cls=sm.Function) +x, v, psi, Ffp, Ffn, F = sm.symbols('x, v, psi, Ffp, Ffn, F', cls=sm.Function) state_symbols = (x(t), v(t)) constant_symbols = (m, mu, g) -specified_symbols = (F(t), Fn(t), Fp(t), psi(t)) +specified_symbols = (F(t), Ffn(t), Ffp(t), psi(t)) eom = sm.Matrix([ + # equations of motion with positive and negative friction force x(t).diff(t) - v(t), - m*v(t).diff(t) - Fp(t) + Fn(t) - F(t), + m*v(t).diff(t) - Ffp(t) + Ffn(t) - F(t), # following two lines ensure: psi >= abs(v) psi(t) + v(t), # >= 0 psi(t) - v(t), # >= 0 - # mu*m*g >= Fp + Fn - mu*m*g - Fp(t) - Fn(t), # >= 0 - # mu*m*g*psi = (Fp + Fn)*psi -> mu*m*g = Fn v > 0 & mu*m*g = Fp if v < 0 - (mu*m*g - Fp(t) - Fn(t))*psi(t), - # Fp*psi = -Fp*v -> Fp is zero if v > 0 - Fp(t)*(psi(t) + v(t)), - # Fn*psi = Fn*v -> Fn is zero if v < 0 - Fn(t)*(psi(t) - v(t)), + # mu*m*g >= Ffp + Ffn + mu*m*g - Ffp(t) - Ffn(t), # >= 0 + # mu*m*g*psi = (Ffp + Ffn)*psi -> mu*m*g = Ffn v > 0 & mu*m*g = Ffp if v < 0 + (mu*m*g - Ffp(t) - Ffn(t))*psi(t), + # Ffp*psi = -Ffp*v -> Ffp is zero if v > 0 + Ffp(t)*(psi(t) + v(t)), + # Ffn*psi = Ffn*v -> Ffn is zero if v < 0 + Ffn(t)*(psi(t) - v(t)), ]) sm.pprint(eom) MathJaxRepr(eom) @@ -73,7 +77,7 @@ def obj_grad(free): # using node numbers 0 to N - 1 # %% # Start with defining the fixed duration and number of nodes. -N = 400 +N = 100 t0, tm, tf = 0*h, (N//2)*h, (N - 1)*h instance_constraints = ( @@ -87,7 +91,7 @@ def obj_grad(free): bounds = { F(t): (-400.0, 400.0), - h: (0.0, 1.0), + h: (0.0, 0.2), } eom_bounds = { @@ -103,10 +107,9 @@ def obj_grad(free): instance_constraints=instance_constraints, time_symbol=t, bounds=bounds, eom_bounds=eom_bounds, - backend='numpy', - ) + backend='numpy') -prob.add_option('max_iter', 10000) +prob.add_option('max_iter', 4000) # %% # Use a zero as an initial guess. @@ -119,22 +122,22 @@ def obj_grad(free): initial_guess[1*N:2*N - half] = 10.0 # v initial_guess[2*N - half:2*N] = -10.0 # v -initial_guess[2*N:3*N - half] = 5.0 # F -initial_guess[3*N - half:3*N] = -5.0 # F +initial_guess[2*N:3*N - half] = 10.0 # F +initial_guess[3*N - half:3*N] = -10.0 # F -initial_guess[3*N:4*N - half] = 0.6 # Fn -initial_guess[4*N - half:4*N] = 0.0 # Fn +initial_guess[3*N:4*N - half] = 1.0 # Ffn +initial_guess[4*N - half:4*N] = 0.0 # Ffn -initial_guess[4*N:5*N - half] = 0.0 # Fp -initial_guess[5*N - half:5*N] = 0.6 # Fp +initial_guess[4*N:5*N - half] = 0.0 # Ffp +initial_guess[5*N - half:5*N] = 1.0 # Ffp initial_guess[5*N:6*N - half] = 10.0 # psi initial_guess[6*N - half:6*N] = 10.0 # psi -initial_guess[-1] = 0.01 +initial_guess[-1] = 0.005 # %% -# Plot the optimal state and input trajectories. +# Plot the initial guess. prob.plot_trajectories(initial_guess) # %% @@ -155,4 +158,13 @@ def obj_grad(free): # Plot the optimal state and input trajectories. prob.plot_trajectories(solution) +# %% +# Plot the friction force. +xs, rs, _, _ = prob.parse_free(solution) +ts = prob.time_vector(solution) +fig, ax = plt.subplots() +ax.plot(ts, -rs[1] + rs[2]) +ax.set_ylabel(r'$F_f$ [N]') +ax.set_xlabel('Time [s]') + plt.show()