Merge pull request #358 from Peter230655/bounds-conflict

check that initial guesses are within bounds. Fixes #355

examples-gallery/advanced/plot_car_around_pylons.py

@@ -311,13 +311,13 @@ def obj_grad(free):
     Fb: (-grenze, grenze),
     Tf: (-grenze, grenze),
     # restrict the steering angle
-    qf: (-np.pi/2. + delta, np.pi/2. - delta),
+    qf: (-np.pi/2. + delta - 1.e-5, np.pi/2. - delta + 1.e-5),
     x: (-20, 20),
     y: (-15, 30),
     h: (0.0, 0.5),
     acc_f: (-grenze1, grenze1),
     acc_b: (-grenze1, grenze1),
-    forward: (0.0, np.inf),
+    forward: (-1.e-5, np.inf),
     h1: (1.0, 5.0),
     h2: (1.0, 5.0),
 }

examples-gallery/advanced/plot_car_in_garage.py

@@ -318,17 +318,18 @@ def in_0_1(x):
 
 grenze = 25.0
 delta = np.pi/4.
+epsilon = 1.e-5
 bounds1 = {
         Fb: (-grenze, grenze),
         Tf: (-grenze, grenze),
         # restrict the steering angle to avoid locking
-        qf: (-np.pi/2. + delta, np.pi/2. - delta),
+        qf: (-np.pi/2. + delta - epsilon, np.pi/2. - delta + epsilon),
         # these bounds on x, y help convergence a lot!
         x: (-10, 10),
         y: (0.0, 25),
 }
 
