Skip to content

Commit 8711900

Browse files
murrayrmmurrayrm
committed
initial minimal implementation (working)
1 parent 23cb793 commit 8711900

2 files changed

Lines changed: 289 additions & 142 deletions

File tree

‎control/obc.py‎

Lines changed: 217 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,217 @@
1+
# obc.py - optimization based control module
2+
#
3+
# RMM, 11 Feb 2021
4+
#
5+
6+
"""The "mod:`~control.obc` module provides support for optimization-based
7+
controllers for nonlinear systems with state and input constraints.
8+
9+
"""
10+
11+
import numpy as np
12+
import scipy as sp
13+
import scipy.optimize as opt
14+
import control as ct
15+
import warnings
16+
17+
from .timeresp import _process_time_response
18+
19+
class ModelPredictiveController():
20+
"""The :class:`ModelPredictiveController` class is a front end for computing
21+
an optimal control input for a nonilinear system with a user-defined cost
22+
function and state and input constraints.
23+
24+
"""
25+
def __init__(
26+
self, sys, time, integral_cost, trajectory_constraints=[],
27+
terminal_cost=None, terminal_constraints=[]):
28+
29+
self.system = sys
30+
self.time_vector = time
31+
self.integral_cost = integral_cost
32+
self.trajectory_constraints = trajectory_constraints
33+
self.terminal_cost = terminal_cost
34+
self.terminal_constraints = terminal_constraints
35+
36+
#
37+
# The approach that we use here is to set up an optimization over the
38+
# inputs at each point in time, using the integral and terminal costs
39+
# as well as the trajectory and terminal constraints. The main work
40+
# of this method is to create the optimization problem that can be
41+
# solved with scipy.optimize.minimize().
42+
#
43+
44+
# Gather together all of the constraints
45+
constraint_lb, constraint_ub = [], []
46+
for time in self.time_vector:
47+
for constraint in self.trajectory_constraints:
48+
type, fun, lb, ub = constraint
49+
constraint_lb.append(lb)
50+
constraint_ub.append(ub)
51+
for constraint in self.terminal_constraints:
52+
type, fun, lb, ub = constraint
53+
constraint_lb.append(lb)
54+
constraint_ub.append(ub)
55+
56+
# Turn constraint vectors into 1D arrays
57+
self.constraint_lb = np.hstack(constraint_lb)
58+
self.constraint_ub = np.hstack(constraint_ub)
59+
60+
# Create the new constraint
61+
self.constraints = sp.optimize.NonlinearConstraint(
62+
self.constraint_function, self.constraint_lb, self.constraint_ub)
63+
64+
# Initial guess
65+
self.initial_guess = np.zeros(
66+
self.system.ninputs * self.time_vector.size)
67+
68+
#
69+
# Cost function
70+
#
71+
# Given the input U = [u[0], ... u[N]], we need to compute the cost of
72+
# the trajectory generated by that input. This means we have to
73+
# simulate the system to get the state trajectory X = [x[0], ...,
74+
# x[N]] and then compute the cost at each point:
75+
#
76+
# Cost = sum_k integral_cost(x[k], u[k]) + terminal_cost(x[N], u[N])
77+
#
78+
def cost_function(self, inputs):
79+
# Reshape the input vector
80+
inputs = inputs.reshape(
81+
(self.system.ninputs, self.time_vector.size))
82+
83+
# Simulate the system to get the state
84+
_, _, states = ct.input_output_response(
85+
self.system, self.time_vector, inputs, self.x, return_x=True)
86+
87+
# Trajectory cost
88+
# TODO: vectorize
89+
cost = 0
90+
for i, time in enumerate(self.time_vector):
91+
cost += self.integral_cost(states[:,i], inputs[:,i])
92+
93+
# Terminal cost
94+
if self.terminal_cost is not None:
95+
cost += self.terminal_cost(states[:,-1], inputs[:,-1])
96+
97+
# Return the total cost for this input sequence
98+
return cost
99+
100+
#
101+
# Constraints
102+
#
103+
# We are given the constraints along the trajectory and the terminal
104+
# constraints, which each take inputs [x, u] and evaluate the
105+
# constraint. How we handle these depends on the type of constraint:
106+
#
107+
# We have stored the form of the constraint at a single point, but we
108+
# now need to extend this to apply to each point in the trajectory.
109+
# This means that we need to create N constraints, each of which holds
110+
# at a specific point in time, and implements the original constraint.
111+
#
112+
# To do this, we basically create a function that simulates the system
113+
# dynamics and returns a vector of values corresponding to the value
114+
# of the function at each time. We also replicate the upper and lower
115+
# bounds for each point in time.
116+
#
117+
118+
# Define a function to evaluate all of the constraints
119+
def constraint_function(self, inputs):
120+
# Reshape the input vector
121+
inputs = inputs.reshape(
122+
(self.system.ninputs, self.time_vector.size))
123+
124+
# Simulate the system to get the state
125+
_, _, states = ct.input_output_response(
126+
self.system, self.time_vector, inputs, self.x, return_x=True)
127+
128+
value = []
129+
for i, time in enumerate(self.time_vector):
130+
for constraint in self.trajectory_constraints:
131+
type, fun, lb, ub = constraint
132+
if type == opt.LinearConstraint:
133+
value.append(
134+
np.dot(fun, np.hstack([states[:,i], inputs[:,i]])))
135+
else:
136+
raise TypeError("unknown constraint type %s" %
137+
constraint[0])
138+
139+
for constraint in self.terminal_constraints:
140+
type, fun, lb, ub = constraint
141+
if type == opt.LinearConstraint:
142+
value.append(
143+
np.dot(fun, np.hstack([states[:,i], inputs[:,i]])))
144+
else:
145+
raise TypeError("unknown constraint type %s" %
146+
constraint[0])
147+
148+
# Return the value of the constraint function
149+
return np.hstack(value)
150+
151+
def __call__(self, x):
152+
"""Compute the optimal input at state x"""
153+
# Store the starting point
154+
# TODO: call compute_trajectory?
155+
self.x = x
156+
157+
# Call ScipPy optimizer
158+
res = sp.optimize.minimize(
159+
self.cost_function, self.initial_guess,
160+
constraints=self.constraints)
161+
162+
# Return the result
163+
if res.success:
164+
return res.x[0]
165+
else:
166+
warnings.warn(res.message)
167+
return None
168+
169+
def compute_trajectory(
170+
self, x, squeeze=None, transpose=None, return_x=None):
171+
"""Compute the optimal input at state x"""
172+
# Store the starting point
173+
self.x = x
174+
175+
# Call ScipPy optimizer
176+
res = sp.optimize.minimize(
177+
self.cost_function, self.initial_guess,
178+
constraints=self.constraints)
179+
180+
# See if we got an answer
181+
if not res.success:
182+
warnings.warn(res.message)
183+
return None
184+
185+
# Reshape the input vector
186+
inputs = res.x.reshape(
187+
(self.system.ninputs, self.time_vector.size))
188+
189+
return _process_time_response(
190+
self.system, self.time_vector, inputs, None,
191+
transpose=transpose, return_x=return_x, squeeze=squeeze)
192+
193+
def state_poly_constraint(sys, polytope):
194+
"""Create state constraint from polytope"""
195+
# TODO: make sure the system and constraints are compatible
196+
197+
# Return a linear constraint object based on the polynomial
198+
return (opt.LinearConstraint,
199+
np.hstack(
200+
[polytope.A, np.zeros((polytope.A.shape[0], sys.ninputs))]),
201+
np.full(polytope.A.shape[0], -np.inf), polytope.b)
202+
203+
204+
def input_poly_constraint(sys, polytope):
205+
"""Create input constraint from polytope"""
206+
# TODO: make sure the system and constraints are compatible
207+
208+
# Return a linear constraint object based on the polynomial
209+
return (opt.LinearConstraint,
210+
np.hstack(
211+
[np.zeros((polytope.A.shape[0], sys.nstates)), polytope.A]),
212+
np.full(polytope.A.shape[0], -np.inf), polytope.b)
213+
214+
215+
def quadratic_cost(sys, Q, R):
216+
"""Create quadratic cost function"""
217+
return lambda x, u: x @ Q @ x + u @ R @ u

0 commit comments

Comments
 (0)