'optimal.minimize_method': None,

'optimal.minimize_options': {},

'optimal.minimize_kwargs': {},

'optimal.solve_ivp_method': None,

'optimal.solve_ivp_options': {},

"""Description of a finite horizon, optimal control problem.

def __init__(

self, sys, timepts, integral_cost, trajectory_constraints=None,

terminal_cost=None, terminal_constraints=None, initial_guess=None,

trajectory_method=None, basis=None, log=False, _kwargs_check=True,

**kwargs):

"""Set up an optimal control problem."""

# Save the basic information for use later

self.system = sys

self.timepts = timepts

self.integral_cost = integral_cost

self.terminal_cost = terminal_cost

self.terminal_constraints = terminal_constraints

self.basis = basis

# Keep track of what type of trajectory method we are using

if trajectory_method is None:

trajectory_method = 'collocation' if sys.isctime() else 'shooting'

elif trajectory_method not in _optimal_trajectory_methods:

raise NotImplementedError(f"Unknown method {trajectory_method}")

self.shooting = trajectory_method in {'shooting'}

self.collocation = trajectory_method in {'collocation'}

# Process keyword arguments

self.solve_ivp_kwargs = {}

self.solve_ivp_kwargs['method'] = kwargs.pop(

'solve_ivp_method', config.defaults['optimal.solve_ivp_method'])

self.solve_ivp_kwargs.update(kwargs.pop(

'solve_ivp_kwargs', config.defaults['optimal.solve_ivp_options']))

self.minimize_kwargs = {}

self.minimize_kwargs['method'] = kwargs.pop(

'minimize_method', config.defaults['optimal.minimize_method'])

self.minimize_kwargs['options'] = kwargs.pop(

'minimize_options', config.defaults['optimal.minimize_options'])

self.minimize_kwargs.update(kwargs.pop(

'minimize_kwargs', config.defaults['optimal.minimize_kwargs']))

# Check to make sure arguments for discrete-time systems are OK

if sys.isdtime(strict=True):

if self.solve_ivp_kwargs['method'] is not None or \

len(self.solve_ivp_kwargs) > 1:

raise TypeError(

"solve_ivp method, kwargs not allowed for"

" discrete-time systems")

# Make sure there were no extraneous keywords

if _kwargs_check and kwargs:

raise TypeError("unrecognized keyword(s): ", str(kwargs))

self.trajectory_constraints = _process_constraints(

trajectory_constraints, "trajectory")

self.terminal_constraints = _process_constraints(

terminal_constraints, "terminal")

#

# Compute and store constraints

#

# While the constraints are evaluated during the execution of the

# SciPy optimization method itself, we go ahead and pre-compute the

# `scipy.optimize.NonlinearConstraint` function that will be passed to

# the optimizer on initialization, since it doesn't change. This is

# mainly a matter of computing the lower and upper bound vectors,

# which we need to "stack" to account for the evaluation at each

# trajectory time point plus any terminal constraints (in a way that

# 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

for t in self.timepts:

for type, fun, lb, ub in self.trajectory_constraints:

if np.all(lb == ub):

# Equality constraint

eqconst_value.append(lb)

else:

# Inequality constraint

constraint_lb.append(lb)

constraint_ub.append(ub)

# Add on the terminal constraints

for type, fun, lb, ub in self.terminal_constraints:

if np.all(lb == ub):

# Equality constraint

eqconst_value.append(lb)

else:

# Inequality constraint

constraint_lb.append(lb)

constraint_ub.append(ub)

# Turn constraint vectors into 1D arrays

self.constraint_lb = np.hstack(constraint_lb) if constraint_lb else []

self.constraint_ub = np.hstack(constraint_ub) if constraint_ub else []

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)

self.colloc_vals = np.zeros((sys.nstates, self.timepts.size))

self.constraints.append(sp.optimize.NonlinearConstraint(

self._collocation_constraint, colloc_zeros, colloc_zeros))

# Initialize run-time statistics

self._reset_statistics(log)

# Process the initial guess

self.initial_guess = self._process_initial_guess(initial_guess)

# 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)

# Log information

if log:

logging.info("New optimal control problem initialized")

#

# Cost function

#

# 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[t_0], ... u[t_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[t_0], ..., x[t_N]] and then compute the cost at each point:

#

# cost = sum_k integral_cost(x[t_k], u[t_k])

# + terminal_cost(x[t_N], u[t_N])

#

# The actual calculation is a bit more complex: for continuous-time

# systems, we use a trapezoidal approximation for the integral cost.

#

# The initial state used for generating the simulation is stored in the

# class parameter `x` prior to calling the optimization algorithm.

#

def _cost_function(self, coeffs):

if self.log:

start_time = time.process_time()

logging.info("_cost_function called at: %g", start_time)

# Compute the states and inputs

states, inputs = self._compute_states_inputs(coeffs)

# Trajectory cost

if ct.isctime(self.system):

# Evaluate the costs

costs = [self.integral_cost(states[:, i], inputs[:, i]) for

i in range(self.timepts.size)]

# Compute the time intervals

dt = np.diff(self.timepts)

# Integrate the cost

costs = np.array(costs)

# Approximate the integral using trapezoidal rule

cost = np.sum(0.5 * (costs[:-1] + costs[1:]) * dt)

else:

# Sum the integral cost over the time (second) indices

# cost += self.integral_cost(states[:,i], inputs[:,i])

cost = sum(map(

self.integral_cost, states[:, :-1].transpose(),

inputs[:, :-1].transpose()))

# Terminal cost

if self.terminal_cost is not None:

cost += self.terminal_cost(states[:, -1], inputs[:, -1])

# Update statistics

self.cost_evaluations += 1

if self.log:

stop_time = time.process_time()

self.cost_process_time += stop_time - start_time

logging.info(

"_cost_function returning %g; elapsed time: %g",

cost, stop_time - start_time)

# Return the total cost for this input sequence

return cost

#

# Constraints

#

# We are given the constraints along the trajectory and the terminal

# constraints, which each take inputs [x, u] and evaluate the

# constraint. How we handle these depends on the type of constraint:

#

# * For linear constraints (LinearConstraint), a combined (hstack'd)

# vector of the state and input is multiplied by the polytope A matrix

# for comparison against the upper and lower bounds.

#

# * For nonlinear constraints (NonlinearConstraint), a user-specific

# constraint function having the form

#

# constraint_fun(x, u)

#

# 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.

#

# 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

# that for N time points with m trajectory constraints and p terminal

# constraints we need to compute N*m + p constraints, each of which

# holds at a specific point in time, and implements the original

# constraint.

#

# 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.

#

def _constraint_function(self, coeffs):

if self.log:

start_time = time.process_time()

logging.info("_constraint_function called at: %g", start_time)

# Compute the states and inputs

states, inputs = self._compute_states_inputs(coeffs)

#

# Evaluate the constraint function along the trajectory

#

value = []

for i, t in enumerate(self.timepts):

for ctype, fun, lb, ub in self.trajectory_constraints:

if np.all(lb == ub):

# Skip equality constraints

continue

elif ctype == opt.LinearConstraint:

# `fun` is the A matrix associated with the polytope...

value.append(fun @ np.hstack([states[:, i], inputs[:, i]]))

elif ctype == opt.NonlinearConstraint:

value.append(fun(states[:, i], inputs[:, i]))

else: # pragma: no cover

# Checked above => we should never get here

raise TypeError(f"unknown constraint type {ctype}")

# Evaluate the terminal constraint functions

for ctype, fun, lb, ub in self.terminal_constraints:

if np.all(lb == ub):

# Skip equality constraints

continue

elif ctype == opt.LinearConstraint:

value.append(fun @ np.hstack([states[:, -1], inputs[:, -1]]))

elif ctype == opt.NonlinearConstraint:

value.append(fun(states[:, -1], inputs[:, -1]))

else: # pragma: no cover

# Checked above => we should never get here

raise TypeError(f"unknown constraint type {ctype}")

# Update statistics

self.constraint_evaluations += 1

if self.log:

stop_time = time.process_time()

self.constraint_process_time += stop_time - start_time

logging.info(

"_constraint_function elapsed time: %g",

stop_time - start_time)

# Debugging information

logging.debug(

"constraint values\n" + str(value) + "\n" +

"lb, ub =\n" + str(self.constraint_lb) + "\n" +

str(self.constraint_ub))

# Return the value of the constraint function

return np.hstack(value)

def _eqconst_function(self, coeffs):

if self.log:

start_time = time.process_time()

logging.info("_eqconst_function called at: %g", start_time)

# Compute the states and inputs

states, inputs = self._compute_states_inputs(coeffs)

# Evaluate the constraint function along the trajectory

value = []

for i, t in enumerate(self.timepts):

for ctype, fun, lb, ub in self.trajectory_constraints:

if np.any(lb != ub):

# Skip inequality constraints

continue

elif ctype == opt.LinearConstraint:

# `fun` is the A matrix associated with the polytope...

value.append(fun @ np.hstack([states[:, i], inputs[:, i]]))

elif ctype == opt.NonlinearConstraint:

value.append(fun(states[:, i], inputs[:, i]))

else: # pragma: no cover

# Checked above => we should never get here

raise TypeError(f"unknown constraint type {ctype}")

# Evaluate the terminal constraint functions

for ctype, fun, lb, ub in self.terminal_constraints:

if np.any(lb != ub):

# Skip inequality constraints

continue

elif ctype == opt.LinearConstraint:

value.append(fun @ np.hstack([states[:, -1], inputs[:, -1]]))

elif ctype == opt.NonlinearConstraint:

value.append(fun(states[:, -1], inputs[:, -1]))

else: # pragma: no cover

# Checked above => we should never get here

raise TypeError("unknown constraint type {ctype}")

# Update statistics

self.eqconst_evaluations += 1

if self.log:

stop_time = time.process_time()

self.eqconst_process_time += stop_time - start_time

logging.info(

"_eqconst_function elapsed time: %g", stop_time - start_time)

# Debugging information

logging.debug(

"eqconst values\n" + str(value) + "\n" +

"desired =\n" + str(self.eqconst_value))

# Return the value of the constraint function

return np.hstack(value)

def _collocation_constraint(self, coeffs):

# Compute the states and inputs

states, inputs = self._compute_states_inputs(coeffs)

if self.system.isctime():

# Compute the collocation constraints

for i, t in enumerate(self.timepts):

if i == 0:

# Initial condition constraint (self.x = initial point)

self.colloc_vals[:, 0] = states[:, 0] - self.x

fk = self.system._rhs(

self.timepts[0], states[:, 0], inputs[:, 0])

continue

# 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] - \

0.5 * (self.timepts[i] - self.timepts[i-1]) * (fkp1 + fk)

fk = fkp1

else:

raise NotImplementedError(

"collocation not yet implemented for discrete-time systems")

# Return the value of the constraint function

return self.colloc_vals.reshape(-1)

#

# 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.

#

# 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 collocation 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).

#

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 \

isinstance(initial_guess, tuple):

state_guess, input_guess = initial_guess

else:

state_guess, input_guess = None, initial_guess

# Process the input guess

if input_guess is not None:

input_guess = self._broadcast_initial_guess(

input_guess, (self.system.ninputs, self.timepts.size))

# If we were given a basis, project onto the basis elements

if self.basis is not None:

input_guess = self._inputs_to_coeffs(input_guess)

else:

input_guess = np.zeros(

self.system.ninputs *

(self.timepts.size if self.basis is None else self.basis.N))

# Process the state guess

if self.collocation:

if state_guess is None:

# Run a simulation to get the initial guess

if self.basis:

inputs = self._coeffs_to_inputs(input_guess)

else:

inputs = input_guess.reshape(self.system.ninputs, -1)

state_guess = self._simulate_states(

np.zeros(self.system.nstates), inputs)

else:

state_guess = self._broadcast_initial_guess(

state_guess, (self.system.nstates, self.timepts.size))

# Reshape for use by scipy.optimize.minimize()

return np.hstack([

input_guess.reshape(-1), state_guess.reshape(-1)])

else:

# 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)

# See whether we got entire guess or just first time point

if initial_guess.ndim == 1:

# Broadcast inputs to entire time vector

try:

initial_guess = np.broadcast_to(

initial_guess.reshape(-1, 1), shape)

except ValueError:

raise ValueError("initial guess is the wrong shape")

elif initial_guess.shape != shape:

raise ValueError("initial guess is the wrong shape")

return initial_guess

#

# Utility function to convert input vector to coefficient vector

#

# 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).

#

def _inputs_to_coeffs(self, inputs):

# If there is no basis function, just return inputs as coeffs

if self.basis is None:

return inputs

# Solve least squares problems (M x = b) for coeffs on each input

coeffs = []

for i in range(self.system.ninputs):

# Set up the matrices to get inputs

M = np.zeros((self.timepts.size, self.basis.var_ncoefs(i)))

b = np.zeros(self.timepts.size)

# Evaluate at each time point and for each basis function

# TODO: vectorize

for j, t in enumerate(self.timepts):

for k in range(self.basis.var_ncoefs(i)):

M[j, k] = self.basis(k, t)

b[j] = inputs[i, j]

# Solve a least squares problem for the coefficients

alpha, residuals, rank, s = np.linalg.lstsq(M, b, rcond=None)

coeffs.append(alpha)

return np.hstack(coeffs)

# 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):

length = self.basis.var_ncoefs(i)

for j, t in enumerate(self.timepts):

for k in range(length):

inputs[i, j] += coeffs[offset + k] * self.basis(k, t)

offset += length

return inputs

#

# Log and statistics

#

# To allow some insight into where time is being spent, we keep track of

# the number of times that various functions are called and (optionally)

# how long we spent inside each function.

#

def _reset_statistics(self, log=False):

"""Reset counters for keeping track of statistics"""

self.log = log

self.cost_evaluations, self.cost_process_time = 0, 0

self.constraint_evaluations, self.constraint_process_time = 0, 0

self.eqconst_evaluations, self.eqconst_process_time = 0, 0

self.system_simulations = 0

def _print_statistics(self, reset=True):

"""Print out summary statistics from last run"""

print("Summary statistics:")

print("* Cost function calls:", self.cost_evaluations)

if self.log:

print("* Cost function process time:", self.cost_process_time)

if self.constraint_evaluations:

print("* Constraint calls:", self.constraint_evaluations)

if self.log:

print(

"* Constraint process time:", self.constraint_process_time)

if self.eqconst_evaluations:

print("* Eqconst calls:", self.eqconst_evaluations)

if self.log:

print(

"* Eqconst process time:", self.eqconst_process_time)

print("* System simulations:", self.system_simulations)

if reset:

self._reset_statistics(self.log)

#

# Compute the states and inputs from the coefficient vector

#

# These internal functions return the states and inputs at the

# collocation points given the coefficient (optimizer state) vector.

# They keep track of whether a shooting method is being used or not and

# simulate the dynamics if needed.

#

# Compute the states and inputs from the coefficients

def _compute_states_inputs(self, coeffs):

#

# Compute out the states and inputs

#

if self.collocation:

# States are appended to end of (input) coefficients

states = coeffs[-self.system.nstates * self.timepts.size:].reshape(

self.system.nstates, -1)

coeffs = coeffs[:-self.system.nstates * self.timepts.size]

# Compute input at time points

if self.basis:

inputs = self._coeffs_to_inputs(coeffs)

else:

inputs = coeffs.reshape((self.system.ninputs, -1))

if self.shooting:

# See if we already have a simulation for this condition

if np.array_equal(coeffs, self.last_coeffs) \

and np.array_equal(self.x, self.last_x):

states = self.last_states

else:

states = self._simulate_states(self.x, inputs)

self.last_x = self.x

self.last_states = states

self.last_coeffs = coeffs

return states, inputs

# Simulate the system dynamics to retrieve the state

def _simulate_states(self, x0, inputs):

if self.log:

logging.debug(

"calling input_output_response from state\n" + str(x0))

logging.debug("input =\n" + str(inputs))

# Simulate the system to get the state

# TODO: update to use response object; remove return_x

_, _, states = ct.input_output_response(

self.system, self.timepts, inputs, x0, return_x=True,

solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts)

self.system_simulations += 1

if self.log:

logging.debug(

"input_output_response returned states\n" + str(states))

return states

#

# Optimal control computations

#

# Compute the optimal trajectory from the current state

def compute_trajectory(

self, x, squeeze=None, transpose=None, return_states=True,

initial_guess=None, print_summary=True, **kwargs):

"""Compute the optimal trajectory starting at state x.

