def test_ss2io(self):
# Create an input/output system from the linear system
linsys = self.siso_linsys
iosys = ct.ss2io(linsys)
np.testing.assert_array_equal(linsys.A, iosys.A)
np.testing.assert_array_equal(linsys.B, iosys.B)
np.testing.assert_array_equal(linsys.C, iosys.C)
np.testing.assert_array_equal(linsys.D, iosys.D)
# Try adding names to things
iosys_named = ct.ss2io(linsys,
inputs='u',
outputs='y',
states=['x1', 'x2'],
name='iosys_named')
self.assertEqual(iosys_named.find_input('u'), 0)
self.assertEqual(iosys_named.find_input('x'), None)
self.assertEqual(iosys_named.find_output('y'), 0)
self.assertEqual(iosys_named.find_output('u'), None)
self.assertEqual(iosys_named.find_state('x0'), None)
self.assertEqual(iosys_named.find_state('x1'), 0)
self.assertEqual(iosys_named.find_state('x2'), 1)
np.testing.assert_array_equal(linsys.A, iosys_named.A)
np.testing.assert_array_equal(linsys.B, iosys_named.B)
np.testing.assert_array_equal(linsys.C, iosys_named.C)
np.testing.assert_array_equal(linsys.D, iosys_named.D)
def test_discrete_lqr():
# oscillator model defined in 2D
# Source: https://www.mpt3.org/UI/RegulationProblem
A = [[0.5403, -0.8415], [0.8415, 0.5403]]
B = [[-0.4597], [0.8415]]
C = [[1, 0]]
D = [[0]]
# Linear discrete-time model with sample time 1
sys = ct.ss2io(ct.ss(A, B, C, D, 1))
# Include weights on states/inputs
Q = np.eye(2)
R = 1
K, S, E = ct.dlqr(A, B, Q, R)
# Compute the integral and terminal cost
integral_cost = opt.quadratic_cost(sys, Q, R)
terminal_cost = opt.quadratic_cost(sys, S, None)
# Solve the LQR problem
lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1))
# Generate a simulation of the LQR controller
time = np.arange(0, 5, 1)
x0 = np.array([1, 1])
_, _, lqr_x = ct.input_output_response(lqr_sys, time, 0, x0, return_x=True)
# Use LQR input as initial guess to avoid convergence/precision issues
lqr_u = np.array(-K @ lqr_x[0:time.size]) # convert from matrix
# Formulate the optimal control problem and compute optimal trajectory
optctrl = opt.OptimalControlProblem(sys,
time,
integral_cost,
terminal_cost=terminal_cost,
initial_guess=lqr_u)
res1 = optctrl.compute_trajectory(x0, return_states=True)
# Compare to make sure results are the same
np.testing.assert_almost_equal(res1.inputs, lqr_u[0])
np.testing.assert_almost_equal(res1.states, lqr_x)
# Add state and input constraints
trajectory_constraints = [
(sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -.5], [5, 5, 0.5]),
]
# Re-solve
res2 = opt.solve_ocp(sys,
time,
x0,
integral_cost,
trajectory_constraints,
terminal_cost=terminal_cost,
initial_guess=lqr_u)
# Make sure we got a different solution
assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def test_discrete_lqr():
# oscillator model defined in 2D
# Source: https://www.mpt3.org/UI/RegulationProblem
A = [[0.5403, -0.8415], [0.8415, 0.5403]]
B = [[-0.4597], [0.8415]]
C = [[1, 0]]
D = [[0]]
# Linear discrete-time model with sample time 1
sys = ct.ss2io(ct.ss(A, B, C, D, 1))
# Include weights on states/inputs
Q = np.eye(2)
R = 1
K, S, E = ct.lqr(A, B, Q, R) # note: *continuous* time LQR
# Compute the integral and terminal cost
integral_cost = opt.quadratic_cost(sys, Q, R)
terminal_cost = opt.quadratic_cost(sys, S, None)
# Formulate finite horizon MPC problem
time = np.arange(0, 5, 1)
x0 = np.array([1, 1])
optctrl = opt.OptimalControlProblem(sys,
time,
integral_cost,
terminal_cost=terminal_cost)
res1 = optctrl.compute_trajectory(x0, return_states=True)
with pytest.xfail("discrete LQR not implemented"):
# Result should match LQR
K, S, E = ct.dlqr(A, B, Q, R)
lqr_sys = ct.ss2io(ct.ss(A - B @ K, B, C, D, 1))
_, _, lqr_x = ct.input_output_response(lqr_sys,
time,
0,
x0,
return_x=True)
np.testing.assert_almost_equal(res1.states, lqr_x)
# Add state and input constraints
trajectory_constraints = [
(sp.optimize.LinearConstraint, np.eye(3), [-10, -10, -1], [10, 10, 1]),
]
# Re-solve
res2 = opt.solve_ocp(sys,
time,
x0,
integral_cost,
constraints,
terminal_cost=terminal_cost)
# Make sure we got a different solution
assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
def test_mpc_iosystem():
# model of an aircraft discretized with 0.2s sampling time
# Source: https://www.mpt3.org/UI/RegulationProblem
A = [[0.99, 0.01, 0.18, -0.09, 0],
[ 0, 0.94, 0, 0.29, 0],
[ 0, 0.14, 0.81, -0.9, 0],
[ 0, -0.2, 0, 0.95, 0],
[ 0, 0.09, 0, 0, 0.9]]
B = [[ 0.01, -0.02],
[-0.14, 0],
[ 0.05, -0.2],
[ 0.02, 0],
[-0.01, 0]]
C = [[0, 1, 0, 0, -1],
[0, 0, 1, 0, 0],
[0, 0, 0, 1, 0],
[1, 0, 0, 0, 0]]
model = ct.ss2io(ct.ss(A, B, C, 0, 0.2))
# For the simulation we need the full state output
sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2))
# compute the steady state values for a particular value of the input
ud = np.array([0.8, -0.3])
xd = np.linalg.inv(np.eye(5) - A) @ B @ ud
yd = C @ xd
# provide constraints on the system signals
constraints = [opt.input_range_constraint(sys, [-5, -6], [5, 6])]
# provide penalties on the system signals
Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C
R = np.diag([3, 2])
cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud)
# online MPC controller object is constructed with a horizon 6
ctrl = opt.create_mpc_iosystem(
model, np.arange(0, 6) * 0.2, cost, constraints)
# Define an I/O system implementing model predictive control
loop = ct.feedback(sys, ctrl, 1)
# Choose a nearby initial condition to speed up computation
X0 = np.hstack([xd, np.kron(ud, np.ones(6))]) * 0.99
Nsim = 12
tout, xout = ct.input_output_response(
loop, np.arange(0, Nsim) * 0.2, 0, X0)
# Make sure the system converged to the desired state
np.testing.assert_allclose(
xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01)
def test_ocp_argument_errors():
sys = ct.ss2io(ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1))
# State and input constraints
constraints = [
(sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -1], [5, 5, 1]),
]
# Quadratic state and input penalty
Q = [[1, 0], [0, 1]]
R = [[1]]
cost = opt.quadratic_cost(sys, Q, R)
# Set up the optimal control problem
time = np.arange(0, 5, 1)
x0 = [4, 0]
# Trajectory constraints not in the right form
with pytest.raises(TypeError, match="constraints must be a list"):
res = opt.solve_ocp(sys, time, x0, cost, np.eye(2))
# Terminal constraints not in the right form
with pytest.raises(TypeError, match="constraints must be a list"):
res = opt.solve_ocp(
sys, time, x0, cost, constraints, terminal_constraints=np.eye(2))
# Initial guess in the wrong shape
with pytest.raises(ValueError, match="initial guess is the wrong shape"):
res = opt.solve_ocp(
sys, time, x0, cost, constraints, initial_guess=np.zeros((4,1,1)))
def test_constraint_specification(constraint_list):
sys = ct.ss2io(ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1))
"""Test out different forms of constraints on a simple problem"""
# Parse out the constraint
constraints = []
for constraint_setup in constraint_list:
if constraint_setup[0] in \
(sp.optimize.LinearConstraint, sp.optimize.NonlinearConstraint):
# No processing required
constraints.append(constraint_setup)
else:
# Call the function in the first argument to set up the constraint
constraints.append(constraint_setup[0](sys, *constraint_setup[1:]))
# Quadratic state and input penalty
Q = [[1, 0], [0, 1]]
R = [[1]]
cost = opt.quadratic_cost(sys, Q, R)
# Create a model predictive controller system
time = np.arange(0, 5, 1)
optctrl = opt.OptimalControlProblem(sys, time, cost, constraints)
# Compute optimal control and compare against MPT3 solution
x0 = [4, 0]
res = optctrl.compute_trajectory(x0, squeeze=True)
t, u_openloop = res.time, res.inputs
np.testing.assert_almost_equal(
u_openloop, [-1, -1, 0.1393, 0.3361, -5.204e-16], decimal=3)
def test_finite_horizon_simple():
# Define a linear system with constraints
# Source: https://www.mpt3.org/UI/RegulationProblem
# LTI prediction model
sys = ct.ss2io(ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1))
# State and input constraints
constraints = [
(sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -1], [5, 5, 1]),
]
# Quadratic state and input penalty
Q = [[1, 0], [0, 1]]
R = [[1]]
cost = opt.quadratic_cost(sys, Q, R)
# Set up the optimal control problem
time = np.arange(0, 5, 1)
x0 = [4, 0]
# Retrieve the full open-loop predictions
res = opt.solve_ocp(
sys, time, x0, cost, constraints, squeeze=True)
t, u_openloop = res.time, res.inputs
np.testing.assert_almost_equal(
u_openloop, [-1, -1, 0.1393, 0.3361, -5.204e-16], decimal=4)
def test_optimal_logging(capsys):
"""Test logging functions (mainly for code coverage)"""
sys = ct.ss2io(ct.ss(np.eye(2), np.eye(2), np.eye(2), 0, 1))
# Set up the optimal control problem
cost = opt.quadratic_cost(sys, 1, 1)
state_constraint = opt.state_range_constraint(sys, [-np.inf, 1], [10, 1])
input_constraint = opt.input_range_constraint(sys, [-100, -100],
[100, 100])
time = np.arange(0, 3, 1)
x0 = [-1, 1]
# Solve it, with logging turned on (with warning due to mixed constraints)
with pytest.warns(sp.optimize.optimize.OptimizeWarning,
match="Equality and inequality .* same element"):
res = opt.solve_ocp(sys,
time,
x0,
cost,
input_constraint,
terminal_cost=cost,
terminal_constraints=state_constraint,
log=True)
# Make sure the output has info available only with logging turned on
captured = capsys.readouterr()
assert captured.out.find("process time") != -1
def test_equality_constraints():
"""Test out the ability to handle equality constraints"""
# Create the system (double integrator, continuous time)
sys = ct.ss2io(ct.ss(np.zeros((2, 2)), np.eye(2), np.eye(2), 0))
# Shortest path to a point is a line
Q = np.zeros((2, 2))
R = np.eye(2)
cost = opt.quadratic_cost(sys, Q, R)
# Set up the terminal constraint to be the origin
final_point = [opt.state_range_constraint(sys, [0, 0], [0, 0])]
# Create the optimal control problem
time = np.arange(0, 3, 1)
optctrl = opt.OptimalControlProblem(sys,
time,
cost,
terminal_constraints=final_point)
# Find a path to the origin
x0 = np.array([4, 3])
res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
t, u1, x1 = res.time, res.inputs, res.states
# Bug prior to SciPy 1.6 will result in incorrect results
if NumpyVersion(sp.__version__) < '1.6.0':
pytest.xfail("SciPy 1.6 or higher required")
np.testing.assert_almost_equal(x1[:, -1], 0, decimal=4)
# Set up terminal constraints as a nonlinear constraint
def final_point_eval(x, u):
return x
final_point = [(sp.optimize.NonlinearConstraint, final_point_eval, [0, 0],
[0, 0])]
optctrl = opt.OptimalControlProblem(sys,
time,
cost,
terminal_constraints=final_point)
# Find a path to the origin
x0 = np.array([4, 3])
res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
t, u2, x2 = res.time, res.inputs, res.states
np.testing.assert_almost_equal(x2[:, -1], 0, decimal=4)
np.testing.assert_almost_equal(u1, u2)
np.testing.assert_almost_equal(x1, x2)
# Try passing and unknown constraint type
final_point = [(None, final_point_eval, [0, 0], [0, 0])]
with pytest.raises(TypeError, match="unknown constraint type"):
optctrl = opt.OptimalControlProblem(sys,
time,
cost,
terminal_constraints=final_point)
res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
def test_optimal_basis_simple():
sys = ct.ss2io(ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1))
# State and input constraints
constraints = [
(sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -1], [5, 5, 1]),
]
# Quadratic state and input penalty
Q = [[1, 0], [0, 1]]
R = [[1]]
cost = opt.quadratic_cost(sys, Q, R)
# Set up the optimal control problem
Tf = 5
time = np.arange(0, Tf, 1)
x0 = [4, 0]
# Basic optimal control problem
res1 = opt.solve_ocp(sys,
time,
x0,
cost,
constraints,
basis=flat.BezierFamily(4, Tf),
return_x=True)
assert res1.success
# Make sure the constraints were satisfied
np.testing.assert_array_less(np.abs(res1.states[0]), 5 + 1e-6)
np.testing.assert_array_less(np.abs(res1.states[1]), 5 + 1e-6)
np.testing.assert_array_less(np.abs(res1.inputs[0]), 1 + 1e-6)
# Pass an initial guess and rerun
res2 = opt.solve_ocp(sys,
time,
x0,
cost,
constraints,
initial_guess=0.99 * res1.inputs,
basis=flat.BezierFamily(4, Tf),
return_x=True)
assert res2.success
np.testing.assert_allclose(res2.inputs, res1.inputs, atol=0.01, rtol=0.01)
# Run with logging turned on for code coverage
res3 = opt.solve_ocp(sys,
time,
x0,
cost,
constraints,
basis=flat.BezierFamily(4, Tf),
return_x=True,
log=True)
assert res3.success
np.testing.assert_almost_equal(res3.inputs, res1.inputs, decimal=3)
def test_change_default_dt_static(self):
"""Test that static gain systems always have dt=None"""
ct.set_defaults('control', default_dt=0)
assert ct.tf(1, 1).dt is None
assert ct.ss([], [], [], 1).dt is None
# Make sure static gain is preserved for the I/O system
sys = ct.ss([], [], [], 1)
sys_io = ct.ss2io(sys)
assert sys_io.dt is None
def test_iosys_print(self):
# Send the output to /dev/null
import os
f = open(os.devnull,"w")
# Simple I/O system
iosys = ct.ss2io(self.siso_linsys)
print(iosys, file=f)
# I/O system without ninputs, noutputs
ios_unspecified = ios.NonlinearIOSystem(secord_update, secord_output)
print(ios_unspecified, file=f)
# I/O system with derived inputs and outputs
ios_linearized = ios.linearize(ios_unspecified, [0, 0], [0])
print(ios_linearized, file=f)
f.close()
def test_interconnect_implicit():
"""Test the use of implicit connections in interconnect()"""
import random
# System definition
P = ct.ss2io(
ct.rss(2, 1, 1, strictly_proper=True),
inputs='u', outputs='y', name='P')
kp = ct.tf(random.uniform(1, 10), [1])
ki = ct.tf(random.uniform(1, 10), [1, 0])
C = ct.tf2io(kp + ki, inputs='e', outputs='u', name='C')
# Block diagram computation
Tss = ct.feedback(P * C, 1)
# Construct the interconnection explicitly
Tio_exp = ct.interconnect(
(C, P),
connections = [['P.u', 'C.u'], ['C.e', '-P.y']],
inplist='C.e', outlist='P.y')
# Compare to bdalg computation
np.testing.assert_almost_equal(Tio_exp.A, Tss.A)
np.testing.assert_almost_equal(Tio_exp.B, Tss.B)
np.testing.assert_almost_equal(Tio_exp.C, Tss.C)
np.testing.assert_almost_equal(Tio_exp.D, Tss.D)
# Construct the interconnection via a summing junction
sumblk = ct.summing_junction(inputs=['r', '-y'], output='e', name="sum")
Tio_sum = ct.interconnect(
(C, P, sumblk), inplist=['r'], outlist=['y'])
np.testing.assert_almost_equal(Tio_sum.A, Tss.A)
np.testing.assert_almost_equal(Tio_sum.B, Tss.B)
np.testing.assert_almost_equal(Tio_sum.C, Tss.C)
np.testing.assert_almost_equal(Tio_sum.D, Tss.D)
# Setting connections to False should lead to an empty connection map
empty = ct.interconnect(
(C, P, sumblk), connections=False, inplist=['r'], outlist=['y'])
np.testing.assert_array_equal(empty.connect_map, np.zeros((4, 3)))
# Implicit summation across repeated signals
kp_io = ct.tf2io(kp, inputs='e', outputs='u', name='kp')
ki_io = ct.tf2io(ki, inputs='e', outputs='u', name='ki')
Tio_sum = ct.interconnect(
(kp_io, ki_io, P, sumblk), inplist=['r'], outlist=['y'])
np.testing.assert_almost_equal(Tio_sum.A, Tss.A)
np.testing.assert_almost_equal(Tio_sum.B, Tss.B)
np.testing.assert_almost_equal(Tio_sum.C, Tss.C)
np.testing.assert_almost_equal(Tio_sum.D, Tss.D)
# TODO: interconnect a MIMO system using implicit connections
# P = control.ss2io(
# control.rss(2, 2, 2, strictly_proper=True),
# input_prefix='u', output_prefix='y', name='P')
# C = control.ss2io(
# control.rss(2, 2, 2),
# input_prefix='e', output_prefix='u', name='C')
# sumblk = control.summing_junction(
# inputs=['r', '-y'], output='e', dimension=2)
# S = control.interconnect([P, C, sumblk], inplist='r', outlist='y')
# Make sure that repeated inplist/outlist names generate an error
# Input not unique
Cbad = ct.tf2io(ct.tf(10, [1, 1]), inputs='r', outputs='x', name='C')
with pytest.raises(ValueError, match="not unique"):
Tio_sum = ct.interconnect(
(Cbad, P, sumblk), inplist=['r'], outlist=['y'])
# Output not unique
Cbad = ct.tf2io(ct.tf(10, [1, 1]), inputs='e', outputs='y', name='C')
with pytest.raises(ValueError, match="not unique"):
Tio_sum = ct.interconnect(
(Cbad, P, sumblk), inplist=['r'], outlist=['y'])
# Signal not found
with pytest.raises(ValueError, match="could not find"):
Tio_sum = ct.interconnect(
(C, P, sumblk), inplist=['x'], outlist=['y'])
with pytest.raises(ValueError, match="could not find"):
Tio_sum = ct.interconnect(
(C, P, sumblk), inplist=['r'], outlist=['x'])
def test_constraint_constructor_errors(fun, args, exception, match):
"""Test various error conditions for constraint constructors"""
sys = ct.ss2io(ct.rss(2, 2, 2))
with pytest.raises(exception, match=match):
fun(sys, *args)
def test_terminal_constraints(sys_args):
"""Test out the ability to handle terminal constraints"""
# Create the system
sys = ct.ss2io(ct.ss(*sys_args))
# Shortest path to a point is a line
Q = np.zeros((2, 2))
R = np.eye(2)
cost = opt.quadratic_cost(sys, Q, R)
# Set up the terminal constraint to be the origin
final_point = [opt.state_range_constraint(sys, [0, 0], [0, 0])]
# Create the optimal control problem
time = np.arange(0, 3, 1)
optctrl = opt.OptimalControlProblem(
sys, time, cost, terminal_constraints=final_point)
# Find a path to the origin
x0 = np.array([4, 3])
res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
t, u1, x1 = res.time, res.inputs, res.states
# Bug prior to SciPy 1.6 will result in incorrect results
if NumpyVersion(sp.__version__) < '1.6.0':
pytest.xfail("SciPy 1.6 or higher required")
np.testing.assert_almost_equal(x1[:,-1], 0, decimal=4)
# Make sure it is a straight line
Tf = time[-1]
if ct.isctime(sys):
# Continuous time is not that accurate on the input, so just skip test
pass
else:
# Final point doesn't affect cost => don't need to test
np.testing.assert_almost_equal(
u1[:, 0:-1],
np.kron((-x0/Tf).reshape((2, 1)), np.ones(time.shape))[:, 0:-1])
np.testing.assert_allclose(
x1, np.kron(x0.reshape((2, 1)), time[::-1]/Tf), atol=0.1, rtol=0.01)
# Re-run using initial guess = optional and make sure nothing changes
res = optctrl.compute_trajectory(x0, initial_guess=u1)
np.testing.assert_almost_equal(res.inputs, u1)
# Re-run using a basis function and see if we get the same answer
res = opt.solve_ocp(sys, time, x0, cost, terminal_constraints=final_point,
basis=flat.BezierFamily(4, Tf))
np.testing.assert_almost_equal(res.inputs, u1, decimal=2)
# Impose some cost on the state, which should change the path
Q = np.eye(2)
R = np.eye(2) * 0.1
cost = opt.quadratic_cost(sys, Q, R)
optctrl = opt.OptimalControlProblem(
sys, time, cost, terminal_constraints=final_point)
# Turn off warning messages, since we sometimes don't get convergence
with warnings.catch_warnings():
warnings.filterwarnings(
"ignore", message="unable to solve", category=UserWarning)
# Find a path to the origin
res = optctrl.compute_trajectory(
x0, squeeze=True, return_x=True, initial_guess=u1)
t, u2, x2 = res.time, res.inputs, res.states
# Not all configurations are able to converge (?)
if res.success:
np.testing.assert_almost_equal(x2[:,-1], 0)
# Make sure that it is *not* a straight line path
assert np.any(np.abs(x2 - x1) > 0.1)
assert np.any(np.abs(u2) > 1) # Make sure next test is useful
# Add some bounds on the inputs
constraints = [opt.input_range_constraint(sys, [-1, -1], [1, 1])]
optctrl = opt.OptimalControlProblem(
sys, time, cost, constraints, terminal_constraints=final_point)
res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
t, u3, x3 = res.time, res.inputs, res.states
# Check the answers only if we converged
if res.success:
np.testing.assert_almost_equal(x3[:,-1], 0, decimal=4)
# Make sure we got a new path and didn't violate the constraints
assert np.any(np.abs(x3 - x1) > 0.1)
np.testing.assert_array_less(np.abs(u3), 1 + 1e-6)
# Make sure that infeasible problems are handled sensibly
x0 = np.array([10, 3])
with pytest.warns(UserWarning, match="unable to solve"):
res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
assert not res.success
