Skip to content

Commit 0d14642

Browse files
murrayrmmurrayrm
committed
add'l unit tests, cache sim results, equality constraint support
1 parent bd322e7 commit 0d14642

2 files changed

Lines changed: 238 additions & 26 deletions

File tree

‎control/obc.py‎

Lines changed: 131 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -76,28 +76,46 @@ def __init__(
7676
# is consistent with the `constraint_function` that is used at
7777
# evaluation time.
7878
#
79-
constraint_lb, constraint_ub = [], []
79+
constraint_lb, constraint_ub, eqconst_value = [], [], []
8080

8181
# Go through each time point and stack the bounds
8282
for time in self.time_vector:
8383
for constraint in self.trajectory_constraints:
8484
type, fun, lb, ub = constraint
85-
constraint_lb.append(lb)
86-
constraint_ub.append(ub)
85+
if np.all(lb == ub):
86+
# Equality constraint
87+
eqconst_value.append(lb)
88+
else:
89+
# Inequality constraint
90+
constraint_lb.append(lb)
91+
constraint_ub.append(ub)
8792

8893
# Add on the terminal constraints
8994
for constraint in self.terminal_constraints:
9095
type, fun, lb, ub = constraint
91-
constraint_lb.append(lb)
92-
constraint_ub.append(ub)
96+
if np.all(lb == ub):
97+
# Equality constraint
98+
eqconst_value.append(lb)
99+
else:
100+
# Inequality constraint
101+
constraint_lb.append(lb)
102+
constraint_ub.append(ub)
93103

94104
# Turn constraint vectors into 1D arrays
95-
self.constraint_lb = np.hstack(constraint_lb)
96-
self.constraint_ub = np.hstack(constraint_ub)
97-
98-
# Create the new constraint
99-
self.constraints = sp.optimize.NonlinearConstraint(
100-
self.constraint_function, self.constraint_lb, self.constraint_ub)
105+
self.constraint_lb = np.hstack(constraint_lb) if constraint_lb else []
106+
self.constraint_ub = np.hstack(constraint_ub) if constraint_ub else []
107+
self.eqconst_value = np.hstack(eqconst_value) if eqconst_value else []
108+
109+
# Create the constraints (inequality and equality)
110+
self.constraints = []
111+
if len(self.constraint_lb) != 0:
112+
self.constraints.append(sp.optimize.NonlinearConstraint(
113+
self.constraint_function, self.constraint_lb,
114+
self.constraint_ub))
115+
if len(self.eqconst_value) != 0:
116+
self.constraints.append(sp.optimize.NonlinearConstraint(
117+
self.eqconst_function, self.eqconst_value,
118+
self.eqconst_value))
101119

102120
#
103121
# Initial guess
@@ -109,6 +127,10 @@ def __init__(
109127
self.initial_guess = np.zeros(
110128
self.system.ninputs * self.time_vector.size)
111129

130+
# Store states, input to minimize re-computation
131+
self.last_x = np.full(self.system.nstates, np.nan)
132+
self.last_inputs = np.full(self.initial_guess.shape, np.nan)
133+
112134
#
113135
# Cost function
114136
#
@@ -117,7 +139,7 @@ def __init__(
117139
# simulate the system to get the state trajectory X = [x[0], ...,
118140
# x[N]] and then compute the cost at each point:
119141
#
120-
# Cost = sum_k integral_cost(x[k], u[k]) + terminal_cost(x[N], u[N])
142+
# cost = sum_k integral_cost(x[k], u[k]) + terminal_cost(x[N], u[N])
121143
#
122144
# The initial state is for generating the simulation is store in the class
123145
# parameter `x` prior to calling the optimization algorithm.
@@ -128,9 +150,17 @@ def cost_function(self, inputs):
128150
inputs = inputs.reshape(
129151
(self.system.ninputs, self.time_vector.size))
130152

131-
# Simulate the system to get the state
132-
_, _, states = ct.input_output_response(
133-
self.system, self.time_vector, inputs, x, return_x=True)
153+
# See if we already have a simulation for this condition
154+
if np.array_equal(x, self.last_x) and \
155+
np.array_equal(inputs, self.last_inputs):
156+
states = self.last_states
157+
else:
158+
# Simulate the system to get the state
159+
_, _, states = ct.input_output_response(
160+
self.system, self.time_vector, inputs, x, return_x=True)
161+
self.last_x = x
162+
self.last_inputs = inputs
163+
self.last_states = states
134164

135165
# Trajectory cost
136166
# TODO: vectorize
@@ -160,11 +190,17 @@ def cost_function(self, inputs):
160190
# * For nonlinear constraints (NonlinearConstraint), a user-specific
161191
# constraint function having the form
162192
#
163-
# constraint_fun(x, u)
193+
# constraint_fun(x, u) TODO: convert from [x, u] to (x, u)
164194
#
165195
# is called at each point along the trajectory and compared against the
166196
# upper and lower bounds.
167197
#
198+
# * If the upper and lower bound for the constraint is identical, then we
199+
# separate out the evaluation into two different constraints, which
200+
# allows the SciPy optimizers to be more efficient (and stops them from
201+
# generating a warning about mixed constraints). This is handled
202+
# through the use of the `eqconst_function` and `eqconst_value` members.
203+
#
168204
# In both cases, the constraint is specified at a single point, but we
169205
# extend this to apply to each point in the trajectory. This means
170206
# that for N time points with m trajectory constraints and p terminal
@@ -189,42 +225,109 @@ def constraint_function(self, inputs):
189225
inputs = inputs.reshape(
190226
(self.system.ninputs, self.time_vector.size))
191227

192-
# Simulate the system to get the state
193-
_, _, states = ct.input_output_response(
194-
self.system, self.time_vector, inputs, x, return_x=True)
228+
# See if we already have a simulation for this condition
229+
if np.array_equal(x, self.last_x) and \
230+
np.array_equal(inputs, self.last_inputs):
231+
states = self.last_states
232+
else:
233+
# Simulate the system to get the state
234+
_, _, states = ct.input_output_response(
235+
self.system, self.time_vector, inputs, x, return_x=True)
236+
self.last_x = x
237+
self.last_inputs = inputs
238+
self.last_states = states
195239

196240
# Evaluate the constraint function along the trajectory
197241
value = []
198242
for i, time in enumerate(self.time_vector):
199243
for constraint in self.trajectory_constraints:
200244
type, fun, lb, ub = constraint
201-
if type == opt.LinearConstraint:
245+
if np.all(lb == ub):
246+
# Skip equality constraints
247+
continue
248+
elif type == opt.LinearConstraint:
202249
# `fun` is the A matrix associated with the polytope...
203250
value.append(
204251
np.dot(fun, np.hstack([states[:,i], inputs[:,i]])))
205252
elif type == opt.NonlinearConstraint:
206-
value.append(
207-
fun(np.hstack([states[:,i], inputs[:,i]])))
253+
value.append(fun(states[:,i], inputs[:,i]))
208254
else:
209255
raise TypeError("unknown constraint type %s" %
210256
constraint[0])
211257

212258
# Evaluate the terminal constraint functions
213259
for constraint in self.terminal_constraints:
214260
type, fun, lb, ub = constraint
215-
if type == opt.LinearConstraint:
261+
if np.all(lb == ub):
262+
# Skip equality constraints
263+
continue
264+
elif type == opt.LinearConstraint:
216265
value.append(
217266
np.dot(fun, np.hstack([states[:,i], inputs[:,i]])))
218267
elif type == opt.NonlinearConstraint:
268+
value.append(fun(states[:,i], inputs[:,i]))
269+
else:
270+
raise TypeError("unknown constraint type %s" %
271+
constraint[0])
272+
273+
# Return the value of the constraint function
274+
return np.hstack(value)
275+
276+
def eqconst_function(self, inputs):
277+
# Retrieve the initial state and reshape the input vector
278+
x = self.x
279+
inputs = inputs.reshape(
280+
(self.system.ninputs, self.time_vector.size))
281+
282+
# See if we already have a simulation for this condition
283+
if np.array_equal(x, self.last_x) and \
284+
np.array_equal(inputs, self.last_inputs):
285+
states = self.last_states
286+
else:
287+
# Simulate the system to get the state
288+
_, _, states = ct.input_output_response(
289+
self.system, self.time_vector, inputs, x, return_x=True)
290+
self.last_x = x
291+
self.last_inputs = inputs
292+
self.last_states = states
293+
294+
# Evaluate the constraint function along the trajectory
295+
value = []
296+
for i, time in enumerate(self.time_vector):
297+
for constraint in self.trajectory_constraints:
298+
type, fun, lb, ub = constraint
299+
if np.any(lb != ub):
300+
# Skip iniquality constraints
301+
continue
302+
elif type == opt.LinearConstraint:
303+
# `fun` is the A matrix associated with the polytope...
304+
value.append(
305+
np.dot(fun, np.hstack([states[:,i], inputs[:,i]])))
306+
elif type == opt.NonlinearConstraint:
307+
value.append(fun(states[:,i], inputs[:,i]))
308+
else:
309+
raise TypeError("unknown constraint type %s" %
310+
constraint[0])
311+
312+
# Evaluate the terminal constraint functions
313+
for constraint in self.terminal_constraints:
314+
type, fun, lb, ub = constraint
315+
if np.any(lb != ub):
316+
# Skip inequality constraints
317+
continue
318+
elif type == opt.LinearConstraint:
219319
value.append(
220-
fun(np.hstack([states[:,i], inputs[:,i]])))
320+
np.dot(fun, np.hstack([states[:,i], inputs[:,i]])))
321+
elif type == opt.NonlinearConstraint:
322+
value.append(fun(states[:,i], inputs[:,i]))
221323
else:
222324
raise TypeError("unknown constraint type %s" %
223325
constraint[0])
224326

225327
# Return the value of the constraint function
226328
return np.hstack(value)
227329

330+
228331
# Allow optctrl(x) as a replacement for optctrl.mpc(x)
229332
def __call__(self, x, squeeze=None):
230333
"""Compute the optimal input at state x"""
@@ -250,7 +353,9 @@ def compute_trajectory(
250353

251354
# See if we got an answer
252355
if not res.success:
253-
warnings.warn(res.message)
356+
warnings.warn(
357+
"unable to solve optimal control problem\n"
358+
"scipy.optimize.minimize returned " + res.message, UserWarning)
254359
return None
255360

256361
# Reshape the input vector
@@ -309,7 +414,7 @@ def state_poly_constraint(sys, A, b):
309414
# Return a linear constraint object based on the polynomial
310415
return (opt.LinearConstraint,
311416
np.hstack([A, np.zeros((A.shape[0], sys.ninputs))]),
312-
np.full(A.shape[0], -np.inf), polytope.b)
417+
np.full(A.shape[0], -np.inf), b)
313418

314419

315420
def state_range_constraint(sys, lb, ub):

‎control/tests/obc_test.py‎

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
import control.obc as obc
1313
from control.tests.conftest import slycotonly
1414

15+
1516
def test_finite_horizon_mpc_simple():
1617
# Define a linear system with constraints
1718
# Source: https://www.mpt3.org/UI/RegulationProblem
@@ -139,3 +140,109 @@ def test_mpc_iosystem():
139140

140141
# Make sure the system converged to the desired state
141142
np.testing.assert_almost_equal(xout[0:sys.nstates, -1], xd, decimal=1)
143+
144+
145+
# Test various constraint combinations; need to use a somewhat convoluted
146+
# parametrization due to the need to define sys instead the test function
147+
@pytest.mark.parametrize("constraint_list", [
148+
[(sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -1], [5, 5, 1],)],
149+
[(obc.state_range_constraint, [-5, -5], [5, 5]),
150+
(obc.input_range_constraint, [-1], [1])],
151+
[(obc.state_range_constraint, [-5, -5], [5, 5]),
152+
(obc.input_poly_constraint, np.array([[1], [-1]]), [1, 1])],
153+
[(obc.state_poly_constraint,
154+
np.array([[1, 0], [0, 1], [-1, 0], [0, -1]]), [5, 5, 5, 5]),
155+
(obc.input_poly_constraint, np.array([[1], [-1]]), [1, 1])],
156+
[(sp.optimize.NonlinearConstraint,
157+
lambda x, u: np.array([abs(x[0]), x[1], u[0]**2]),
158+
[-np.inf, -5, -1e-12], [5, 5, 1],)], # -1e-12 for SciPy bug?
159+
])
160+
def test_constraint_specification(constraint_list):
161+
sys = ct.ss2io(ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1))
162+
163+
"""Test out different forms of constraints on a simple problem"""
164+
# Parse out the constraint
165+
constraints = []
166+
for constraint_setup in constraint_list:
167+
if constraint_setup[0] in \
168+
(sp.optimize.LinearConstraint, sp.optimize.NonlinearConstraint):
169+
# No processing required
170+
constraints.append(constraint_setup)
171+
else:
172+
# Call the function in the first argument to set up the constraint
173+
constraints.append(constraint_setup[0](sys, *constraint_setup[1:]))
174+
175+
# Quadratic state and input penalty
176+
Q = [[1, 0], [0, 1]]
177+
R = [[1]]
178+
cost = obc.quadratic_cost(sys, Q, R)
179+
180+
# Create a model predictive controller system
181+
time = np.arange(0, 5, 1)
182+
optctrl = obc.OptimalControlProblem(sys, time, cost, constraints)
183+
184+
# Compute optimal control and compare against MPT3 solution
185+
x0 = [4, 0]
186+
t, u_openloop = optctrl.compute_trajectory(x0, squeeze=True)
187+
np.testing.assert_almost_equal(
188+
u_openloop, [-1, -1, 0.1393, 0.3361, -5.204e-16], decimal=3)
189+
190+
191+
def test_terminal_constraints():
192+
"""Test out the ability to handle terminal constraints"""
193+
# Discrete time "integrator" with 2 states, 2 inputs
194+
sys = ct.ss2io(ct.ss([[1, 0], [0, 1]], np.eye(2), np.eye(2), 0, True))
195+
196+
# Shortest path to a point is a line
197+
Q = np.zeros((2, 2))
198+
R = np.eye(2)
199+
cost = obc.quadratic_cost(sys, Q, R)
200+
201+
# Set up the terminal constraint to be the origin
202+
final_point = [obc.state_range_constraint(sys, [0, 0], [0, 0])]
203+
204+
# Create the optimal control problem
205+
time = np.arange(0, 5, 1)
206+
optctrl = obc.OptimalControlProblem(
207+
sys, time, cost, terminal_constraints=final_point)
208+
209+
# Find a path to the origin
210+
x0 = np.array([4, 3])
211+
t, u1, x1 = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
212+
np.testing.assert_almost_equal(x1[:,-1], 0)
213+
214+
# Make sure it is a straight line
215+
np.testing.assert_almost_equal(
216+
x1, np.kron(x0.reshape((2, 1)), time[::-1]/4))
217+
218+
# Impose some cost on the state, which should change the path
219+
Q = np.eye(2)
220+
R = np.eye(2) * 0.1
221+
cost = obc.quadratic_cost(sys, Q, R)
222+
optctrl = obc.OptimalControlProblem(
223+
sys, time, cost, terminal_constraints=final_point)
224+
225+
# Find a path to the origin
226+
t, u2, x2 = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
227+
np.testing.assert_almost_equal(x2[:,-1], 0)
228+
229+
# Make sure that it is *not* a straight line path
230+
assert np.any(np.abs(x2 - x1) > 0.1)
231+
assert np.any(np.abs(u2) > 1) # To make sure next test is useful
232+
233+
# Add some bounds on the inputs
234+
constraints = [obc.input_range_constraint(sys, [-1, -1], [1, 1])]
235+
optctrl = obc.OptimalControlProblem(
236+
sys, time, cost, constraints, terminal_constraints=final_point)
237+
t, u3, x3 = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
238+
np.testing.assert_almost_equal(x2[:,-1], 0)
239+
240+
# Make sure we got a new path and didn't violate the constraints
241+
assert np.any(np.abs(x3 - x1) > 0.1)
242+
np.testing.assert_array_less(np.abs(u3), 1 + 1e-12)
243+
244+
# Make sure that infeasible problems are handled sensibly
245+
x0 = np.array([10, 3])
246+
with pytest.warns(UserWarning, match="unable to solve"):
247+
res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
248+
assert res == None

0 commit comments

Comments
 (0)