# Allow 'return_x` as a synonym for 'return_states'

return_states = ct.config._get_param(

'optimal', 'return_x', kwargs, return_states, pop=True, last=True)

# Store the initial state (for use in _constraint_function)

self.x = x

# Allow the initial guess to be overridden

if initial_guess is None:

initial_guess = self.initial_guess

else:

initial_guess = self._process_initial_guess(initial_guess)

# Call SciPy optimizer

res = sp.optimize.minimize(

self._cost_function, initial_guess,

constraints=self.constraints, **self.minimize_kwargs)

# Process and return the results

return OptimalControlResult(

self, res, transpose=transpose, return_states=return_states,

squeeze=squeeze, print_summary=print_summary)

# Compute the current input to apply from the current state (MPC style)

def compute_mpc(self, x, squeeze=None):

"""Compute the optimal input at state x.

res = self.compute_trajectory(x, squeeze=squeeze)

return res.inputs[:, 0]

# Create an input/output system implementing an MPC controller

def create_mpc_iosystem(self, **kwargs):

"""Create an I/O system implementing an MPC controller.

# Check to make sure we are in discrete time

if self.system.dt == 0:

raise ct.ControlNotImplemented(

"MPC for continuous-time systems not implemented")

def _update(t, x, u, params={}):

coeffs = x.reshape((self.system.ninputs, -1))

