def test_duplicates(self): nlios = ios.NonlinearIOSystem(lambda t,x,u,params: x, \ lambda t, x, u, params: u*u, \ inputs=1, outputs=1, states=1, name="sys") # Turn off deprecation warnings warnings.simplefilter("ignore", category=DeprecationWarning) warnings.simplefilter("ignore", category=PendingDeprecationWarning) # Duplicate objects with warnings.catch_warnings(record=True) as warnval: # Trigger a warning ios_series = nlios * nlios # Verify that we got a warning self.assertEqual(len(warnval), 1) self.assertTrue(issubclass(warnval[-1].category, UserWarning)) self.assertTrue("Duplicate object" in str(warnval[-1].message)) # Nonduplicate objects nlios1 = nlios.copy() nlios2 = nlios.copy() with warnings.catch_warnings(record=True) as warnval: ios_series = nlios1 * nlios2 self.assertEquals(len(warnval), 1) # when subsystems have the same name, duplicates are # renamed <subsysname_i> self.assertTrue("copy of sys_1.x[0]" in ios_series.state_index.keys()) self.assertTrue("copy of sys.x[0]" in ios_series.state_index.keys()) # Duplicate names iosys_siso = ct.LinearIOSystem(self.siso_linsys) nlios1 = ios.NonlinearIOSystem(None, \ lambda t, x, u, params: u*u, inputs=1, outputs=1, name="sys") nlios2 = ios.NonlinearIOSystem(None, \ lambda t, x, u, params: u*u, inputs=1, outputs=1, name="sys") with warnings.catch_warnings(record=True) as warnval: # Trigger a warning iosys = ct.InterconnectedSystem( (nlios1, iosys_siso, nlios2), inputs=0, outputs=0, states=0) # Verify that we got a warning self.assertEqual(len(warnval), 1) self.assertTrue(issubclass(warnval[-1].category, UserWarning)) self.assertTrue("Duplicate name" in str(warnval[-1].message)) # Same system, different names => everything should be OK nlios1 = ios.NonlinearIOSystem(None, \ lambda t, x, u, params: u*u, inputs=1, outputs=1, name="nlios1") nlios2 = ios.NonlinearIOSystem(None, \ lambda t, x, u, params: u*u, inputs=1, outputs=1, name="nlios2") with warnings.catch_warnings(record=True) as warnval: iosys = ct.InterconnectedSystem( (nlios1, iosys_siso, nlios2), inputs=0, outputs=0, states=0) self.assertEqual(len(warnval), 0)
def test_interconnect_exceptions(): # First make sure the docstring example works P = ct.tf2io(ct.tf(1, [1, 0]), input='u', output='y') C = ct.tf2io(ct.tf(10, [1, 1]), input='e', output='u') sumblk = ct.summing_junction(inputs=['r', '-y'], output='e') T = ct.interconnect((P, C, sumblk), input='r', output='y') assert (T.ninputs, T.noutputs, T.nstates) == (1, 1, 2) # Unrecognized arguments # LinearIOSystem with pytest.raises(TypeError, match="unknown parameter"): P = ct.LinearIOSystem(ct.rss(2, 1, 1), output_name='y') # Interconnect with pytest.raises(TypeError, match="unknown parameter"): T = ct.interconnect((P, C, sumblk), input_name='r', output='y') # Interconnected system with pytest.raises(TypeError, match="unknown parameter"): T = ct.InterconnectedSystem((P, C, sumblk), input_name='r', output='y') # NonlinearIOSytem with pytest.raises(TypeError, match="unknown parameter"): nlios = ct.NonlinearIOSystem( None, lambda t, x, u, params: u*u, input_count=1, output_count=1) # Summing junction with pytest.raises(TypeError, match="input specification is required"): sumblk = ct.summing_junction() with pytest.raises(TypeError, match="unknown parameter"): sumblk = ct.summing_junction(input_count=2, output_count=2)
def closeLoop(tf,k_p,h_multiplier=1): H=control.TransferFunction([h_multiplier],[1]) H=control.tf2io(H) H.name='H' H.set_inputs(['in']) H.set_outputs(['out']) D=control.TransferFunction([k_p],[1]) D=control.tf2io(D) D.name='D' D.set_inputs(['in']) D.set_outputs(['out']) system=control.tf2io(tf) system.name='system' system.set_inputs(['in']) system.set_outputs(['out']) # + # in ---->o---->--D-->system----> out # |- | # -------H------------ system_MF = control.InterconnectedSystem([H,D,system] ,name='system_MF', connections=[ ['H.in','system.out'], ['D.in','-H.out'], ['system.in','D.out'], ], inplist=['D.in'], inputs=['in'], outlist=['system.out','D.out','-H.out'], outputs=['out','control','error']) return system_MF
outputs=('u', 'j_hat', 'b_hat'), states=3, name='control', dt=0 ) # Define the closed-loop system # x_cl = [plant.x, control.x[0], control.x[1], control.x[2]] # = [motorx, j_hat, b_hat, e_i] # u_cl = [xd, xddot] # y_cl = [motorx, Jhat, Bhat] IO_CLOSED = control.InterconnectedSystem( (IO_DC_MOTOR, IO_ADAPTIVE_PI), connections=( ('plant.u', 'control.u'), ('control.u[2]', 'plant.x') ), inplist=('control.u[0]', 'control.u[1]'), outlist=('plant.x', 'control.j_hat', 'control.b_hat', 'control.u'), dt=0 ) # Set simulation duration and time steps N_POINTS = 3000 T_F = 60 # Set initial conditions X0 = np.zeros((4, 1)) X0[0] = 1 X0[1] = J_HAT_0 X0[2] = B_HAT_0
# # We construct the system using the InterconnectedSystem constructor and using # signal labels to keep track of everything. steering = ct.InterconnectedSystem( # List of subsystems (trajgen, controller, vehicle), name='steering', # Interconnections between subsystems connections=(('controller.ex', 'trajgen.xd', '-vehicle.x'), ('controller.ey', 'trajgen.yd', '-vehicle.y'), ('controller.etheta', 'trajgen.thetad', '-vehicle.theta'), ('controller.vd', 'trajgen.vd'), ('controller.phid', 'trajgen.phid'), ('vehicle.v', 'controller.v'), ('vehicle.phi', 'controller.phi')), # System inputs inplist=['trajgen.vref', 'trajgen.yref'], inputs=['yref', 'vref'], # System outputs outlist=[ 'vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v', 'controller.phi' ], outputs=['x', 'y', 'theta', 'v', 'phi']) # Set up the simulation conditions yref = 1 T = np.linspace(0, 5, 100)
outputs=('y_r', 'psi_ref', 'delta_ref', 'v_r')) ############################################################################### # System Connect ############################################################################### LatSlidingModeControl = ct.InterconnectedSystem( # List of subsystems (target, controller, vehicle), name='LatSlidingModeControl', # Interconnections between subsystems connections=( ('target.x_ref', 'vehicle.x'), ('vehicle.v', 'target.v_r'), ('controller.e_y', 'target.y_r', '-vehicle.y'), ('controller.psi_r', 'target.psi_ref'), ('controller.psi', 'vehicle.psi'), ('vehicle.delta_rate', 'controller.delta_rate'), ), # System inputs inplist=['target.v_ref'], inputs=['vref'], # System outputs outlist=['vehicle.x', 'vehicle.y', 'vehicle.psi', 'vehicle.delta'], outputs=['x', 'y', 'psi', 'delta']) ############################################################################### # Input Output Response ############################################################################### # time of response
adaptive_output, inputs=3, outputs=('u', 'theta', 'k', 'e', 'delta_u', 'u_c', 'e_delta', 'e_u'), states=4, name='control', dt=0) # Define the closed-loop system # x_cl = [plant.x_p, ref_model.x_m, control.theta, control.k, control.beta_delta, control.e_delta] IO_CLOSED = control.InterconnectedSystem( (IO_PLANT, IO_REF_MODEL, IO_ADAPTIVE), connections=(('plant.u', 'control.u'), ('control.u[1]', 'plant.x_p'), ('control.u[2]', 'ref_model.x_m')), inplist=('ref_model.r', 'control.u[0]'), outlist=('plant.x_p', 'ref_model.x_m', 'control.u', 'control.theta', 'control.k', 'control.e', 'control.u_c', 'control.e_delta', 'control.e_u'), dt=0) # Set simulation duration and time steps N_POINTS = 3000 T_F = 20 # Set initial conditions X0 = np.zeros((6, 1)) X0[0] = X_P_0 X0[1] = X_M_0 X0[2] = THETA_0 X0[3] = K_0
# System Connect ############################################################################### LatRearWheelFeedbackControl = ct.InterconnectedSystem( # List of subsystems (target, controller, vehicle), name='LatRearWheelFeedbackControl', # Interconnections between subsystems connections=( ('target.x','vehicle.x'), ('target.y','vehicle.y'), ('controller.e_x','vehicle.x','-target.x_r'), # e_x:x轴方向偏差 ('controller.e_y','vehicle.y','-target.y_r'), # e_y:y轴方向偏差 ('controller.e_yaw','vehicle.yaw','-target.yaw_r'), # e_yaw:偏航角偏差 ('controller.yaw_ref','target.yaw_r'), ('controller.k_ref','target.k_r'), ('controller.v_ref','target.v_r'), ('vehicle.delta', 'controller.delta'), ('vehicle.v', 'controller.v') ), # System inputs inplist=['target.v_ref'], inputs=['v_ref'], # System outputs outlist=['vehicle.x', 'vehicle.y' , 'vehicle.yaw','controller.delta','target.yaw_r'], outputs=['x', 'y', 'psi', 'delta', 'psi_ref'] ) ###############################################################################
# Construct a PI controller with rolloff, as a transfer function Kp = 0.5 # proportional gain Ki = 0.1 # integral gain control_tf = ct.tf2io( ct.TransferFunction([Kp, Ki], [1, 0.01*Ki/Kp]), name='control', inputs='u', outputs='y') # Construct the closed loop control system # Inputs: vref, gear, theta # Outputs: v (vehicle velocity) cruise_tf = ct.InterconnectedSystem( (control_tf, vehicle), name='cruise', connections=( ['control.u', '-vehicle.v'], ['vehicle.u', 'control.y']), inplist=('control.u', 'vehicle.gear', 'vehicle.theta'), inputs=('vref', 'gear', 'theta'), outlist=('vehicle.v', 'vehicle.u'), outputs=('v', 'u')) # Define the time and input vectors T = np.linspace(0, 25, 101) vref = 20 * np.ones(T.shape) gear = 4 * np.ones(T.shape) theta0 = np.zeros(T.shape) # Now simulate the effect of a hill at t = 5 seconds plt.figure() plt.suptitle('Response to change in road slope') vel_axes = plt.subplot(2, 1, 1)
# x_cl = [plant[5], reference[2], input_filter[4], output_filter[4], controller[12]] IO_CLOSED = control.InterconnectedSystem( (IO_PLANT, IO_REF_MODEL, IO_INPUT_FILTER, IO_OUTPUT_FILTER, IO_ADAPTIVE), connections=(('plant.u[0]', 'control.y[0]'), ('plant.u[1]', 'control.y[1]'), ('input_filter.u[0]', 'control.y[0]'), ('input_filter.u[1]', 'control.y[1]'), ('output_filter.u[0]', 'plant.y[0]'), ('output_filter.u[1]', 'plant.y[1]'), ('control.u[2]', 'input_filter.y[0]'), ('control.u[3]', 'input_filter.y[1]'), ('control.u[4]', 'input_filter.y[2]'), ('control.u[5]', 'input_filter.y[3]'), ('control.u[6]', 'output_filter.y[0]'), ('control.u[7]', 'output_filter.y[1]'), ('control.u[8]', 'output_filter.y[2]'), ('control.u[9]', 'output_filter.y[3]'), ('control.u[10]', 'output_filter.y[4]'), ('control.u[11]', 'output_filter.y[5]'), ('control.u[12]', 'plant.y[0]'), ('control.u[13]', 'plant.y[1]'), ('control.u[14]', 'ref_model.y[0]'), ('control.u[15]', 'ref_model.y[1]')), inplist=('ref_model.u[0]', 'ref_model.u[1]', 'control.u[0]', 'control.u[1]'), outlist=('plant.y[0]', 'plant.y[1]', 'ref_model.y[0]', 'ref_model.y[1]'), dt=0)
name='controlLQR') io_closed = control.InterconnectedSystem( # (sysFull, controlLQR), # (sysPartKF, controlLQR), # (sysFullNonLin, sysPartKF, controlLQR), (sysFull, sysPartKF, controlLQR), connections=( # ('sysFullNonLin.u', 'controlLQR.f'), ('sysPartKF.u', 'controlLQR.f'), ('sysPartKF.uKF', 'controlLQR.f'), ('controlLQR.x', 'sysPartKF.x_h'), ('controlLQR.xdot', 'sysPartKF.xdot_h'), ('controlLQR.theta', 'sysPartKF.theta_h'), ('controlLQR.thetadot', 'sysPartKF.thetadot_h'), ('sysFull.u', 'controlLQR.f'), # ('controlLQR.x', 'sysFull.x'), # ('controlLQR.xdot', 'sysFull.xdot'), # ('controlLQR.theta', 'sysFull.theta'), # ('controlLQR.thetadot', 'sysFull.thetadot') ), inplist=[ 'sysPartKF.vd0', 'sysPartKF.vd1', 'sysPartKF.vd2', 'sysPartKF.vd3', 'sysPartKF.vn', 'sysFull.vd0', 'sysFull.vd1', 'sysFull.vd2', 'sysFull.vd3', 'sysFull.vn' ], # outlist=('sysPartKF.x_h', 'sysPartKF.xdot_h', 'sysPartKF.theta_h', 'sysPartKF.thetadot_h') # outlist=('sysFull.x', 'sysFull.xdot', 'sysFull.theta', 'sysFull.thetadot') outlist=('sysFull.x', 'sysFull.xdot', 'sysFull.theta', 'sysFull.thetadot'))