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