if self.basis:

# Keep the coefficients unchanged

# TODO: could compute input vector, shift, and re-project (?)

self.initial_guess = coeffs

else:

# Shift the basis elements by one time step

self.initial_guess = np.hstack(

[coeffs[:, 1:], coeffs[:, -1:]]).reshape(-1)

res = self.compute_trajectory(u, print_summary=False)

# New state is the new input vector

return res.inputs.reshape(-1)

def _output(t, x, u, params={}):

# Start with initial guess and recompute based on input state (u)

self.initial_guess = x

res = self.compute_trajectory(u, print_summary=False)

return res.inputs[:, 0]

# Define signal names, if they are not already given

if kwargs.get('inputs') is None:

kwargs['inputs'] = self.system.state_labels

if kwargs.get('outputs') is None:

kwargs['outputs'] = self.system.input_labels

if kwargs.get('states') is None:

kwargs['states'] = self.system.ninputs * \

(self.timepts.size if self.basis is None else self.basis.N)

return ct.NonlinearIOSystem(

"""Result from solving an optimal control problem.

def __init__(

self, ocp, res, return_states=True, print_summary=False,

transpose=None, squeeze=None):

"""Create a OptimalControlResult object"""

# Copy all of the fields we were sent by sp.optimize.minimize()

for key, val in res.items():

setattr(self, key, val)

# Remember the optimal control problem that we solved

self.problem = ocp

# Parse the optimization variables into states and inputs

states, inputs = ocp._compute_states_inputs(res.x)

# See if we got an answer

if not res.success:

warnings.warn(

"unable to solve optimal control problem\n"

"scipy.optimize.minimize returned " + res.message, UserWarning)