Skip to content

Commit cfe21de

Browse files
murrayrmmurrayrm
authored
Merge pull request #712 from murrayrm/optimal_18Mar2022
Optimal control enhancements
2 parents 2f3ca18 + f3d46bc commit cfe21de

12 files changed

+321
-86
lines changed

‎control/config.py

+3
Original file line numberDiff line numberDiff line change
@@ -103,6 +103,9 @@ def reset_defaults():
103103
from .iosys import _iosys_defaults
104104
defaults.update(_iosys_defaults)
105105

106+
from .optimal import _optimal_defaults
107+
defaults.update(_optimal_defaults)
108+
106109

107110
def _get_param(module, param, argval=None, defval=None, pop=False, last=False):
108111
"""Return the default value for a configuration option.

‎control/iosys.py

+80-46
Original file line numberDiff line numberDiff line change
@@ -1571,7 +1571,7 @@ def __init__(self, io_sys, ss_sys=None):
15711571
def input_output_response(
15721572
sys, T, U=0., X0=0, params={},
15731573
transpose=False, return_x=False, squeeze=None,
1574-
solve_ivp_kwargs={}, **kwargs):
1574+
solve_ivp_kwargs={}, t_eval='T', **kwargs):
15751575
"""Compute the output response of a system to a given input.
15761576
15771577
Simulate a dynamical system with a given input and return its output
@@ -1654,50 +1654,57 @@ def input_output_response(
16541654
if kwargs.get('solve_ivp_method', None):
16551655
if kwargs.get('method', None):
16561656
raise ValueError("ivp_method specified more than once")
1657-
solve_ivp_kwargs['method'] = kwargs['solve_ivp_method']
1657+
solve_ivp_kwargs['method'] = kwargs.pop('solve_ivp_method')
16581658

16591659
# Set the default method to 'RK45'
16601660
if solve_ivp_kwargs.get('method', None) is None:
16611661
solve_ivp_kwargs['method'] = 'RK45'
16621662

1663+
# Make sure all input arguments got parsed
1664+
if kwargs:
1665+
raise TypeError("unknown parameters %s" % kwargs)
1666+
16631667
# Sanity checking on the input
16641668
if not isinstance(sys, InputOutputSystem):
16651669
raise TypeError("System of type ", type(sys), " not valid")
16661670

16671671
# Compute the time interval and number of steps
16681672
T0, Tf = T[0], T[-1]
1669-
n_steps = len(T)
1673+
ntimepts = len(T)
1674+
1675+
# Figure out simulation times (t_eval)
1676+
if solve_ivp_kwargs.get('t_eval'):
1677+
if t_eval == 'T':
1678+
# Override the default with the solve_ivp keyword
1679+
t_eval = solve_ivp_kwargs.pop('t_eval')
1680+
else:
1681+
raise ValueError("t_eval specified more than once")
1682+
if isinstance(t_eval, str) and t_eval == 'T':
1683+
# Use the input time points as the output time points
1684+
t_eval = T
16701685

16711686
# Check and convert the input, if needed
16721687
# TODO: improve MIMO ninputs check (choose from U)
16731688
if sys.ninputs is None or sys.ninputs == 1:
1674-
legal_shapes = [(n_steps,), (1, n_steps)]
1689+
legal_shapes = [(ntimepts,), (1, ntimepts)]
16751690
else:
1676-
legal_shapes = [(sys.ninputs, n_steps)]
1691+
legal_shapes = [(sys.ninputs, ntimepts)]
16771692
U = _check_convert_array(U, legal_shapes,
16781693
'Parameter ``U``: ', squeeze=False)
1679-
U = U.reshape(-1, n_steps)
1680-
1681-
# Check to make sure this is not a static function
1682-
nstates = _find_size(sys.nstates, X0)
1683-
if nstates == 0:
1684-
# No states => map input to output
1685-
u = U[0] if len(U.shape) == 1 else U[:, 0]
1686-
y = np.zeros((np.shape(sys._out(T[0], X0, u))[0], len(T)))
1687-
for i in range(len(T)):
1688-
u = U[i] if len(U.shape) == 1 else U[:, i]
1689-
y[:, i] = sys._out(T[i], [], u)
1690-
return TimeResponseData(
1691-
T, y, None, U, issiso=sys.issiso(),
1692-
output_labels=sys.output_index, input_labels=sys.input_index,
1693-
transpose=transpose, return_x=return_x, squeeze=squeeze)
1694+
U = U.reshape(-1, ntimepts)
1695+
ninputs = U.shape[0]
16941696

16951697
# create X0 if not given, test if X0 has correct shape
1698+
nstates = _find_size(sys.nstates, X0)
16961699
X0 = _check_convert_array(X0, [(nstates,), (nstates, 1)],
16971700
'Parameter ``X0``: ', squeeze=True)
16981701

1699-
# Update the parameter values
1700-
sys._update_params(params)
1702+
# Figure out the number of outputs
1703+
if sys.noutputs is None:
1704+
# Evaluate the output function to find number of outputs
1705+
noutputs = np.shape(sys._out(T[0], X0, U[:, 0]))[0]
1706+
else:
1707+
noutputs = sys.noutputs
17011708

17021709
#
17031710
# Define a function to evaluate the input at an arbitrary time
@@ -1714,6 +1721,31 @@ def ufun(t):
17141721
dt = (t - T[idx-1]) / (T[idx] - T[idx-1])
17151722
return U[..., idx-1] * (1. - dt) + U[..., idx] * dt
17161723

1724+
# Check to make sure this is not a static function
1725+
if nstates == 0: # No states => map input to output
1726+
# Make sure the user gave a time vector for evaluation (or 'T')
1727+
if t_eval is None:
1728+
# User overrode t_eval with None, but didn't give us the times...
1729+
warn("t_eval set to None, but no dynamics; using T instead")
1730+
t_eval = T
1731+
1732+
# Allocate space for the inputs and outputs
1733+
u = np.zeros((ninputs, len(t_eval)))
1734+
y = np.zeros((noutputs, len(t_eval)))
1735+
1736+
# Compute the input and output at each point in time
1737+
for i, t in enumerate(t_eval):
1738+
u[:, i] = ufun(t)
1739+
y[:, i] = sys._out(t, [], u[:, i])
1740+
1741+
return TimeResponseData(
1742+
t_eval, y, None, u, issiso=sys.issiso(),
1743+
output_labels=sys.output_index, input_labels=sys.input_index,
1744+
transpose=transpose, return_x=return_x, squeeze=squeeze)
1745+
1746+
# Update the parameter values
1747+
sys._update_params(params)
1748+
17171749
# Create a lambda function for the right hand side
17181750
def ivp_rhs(t, x):
17191751
return sys._rhs(t, x, ufun(t))
@@ -1724,27 +1756,27 @@ def ivp_rhs(t, x):
17241756
raise NameError("scipy.integrate.solve_ivp not found; "
17251757
"use SciPy 1.0 or greater")
17261758
soln = sp.integrate.solve_ivp(
1727-
ivp_rhs, (T0, Tf), X0, t_eval=T,
1759+
ivp_rhs, (T0, Tf), X0, t_eval=t_eval,
17281760
vectorized=False, **solve_ivp_kwargs)
1761+
if not soln.success:
1762+
raise RuntimeError("solve_ivp failed: " + soln.message)
17291763

1730-
if not soln.success or soln.status != 0:
1731-
# Something went wrong
1732-
warn("sp.integrate.solve_ivp failed")
1733-
print("Return bunch:", soln)
1734-
1735-
# Compute the output associated with the state (and use sys.out to
1736-
# figure out the number of outputs just in case it wasn't specified)
1737-
u = U[0] if len(U.shape) == 1 else U[:, 0]
1738-
y = np.zeros((np.shape(sys._out(T[0], X0, u))[0], len(T)))
1739-
for i in range(len(T)):
1740-
u = U[i] if len(U.shape) == 1 else U[:, i]
1741-
y[:, i] = sys._out(T[i], soln.y[:, i], u)
1764+
# Compute inputs and outputs for each time point
1765+
u = np.zeros((ninputs, len(soln.t)))
1766+
y = np.zeros((noutputs, len(soln.t)))
1767+
for i, t in enumerate(soln.t):
1768+
u[:, i] = ufun(t)
1769+
y[:, i] = sys._out(t, soln.y[:, i], u[:, i])
17421770

17431771
elif isdtime(sys):
1772+
# If t_eval was not specified, use the sampling time
1773+
if t_eval is None:
1774+
t_eval = np.arange(T[0], T[1] + sys.dt, sys.dt)
1775+
17441776
# Make sure the time vector is uniformly spaced
1745-
dt = T[1] - T[0]
1746-
if not np.allclose(T[1:] - T[:-1], dt):
1747-
raise ValueError("Parameter ``T``: time values must be "
1777+
dt = t_eval[1] - t_eval[0]
1778+
if not np.allclose(t_eval[1:] - t_eval[:-1], dt):
1779+
raise ValueError("Parameter ``t_eval``: time values must be "
17481780
"equally spaced.")
17491781

17501782
# Make sure the sample time matches the given time
@@ -1764,21 +1796,23 @@ def ivp_rhs(t, x):
17641796

17651797
# Compute the solution
17661798
soln = sp.optimize.OptimizeResult()
1767-
soln.t = T # Store the time vector directly
1768-
x = X0 # Initilize state
1799+
soln.t = t_eval # Store the time vector directly
1800+
x = np.array(X0) # State vector (store as floats)
17691801
soln.y = [] # Solution, following scipy convention
1770-
y = [] # System output
1771-
for i in range(len(T)):
1772-
# Store the current state and output
1802+
u, y = [], [] # System input, output
1803+
for t in t_eval:
1804+
# Store the current input, state, and output
17731805
soln.y.append(x)
1774-
y.append(sys._out(T[i], x, ufun(T[i])))
1806+
u.append(ufun(t))
1807+
y.append(sys._out(t, x, u[-1]))
17751808

17761809
# Update the state for the next iteration
1777-
x = sys._rhs(T[i], x, ufun(T[i]))
1810+
x = sys._rhs(t, x, u[-1])
17781811

17791812
# Convert output to numpy arrays
17801813
soln.y = np.transpose(np.array(soln.y))
17811814
y = np.transpose(np.array(y))
1815+
u = np.transpose(np.array(u))
17821816

17831817
# Mark solution as successful
17841818
soln.success = True # No way to fail
@@ -1787,7 +1821,7 @@ def ivp_rhs(t, x):
17871821
raise TypeError("Can't determine system type")
17881822

17891823
return TimeResponseData(
1790-
soln.t, y, soln.y, U, issiso=sys.issiso(),
1824+
soln.t, y, soln.y, u, issiso=sys.issiso(),
17911825
output_labels=sys.output_index, input_labels=sys.input_index,
17921826
state_labels=sys.state_index,
17931827
transpose=transpose, return_x=return_x, squeeze=squeeze)

‎control/optimal.py

+55-15
Original file line numberDiff line numberDiff line change
@@ -16,10 +16,21 @@
1616
import logging
1717
import time
1818

19+
from . import config
20+
from .exception import ControlNotImplemented
1921
from .timeresp import TimeResponseData
2022

2123
__all__ = ['find_optimal_input']
2224

25+
# Define module default parameter values
26+
_optimal_defaults = {
27+
'optimal.minimize_method': None,
28+
'optimal.minimize_options': {},
29+
'optimal.minimize_kwargs': {},
30+
'optimal.solve_ivp_method': None,
31+
'optimal.solve_ivp_options': {},
32+
}
33+
2334

2435
class OptimalControlProblem():
2536
"""Description of a finite horizon, optimal control problem.
@@ -110,6 +121,10 @@ class OptimalControlProblem():
110121
values of the input at the specified times (using linear interpolation for
111122
continuous systems).
112123
124+
The default values for ``minimize_method``, ``minimize_options``,
125+
``minimize_kwargs``, ``solve_ivp_method``, and ``solve_ivp_options`` can
126+
be set using config.defaults['optimal.<keyword>'].
127+
113128
"""
114129
def __init__(
115130
self, sys, timepts, integral_cost, trajectory_constraints=[],
@@ -126,17 +141,22 @@ def __init__(
126141

127142
# Process keyword arguments
128143
self.solve_ivp_kwargs = {}
129-
self.solve_ivp_kwargs['method'] = kwargs.pop('solve_ivp_method', None)
130-
self.solve_ivp_kwargs.update(kwargs.pop('solve_ivp_kwargs', {}))
144+
self.solve_ivp_kwargs['method'] = kwargs.pop(
145+
'solve_ivp_method', config.defaults['optimal.solve_ivp_method'])
146+
self.solve_ivp_kwargs.update(kwargs.pop(
147+
'solve_ivp_kwargs', config.defaults['optimal.solve_ivp_options']))
131148

132149
self.minimize_kwargs = {}
133-
self.minimize_kwargs['method'] = kwargs.pop('minimize_method', None)
134-
self.minimize_kwargs['options'] = kwargs.pop('minimize_options', {})
135-
self.minimize_kwargs.update(kwargs.pop('minimize_kwargs', {}))
150+
self.minimize_kwargs['method'] = kwargs.pop(
151+
'minimize_method', config.defaults['optimal.minimize_method'])
152+
self.minimize_kwargs['options'] = kwargs.pop(
153+
'minimize_options', config.defaults['optimal.minimize_options'])
154+
self.minimize_kwargs.update(kwargs.pop(
155+
'minimize_kwargs', config.defaults['optimal.minimize_kwargs']))
136156

137-
if len(kwargs) > 0:
138-
raise ValueError(
139-
f'unrecognized keyword(s): {list(kwargs.keys())}')
157+
# Make sure all input arguments got parsed
158+
if kwargs:
159+
raise TypeError("unrecognized keyword(s): ", str(kwargs))
140160

141161
# Process trajectory constraints
142162
if isinstance(trajectory_constraints, tuple):
@@ -271,9 +291,10 @@ def _cost_function(self, coeffs):
271291
logging.debug("initial input[0:3] =\n" + str(inputs[:, 0:3]))
272292

273293
# Simulate the system to get the state
294+
# TODO: try calling solve_ivp directly for better speed?
274295
_, _, states = ct.input_output_response(
275296
self.system, self.timepts, inputs, x, return_x=True,
276-
solve_ivp_kwargs=self.solve_ivp_kwargs)
297+
solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts)
277298
self.system_simulations += 1
278299
self.last_x = x
279300
self.last_coeffs = coeffs
@@ -393,7 +414,7 @@ def _constraint_function(self, coeffs):
393414
# Simulate the system to get the state
394415
_, _, states = ct.input_output_response(
395416
self.system, self.timepts, inputs, x, return_x=True,
396-
solve_ivp_kwargs=self.solve_ivp_kwargs)
417+
solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts)
397418
self.system_simulations += 1
398419
self.last_x = x
399420
self.last_coeffs = coeffs
@@ -475,7 +496,7 @@ def _eqconst_function(self, coeffs):
475496
# Simulate the system to get the state
476497
_, _, states = ct.input_output_response(
477498
self.system, self.timepts, inputs, x, return_x=True,
478-
solve_ivp_kwargs=self.solve_ivp_kwargs)
499+
solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts)
479500
self.system_simulations += 1
480501
self.last_x = x
481502
self.last_coeffs = coeffs
@@ -548,7 +569,7 @@ def _process_initial_guess(self, initial_guess):
548569
initial_guess = np.atleast_1d(initial_guess)
549570

550571
# See whether we got entire guess or just first time point
551-
if len(initial_guess.shape) == 1:
572+
if initial_guess.ndim == 1:
552573
# Broadcast inputs to entire time vector
553574
try:
554575
initial_guess = np.broadcast_to(
@@ -804,6 +825,15 @@ class OptimalControlResult(sp.optimize.OptimizeResult):
804825
Whether or not the optimizer exited successful.
805826
problem : OptimalControlProblem
806827
Optimal control problem that generated this solution.
828+
cost : float
829+
Final cost of the return solution.
830+
system_simulations, {cost, constraint, eqconst}_evaluations : int
831+
Number of system simulations and evaluations of the cost function,
832+
(inequality) constraint function, and equality constraint function
833+
performed during the optimzation.
834+
{cost, constraint, eqconst}_process_time : float
835+
If logging was enabled, the amount of time spent evaluating the cost
836+
and constraint functions.
807837
808838
"""
809839
def __init__(
@@ -833,15 +863,19 @@ def __init__(
833863
"unable to solve optimal control problem\n"
834864
"scipy.optimize.minimize returned " + res.message, UserWarning)
835865

866+
# Save the final cost
867+
self.cost = res.fun
868+
836869
# Optionally print summary information
837870
if print_summary:
838871
ocp._print_statistics()
872+
print("* Final cost:", self.cost)
839873

840874
if return_states and inputs.shape[1] == ocp.timepts.shape[0]:
841875
# Simulate the system if we need the state back
842876
_, _, states = ct.input_output_response(
843877
ocp.system, ocp.timepts, inputs, ocp.x, return_x=True,
844-
solve_ivp_kwargs=ocp.solve_ivp_kwargs)
878+
solve_ivp_kwargs=ocp.solve_ivp_kwargs, t_eval=ocp.timepts)
845879
ocp.system_simulations += 1
846880
else:
847881
states = None
@@ -858,7 +892,7 @@ def __init__(
858892

859893
# Compute the input for a nonlinear, (constrained) optimal control problem
860894
def solve_ocp(
861-
sys, horizon, X0, cost, trajectory_constraints=[], terminal_cost=None,
895+
sys, horizon, X0, cost, trajectory_constraints=None, terminal_cost=None,
862896
terminal_constraints=[], initial_guess=None, basis=None, squeeze=None,
863897
transpose=None, return_states=False, log=False, **kwargs):
864898

@@ -960,12 +994,18 @@ def solve_ocp(
960994
# Process keyword arguments
961995
if trajectory_constraints is None:
962996
# Backwards compatibility
963-
trajectory_constraints = kwargs.pop('constraints', None)
997+
trajectory_constraints = kwargs.pop('constraints', [])
964998

965999
# Allow 'return_x` as a synonym for 'return_states'
9661000
return_states = ct.config._get_param(
9671001
'optimal', 'return_x', kwargs, return_states, pop=True)
9681002

1003+
# Process (legacy) method keyword
1004+
if kwargs.get('method'):
1005+
if kwargs.get('minimize_method'):
1006+
raise ValueError("'minimize_method' specified more than once")
1007+
kwargs['minimize_method'] = kwargs.pop('method')
1008+
9691009
# Set up the optimal control problem
9701010
ocp = OptimalControlProblem(
9711011
sys, horizon, cost, trajectory_constraints=trajectory_constraints,

‎control/statefbk.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -734,7 +734,7 @@ def dlqr(*args, **kwargs):
734734
The dlqr() function computes the optimal state feedback controller
735735
u[n] = - K x[n] that minimizes the quadratic cost
736736
737-
.. math:: J = \\Sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n])
737+
.. math:: J = \\sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n])
738738
739739
The function can be called with either 3, 4, or 5 arguments:
740740

0 commit comments

Comments
 (0)