Skip to content

Commit

Permalink
corrected error with instance = 0, added two examples
Browse files Browse the repository at this point in the history
  • Loading branch information
@Peter230655
Peter230655 committed Feb 19, 2025
1 parent 1a17afb commit f2235d6
Show file tree
Hide file tree
Showing 4 changed files with 85 additions and 68 deletions.
6 changes: 3 additions & 3 deletions examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
14 changes: 7 additions & 7 deletions examples-gallery/beginner/plot_sliding_block.py
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
# %%
"""
Block Sliding Over a Hill
=========================
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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.
Expand All @@ -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.
Expand Down
65 changes: 50 additions & 15 deletions opty/direct_collocation.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -709,13 +714,18 @@ 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:
instance_matrix = sm.Matrix(self.collocator.instance_constraints)
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
Expand All @@ -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
Expand All @@ -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)
Expand All @@ -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:
Expand All @@ -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)
Expand All @@ -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:
Expand Down Expand Up @@ -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


Expand Down
Loading

0 comments on commit f2235d6

Please sign in to comment.