-bounds2 = {py[i]: (0, 100) for i in range(number)}
+bounds2 = {py[i]: (0 - epsilon, 100 + epsilon) for i in range(number)}
 bounds = {**bounds1, **bounds2}
 
 prob = Problem(

examples-gallery/advanced/plot_human_gait.py

@@ -135,6 +135,7 @@
 #   height.
 # - Only let the hip, knee, and ankle flex and extend to realistic limits.
 # - Put a maximum on the peak torque values.
+
 bounds = {
     h: (0.001, 0.1),
     delt: (0.0, 10.0),
@@ -374,7 +375,7 @@ def update(i):
 pprint.pprint(prob.collocator.known_parameter_map)
 
 # %%
-# Use the Earth solution as an initial guess.
+# Use earth's solution as initial guess.
 solution, info = prob.solve(solution)
 
 # %%

examples-gallery/advanced/plot_one_legged_time_trial.py

@@ -725,7 +725,7 @@ def plot_configuration(q_vals, p_vals, ax=None):
 fig, axes = plt.subplots(16, 1, sharex=True,
                          figsize=(6.4, 0.8*16),
                          layout='compressed')
-problem.plot_trajectories(initial_guess, axes=axes)
+_ = problem.plot_trajectories(initial_guess, axes=axes)
 
 # %%
 # Solve the Optimal Control Problem
@@ -740,18 +740,18 @@ def plot_configuration(q_vals, p_vals, ax=None):
 # %%
 # Plot the Solution
 # -----------------
-problem.plot_objective_value()
+_ = problem.plot_objective_value()
 
 # %%
 fig, axes = plt.subplots(3, 1, figsize=(12.8, 10),
                          layout='constrained')
-problem.plot_constraint_violations(solution, axes=axes)
+_ = problem.plot_constraint_violations(solution, axes=axes)
 
 # %%
 fig, axes = plt.subplots(16, 1, sharex=True,
                          figsize=(6.4, 0.8*16),
                          layout='compressed')
-problem.plot_trajectories(solution, axes=axes)
+_ = problem.plot_trajectories(solution, axes=axes)
 
 # %%
 # Plot Musculo-tendon Behavior

examples-gallery/advanced/plot_park2004.py

@@ -166,6 +166,7 @@ def obj_grad(free):
 initial_guess = np.hstack((x_meas_vec,
                            (h.gain_scale_factors*h.numerical_gains).flatten()))
 
+
 # %%
 # Find the optimal solution and compare to the values used to generate the
 # data.

examples-gallery/advanced/plot_particle_in_tube.py

@@ -297,6 +297,7 @@ def distance(N, r1, curve, point):
 # Add some physical limits to the force, other bounds as needed to realize
 # the inequalities.
 grenze = 100.0
+
 bounds = {
     Fx: (-grenze, grenze),
     Fy: (-grenze, grenze),

examples-gallery/intermediate/plot_non_contiguous_parameter_identification.py

@@ -182,7 +182,6 @@ def obj_grad(free):
     integration_method='midpoint',
     bounds=bounds,
 )
-
 # %%
 # Create an Initial Guess
 # -----------------------
@@ -205,10 +204,10 @@ def obj_grad(free):
 print(info['status_msg'])
 
 # %%
-problem.plot_objective_value()
+_ = problem.plot_objective_value()
 
 # %%
-problem.plot_constraint_violations(solution)
+_ = problem.plot_constraint_violations(solution)
 
 # %%
 # The identified parameters are:

opty/direct_collocation.py

@@ -199,7 +199,7 @@ def __init__(self, obj, obj_grad, equations_of_motion, state_symbols,
 
         self.obj_value = []
 
-    def solve(self, free, lagrange=[], zl=[], zu=[]):
+    def solve(self, free, lagrange=[], zl=[], zu=[], respect_bounds=False):
         """Returns the optimal solution and an info dictionary.
 
         Solves the posed optimization problem starting at point x.
@@ -221,6 +221,11 @@ def solve(self, free, lagrange=[], zl=[], zu=[]):
             Initial values for the multipliers for upper variable bounds (only
             if warm start option is chosen).
 
+        respect_bounds : bool, optional (default=False)
+            If True, the initial guess is checked to ensure that it is within
+            the bounds, and a ValueError is raised if it is not. If False, the
+            initial guess is not checked.
+
         Returns
         -------
         x : :py:class:`numpy.ndarray`, shape `(n*N + q*N + r + s, )`
@@ -244,9 +249,81 @@ def solve(self, free, lagrange=[], zl=[], zu=[]):
                 gives the status of the algorithm as a message
 
         """
-
+        if respect_bounds:
+            self.check_bounds_conflict(free)
         return super().solve(free, lagrange=lagrange, zl=zl, zu=zu)
 
+    def check_bounds_conflict(self, free):
+        """
+        Ascertains that the initial guesses for all variables are within the
+        limits prescribed by their respective bounds. Raises a ValueError if
+        for any variable the initial guess is outside its bounds, or if the
+        lower bound is greater than the upper bound.
+
+        Parameters
+        ----------
+        free : array_like, shape(n*N + q*N + r + s, )
+            Initial guess given to solve.
+
+        Raises
+        ------
+        ValueError
+            If the lower bound for variable is greater than its upper bound,
+            ``opty`` may not break, but the solution will likely not be correct.
+            Hence a ValueError is raised in such as case.
+
+            If the initial guess for any variable is outside its bounds,
+            a ValueError is raised.
+
+        """
+        if self.bounds is not None:
+            errors = []
+            # check for reversed bounds
+            for key in self.bounds.keys():
+                if self.bounds[key][0] > self.bounds[key][1]:
+                    errors.append(key)
+            if len(errors) > 0:
+                msg = (f'The lower bound(s) for {errors} is (are) greater than'
+                           ' the upper bound(s).')
+                raise ValueError(msg)
+
+            violating_variables = []
+
+            if self.collocator._variable_duration:
+                local_ts = self.collocator.time_interval_symbol
+                if local_ts in self.bounds.keys():
+                    if (free[-1] < self.bounds[local_ts][0]
+                        or free[-1] > self.bounds[local_ts][1]):
+                        violating_variables.append(local_ts)
+
+            symbole = (self.collocator.state_symbols +
+                self.collocator.unknown_input_trajectories)
+            for symb in symbole:
+                if symb in self.bounds.keys():
+                    idx = symbole.index(symb)
+                    feld = free[idx*self.collocator.num_collocation_nodes:
+                            (idx+1)*self.collocator.num_collocation_nodes]
+                    if (np.any(feld < self.bounds[symb][0])
+                        or np.any(feld > self.bounds[symb][1])):
+                        violating_variables.append(symb)
+
+            # check that initial guesses for unknown parameters are within
+            startidx = len(symbole) * self.collocator.num_collocation_nodes
+            for symb in self.collocator.unknown_parameters:
+                if symb in self.bounds.keys():
+                    idx = self.collocator.unknown_parameters.index(symb)
+                    if (free[startidx+idx] < self.bounds[symb][0]
+                        or free[startidx+idx] > self.bounds[symb][1]):
+                        violating_variables.append(symb)
+
+            if len(violating_variables) > 0:
+                msg = (f'The initial guesses for {violating_variables} are in '
+                f'conflict with their bounds.')
+                raise ValueError(msg)
+
+        else:
+            pass
+
     def _generate_bound_arrays(self):
         lb = -self.INF * np.ones(self.num_free)
         ub = self.INF * np.ones(self.num_free)