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 time_steering_terminal_cost():
# Define cost and constraints
traj_cost = opt.quadratic_cost(vehicle, None, np.diag([0.1, 1]), u0=uf)
term_cost = opt.quadratic_cost(vehicle, np.diag([1, 10, 10]), None, x0=xf)
constraints = [opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1])]
res = opt.solve_ocp(
vehicle,
horizon,
x0,
traj_cost,
constraints,
terminal_cost=term_cost,
initial_guess=bend_left,
print_summary=False,
solve_ivp_kwargs={
'atol': 1e-4,
'rtol': 1e-2
},
# minimize_method='SLSQP', minimize_options={'eps': 0.01}
minimize_method='trust-constr',
minimize_options={'finite_diff_rel_step': 0.01},
)
# Only count this as a benchmark if we converged
assert res.success
def time_steering_bezier_basis(nbasis, ntimes):
# Set up costs and constriants
Q = np.diag([.1, 10, .1]) # keep lateral error low
R = np.diag([1, 1]) # minimize applied inputs
cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
constraints = [opt.input_range_constraint(vehicle, [0, -0.1], [20, 0.1])]
terminal = [opt.state_range_constraint(vehicle, xf, xf)]
# Set up horizon
horizon = np.linspace(0, Tf, ntimes, endpoint=True)
# Set up the optimal control problem
res = opt.solve_ocp(
vehicle,
horizon,
x0,
cost,
constraints,
terminal_constraints=terminal,
initial_guess=bend_left,
basis=flat.BezierFamily(nbasis, T=Tf),
# solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2},
minimize_method='trust-constr',
minimize_options={'finite_diff_rel_step': 0.01},
# minimize_method='SLSQP', minimize_options={'eps': 0.01},
return_states=True,
print_summary=False)
t, u, x = res.time, res.inputs, res.states
# Make sure we found a valid solution
assert res.success
def time_steering_terminal_constraint(integrator_name, minimizer_name):
# Get the integrator and minimizer parameters to use
integrator = integrator_table[integrator_name]
minimizer = minimizer_table[minimizer_name]
# Input cost and terminal constraints
R = np.diag([1, 1]) # minimize applied inputs
cost = opt.quadratic_cost(vehicle, np.zeros((3, 3)), R, u0=uf)
constraints = [opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1])]
terminal = [opt.state_range_constraint(vehicle, xf, xf)]
res = opt.solve_ocp(
vehicle,
horizon,
x0,
cost,
constraints,
terminal_constraints=terminal,
initial_guess=bend_left,
log=False,
solve_ivp_method=integrator[0],
solve_ivp_kwargs=integrator[1],
minimize_method=minimizer[0],
minimize_options=minimizer[1],
)
# Only count this as a benchmark if we converged
assert res.success
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 time_steering_cost():
# Define cost and constraints
traj_cost = opt.quadratic_cost(vehicle, None, np.diag([0.1, 1]), u0=uf)
constraints = [opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1])]
traj = flat.point_to_point(vehicle,
timepts,
x0,
u0,
xf,
uf,
cost=traj_cost,
constraints=constraints,
basis=flat.PolyFamily(8))
# Verify that the trajectory computation is correct
x, u = traj.eval([0, Tf])
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, 1])
np.testing.assert_array_almost_equal(uf, u[:, 1])
def test_point_to_point_errors(self):
"""Test error and warning conditions in point_to_point()"""
# Double integrator system
sys = ct.ss([[0, 1], [0, 0]], [[0], [1]], [[1, 0]], 0)
flat_sys = fs.LinearFlatSystem(sys)
# Define the endpoints of the trajectory
x0 = [1, 0]
u0 = [0]
xf = [0, 0]
uf = [0]
Tf = 10
T = np.linspace(0, Tf, 500)
# Cost function
timepts = np.linspace(0, Tf, 10)
cost_fcn = opt.quadratic_cost(flat_sys,
np.diag([1, 1]),
1,
x0=xf,
u0=uf)
# Solving without basis specified should be OK
traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf, uf)
x, u = traj.eval(timepts)
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])
# Adding a cost function generates a warning
with pytest.warns(UserWarning, match="optimization not possible"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn)
# Make sure we still solved the problem
x, u = traj.eval(timepts)
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])
# Try to optimize with insufficient degrees of freedom
with pytest.warns(UserWarning, match="optimization not possible"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(6))
# Make sure we still solved the problem
x, u = traj.eval(timepts)
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])
# Solve with the errors in the various input arguments
with pytest.raises(ValueError, match="Initial state: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, np.zeros(3), u0, xf,
uf)
with pytest.raises(ValueError, match="Initial input: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, x0, np.zeros(3), xf,
uf)
with pytest.raises(ValueError, match="Final state: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, x0, u0, np.zeros(3),
uf)
with pytest.raises(ValueError, match="Final input: Wrong shape"):
traj = fs.point_to_point(flat_sys, timepts, x0, u0, xf,
np.zeros(3))
# Different ways of describing constraints
constraint = opt.input_range_constraint(flat_sys, -100, 100)
with pytest.warns(UserWarning, match="optimization not possible"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=constraint,
basis=fs.PolyFamily(6))
x, u = traj.eval(timepts)
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, -1])
np.testing.assert_array_almost_equal(uf, u[:, -1])
# Constraint that isn't a constraint
with pytest.raises(TypeError, match="must be a list"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=np.eye(2),
basis=fs.PolyFamily(8))
# Unknown constraint type
with pytest.raises(TypeError, match="unknown constraint type"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=[(None, 0, 0, 0)],
basis=fs.PolyFamily(8))
# Unsolvable optimization
constraint = [opt.input_range_constraint(flat_sys, -0.01, 0.01)]
with pytest.raises(RuntimeError, match="Unable to solve optimal"):
traj = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
constraints=constraint,
basis=fs.PolyFamily(8))
# Method arguments, parameters
traj_method = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(8),
minimize_method='slsqp')
traj_kwarg = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
cost=cost_fcn,
basis=fs.PolyFamily(8),
minimize_kwargs={'method': 'slsqp'})
np.testing.assert_allclose(traj_method.eval(timepts)[0],
traj_kwarg.eval(timepts)[0],
atol=1e-5)
# Unrecognized keywords
with pytest.raises(TypeError, match="unrecognized keyword"):
traj_method = fs.point_to_point(flat_sys,
timepts,
x0,
u0,
xf,
uf,
solve_ivp_method=None)
bezier = fs.BezierFamily(8)
traj2 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=bezier)
plot_vehicle_lanechange(traj2)
# ### Added cost function
# In[ ]:
timepts = np.linspace(0, Tf, 12)
poly = fs.PolyFamily(8)
traj_cost = opt.quadratic_cost(vehicle_flat,
np.diag([0, 0.1, 0]),
np.diag([0.1, 10]),
x0=xf,
u0=uf)
constraints = [opt.input_range_constraint(vehicle_flat, [8, -0.1], [12, 0.1])]
traj3 = fs.point_to_point(vehicle_flat,
timepts,
x0,
u0,
xf,
uf,
cost=traj_cost,
basis=poly)
plot_vehicle_lanechange(traj3)
#
#######################
# TODO just be a line
#######################
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
def test_optimal_doc():
"""Test optimal control problem from documentation"""
def vehicle_update(t, x, u, params):
# Get the parameters for the model
l = params.get('wheelbase', 3.) # vehicle wheelbase
phimax = params.get('maxsteer', 0.5) # max steering angle (rad)
# Saturate the steering input
phi = np.clip(u[1], -phimax, phimax)
# Return the derivative of the state
return np.array([
np.cos(x[2]) * u[0], # xdot = cos(theta) v
np.sin(x[2]) * u[0], # ydot = sin(theta) v
(u[0] / l) * np.tan(phi) # thdot = v/l tan(phi)
])
def vehicle_output(t, x, u, params):
return x # return x, y, theta (full state)
# Define the vehicle steering dynamics as an input/output system
vehicle = ct.NonlinearIOSystem(vehicle_update,
vehicle_output,
states=3,
name='vehicle',
inputs=('v', 'phi'),
outputs=('x', 'y', 'theta'))
# Define the initial and final points and time interval
x0 = [0., -2., 0.]
u0 = [10., 0.]
xf = [100., 2., 0.]
uf = [10., 0.]
Tf = 10
# Define the cost functions
Q = np.diag([0, 0, 0.1]) # don't turn too sharply
R = np.diag([1, 1]) # keep inputs small
P = np.diag([1000, 1000, 1000]) # get close to final point
traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
# Define the constraints
constraints = [opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1])]
# Solve the optimal control problem
horizon = np.linspace(0, Tf, 3, endpoint=True)
result = opt.solve_ocp(vehicle,
horizon,
x0,
traj_cost,
constraints,
terminal_cost=term_cost,
initial_guess=u0)
# Make sure the resulting trajectory generate a good solution
resp = ct.input_output_response(vehicle,
horizon,
result.inputs,
x0,
t_eval=np.linspace(0, Tf, 10))
t, y = resp
assert (y[0, -1] - xf[0]) / xf[0] < 0.01
assert (y[1, -1] - xf[1]) / xf[1] < 0.01
assert y[2, -1] < 0.1
