From f2235d6f79cd3985d37fbc2bdff93fd3b495a000 Mon Sep 17 00:00:00 2001 From: Peter Stahlecker Date: Wed, 19 Feb 2025 14:37:45 +0100 Subject: [PATCH] corrected error with instance = 0, added two examples --- .../plot_pendulum_swing_up_fixed_duration.py | 6 +- .../beginner/plot_sliding_block.py | 14 ++-- opty/direct_collocation.py | 65 ++++++++++++++---- opty/tests/test_direct_collocation.py | 68 +++++++------------ 4 files changed, 85 insertions(+), 68 deletions(-) diff --git a/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py b/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py index e771df51..49a06636 100644 --- a/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py +++ b/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py @@ -78,9 +78,9 @@ time_symbol=t) # %% -# Use a random positive initial guess. -initial_guess = np.random.randn(prob.num_free) - +# Use a linear guess. +# %% +initial_guess = prob.create_linear_initial_guess(plot=True) # %% # Find the optimal solution. solution, info = prob.solve(initial_guess) diff --git a/examples-gallery/beginner/plot_sliding_block.py b/examples-gallery/beginner/plot_sliding_block.py index b9a9ded7..d6078969 100644 --- a/examples-gallery/beginner/plot_sliding_block.py +++ b/examples-gallery/beginner/plot_sliding_block.py @@ -1,4 +1,3 @@ -# %% """ Block Sliding Over a Hill ========================= @@ -174,6 +173,7 @@ def obj_grad(free): bounds=bounds, ) + initial_guess = prob.create_linear_initial_guess() solution, info = prob.solve(initial_guess) solution_list.append(solution) info_list.append(info) @@ -273,15 +273,15 @@ def update(frame): print(f'Optimal h value is: {solution_list[selection][-1]:.3f}') # %% -prob_list[selection].plot_objective_value() +_ = prob_list[selection].plot_objective_value() # %% # Plot errors in the solution. -prob_list[selection].plot_constraint_violations(solution_list[selection]) +_ = prob_list[selection].plot_constraint_violations(solution_list[selection]) # %% # Plot the trajectories of the block. -prob_list[selection].plot_trajectories(solution_list[selection]) +_ = prob_list[selection].plot_trajectories(solution_list[selection]) # %% # Animate the solution. @@ -294,15 +294,15 @@ def update(frame): print('Message from optimizer:', info_list[selection]['status_msg']) # %% -prob_list[selection].plot_objective_value() +_ = prob_list[selection].plot_objective_value() # %% # Plot errors in the solution. -prob_list[selection].plot_constraint_violations(solution_list[selection]) +_ =prob_list[selection].plot_constraint_violations(solution_list[selection]) # %% # Plot the trajectories of the block. -prob_list[selection].plot_trajectories(solution_list[selection]) +_ =prob_list[selection].plot_trajectories(solution_list[selection]) # %% # Animate the solution. diff --git a/opty/direct_collocation.py b/opty/direct_collocation.py index fa6a8ccf..f58ad820 100644 --- a/opty/direct_collocation.py +++ b/opty/direct_collocation.py @@ -685,13 +685,18 @@ def parse_free(self, free): return parse_free(free, n, q, N, variable_duration) - def create_linear_initial_guess(self): + def create_linear_initial_guess(self, plot=False): """Creates an initial guess that is the linear interpolation between exact instance constraints. Please see the notes for more information. + Parameters + ---------- + plot : bool, optional (default=False) + If True, the initial guess will be plotted. + Returns ------- - initial_guess : (n*N + q*N + r + s)-ndarray + initial_guess : ndarray shape(n*N + q*N + r + s) The initial guess for the free variables in the optimization problem. Notes @@ -709,6 +714,7 @@ def create_linear_initial_guess(self): - All else is set to zero. """ + hilfs = sm.symbols('hilfs') if self.collocator.instance_constraints is None: instance_matrix = sm.Matrix([]) else: @@ -716,6 +722,10 @@ def create_linear_initial_guess(self): par_map = self.collocator.known_parameter_map instance_matrix = instance_matrix.subs({key: par_map[key] for key in par_map.keys()}) + # setting the variable time interval to 1.0 makes getting the node + # numbers of the instance times easy. + instance_matrix = instance_matrix.subs({self.collocator. + node_time_interval: 1.0}) initial_guess = np.zeros(self.num_free) num_nodes = self.collocator.num_collocation_nodes @@ -737,6 +747,16 @@ def create_linear_initial_guess(self): M_new.row_del(row) instance_matrix = M_new + # If the instance constraint is of the from ``x(t) - 0``, or ``x(t)`` + # a dummy ``hilfs`` is added so all constraints have the same form. + # This dummy will be set to zero later. + for i, expr in enumerate(instance_matrix): + zaehler = 0 + for a in sm.preorder_traversal(expr): + zaehler += 1 + if zaehler == 2: + instance_matrix[i, 0] = instance_matrix[i, 0] + hilfs + if self.collocator.instance_constraints is not None: if not isinstance(self.collocator.node_time_interval, sm.Symbol): # fixed time interval @@ -759,7 +779,6 @@ def create_linear_initial_guess(self): name_rank = {name.name: i for i, name in enumerate( self.collocator.state_symbols)} liste3 = sorted(liste1, key=lambda x: (name_rank[x[3]], x[1])) - duration = self.collocator.node_time_interval * (num_nodes-1) for state in self.collocator.state_symbols: state_idx = self.collocator.state_symbols.index(state) @@ -768,7 +787,10 @@ def create_linear_initial_guess(self): for i in liste3: if state.name == i[3]: times.append(i[1]) - values.append(-i[2]) + if i[-2] != hilfs: + values.append(-i[2]) + else: + values.append(0) if len(times) == 0: pass elif len(times) == 1: @@ -787,12 +809,9 @@ def create_linear_initial_guess(self): else: liste = [] liste1 = [] - instance_matrix = instance_matrix.subs({self.collocator. - node_time_interval: 1.0}) for exp1 in instance_matrix: for a in sm.preorder_traversal(exp1): liste.append(a) - for entry in liste: if isinstance(entry, sm.Function): idx = liste.index(entry) @@ -815,7 +834,10 @@ def create_linear_initial_guess(self): for i in liste3: if state.name == i[3]: times.append(i[1]) - values.append(-i[2]) + if i[-2] != hilfs: + values.append(-i[2]) + else: + values.append(0) if len(times) == 0: pass elif len(times) == 1: @@ -863,22 +885,35 @@ def create_linear_initial_guess(self): initial_guess[start_idx + idx] = wert # set the value of the variable time interval. - if self.bounds is not None and \ - isinstance(self.collocator.node_time_interval, sm.Symbol): + if isinstance(self.collocator.node_time_interval, sm.Symbol): + if self.bounds is not None: if self.collocator.node_time_interval in self.bounds.keys(): if self.bounds[self.collocator.node_time_interval][0] \ == -np.inf: - wert = self.bounds[self.collocator.node_time_interval][1] + wert = self.bounds[self.collocator. + node_time_interval][1] elif self.bounds[self.collocator.node_time_interval][1] \ == np.inf: - wert = self.bounds[self.collocator.node_time_interval][0] + wert = self.bounds[self.collocator. + node_time_interval][0] else: wert = 0.5*(self.bounds[self.collocator. - node_time_interval][0] + - self.bounds[self.collocator. - node_time_interval][1]) + node_time_interval][0] + + self.bounds[self.collocator. + node_time_interval][1]) initial_guess[-1] = wert + if self.bounds is None: + initial_guess[-1] = 1.0 / (num_nodes-1) + + elif self.collocator.node_time_interval not in self.bounds.keys(): + initial_guess[-1] = 1.0 / (num_nodes-1) + else: + pass + + if plot: + self.plot_trajectories(initial_guess) + return initial_guess diff --git a/opty/tests/test_direct_collocation.py b/opty/tests/test_direct_collocation.py index cd37bfbc..2852b632 100644 --- a/opty/tests/test_direct_collocation.py +++ b/opty/tests/test_direct_collocation.py @@ -1928,7 +1928,7 @@ def test_linear_initial_guess(plot=False): state_symbols = (x, y, ux, uy) par_map = {b1: 1.0, b2: 2.0, - } + } num_nodes = 61 # A: CONSTANT TIME INTERVAL @@ -1946,7 +1946,7 @@ def obj_grad(free): return grad instance_constraints = ( - x.func(t0) - 3.0, + x.func(t0), y.func(t0) - 2.0, ux.func(t0) - 5.0, @@ -1956,10 +1956,10 @@ def obj_grad(free): uy.func(t1) - 4.5, x.func(t2) + 2.0, - y.func(t2) - 3.0, + y.func(t2), - x.func(tf) + 1.0, - ux.func(tf) + 5.0, + x.func(tf), + ux.func(tf), ) bounds= { @@ -1994,7 +1994,7 @@ def obj_grad(free): # x - guess start = round(t0/duration*num_nodes) ende = round(t1/duration*num_nodes) - werte = np.linspace(3.0, 1.5*par_map[b1], ende-start) + werte = np.linspace(0.0, 1.5*par_map[b1], ende-start) expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte start = round(t1/duration*num_nodes) ende = round(t2/duration*num_nodes) @@ -2002,7 +2002,7 @@ def obj_grad(free): expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte start = round(t2/duration*num_nodes) ende = round(tf/duration*num_nodes) - werte = np.linspace(-2.0, -1.0, ende-start) + werte = np.linspace(-2.0, 0.0, ende-start) expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte # y - guess start = round(t0/duration*num_nodes) @@ -2011,7 +2011,7 @@ def obj_grad(free): expected_guess[1*num_nodes+start:1*num_nodes+ende] = werte start = round(t1/duration*num_nodes) ende = round(t2/duration*num_nodes) - werte = np.linspace(4.0, 3.0, ende-start) + werte = np.linspace(4.0, 0.0, ende-start) expected_guess[1*num_nodes+start:1*num_nodes+ende] = werte # ux - guess start = round(t0/duration*num_nodes) @@ -2020,7 +2020,7 @@ def obj_grad(free): expected_guess[2*num_nodes+start:2*num_nodes+ende] = werte start = round(t1/duration*num_nodes) ende = round(tf/duration*num_nodes) - werte = np.linspace(1.0-par_map[b2], -5.0, ende-start) + werte = np.linspace(1.0-par_map[b2], 0.0, ende-start) expected_guess[2*num_nodes+start:2*num_nodes+ende] = werte # uy - guess expected_guess[3*num_nodes:4*num_nodes] = 4.5 @@ -2033,13 +2033,9 @@ def obj_grad(free): # a2 - guess expected_guess[6*num_nodes+1] = 11.0/2.0 - initial_guess = prob.create_linear_initial_guess() np.testing.assert_allclose(initial_guess, expected_guess) - if plot: - prob.plot_trajectories(initial_guess) - # A2: np.inf, -np.inf in bounds bounds[a1] = (-np.inf, 10.0) bounds[a2] = (-10.0, np.inf) @@ -2084,9 +2080,6 @@ def obj_grad(free): initial_guess = prob.create_linear_initial_guess() np.testing.assert_allclose(initial_guess, expected_guess) - if plot: - prob.plot_trajectories(initial_guess) - # A4: state instances in instance_constraints instance_constraints = ( x.func(t0) - 3.0 + ux.func(tf), @@ -2099,10 +2092,10 @@ def obj_grad(free): uy.func(t1) - 4.5, x.func(t2) + 2.0, - y.func(t2) - 3.0, + y.func(t2), - x.func(tf) + 1.0, - ux.func(tf) + 5.0, + x.func(tf), + ux.func(tf), ) start = round(t0/duration*num_nodes) @@ -2124,9 +2117,7 @@ def obj_grad(free): initial_guess = prob.create_linear_initial_guess() np.testing.assert_allclose(initial_guess, expected_guess) - if plot: - prob.plot_trajectories(initial_guess) - + # ======================================================================== # B: VARIABLE TIME INTERVAL # B1: normal h = sym.symbols('h') @@ -2143,8 +2134,8 @@ def obj_grad(free): return grad instance_constraints = ( - x.func(t00) - 3.0, - y.func(t00) - 2.0, + x.func(t00), + y.func(t00), ux.func(t00) - 5.0, x.func(t10) - 1.5*b1, @@ -2152,7 +2143,7 @@ def obj_grad(free): ux.func(t10) - 1.0+b2, uy.func(t10) - 4.5, - x.func(t20) + 2.0, + x.func(t20), y.func(t20) - 3.0, x.func(tf0) + 1.0, @@ -2194,20 +2185,20 @@ def obj_grad(free): # x - guess start = t0 ende = t1 - werte = np.linspace(3.0, 1.5*par_map[b1], ende-start) + werte = np.linspace(0.0, 1.5*par_map[b1], ende-start) expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte start = t1 ende = t2 - werte = np.linspace(1.5*par_map[b1], -2.0, ende-start) + werte = np.linspace(1.5*par_map[b1], 0.0, ende-start) expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte start = t2 ende = tf - werte = np.linspace(-2.0, -1.0, ende-start) + werte = np.linspace(0.0, -1.0, ende-start) expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte # y - guess start = t0 ende = t1 - werte = np.linspace(2.0, 4.0, ende-start) + werte = np.linspace(0.0, 4.0, ende-start) expected_guess[1*num_nodes+start:1*num_nodes+ende] = werte start = t1 ende = t2 @@ -2238,9 +2229,6 @@ def obj_grad(free): initial_guess = prob.create_linear_initial_guess() np.testing.assert_allclose(initial_guess, expected_guess) - if plot: - prob.plot_trajectories(initial_guess) - # B2: np.inf, -np.inf in bounds bounds[a1] = (-np.inf, 10.0) bounds[a2] = (-10.0, np.inf) @@ -2264,15 +2252,12 @@ def obj_grad(free): initial_guess = prob.create_linear_initial_guess() np.testing.assert_allclose(initial_guess, expected_guess) - if plot: - prob.plot_trajectories(initial_guess) - # B3: no bounds expected_guess[4*num_nodes: 5*num_nodes] = 0.0 expected_guess[5*num_nodes: 6*num_nodes] = 0.0 expected_guess[6*num_nodes] = 0.0 expected_guess[6*num_nodes+1] = 0.0 - expected_guess[-1] = 0.0 + expected_guess[-1] = 1.0 / (num_nodes-1) prob = Problem( obj, @@ -2288,13 +2273,10 @@ def obj_grad(free): initial_guess = prob.create_linear_initial_guess() np.testing.assert_allclose(initial_guess, expected_guess) - if plot: - prob.plot_trajectories(initial_guess) - # B4: state instances in instance_constraints instance_constraints = ( x.func(t00) - 3.0 + ux.func(tf0), - y.func(t00) - 2.0, + y.func(t00), ux.func(t00) - 5.0, x.func(t10) - 1.5*b1, @@ -2302,11 +2284,11 @@ def obj_grad(free): ux.func(t10) - 1.0+b2, uy.func(t10) - 4.5, - x.func(t20) + 2.0, + x.func(t20), y.func(t20) - 3.0, - x.func(tf0) + 1.0, - ux.func(tf0) + 5.0, + x.func(tf0)+ 1.0, + ux.func(tf0)+ 5.0, ) start = t0