Skip to content

Add collocation method for optimal control problems #799

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
murrayrm merged 6 commits into from
Dec 2, 2022
Merged

Add collocation method for optimal control problems #799

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
d9ec3ec
add trajectory_method='shooting'
murrayrm Nov 24, 2022
ce5e67a
add trajectory_method='collocation'
murrayrm Nov 24, 2022
8256fa0
set default trajectory_methood for ctime systems to collocation
murrayrm Nov 25, 2022
d121102
updated examples + code cleanup
murrayrm Nov 25, 2022
e0f71f4
update documentation + allow constraints in scipy.optimize form
murrayrm Nov 26, 2022
dd5e42d
add benchmarks for shooting vs collocation
murrayrm Nov 27, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
updated examples + code cleanup
  • Loading branch information
@murrayrm
murrayrm committed Nov 26, 2022
commit d121102cef6a9a8f44a85151ff852fa4f08fe83d
148 changes: 76 additions & 72 deletions control/optimal.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
# Define module default parameter values
_optimal_trajectory_methods = {'shooting', 'collocation'}
_optimal_defaults = {
'optimal.trajectory_method': 'shooting',
'optimal.minimize_method': None,
'optimal.minimize_options': {},
'optimal.minimize_kwargs': {},
Expand Down Expand Up @@ -147,9 +146,8 @@ def __init__(
self.terminal_constraints = terminal_constraints
self.basis = basis

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

# Process trajectory constraints
if isinstance(trajectory_constraints, tuple):
self.trajectory_constraints = [trajectory_constraints]
elif not isinstance(trajectory_constraints, list):
raise TypeError("trajectory constraints must be a list")
else:
self.trajectory_constraints = trajectory_constraints
def _process_constraints(constraint_list, name):
if isinstance(constraint_list, tuple):
constraint_list = [constraint_list]
elif not isinstance(constraint_list, list):
raise TypeError(f"{name} constraints must be a list")

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

# Process terminal constraints
if isinstance(terminal_constraints, tuple):
self.terminal_constraints = [terminal_constraints]
elif not isinstance(terminal_constraints, list):
raise TypeError("terminal constraints must be a list")
else:
self.terminal_constraints = terminal_constraints
return constraint_list

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

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

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

# Create the constraints (inequality and equality)
# TODO: for collocation method, keep track of linear vs nonlinear
self.constraints = []

if len(self.constraint_lb) != 0:
self.constraints.append(sp.optimize.NonlinearConstraint(
self._constraint_function, self.constraint_lb,
self.constraint_ub))

if len(self.eqconst_value) != 0:
self.constraints.append(sp.optimize.NonlinearConstraint(
self._eqconst_function, self.eqconst_value,
self.eqconst_value))

if self.collocation:
# Add the collocation constraints
colloc_zeros = np.zeros(sys.nstates * self.timepts.size)
Expand All @@ -275,7 +273,7 @@ def __init__(
# Process the initial guess
self.initial_guess = self._process_initial_guess(initial_guess)

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

Expand All @@ -286,10 +284,14 @@ def __init__(
#
# Cost function
#
# Given the input U = [u[0], ... u[N]], we need to compute the cost of
# the trajectory generated by that input. This means we have to
# simulate the system to get the state trajectory X = [x[0], ...,
# x[N]] and then compute the cost at each point:
# For collocation methods we are given the states and inputs at each
# time point and we use a trapezoidal approximation to compute the
# integral cost, then add on the terminal cost.
#
# For shooting methods, given the input U = [u[0], ... u[N]] we need to
# compute the cost of the trajectory generated by that input. This
# means we have to simulate the system to get the state trajectory X =
# [x[0], ..., x[N]] and then compute the cost at each point:
#
# cost = sum_k integral_cost(x[k], u[k]) + terminal_cost(x[N], u[N])
#
Expand Down Expand Up @@ -362,11 +364,12 @@ def _cost_function(self, coeffs):
# is called at each point along the trajectory and compared against the
# upper and lower bounds.
#
# * If the upper and lower bound for the constraint are identical, then we
# separate out the evaluation into two different constraints, which
# allows the SciPy optimizers to be more efficient (and stops them from
# generating a warning about mixed constraints). This is handled
# through the use of the `_eqconst_function` and `eqconst_value` members.
# * If the upper and lower bound for the constraint are identical, then
# we separate out the evaluation into two different constraints, which
# allows the SciPy optimizers to be more efficient (and stops them
# from generating a warning about mixed constraints). This is handled
# through the use of the `_eqconst_function` and `eqconst_value`
# members.
#
# In both cases, the constraint is specified at a single point, but we
# extend this to apply to each point in the trajectory. This means
Expand All @@ -378,16 +381,12 @@ def _cost_function(self, coeffs):
# For collocation methods, we can directly evaluate the constraints at
# the collocation points.
#
# For shooting methods, we do this by creating a function that
# simulates the system dynamics and returns a vector of values
# corresponding to the value of the function at each time. The
# class initialization methods takes care of replicating the upper
# and lower bounds for each point in time so that the SciPy
# optimization algorithm can do the proper evaluation.
#
# In addition, since SciPy's optimization function does not allow us to
# pass arguments to the constraint function, we have to store the initial
# state prior to optimization and retrieve it here.
# For shooting methods, we do this by creating a function that simulates
# the system dynamics and returns a vector of values corresponding to
# the value of the function at each time. The class initialization
# methods takes care of replicating the upper and lower bounds for each
# point in time so that the SciPy optimization algorithm can do the
# proper evaluation.
#
def _constraint_function(self, coeffs):
if self.log:
Expand Down Expand Up @@ -437,8 +436,7 @@ def _constraint_function(self, coeffs):
"_constraint_function elapsed time: %g",
stop_time - start_time)

# Debugging information
if self.log:
# Debugging information
logging.debug(
"constraint values\n" + str(value) + "\n" +
"lb, ub =\n" + str(self.constraint_lb) + "\n" +
Expand Down Expand Up @@ -492,8 +490,7 @@ def _eqconst_function(self, coeffs):
logging.info(
"_eqconst_function elapsed time: %g", stop_time - start_time)

# Debugging information
if self.log:
# Debugging information
logging.debug(
"eqconst values\n" + str(value) + "\n" +
"desired =\n" + str(self.eqconst_value))
Expand All @@ -515,7 +512,7 @@ def _collocation_constraint(self, coeffs):
self.timepts[0], states[:, 0], inputs[:, 0])
continue

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

#
# Initial guess
# Initial guess processing
#
# We store an initial guess in case it is not specified later. Note
# that create_mpc_iosystem() will reset the initial guess based on
# the current state of the MPC controller.
#
# Note: the initial guess is passed as the inputs at the given time
# The functions below are used to process the initial guess, which can
# either consist of an input only (for shooting methods) or an input
# and/or state trajectory (for collocaiton methods).
#
# Note: The initial input guess is passed as the inputs at the given time
# vector. If a basis is specified, this is converted to coefficient
# values (which are generally of smaller dimension).
#
# TODO: figure out how to modify this for collocation
#
def _process_initial_guess(self, initial_guess):
# Sort out the input guess and the state guess
if self.collocation and initial_guess is not None and \
Expand Down Expand Up @@ -583,6 +582,7 @@ def _process_initial_guess(self, initial_guess):
# Reshape for use by scipy.optimize.minimize()
return input_guess.reshape(-1)

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

# Compute the states and inputs from the coefficients
Expand Down Expand Up @@ -738,6 +739,7 @@ def _simulate_states(self, x0, inputs):
if self.log:
logging.debug(
"input_output_response returned states\n" + str(states))

return states

#
Expand Down Expand Up @@ -930,16 +932,6 @@ def __init__(
ocp._print_statistics()
print("* Final cost:", self.cost)

if return_states and inputs.shape[1] == ocp.timepts.shape[0]:
# Simulate the system if we need the state back
# TODO: this is probably not needed due to compute_states_inputs()
_, _, states = ct.input_output_response(
ocp.system, ocp.timepts, inputs, ocp.x, return_x=True,
solve_ivp_kwargs=ocp.solve_ivp_kwargs, t_eval=ocp.timepts)
ocp.system_simulations += 1
else:
states = None

# Process data as a time response (with "outputs" = inputs)
response = TimeResponseData(
ocp.timepts, inputs, states, issiso=ocp.system.issiso(),
Expand Down Expand Up @@ -1065,10 +1057,22 @@ def solve_ocp(
'optimal', 'return_x', kwargs, return_states, pop=True)

# Process (legacy) method keyword
if kwargs.get('method') and method not in optimal_methods:
if kwargs.get('minimize_method'):
raise ValueError("'minimize_method' specified more than once")
kwargs['minimize_method'] = kwargs.pop('method')
if kwargs.get('method'):
method = kwargs.pop('method')
if method not in optimal_methods:
if kwargs.get('minimize_method'):
raise ValueError("'minimize_method' specified more than once")
warnings.warn(
"'method' parameter is deprecated; assuming minimize_method",
DeprecationWarning)
kwargs['minimize_method'] = method
else:
if kwargs.get('trajectory_method'):
raise ValueError("'trajectory_method' specified more than once")
warnings.warn(
"'method' parameter is deprecated; assuming trajectory_method",
DeprecationWarning)
kwargs['trajectory_method'] = method

# Set up the optimal control problem
ocp = OptimalControlProblem(
Expand Down
Loading