Skip to content

Fixes to various optimization-based control functions #709

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 9 commits into from
Mar 19, 2022
Merged

Fixes to various optimization-based control functions #709

Changes from 1 commit
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
0060618
fix handling of endpoint in discrete optimal
murrayrm Jan 25, 2022
395dbbb
check for unused keywords in OptimalControlProblem
murrayrm Jan 25, 2022
de4c75c
generate iosys warning if solve_ivp does not succeed
murrayrm Jan 25, 2022
93a5fae
remove one step delay in create_mpc_iosystem, discrete time only
murrayrm Jan 25, 2022
7396f76
add params to flat systems + updated keywords
murrayrm Feb 5, 2022
994fedc
fix discrete time X0, U processing
murrayrm Mar 1, 2022
d01a52c
fix optimal unit tests (matrix case) + PEP8
murrayrm Mar 12, 2022
be660ce
default outfcn() bug fix + unit tests for flatsys
murrayrm Mar 12, 2022
f807b33
updated optimal constraint handling + unit tests
murrayrm Mar 12, 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
remove one step delay in create_mpc_iosystem, discrete time only
  • Loading branch information
@murrayrm
murrayrm committed Jan 25, 2022
commit 93a5fae4aa3e000479a90be6d99e3471ca720e7f
72 changes: 40 additions & 32 deletions control/optimal.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -639,35 +639,9 @@ def _print_statistics(self, reset=True):
if reset:
self._reset_statistics(self.log)

# Create an input/output system implementing an MPC controller
def _create_mpc_iosystem(self, dt=True):
"""Create an I/O system implementing an MPC controller"""
def _update(t, x, u, params={}):
coeffs = x.reshape((self.system.ninputs, -1))
if self.basis:
# Keep the coeffecients 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)
return res.inputs.reshape(-1)

def _output(t, x, u, params={}):
if self.basis:
# TODO: compute inputs from basis elements
raise NotImplementedError("basis elements not implemented")
else:
inputs = x.reshape((self.system.ninputs, -1))
return inputs[:, 0]

return ct.NonlinearIOSystem(
_update, _output, dt=dt,
inputs=self.system.nstates, outputs=self.system.ninputs,
states=self.system.ninputs * \
(self.timepts.size if self.basis is None else self.basis.N))
#
# Optimal control computations
#

# Compute the optimal trajectory from the current state
def compute_trajectory(
Expand Down Expand Up @@ -762,6 +736,41 @@ def compute_mpc(self, x, squeeze=None):
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):
"""Create an I/O system implementing an MPC controller"""
# Check to make sure we are in discrete time
if self.system.dt == 0:
raise 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 coeffecients 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]

return ct.NonlinearIOSystem(
_update, _output, dt=self.system.dt,
inputs=self.system.nstates, outputs=self.system.ninputs,
states=self.system.ninputs * \
(self.timepts.size if self.basis is None else self.basis.N))


# Optimal control result
class OptimalControlResult(sp.optimize.OptimizeResult):
Expand Down Expand Up @@ -952,7 +961,7 @@ def solve_ocp(
# Create a model predictive controller for an optimal control problem
def create_mpc_iosystem(
sys, horizon, cost, constraints=[], terminal_cost=None,
terminal_constraints=[], dt=True, log=False, **kwargs):
terminal_constraints=[], log=False, **kwargs):
"""Create a model predictive I/O control system

This function creates an input/output system that implements a model
Expand Down Expand Up @@ -1001,15 +1010,14 @@ def create_mpc_iosystem(
:func:`OptimalControlProblem` for more information.

"""

# Set up the optimal control problem
ocp = OptimalControlProblem(
sys, horizon, cost, trajectory_constraints=constraints,
terminal_cost=terminal_cost, terminal_constraints=terminal_constraints,
log=log, **kwargs)

# Return an I/O system implementing the model predictive controller
return ocp._create_mpc_iosystem(dt=dt)
return ocp.create_mpc_iosystem()


#
Expand Down