Skip to content

Commit d121102

Browse files
murrayrmmurrayrm
committed
updated examples + code cleanup
1 parent 8256fa0 commit d121102

5 files changed

Lines changed: 190 additions & 146 deletions

File tree

‎control/optimal.py‎

Lines changed: 76 additions & 72 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,6 @@
2323
# Define module default parameter values
2424
_optimal_trajectory_methods = {'shooting', 'collocation'}
2525
_optimal_defaults = {
26-
'optimal.trajectory_method': 'shooting',
2726
'optimal.minimize_method': None,
2827
'optimal.minimize_options': {},
2928
'optimal.minimize_kwargs': {},
@@ -147,9 +146,8 @@ def __init__(
147146
self.terminal_constraints = terminal_constraints
148147
self.basis = basis
149148

150-
# Keep track of what type of trajector method we are using
149+
# Keep track of what type of trajectory method we are using
151150
if trajectory_method is None:
152-
# TODO: change default
153151
trajectory_method = 'collocation' if sys.isctime() else 'shooting'
154152
elif trajectory_method not in _optimal_trajectory_methods:
155153
raise NotImplementedError(f"Unkown method {method}")
@@ -185,30 +183,23 @@ def __init__(
185183
raise TypeError("unrecognized keyword(s): ", str(kwargs))
186184

187185
# Process trajectory constraints
188-
if isinstance(trajectory_constraints, tuple):
189-
self.trajectory_constraints = [trajectory_constraints]
190-
elif not isinstance(trajectory_constraints, list):
191-
raise TypeError("trajectory constraints must be a list")
192-
else:
193-
self.trajectory_constraints = trajectory_constraints
186+
def _process_constraints(constraint_list, name):
187+
if isinstance(constraint_list, tuple):
188+
constraint_list = [constraint_list]
189+
elif not isinstance(constraint_list, list):
190+
raise TypeError(f"{name} constraints must be a list")
194191

195-
# Make sure that we recognize all of the constraint types
196-
for ctype, fun, lb, ub in self.trajectory_constraints:
197-
if not ctype in [opt.LinearConstraint, opt.NonlinearConstraint]:
198-
raise TypeError(f"unknown constraint type {ctype}")
192+
# Make sure that we recognize all of the constraint types
193+
for ctype, fun, lb, ub in constraint_list:
194+
if not ctype in [opt.LinearConstraint, opt.NonlinearConstraint]:
195+
raise TypeError(f"unknown {name} constraint type {ctype}")
199196

200-
# Process terminal constraints
201-
if isinstance(terminal_constraints, tuple):
202-
self.terminal_constraints = [terminal_constraints]
203-
elif not isinstance(terminal_constraints, list):
204-
raise TypeError("terminal constraints must be a list")
205-
else:
206-
self.terminal_constraints = terminal_constraints
197+
return constraint_list
207198

208-
# Make sure that we recognize all of the constraint types
209-
for ctype, fun, lb, ub in self.terminal_constraints:
210-
if not ctype in [opt.LinearConstraint, opt.NonlinearConstraint]:
211-
raise TypeError(f"unknown constraint type {ctype}")
199+
self.trajectory_constraints = _process_constraints(
200+
trajectory_constraints, "trajectory")
201+
self.terminal_constraints = _process_constraints(
202+
terminal_constraints, "terminal")
212203

213204
#
214205
# Compute and store constraints
@@ -223,10 +214,13 @@ def __init__(
223214
# is consistent with the `_constraint_function` that is used at
224215
# evaluation time.
225216
#
217+
# TODO: when using the collocation method, linear constraints on the
218+
# states and inputs can potentially maintain their linear structure
219+
# rather than being converted to nonlinear constraints.
220+
#
226221
constraint_lb, constraint_ub, eqconst_value = [], [], []
227222

228223
# Go through each time point and stack the bounds
229-
# TODO: for collocation method, keep track of linear vs nonlinear
230224
for t in self.timepts:
231225
for type, fun, lb, ub in self.trajectory_constraints:
232226
if np.all(lb == ub):
@@ -253,15 +247,19 @@ def __init__(
253247
self.eqconst_value = np.hstack(eqconst_value) if eqconst_value else []
254248

255249
# Create the constraints (inequality and equality)
250+
# TODO: for collocation method, keep track of linear vs nonlinear
256251
self.constraints = []
252+
257253
if len(self.constraint_lb) != 0:
258254
self.constraints.append(sp.optimize.NonlinearConstraint(
259255
self._constraint_function, self.constraint_lb,
260256
self.constraint_ub))
257+
261258
if len(self.eqconst_value) != 0:
262259
self.constraints.append(sp.optimize.NonlinearConstraint(
263260
self._eqconst_function, self.eqconst_value,
264261
self.eqconst_value))
262+
265263
if self.collocation:
266264
# Add the collocation constraints
267265
colloc_zeros = np.zeros(sys.nstates * self.timepts.size)
@@ -275,7 +273,7 @@ def __init__(
275273
# Process the initial guess
276274
self.initial_guess = self._process_initial_guess(initial_guess)
277275

278-
# Store states, input, used later to minimize re-computation
276+
# Store states, input (used later to minimize re-computation)
279277
self.last_x = np.full(self.system.nstates, np.nan)
280278
self.last_coeffs = np.full(self.initial_guess.shape, np.nan)
281279

@@ -286,10 +284,14 @@ def __init__(
286284
#
287285
# Cost function
288286
#
289-
# Given the input U = [u[0], ... u[N]], we need to compute the cost of
290-
# the trajectory generated by that input. This means we have to
291-
# simulate the system to get the state trajectory X = [x[0], ...,
292-
# x[N]] and then compute the cost at each point:
287+
# For collocation methods we are given the states and inputs at each
288+
# time point and we use a trapezoidal approximation to compute the
289+
# integral cost, then add on the terminal cost.
290+
#
291+
# For shooting methods, given the input U = [u[0], ... u[N]] we need to
292+
# compute the cost of the trajectory generated by that input. This
293+
# means we have to simulate the system to get the state trajectory X =
294+
# [x[0], ..., x[N]] and then compute the cost at each point:
293295
#
294296
# cost = sum_k integral_cost(x[k], u[k]) + terminal_cost(x[N], u[N])
295297
#
@@ -362,11 +364,12 @@ def _cost_function(self, coeffs):
362364
# is called at each point along the trajectory and compared against the
363365
# upper and lower bounds.
364366
#
365-
# * If the upper and lower bound for the constraint are identical, then we
366-
# separate out the evaluation into two different constraints, which
367-
# allows the SciPy optimizers to be more efficient (and stops them from
368-
# generating a warning about mixed constraints). This is handled
369-
# through the use of the `_eqconst_function` and `eqconst_value` members.
367+
# * If the upper and lower bound for the constraint are identical, then
368+
# we separate out the evaluation into two different constraints, which
369+
# allows the SciPy optimizers to be more efficient (and stops them
370+
# from generating a warning about mixed constraints). This is handled
371+
# through the use of the `_eqconst_function` and `eqconst_value`
372+
# members.
370373
#
371374
# In both cases, the constraint is specified at a single point, but we
372375
# extend this to apply to each point in the trajectory. This means
@@ -378,16 +381,12 @@ def _cost_function(self, coeffs):
378381
# For collocation methods, we can directly evaluate the constraints at
379382
# the collocation points.
380383
#
381-
# For shooting methods, we do this by creating a function that
382-
# simulates the system dynamics and returns a vector of values
383-
# corresponding to the value of the function at each time. The
384-
# class initialization methods takes care of replicating the upper
385-
# and lower bounds for each point in time so that the SciPy
386-
# optimization algorithm can do the proper evaluation.
387-
#
388-
# In addition, since SciPy's optimization function does not allow us to
389-
# pass arguments to the constraint function, we have to store the initial
390-
# state prior to optimization and retrieve it here.
384+
# For shooting methods, we do this by creating a function that simulates
385+
# the system dynamics and returns a vector of values corresponding to
386+
# the value of the function at each time. The class initialization
387+
# methods takes care of replicating the upper and lower bounds for each
388+
# point in time so that the SciPy optimization algorithm can do the
389+
# proper evaluation.
391390
#
392391
def _constraint_function(self, coeffs):
393392
if self.log:
@@ -437,8 +436,7 @@ def _constraint_function(self, coeffs):
437436
"_constraint_function elapsed time: %g",
438437
stop_time - start_time)
439438

440-
# Debugging information
441-
if self.log:
439+
# Debugging information
442440
logging.debug(
443441
"constraint values\n" + str(value) + "\n" +
444442
"lb, ub =\n" + str(self.constraint_lb) + "\n" +
@@ -492,8 +490,7 @@ def _eqconst_function(self, coeffs):
492490
logging.info(
493491
"_eqconst_function elapsed time: %g", stop_time - start_time)
494492

495-
# Debugging information
496-
if self.log:
493+
# Debugging information
497494
logging.debug(
498495
"eqconst values\n" + str(value) + "\n" +
499496
"desired =\n" + str(self.eqconst_value))
@@ -515,7 +512,7 @@ def _collocation_constraint(self, coeffs):
515512
self.timepts[0], states[:, 0], inputs[:, 0])
516513
continue
517514

518-
# M. Kelly, SIAM Review (2017), equation (3.2), i = k+1
515+
# From M. Kelly, SIAM Review (2017), equation (3.2), i = k+1
519516
# x[k+1] - x[k] = 0.5 hk (f(x[k+1], u[k+1] + f(x[k], u[k]))
520517
fkp1 = self.system._rhs(t, states[:, i], inputs[:, i])
521518
self.colloc_vals[:, i] = states[:, i] - states[:, i-1] - \
@@ -529,18 +526,20 @@ def _collocation_constraint(self, coeffs):
529526
return self.colloc_vals.reshape(-1)
530527

531528
#
532-
# Initial guess
529+
# Initial guess processing
533530
#
534531
# We store an initial guess in case it is not specified later. Note
535532
# that create_mpc_iosystem() will reset the initial guess based on
536533
# the current state of the MPC controller.
537534
#
538-
# Note: the initial guess is passed as the inputs at the given time
535+
# The functions below are used to process the initial guess, which can
536+
# either consist of an input only (for shooting methods) or an input
537+
# and/or state trajectory (for collocaiton methods).
538+
#
539+
# Note: The initial input guess is passed as the inputs at the given time
539540
# vector. If a basis is specified, this is converted to coefficient
540541
# values (which are generally of smaller dimension).
541542
#
542-
# TODO: figure out how to modify this for collocation
543-
#
544543
def _process_initial_guess(self, initial_guess):
545544
# Sort out the input guess and the state guess
546545
if self.collocation and initial_guess is not None and \
@@ -583,6 +582,7 @@ def _process_initial_guess(self, initial_guess):
583582
# Reshape for use by scipy.optimize.minimize()
584583
return input_guess.reshape(-1)
585584

585+
# Utility function to broadcast an initial guess to the right shape
586586
def _broadcast_initial_guess(self, initial_guess, shape):
587587
# Convert to a 1D array (or higher)
588588
initial_guess = np.atleast_1d(initial_guess)
@@ -604,11 +604,11 @@ def _broadcast_initial_guess(self, initial_guess, shape):
604604
#
605605
# Utility function to convert input vector to coefficient vector
606606
#
607-
# Initially guesses from the user are passed as input vectors as a
607+
# Initial guesses from the user are passed as input vectors as a
608608
# function of time, but internally we store the guess in terms of the
609609
# basis coefficients. We do this by solving a least squares problem to
610-
# find coefficients that match the input functions at the time points (as
611-
# much as possible, if the problem is under-determined).
610+
# find coefficients that match the input functions at the time points
611+
# (as much as possible, if the problem is under-determined).
612612
#
613613
def _inputs_to_coeffs(self, inputs):
614614
# If there is no basis function, just return inputs as coeffs
@@ -638,6 +638,7 @@ def _inputs_to_coeffs(self, inputs):
638638
# Utility function to convert coefficient vector to input vector
639639
def _coeffs_to_inputs(self, coeffs):
640640
# TODO: vectorize
641+
# TODO: use BasisFamily eval() method (if more efficient)?
641642
inputs = np.zeros((self.system.ninputs, self.timepts.size))
642643
offset = 0
643644
for i in range(self.system.ninputs):
@@ -689,7 +690,7 @@ def _print_statistics(self, reset=True):
689690
# These internal functions return the states and inputs at the
690691
# collocation points given the ceofficient (optimizer state) vector.
691692
# They keep track of whether a shooting method is being used or not and
692-
# simulate the dynamis of needed.
693+
# simulate the dynamics if needed.
693694
#
694695

695696
# Compute the states and inputs from the coefficients
@@ -738,6 +739,7 @@ def _simulate_states(self, x0, inputs):
738739
if self.log:
739740
logging.debug(
740741
"input_output_response returned states\n" + str(states))
742+
741743
return states
742744

743745
#
@@ -930,16 +932,6 @@ def __init__(
930932
ocp._print_statistics()
931933
print("* Final cost:", self.cost)
932934

933-
if return_states and inputs.shape[1] == ocp.timepts.shape[0]:
934-
# Simulate the system if we need the state back
935-
# TODO: this is probably not needed due to compute_states_inputs()
936-
_, _, states = ct.input_output_response(
937-
ocp.system, ocp.timepts, inputs, ocp.x, return_x=True,
938-
solve_ivp_kwargs=ocp.solve_ivp_kwargs, t_eval=ocp.timepts)
939-
ocp.system_simulations += 1
940-
else:
941-
states = None
942-
943935
# Process data as a time response (with "outputs" = inputs)
944936
response = TimeResponseData(
945937
ocp.timepts, inputs, states, issiso=ocp.system.issiso(),
@@ -1065,10 +1057,22 @@ def solve_ocp(
10651057
'optimal', 'return_x', kwargs, return_states, pop=True)
10661058

10671059
# Process (legacy) method keyword
1068-
if kwargs.get('method') and method not in optimal_methods:
1069-
if kwargs.get('minimize_method'):
1070-
raise ValueError("'minimize_method' specified more than once")
1071-
kwargs['minimize_method'] = kwargs.pop('method')
1060+
if kwargs.get('method'):
1061+
method = kwargs.pop('method')
1062+
if method not in optimal_methods:
1063+
if kwargs.get('minimize_method'):
1064+
raise ValueError("'minimize_method' specified more than once")
1065+
warnings.warn(
1066+
"'method' parameter is deprecated; assuming minimize_method",
1067+
DeprecationWarning)
1068+
kwargs['minimize_method'] = method
1069+
else:
1070+
if kwargs.get('trajectory_method'):
1071+
raise ValueError("'trajectory_method' specified more than once")
1072+
warnings.warn(
1073+
"'method' parameter is deprecated; assuming trajectory_method",
1074+
DeprecationWarning)
1075+
kwargs['trajectory_method'] = method
10721076

10731077
# Set up the optimal control problem
10741078
ocp = OptimalControlProblem(

0 commit comments

Comments
 (0)