Skip to content

Commit 614f2a7

Browse files
murrayrmmurrayrm
committed
updated examples + code cleanup
1 parent 329ae47 commit 614f2a7

File tree

5 files changed

+190
-146
lines changed

5 files changed

+190
-146
lines changed

‎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}")
@@ -177,30 +175,23 @@ def __init__(
177175
raise TypeError("unrecognized keyword(s): ", str(kwargs))
178176

179177
# Process trajectory constraints
180-
if isinstance(trajectory_constraints, tuple):
181-
self.trajectory_constraints = [trajectory_constraints]
182-
elif not isinstance(trajectory_constraints, list):
183-
raise TypeError("trajectory constraints must be a list")
184-
else:
185-
self.trajectory_constraints = trajectory_constraints
178+
def _process_constraints(constraint_list, name):
179+
if isinstance(constraint_list, tuple):
180+
constraint_list = [constraint_list]
181+
elif not isinstance(constraint_list, list):
182+
raise TypeError(f"{name} constraints must be a list")
186183

187-
# Make sure that we recognize all of the constraint types
188-
for ctype, fun, lb, ub in self.trajectory_constraints:
189-
if not ctype in [opt.LinearConstraint, opt.NonlinearConstraint]:
190-
raise TypeError(f"unknown constraint type {ctype}")
184+
# Make sure that we recognize all of the constraint types
185+
for ctype, fun, lb, ub in constraint_list:
186+
if not ctype in [opt.LinearConstraint, opt.NonlinearConstraint]:
187+
raise TypeError(f"unknown {name} constraint type {ctype}")
191188

192-
# Process terminal constraints
193-
if isinstance(terminal_constraints, tuple):
194-
self.terminal_constraints = [terminal_constraints]
195-
elif not isinstance(terminal_constraints, list):
196-
raise TypeError("terminal constraints must be a list")
197-
else:
198-
self.terminal_constraints = terminal_constraints
189+
return constraint_list
199190

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

205196
#
206197
# Compute and store constraints
@@ -215,10 +206,13 @@ def __init__(
215206
# is consistent with the `_constraint_function` that is used at
216207
# evaluation time.
217208
#
209+
# TODO: when using the collocation method, linear constraints on the
210+
# states and inputs can potentially maintain their linear structure
211+
# rather than being converted to nonlinear constraints.
212+
#
218213
constraint_lb, constraint_ub, eqconst_value = [], [], []
219214

220215
# Go through each time point and stack the bounds
221-
# TODO: for collocation method, keep track of linear vs nonlinear
222216
for t in self.timepts:
223217
for type, fun, lb, ub in self.trajectory_constraints:
224218
if np.all(lb == ub):
@@ -245,15 +239,19 @@ def __init__(
245239
self.eqconst_value = np.hstack(eqconst_value) if eqconst_value else []
246240

247241
# Create the constraints (inequality and equality)
242+
# TODO: for collocation method, keep track of linear vs nonlinear
248243
self.constraints = []
244+
249245
if len(self.constraint_lb) != 0:
250246
self.constraints.append(sp.optimize.NonlinearConstraint(
251247
self._constraint_function, self.constraint_lb,
252248
self.constraint_ub))
249+
253250
if len(self.eqconst_value) != 0:
254251
self.constraints.append(sp.optimize.NonlinearConstraint(
255252
self._eqconst_function, self.eqconst_value,
256253
self.eqconst_value))
254+
257255
if self.collocation:
258256
# Add the collocation constraints
259257
colloc_zeros = np.zeros(sys.nstates * self.timepts.size)
@@ -267,7 +265,7 @@ def __init__(
267265
# Process the initial guess
268266
self.initial_guess = self._process_initial_guess(initial_guess)
269267

270-
# Store states, input, used later to minimize re-computation
268+
# Store states, input (used later to minimize re-computation)
271269
self.last_x = np.full(self.system.nstates, np.nan)
272270
self.last_coeffs = np.full(self.initial_guess.shape, np.nan)
273271

@@ -278,10 +276,14 @@ def __init__(
278276
#
279277
# Cost function
280278
#
281-
# Given the input U = [u[0], ... u[N]], we need to compute the cost of
282-
# the trajectory generated by that input. This means we have to
283-
# simulate the system to get the state trajectory X = [x[0], ...,
284-
# x[N]] and then compute the cost at each point:
279+
# For collocation methods we are given the states and inputs at each
280+
# time point and we use a trapezoidal approximation to compute the
281+
# integral cost, then add on the terminal cost.
282+
#
283+
# For shooting methods, given the input U = [u[0], ... u[N]] we need to
284+
# compute the cost of the trajectory generated by that input. This
285+
# means we have to simulate the system to get the state trajectory X =
286+
# [x[0], ..., x[N]] and then compute the cost at each point:
285287
#
286288
# cost = sum_k integral_cost(x[k], u[k]) + terminal_cost(x[N], u[N])
287289
#
@@ -354,11 +356,12 @@ def _cost_function(self, coeffs):
354356
# is called at each point along the trajectory and compared against the
355357
# upper and lower bounds.
356358
#
357-
# * If the upper and lower bound for the constraint are identical, then we
358-
# separate out the evaluation into two different constraints, which
359-
# allows the SciPy optimizers to be more efficient (and stops them from
360-
# generating a warning about mixed constraints). This is handled
361-
# through the use of the `_eqconst_function` and `eqconst_value` members.
359+
# * If the upper and lower bound for the constraint are identical, then
360+
# we separate out the evaluation into two different constraints, which
361+
# allows the SciPy optimizers to be more efficient (and stops them
362+
# from generating a warning about mixed constraints). This is handled
363+
# through the use of the `_eqconst_function` and `eqconst_value`
364+
# members.
362365
#
363366
# In both cases, the constraint is specified at a single point, but we
364367
# extend this to apply to each point in the trajectory. This means
@@ -370,16 +373,12 @@ def _cost_function(self, coeffs):
370373
# For collocation methods, we can directly evaluate the constraints at
371374
# the collocation points.
372375
#
373-
# For shooting methods, we do this by creating a function that
374-
# simulates the system dynamics and returns a vector of values
375-
# corresponding to the value of the function at each time. The
376-
# class initialization methods takes care of replicating the upper
377-
# and lower bounds for each point in time so that the SciPy
378-
# optimization algorithm can do the proper evaluation.
379-
#
380-
# In addition, since SciPy's optimization function does not allow us to
381-
# pass arguments to the constraint function, we have to store the initial
382-
# state prior to optimization and retrieve it here.
376+
# For shooting methods, we do this by creating a function that simulates
377+
# the system dynamics and returns a vector of values corresponding to
378+
# the value of the function at each time. The class initialization
379+
# methods takes care of replicating the upper and lower bounds for each
380+
# point in time so that the SciPy optimization algorithm can do the
381+
# proper evaluation.
383382
#
384383
def _constraint_function(self, coeffs):
385384
if self.log:
@@ -429,8 +428,7 @@ def _constraint_function(self, coeffs):
429428
"_constraint_function elapsed time: %g",
430429
stop_time - start_time)
431430

432-
# Debugging information
433-
if self.log:
431+
# Debugging information
434432
logging.debug(
435433
"constraint values\n" + str(value) + "\n" +
436434
"lb, ub =\n" + str(self.constraint_lb) + "\n" +
@@ -484,8 +482,7 @@ def _eqconst_function(self, coeffs):
484482
logging.info(
485483
"_eqconst_function elapsed time: %g", stop_time - start_time)
486484

487-
# Debugging information
488-
if self.log:
485+
# Debugging information
489486
logging.debug(
490487
"eqconst values\n" + str(value) + "\n" +
491488
"desired =\n" + str(self.eqconst_value))
@@ -507,7 +504,7 @@ def _collocation_constraint(self, coeffs):
507504
self.timepts[0], states[:, 0], inputs[:, 0])
508505
continue
509506

510-
# M. Kelly, SIAM Review (2017), equation (3.2), i = k+1
507+
# From M. Kelly, SIAM Review (2017), equation (3.2), i = k+1
511508
# x[k+1] - x[k] = 0.5 hk (f(x[k+1], u[k+1] + f(x[k], u[k]))
512509
fkp1 = self.system._rhs(t, states[:, i], inputs[:, i])
513510
self.colloc_vals[:, i] = states[:, i] - states[:, i-1] - \
@@ -521,18 +518,20 @@ def _collocation_constraint(self, coeffs):
521518
return self.colloc_vals.reshape(-1)
522519

523520
#
524-
# Initial guess
521+
# Initial guess processing
525522
#
526523
# We store an initial guess in case it is not specified later. Note
527524
# that create_mpc_iosystem() will reset the initial guess based on
528525
# the current state of the MPC controller.
529526
#
530-
# Note: the initial guess is passed as the inputs at the given time
527+
# The functions below are used to process the initial guess, which can
528+
# either consist of an input only (for shooting methods) or an input
529+
# and/or state trajectory (for collocaiton methods).
530+
#
531+
# Note: The initial input guess is passed as the inputs at the given time
531532
# vector. If a basis is specified, this is converted to coefficient
532533
# values (which are generally of smaller dimension).
533534
#
534-
# TODO: figure out how to modify this for collocation
535-
#
536535
def _process_initial_guess(self, initial_guess):
537536
# Sort out the input guess and the state guess
538537
if self.collocation and initial_guess is not None and \
@@ -575,6 +574,7 @@ def _process_initial_guess(self, initial_guess):
575574
# Reshape for use by scipy.optimize.minimize()
576575
return input_guess.reshape(-1)
577576

577+
# Utility function to broadcast an initial guess to the right shape
578578
def _broadcast_initial_guess(self, initial_guess, shape):
579579
# Convert to a 1D array (or higher)
580580
initial_guess = np.atleast_1d(initial_guess)
@@ -596,11 +596,11 @@ def _broadcast_initial_guess(self, initial_guess, shape):
596596
#
597597
# Utility function to convert input vector to coefficient vector
598598
#
599-
# Initially guesses from the user are passed as input vectors as a
599+
# Initial guesses from the user are passed as input vectors as a
600600
# function of time, but internally we store the guess in terms of the
601601
# basis coefficients. We do this by solving a least squares problem to
602-
# find coefficients that match the input functions at the time points (as
603-
# much as possible, if the problem is under-determined).
602+
# find coefficients that match the input functions at the time points
603+
# (as much as possible, if the problem is under-determined).
604604
#
605605
def _inputs_to_coeffs(self, inputs):
606606
# If there is no basis function, just return inputs as coeffs
@@ -630,6 +630,7 @@ def _inputs_to_coeffs(self, inputs):
630630
# Utility function to convert coefficient vector to input vector
631631
def _coeffs_to_inputs(self, coeffs):
632632
# TODO: vectorize
633+
# TODO: use BasisFamily eval() method (if more efficient)?
633634
inputs = np.zeros((self.system.ninputs, self.timepts.size))
634635
offset = 0
635636
for i in range(self.system.ninputs):
@@ -681,7 +682,7 @@ def _print_statistics(self, reset=True):
681682
# These internal functions return the states and inputs at the
682683
# collocation points given the ceofficient (optimizer state) vector.
683684
# They keep track of whether a shooting method is being used or not and
684-
# simulate the dynamis of needed.
685+
# simulate the dynamics if needed.
685686
#
686687

687688
# Compute the states and inputs from the coefficients
@@ -730,6 +731,7 @@ def _simulate_states(self, x0, inputs):
730731
if self.log:
731732
logging.debug(
732733
"input_output_response returned states\n" + str(states))
734+
733735
return states
734736

735737
#
@@ -922,16 +924,6 @@ def __init__(
922924
ocp._print_statistics()
923925
print("* Final cost:", self.cost)
924926

925-
if return_states and inputs.shape[1] == ocp.timepts.shape[0]:
926-
# Simulate the system if we need the state back
927-
# TODO: this is probably not needed due to compute_states_inputs()
928-
_, _, states = ct.input_output_response(
929-
ocp.system, ocp.timepts, inputs, ocp.x, return_x=True,
930-
solve_ivp_kwargs=ocp.solve_ivp_kwargs, t_eval=ocp.timepts)
931-
ocp.system_simulations += 1
932-
else:
933-
states = None
934-
935927
# Process data as a time response (with "outputs" = inputs)
936928
response = TimeResponseData(
937929
ocp.timepts, inputs, states, issiso=ocp.system.issiso(),
@@ -1053,10 +1045,22 @@ def solve_ocp(
10531045
'optimal', 'return_x', kwargs, return_states, pop=True)
10541046

10551047
# Process (legacy) method keyword
1056-
if kwargs.get('method') and method not in optimal_methods:
1057-
if kwargs.get('minimize_method'):
1058-
raise ValueError("'minimize_method' specified more than once")
1059-
kwargs['minimize_method'] = kwargs.pop('method')
1048+
if kwargs.get('method'):
1049+
method = kwargs.pop('method')
1050+
if method not in optimal_methods:
1051+
if kwargs.get('minimize_method'):
1052+
raise ValueError("'minimize_method' specified more than once")
1053+
warnings.warn(
1054+
"'method' parameter is deprecated; assuming minimize_method",
1055+
DeprecationWarning)
1056+
kwargs['minimize_method'] = method
1057+
else:
1058+
if kwargs.get('trajectory_method'):
1059+
raise ValueError("'trajectory_method' specified more than once")
1060+
warnings.warn(
1061+
"'method' parameter is deprecated; assuming trajectory_method",
1062+
DeprecationWarning)
1063+
kwargs['trajectory_method'] = method
10601064

10611065
# Set up the optimal control problem
10621066
ocp = OptimalControlProblem(

0 commit comments

Comments
 (0)