def test_discrete(self): """Test discrete time functionality""" # Create some linear and nonlinear systems to play with linsys = ct.StateSpace( [[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], [[0]], True) lnios = ios.LinearIOSystem(linsys) # Set up parameters for simulation T, U, X0 = self.T, self.U, self.X0 # Simulate and compare to LTI output ios_t, ios_y = ios.input_output_response(lnios, T, U, X0) lin_t, lin_y, lin_x = ct.forced_response(linsys, T, U, X0) np.testing.assert_array_almost_equal(ios_t, lin_t, decimal=3) np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3) # Test MIMO system, converted to discrete time linsys = ct.StateSpace(self.mimo_linsys1) linsys.dt = self.T[1] - self.T[0] lnios = ios.LinearIOSystem(linsys) # Set up parameters for simulation T = self.T U = [np.sin(T), np.cos(T)] X0 = 0 # Simulate and compare to LTI output ios_t, ios_y = ios.input_output_response(lnios, T, U, X0) lin_t, lin_y, lin_x = ct.forced_response(linsys, T, U, X0) np.testing.assert_array_almost_equal(ios_t, lin_t, decimal=3) np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
def feedback(tau: float, fb_gain: float) -> None: """Simulation of a negative feedback system, using the package 'control'. 'time', 'in_signal', 'num' and 'den' are taken from the global workspace Parameters ---------- tau : time constant of a first order lag [sec] fb_gain : feedback gain """ # First, define the feedforward transfer function sys = control.TransferFunction(num, den) print(sys) # 1 / (tau*s + 1) # Simulate the response of the feedforward system t_out, y_out, x_out = control.forced_response(sys, T=time, U=in_signal) # Then define a feedback-loop, with gain fb_gain sys_total = control.feedback(sys, fb_gain) print(sys_total) # 1 / (tau*s + (1+k)) # Simulate the response of the feedback system t_total, y_total, x_total = control.forced_response(sys_total, T=time, U=in_signal) # Show the signals plt.plot(time, in_signal, '-', label='Input') plt.plot(t_out, y_out, '--', label='Feedforward') plt.plot(t_total, y_total, '-.', label='Feedback') # Format the plot plt.xlabel('Time [sec]') plt.title(f"First order lag (tau={tau} sec)") plt.text(15, 0.8, "Simulated with 'control' ", fontsize=12) plt.legend()
def test_connect(self): # Define a couple of (linear) systems to interconnection linsys1 = self.siso_linsys iosys1 = ios.LinearIOSystem(linsys1) linsys2 = self.siso_linsys iosys2 = ios.LinearIOSystem(linsys2) # Connect systems in different ways and compare to StateSpace linsys_series = linsys2 * linsys1 iosys_series = ios.InterconnectedSystem( (iosys1, iosys2), # systems ((1, 0),), # interconnection (series) 0, # input = first system 1 # output = second system ) # Run a simulation and compare to linear response T, U = self.T, self.U X0 = np.concatenate((self.X0, self.X0)) ios_t, ios_y, ios_x = ios.input_output_response( iosys_series, T, U, X0, return_x=True) lti_t, lti_y, lti_x = ct.forced_response(linsys_series, T, U, X0) np.testing.assert_array_almost_equal(lti_t, ios_t) np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3) # Connect systems with different timebases linsys2c = self.siso_linsys linsys2c.dt = 0 # Reset the timebase iosys2c = ios.LinearIOSystem(linsys2c) iosys_series = ios.InterconnectedSystem( (iosys1, iosys2c), # systems ((1, 0),), # interconnection (series) 0, # input = first system 1 # output = second system ) self.assertTrue(ct.isctime(iosys_series, strict=True)) ios_t, ios_y, ios_x = ios.input_output_response( iosys_series, T, U, X0, return_x=True) lti_t, lti_y, lti_x = ct.forced_response(linsys_series, T, U, X0) np.testing.assert_array_almost_equal(lti_t, ios_t) np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3) # Feedback interconnection linsys_feedback = ct.feedback(linsys1, linsys2) iosys_feedback = ios.InterconnectedSystem( (iosys1, iosys2), # systems ((1, 0), # input of sys2 = output of sys1 (0, (1, 0, -1))), # input of sys1 = -output of sys2 0, # input = first system 0 # output = first system ) ios_t, ios_y, ios_x = ios.input_output_response( iosys_feedback, T, U, X0, return_x=True) lti_t, lti_y, lti_x = ct.forced_response(linsys_feedback, T, U, X0) np.testing.assert_array_almost_equal(lti_t, ios_t) np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
def testSimulation(self): T = range(100) U = np.sin(T) # For now, just check calling syntax # TODO: add checks on output of simulations tout, yout = step_response(self.siso_ss1d) tout, yout = step_response(self.siso_ss1d, T) tout, yout = impulse_response(self.siso_ss1d, T) tout, yout = impulse_response(self.siso_ss1d) tout, yout, xout = forced_response(self.siso_ss1d, T, U, 0) tout, yout, xout = forced_response(self.siso_ss2d, T, U, 0) tout, yout, xout = forced_response(self.siso_ss3d, T, U, 0)
def calc_average_perf(s, Gp, Gd, Kc, tauI, tauD, tauC, times): Perf = np.linspace(0, 1, 100) Gc = Kc * (1 + 1 / (tauI * s) + tauD * s / (tauC * s + 1)) sys_D = Gd / (1 + Gp * Gc) average_Perf = [] for j in range(0, 100): Ti = dfs[j] _, Y, _ = control.forced_response(sys_D, times, Ti) sys_U = Gc _, U, _ = control.forced_response(sys_U, times, -Y) Perf[j] = sum(abs(Y)) + 0.2 * sum(abs(U)) if sum(abs(Y) >= 5) >= 1 or sum(abs(U) >= 5) >= 1: Perf[j] = 1e6 average_Perf = sum(Perf) / len(Perf) return average_Perf
def test_nonsquare_bdalg(self): # Set up parameters for simulation T = self.T U2 = [np.sin(T), np.cos(T)] U3 = [np.sin(T), np.cos(T), T] X0 = 0 # Set up systems to be composed linsys_2i3o = ct.StateSpace( [[-1, 1, 0], [0, -2, 0], [0, 0, -3]], [[1, 0], [0, 1], [1, 1]], [[1, 0, 0], [0, 1, 0], [0, 0, 1]], np.zeros((3, 2))) iosys_2i3o = ios.LinearIOSystem(linsys_2i3o) linsys_3i2o = ct.StateSpace( [[-1, 1, 0], [0, -2, 0], [0, 0, -3]], [[1, 0, 0], [0, 1, 0], [0, 0, 1]], [[1, 0, 1], [0, 1, -1]], np.zeros((2, 3))) iosys_3i2o = ios.LinearIOSystem(linsys_3i2o) # Multiplication linsys_multiply = linsys_3i2o * linsys_2i3o iosys_multiply = iosys_3i2o * iosys_2i3o lin_t, lin_y, lin_x = ct.forced_response(linsys_multiply, T, U2, X0) ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U2, X0) np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3) linsys_multiply = linsys_2i3o * linsys_3i2o iosys_multiply = iosys_2i3o * iosys_3i2o lin_t, lin_y, lin_x = ct.forced_response(linsys_multiply, T, U3, X0) ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U3, X0) np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3) # Right multiplication # TODO: add real tests once conversion from other types is supported iosys_multiply = ios.InputOutputSystem.__rmul__(iosys_3i2o, iosys_2i3o) ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U3, X0) np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3) # Feedback linsys_multiply = ct.feedback(linsys_3i2o, linsys_2i3o) iosys_multiply = iosys_3i2o.feedback(iosys_2i3o) lin_t, lin_y, lin_x = ct.forced_response(linsys_multiply, T, U3, X0) ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U3, X0) np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3) # Mismatch should generate exception args = (iosys_3i2o, iosys_3i2o) self.assertRaises(ValueError, ct.series, *args)
def y_map_function(self,iterator): # If some combination of 20% done running, checkpoint to console if iterator in self.milestones: self.serialized_checkpoint(iterator) # Create first PRBS outside of loop u = self.uArray[iterator,:,:] # The transfer function from the 2nd input to the 1st output is # (3s + 4) / (6s^2 + 5s + 4). # num = [[[1., 2.], [3., 4.]], [[5., 6.], [7., 8.]]] # Iterate over each of the output dimensions and # add to numerator allY=np.zeros((self.nstep,self.outDim)) for j in range(0,self.outDim): # Iterate over each of the input dimensions # and add to the numerator array b = [[self.b[iterator,self.outDim*j+i]] for i in range(self.inDim)] a = [[1.,-self.a[iterator,self.outDim*j+i]] for i in range(self.inDim)] q = [self.q[iterator,self.outDim*j+i] for i in range(self.inDim)] bigU=np.transpose(u) uSim=np.zeros_like(bigU) for (idx,row) in enumerate(bigU): uSim[idx,:] = np.concatenate((np.zeros(q[idx]),row[:-q[idx]])) numTemp=np.array([b]);denTemp=np.array([a]) sys = control.tf(numTemp,denTemp,1) _,y,_ = control.forced_response(sys,U=uSim,T=self.t) allY[:,j] = self.gauss_noise(y,self.stdev) return allY
def testMarkovSignature(self, matarrayout, matarrayin): U = matarrayin([[1., 1., 1., 1., 1.]]) Y = U m = 3 H = markov(Y, U, m, transpose=False) Htrue = np.array([[1., 0., 0.]]) np.testing.assert_array_almost_equal(H, Htrue) # Make sure that transposed data also works H = markov(np.transpose(Y), np.transpose(U), m, transpose=True) np.testing.assert_array_almost_equal(H, np.transpose(Htrue)) # Generate Markov parameters without any arguments H = markov(Y, U, m) np.testing.assert_array_almost_equal(H, Htrue) # Test example from docstring T = np.linspace(0, 10, 100) U = np.ones((1, 100)) T, Y = forced_response(tf([1], [1, 0.5], True), T, U) H = markov(Y, U, 3, transpose=False) # Test example from issue #395 inp = np.array([1, 2]) outp = np.array([2, 4]) mrk = markov(outp, inp, 1, transpose=False) # Make sure MIMO generates an error U = np.ones((2, 100)) # 2 inputs (Y unchanged, with 1 output) with pytest.raises(ControlMIMONotImplemented): markov(Y, U, m)
def creation_example(): """Example for creation of a FSR model with python-control.""" # creating a step response for our model sys = ctrl.tf([1], [100, 1]) sim_duration = 2000 time = np.arange(sim_duration) _, output = ctrl.step_response(sys, time) # creating the model model = ps.FsrModel(output, t=time) # creating a test input signal u = np.ones(sim_duration) for i in range(len(u)): if i < 500: u[i] = 0.001 * i # getting response from our model t1, y1 = ps.forced_response(model, time, u) # simulating a reference with control t2, y2, _ = ctrl.forced_response(sys, time, u) # show everything ax = plt.subplot(111) plt.plot(t1, y1, label="Model response") plt.plot(t2, y2, label="Reference response") plt.plot(t1, u, label="Input signal") ax.legend() plt.title("pystrem example") plt.show()
def step_system(xopt, label, ax): kp, tz1, tz2, tz3, tp = xopt Co = kp * (tz1 * s + 1) * (tz2 * s + 1) * (tz3 * s + 1) / (s * s * (tp * s + 1)) To = control.minreal(Co * Go / (1 + Co * Go), verbose=False) _, yout = control.forced_response(To, T, U=ramp, squeeze=True) ax.plot(T, yout, label=label)
def err(self, params, div): num = params[:div] den = params[div:] est_sys = control.tf(num, den) tsim, ysim, xsim = control.forced_response(est_sys, T=self.t, U=self.u) numpy.nan_to_num(self.history.append(sum((self.y - ysim)**2))) return self.history[-1]
def solve(problem_spec): """ solution of coprime decomposition :param problem_spec: ProblemSpecification object :return: solution_data: output value of the system and controller function """ s, t, T = sp.symbols("s, t, T") transfer_func = problem_spec.transfer_func() z_func, n_func = transfer_func.expand().as_numer_denom( ) # separate numerator and denominator # tracking controller # numerator and denominator of controller cd_res = cd.coprime_decomposition(z_func, n_func, problem_spec.pol) tf_k = (cd_res.f_func * z_func) / (cd_res.h_func * n_func) # open_loop z_o, n_o = sp.simplify(tf_k).expand().as_numer_denom() n_coeffs_o = [float(c) for c in st.coeffs(z_o, s) ] # coefficients of numerator of open_loop d_coeffs_o = [float(c) for c in st.coeffs(n_o, s) ] # coefficients of denominator of open_loop # feedback close_loop = control.feedback(control.tf(n_coeffs_o, d_coeffs_o)) # simulate system with controller with initial error (190K instead of 200K). y = control.forced_response(close_loop, problem_spec.tt, problem_spec.yr, problem_spec.x0_1) solution_data = SolutionData() solution_data.yy = y[1] solution_data.controller_n = cd_res.f_func solution_data.controller_d = cd_res.h_func solution_data.controller_ceoffs = cd_res.c_coeffs return solution_data
def test_forced_response_legacy(self): # Define a system for testing sys = ct.rss(2, 1, 1) T = np.linspace(0, 10, 10) U = np.sin(T) """Make sure that legacy version of forced_response works""" ct.config.use_legacy_defaults("0.8.4") # forced_response returns x by default t, y = ct.step_response(sys, T) t, y, x = ct.forced_response(sys, T, U) ct.config.use_legacy_defaults("0.9.0") # forced_response returns input/output by default t, y = ct.step_response(sys, T) t, y = ct.forced_response(sys, T, U) t, y, x = ct.forced_response(sys, T, U, return_x=True)
def main(): m = 1500 # Mass. Gives it a bit delay in the beginning. k = 450 # Static gain. Tune so end values are similar to experimental data. c = 240 # Time constant. Higher is slower. Damping. # Plant transfer function. Static gain is numerator. Denominator is c * s + 1. plant = control.tf([k], [m, c, 1]) plant_ss = control.ss(plant) Kp = 2 Ki = 0 Kd = 1 # PID controller transfer function. C(s) = Kp + Ki/s + Kd s = (Kd s^2 + Kp s + Ki) / s controller = control.tf([Kd, Kp, Ki], [1, 0]) sys = control.feedback(plant, controller) # Plot the step responses setpoint = 150 n = 100 # seconds fig, ax = plt.subplots(tight_layout=True) T = np.arange(0, n, 1) # 0 -- n seconds, in steps of 1 second u = np.full( n, fill_value=setpoint) # input vector. Step response so single value. T, y_out, x_out = control.forced_response(sys, T, u) ax.plot(T, y_out, label=str(setpoint)) ax.plot(T, x_out[0], label='x0') ax.plot(T, x_out[1], label='x1') ax.legend() plt.show()
def graficado(valor_ajustado_de_la_deslizadera_del_tiempo, tamaño_impulso, tamaño_escalon, tamaño_rampa, boton_pulsado, G0): fig.clf() retardo = eval(input_del_retardo_en_la_ventana.get()) if boton_pulsado == 'Impulso': t, y = control.impulse_response( G0 * tamaño_impulso, T=(valor_ajustado_de_la_deslizadera_del_tiempo)) if boton_pulsado == 'Escalon': t, y = control.step_response( G0 * tamaño_escalon, T=(valor_ajustado_de_la_deslizadera_del_tiempo)) if boton_pulsado == 'Rampa': t, y = control.step_response( G0 * tamaño_rampa / s, T=(valor_ajustado_de_la_deslizadera_del_tiempo)) if boton_pulsado == 'Arbitraria': t, y, xout = control.forced_response(G0, T=(t_experimental), U=(u_experimental)) t_a_dibujar, y_a_dibujar = ajusta_el_retardo_segun_el_input_de_la_ventana( t, y, retardo) fig.add_subplot(111).plot(t_a_dibujar, y_a_dibujar) canvas.draw()
def speed_control(sys, ref_speeds, curr_speeds, init_values): ''' Effects: get the reference speed, current (measured) speed and initial values Use the difference e = ref_speeds - curr_speeds as the input for the PI controller, derive the new throttle Parameters ---------- sys : control.ss state space controller ref_speeds : list of float the desired speed we need curr_speeds : list of float the current speed init_values : the initial_values of the system DESCRIPTION. Returns ------- throttle : float type DESCRIPTION. ''' U0 = np.array(ref_speeds) - np.array(curr_speeds) #print(U0) _, y0, x0 = control.forced_response( sys, U=U0, X0=init_values[0]) # y0 is the next values, x0 is the state evolution # see https://python-control.readthedocs.io/en/0.8.3/generated/control.forced_response.html#control.forced_response init_values.append(x0[-1]) throttle = y0[-1] return throttle, init_values
def test_nonlinear_iosys(self): # Create a simple nonlinear I/O system nlsys = ios.NonlinearIOSystem(predprey) T = self.T # Start by simulating from an equilibrium point X0 = [0, 0] ios_t, ios_y = ios.input_output_response(nlsys, T, 0, X0) np.testing.assert_array_almost_equal(ios_y, np.zeros(np.shape(ios_y))) # Now simulate from a nonzero point X0 = [0.5, 0.5] ios_t, ios_y = ios.input_output_response(nlsys, T, 0, X0) # # Simulate a linear function as a nonlinear function and compare # # Create a single input/single output linear system linsys = self.siso_linsys # Create a nonlinear system with the same dynamics nlupd = lambda t, x, u, params: \ np.reshape(linsys.A * np.reshape(x, (-1, 1)) + linsys.B * u, (-1,)) nlout = lambda t, x, u, params: \ np.reshape(linsys.C * np.reshape(x, (-1, 1)) + linsys.D * u, (-1,)) nlsys = ios.NonlinearIOSystem(nlupd, nlout) # Make sure that simulations also line up T, U, X0 = self.T, self.U, self.X0 lti_t, lti_y, lti_x = ct.forced_response(linsys, T, U, X0) ios_t, ios_y = ios.input_output_response(nlsys, T, U, X0) np.testing.assert_array_almost_equal(lti_t, ios_t) np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
def getGamma( lc ): A,b = de.lstSqMultiPt( ThetaH[:,:], ThetaA.reshape((-1,1)), df[:,1:], (df[:,0]-.05).reshape((-1,1)), etaP.reshape((-1,))/lc, t, .0125 )[:2] M = A.tocsr() # preconditioning with Jacobi preconditioner D = ss.dia_matrix( (1./np.power(M.multiply(M).sum(0),.5),0), (M.shape[1],M.shape[1]) ) G = (D*M.T*M*D).todense() # Tikhonov matrix for regularization gamma = D*((G)**-1*D*(M.T*b)) reg = co.coupledOde( Lambda.shape[1]-1, etaP.reshape((-1,)), lc, np.ones(Lambda.shape[1]-1), np.ones(1), np.ones(1)*.05, np.array(gamma[:A.shape[1]]).reshape((-1,)) ) sys = control.ss(reg.A().detach(),np.eye(reg.A().shape[0]),np.eye(reg.A().shape[0]),np.zeros(reg.A().shape)) ut = np.vstack(([[0]],np.array(gamma[A.shape[1]:]).reshape((-1,1)))) tp = np.hstack(([0],t)) x0 = np.ones((2236,1)) x0[-1] = 0.05 rv = control.forced_response(sys,np.arange(0,32+5e-2,5e-2),X0=x0) reft,refft,header = ph.parseH( 'Min4data_2iso_0919.csv', do_squeeze=False ) reft = np.vstack([x.reshape((1,-1)) for x in reft]) refft = np.vstack([x.reshape((1,-1)) for x in refft]) residual = rv[1][:-1,(np.power( 2, [0,2,3,4,5] )/5e-2).astype(np.int)] - refft[:,list(range(1,refft.shape[1]))+[0]].T mask = (1 - np.isnan(residual)).nonzero() flatResidual = residual[mask] return gamma,flatResidual,rv
def speed_control(env, sys, ref_speeds, curr_speeds, init_values): ''' Parameters ---------- env : CARLA_ENV DESCRIPTION. sys : control.ss speed controller ref_speeds : list of float the desired speed we need curr_speeds : list of float the current speed init_values : the initial_values of the system DESCRIPTION. Returns ------- throttle : float type DESCRIPTION. ''' U0 = np.array(ref_speeds) - np.array(curr_speeds) #print(U0) _, y0, x0 = control.forced_response(sys, U=U0, X0=init_values[0]) init_values.append(x0[-1]) throttle = y0[-1] return throttle, init_values
def test_import_export(self): sys = ctrl.tf([1], [100, 1]) sim_duration = 2000 time = np.zeros(sim_duration) i = 0 while (i < sim_duration): time[i] = i i += 1 _, output = ctrl.step_response(sys, time) m = ps.FsrModel(output, t=time) # testing our model with a test signal test_sgnl_len = int(2500) u = np.zeros(test_sgnl_len) for i in range(test_sgnl_len): u[i] = 1 time = np.zeros(test_sgnl_len) for i in range(test_sgnl_len): time[i] = i _, comp, _ = ctrl.forced_response(sys, time, u) # 'rb' and 'wb' to avoid problems under Windows! with open("temp", "w") as fh: ps.export_csv(m, fh) with open("temp", "r") as fh: m = ps.import_csv(fh) os.remove("temp") _, y = ps.forced_response(m, time, u) self.assertTrue(np.allclose(y, comp, rtol=1e-2), "import/export broke")
def get_reference_speed(distance_sys,ref_distance, curr_distance, init_values): ''' Parameters ---------- distance_sys : control.ss state space controller. ref_distance : float the reference distance. curr_distance : float current distance between two vehicles. init_values : the initial_values of the system Returns ------- None. ''' U0 = np.array(ref_distance) - np.array(curr_distance) print(U0) _,y0,x0 = control.forced_response(distance_sys,U = U0,X0 = init_values[0]) # y0 is the next values, x0 is the state evolution # see https://python-control.readthedocs.io/en/0.8.3/generated/control.forced_response.html#control.forced_response init_values.append(x0[-1]) ref_speed = y0[-1] return ref_speed, init_values
def test_double_integrator(self): # Define a second order integrator sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0) flatsys = fs.LinearFlatSystem(sys) # Define the endpoints of a trajectory x1 = [0, 0]; u1 = [0]; T1 = 1 x2 = [1, 0]; u2 = [0]; T2 = 2 x3 = [0, 1]; u3 = [0]; T3 = 3 x4 = [1, 1]; u4 = [1]; T4 = 4 # Define the basis set poly = fs.PolyFamily(6) # Plan trajectories for various combinations for x0, u0, xf, uf, Tf in [ (x1, u1, x2, u2, T2), (x1, u1, x3, u3, T3), (x1, u1, x4, u4, T4)]: traj = fs.point_to_point(flatsys, x0, u0, xf, uf, Tf, basis=poly) # 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]) # Simulate the system and make sure we stay close to desired traj T = np.linspace(0, Tf, 100) xd, ud = traj.eval(T) t, y, x = ct.forced_response(sys, T, ud, x0) np.testing.assert_array_almost_equal(x, xd, decimal=3)
def test_IT2_forced_response(self): sys = ctrl.tf([100], [50, 1000, 150, 0]) # IT2 _, o = ctrl.step_response(sys, self.time) m = ps.FsrModel(o, t=self.time, optimize=False) _, y = ps.forced_response(m, self.t, self.u) _, o, _ = ctrl.forced_response(sys, self.t, self.u) self.assertTrue(np.allclose(y, o, rtol=1e-2), "it2 response broke")
def test_PT2_forced_response(self): sys = ctrl.tf([1], [1000, 10, 1]) # PT2 with overshooting _, o = ctrl.step_response(sys, self.time) m = ps.FsrModel(o, t=self.time) _, y = ps.forced_response(m, self.t, self.u) _, o, _ = ctrl.forced_response(sys, self.t, self.u) self.assertTrue(np.allclose(y, o, rtol=1e-2), "pt2 response broke")
def fun(iterator): sys = control.tf([ np.random.randint(10), ], [np.random.randint(10), 1.]) _, yEnd, _ = control.forced_response(sys, U=uArray[iterator, :], T=np.linspace(0, 100, 100)) return yEnd
def make_step(sys, u, t): ti, yi = control.forced_response(sys, U=u, T=[PrevStep.t, t], X0=PrevStep.y * PLANT_DENOMINATOR[0]) PrevStep.t = ti[-1] PrevStep.y = yi[-1] return PrevStep.t, PrevStep.y
def __init__(self, sys, sid): self.sid = sid self.sys = sys self.time = [0,1,2,3,4,5] self.u = [0,0,0,0,0,0] self.count = len(self.time) T, yout, _ = control.forced_response(self.sys, self.time, self.u) socketio.emit('process', {'T': T.tolist(), 'yout': yout.tolist(), 'uout': self.u}, room=self.sid)
def test_bdalg_functions(self): """Test block diagram functions algebra on I/O systems""" # Set up parameters for simulation T = self.T U = [np.sin(T), np.cos(T)] X0 = 0 # Set up systems to be composed linsys1 = self.mimo_linsys1 linio1 = ios.LinearIOSystem(linsys1) linsys2 = self.mimo_linsys2 linio2 = ios.LinearIOSystem(linsys2) # Series interconnection linsys_series = ct.series(linsys1, linsys2) iosys_series = ct.series(linio1, linio2) lin_t, lin_y, lin_x = ct.forced_response(linsys_series, T, U, X0) ios_t, ios_y = ios.input_output_response(iosys_series, T, U, X0) np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3) # Make sure that systems don't commute linsys_series = ct.series(linsys2, linsys1) lin_t, lin_y, lin_x = ct.forced_response(linsys_series, T, U, X0) self.assertFalse((np.abs(lin_y - ios_y) < 1e-3).all()) # Parallel interconnection linsys_parallel = ct.parallel(linsys1, linsys2) iosys_parallel = ct.parallel(linio1, linio2) lin_t, lin_y, lin_x = ct.forced_response(linsys_parallel, T, U, X0) ios_t, ios_y = ios.input_output_response(iosys_parallel, T, U, X0) np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3) # Negation linsys_negate = ct.negate(linsys1) iosys_negate = ct.negate(linio1) lin_t, lin_y, lin_x = ct.forced_response(linsys_negate, T, U, X0) ios_t, ios_y = ios.input_output_response(iosys_negate, T, U, X0) np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3) # Feedback interconnection linsys_feedback = ct.feedback(linsys1, linsys2) iosys_feedback = ct.feedback(linio1, linio2) lin_t, lin_y, lin_x = ct.forced_response(linsys_feedback, T, U, X0) ios_t, ios_y = ios.input_output_response(iosys_feedback, T, U, X0) np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
def test_all_pass_forced_response(self): sys = ctrl.tf([-50, 1], [1000, 10, 1]) # all-pass _, o = ctrl.step_response(sys, self.time) m = ps.FsrModel(o, t=self.time) _, y = ps.forced_response(m, self.t, self.u) _, o, _ = ctrl.forced_response(sys, self.t, self.u) self.assertTrue(np.allclose(y, o, rtol=1e-1), "all-pass response broke")
def step(self, action, sys): # Update time and action vectors self.time.append(self.count) self.u.append(action) # Evaluate and send response T, yout, uout = control.forced_response(self.sys, self.time, self.u) socketio.emit('process', {'T': T[-1].tolist(), 'yout': yout[-1].tolist(), 'uout': self.u[-1]}, room=self.sid) self.inc_count()
def showStepResponse(self, ax=0): T, yout, xout = control.forced_response(self.sys, self.time, [functions.step(self.time)]) if ax != 0: ax.plot(T,yout, label='step response') ax.legend() else: fig, ax1 = plt.subplots(nrows=1, ncols=1) ax1.plot(T,yout, label='step response') ax1.legend() plt.show()
def showTimeResponse(self, ax=0): T, yout, xout = control.forced_response(self.sys, self.time, self.fct(self.time)) if ax != 0: ax.plot(T,yout, label='time response') ax.legend() else: fig, ax1 = plt.subplots(nrows=1, ncols=1) ax1.plot(T,yout, label='time response') ax1.legend() plt.show()
#--------- Uncomment below to compare the response to the exact function with -------- #--------- our n-term fourier approximation -------- # wn = (2*pi)**2 z = 0.1 # Define the system to use in simulation - in transfer function form here num = [2.*z*wn,wn**2]; den = [1,2.*z*wn,wn**2]; sys = control.tf(num,den); # run the simulation - first with the exact input [T_out,yout_exact,xout_exact] = control.forced_response(sys,t,y) # run the simulation - utilize the built-in initial condition response function [T_approx,yout_approx,xout_approx] = control.forced_response(sys,t,approx) # Make the figure pretty, then plot the results # "pretty" parameters selected based on pdf output, not screen output # Many of these setting could also be made default by the .matplotlibrc file fig = figure(figsize=(6,4)) ax = gca() subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96) setp(ax.get_ymajorticklabels(),family='CMU Serif',fontsize=18) setp(ax.get_xmajorticklabels(),family='CMU Serif',fontsize=18) ax.spines['right'].set_color('none') ax.spines['top'].set_color('none') ax.xaxis.set_ticks_position('bottom')
# ltext = leg.get_texts() # plt.setp(ltext,fontsize=18) # Adjust the page layout filling the page using the new tight_layout command plt.tight_layout(pad=0.5) # save the figure as a high-res pdf in the current folder # plt.savefig('mpc_command.pdf') # show the figure plt.show() # Now, let's verify the results using a full simulation of the model x0 = np.array([[x1_init],[x1_dot_init]]) time_out, y_out, x_out = control.forced_response(sys, time, u_total, x0) # Set the plot size - 3x2 aspect ratio is best fig = plt.figure(figsize=(6,4)) ax = plt.gca() plt.subplots_adjust(bottom=0.17, left=0.17, top=0.96, right=0.96) # Change the axis units font plt.setp(ax.get_ymajorticklabels(),fontsize=18) plt.setp(ax.get_xmajorticklabels(),fontsize=18) ax.spines['right'].set_color('none') ax.spines['top'].set_color('none') ax.xaxis.set_ticks_position('bottom')
ki = 10 # The integral gain # Define the closed-loop transfer function numPID = [kd,kp,ki] denPID = [m,(c + kd),(k + kp),ki] sysPID = control.tf(numPID,denPID) # Let's start it at t=0.5s for clarity in plotting Xd = zeros(500) # Define an array of all zeros Xd[50:] = 1 # Make all elements of this array index>50 = 1 (all after 0.5s) Xd[200:] = 0.6 # Make the elements after 2.5s = 0.5 # run the simulation - utilize the built-in forced_response function [T_PID,yout_PID,xout_PID] = control.forced_response(sysPID,t,Xd) # Calculate the error terms error = Xd - yout_PID error_deriv = r_[0,diff(error)] error_int = trapz(error,T_PID) # Calculate the "derivative on measurement" term error_measurement = -r_[0,diff(yout_PID)] # Calculate the force input (the output of the PID controller) force = kp*error + ki*error_int + kd*error_deriv # Direct application # Force from "derivative on measurement" method force_measurement = kp*error + ki*error_int + kd*error_measurement
ylabel('Force (N)',family='CMU Serif',fontsize=22,weight='bold',labelpad=10) # xlim(0,4) ylim(-1.25,1.25) plot(t,F,color='blue',linewidth=2,label='Response') # uncomment below save the figure as a high-res pdf in the current folder savefig('BangCoastBang_Force.png') #show the figure #show() # run the simulation - utilize the built-in forced_response function [T,yout,xout] = control.forced_response(sys,t,F,hmax=0.01) # Make the figure pretty, then plot the results # "pretty" parameters selected based on pdf output, not screen output # Many of these setting could also be made default by the .matplotlibrc file fig = figure(figsize=(6,4)) ax = gca() subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96) setp(ax.get_ymajorticklabels(),family='CMU Serif',fontsize=18) setp(ax.get_xmajorticklabels(),family='CMU Serif',fontsize=18) ax.spines['right'].set_color('none') ax.spines['top'].set_color('none') ax.xaxis.set_ticks_position('bottom') ax.yaxis.set_ticks_position('left') ax.grid(True,linestyle=':',color='0.75') ax.set_axisbelow(True)
sampling_multiple = dt / new_dt # Define the new time vector time = np.arange(0, stop_time, new_dt) sampling_offset = np.ones(int(sampling_multiple),) u_newDt = np.kron(u_total, sampling_offset) u_newDt = u_newDt[int(sampling_multiple):] # Convert the system to digital using the faster sampling rate. new_digital_sys = control.sample_system(sys, new_dt) # Now, simulate the systema at the new higher sampling rate t_out, y_out, x_out = control.forced_response(new_digital_sys, time, u_newDt) # I'm including a message here, so that I can tell from the terminal when it's # done running. Otherwise, the plot windows tend to end up hidden behind others # and I have to dig around to get them. input("\nDone solving... press enter to plot the results.") # Set the plot size - 3x2 aspect ratio is best fig = plt.figure(figsize=(6,4)) ax = plt.gca() plt.subplots_adjust(bottom=0.17, left=0.17, top=0.96, right=0.96) # Change the axis units font plt.setp(ax.get_ymajorticklabels(),fontsize=18) plt.setp(ax.get_xmajorticklabels(),fontsize=18)
# Set up simulation parameters t = r_[0:5:500j] # time for simulation, 0-5s with 500 points in-between # ----- Open-Loop input the step response ----------------------------------------------- # # Define the open-loop system to use in simulation - in transfer function form here num = [1] den = [m, c, k] sys = control.tf(num, den) # Let's start it at t=0.5s for clarity in plotting F = zeros(500) # Define an array of all zeros F[25:] = 1 # Make all elements of this array index>50 = 1 (all after 0.5s) # run the simulation - utilize the built-in forced_response function [T, yout, xout] = control.forced_response(sys, t, F) # Make the figure pretty, then plot the results # "pretty" parameters selected based on pdf output, not screen output # Many of these setting could also be made default by the .matplotlibrc file fig = figure(figsize=(6, 4)) ax = gca() subplots_adjust(bottom=0.17, left=0.17, top=0.96, right=0.96) setp(ax.get_ymajorticklabels(), family="CMU Serif", fontsize=18) setp(ax.get_xmajorticklabels(), family="CMU Serif", fontsize=18) ax.spines["right"].set_color("none") ax.spines["top"].set_color("none") ax.xaxis.set_ticks_position("bottom") ax.yaxis.set_ticks_position("left") ax.grid(True, linestyle=":", color="0.75") ax.set_axisbelow(True)
def input_sim(self,u=0,T_intval=0.01): T, yout, xout=control.forced_response(self.agent_model, np.array([0,T_intval]), u, self.state) self.state=yout[-1]
den = [1.,0.,wn**2]; sys = control.tf(num,den); # Set up simulation parameters t = r_[0:5:0.001] # time for simulation, 0-5s with 500 points in-between #----- Look at a step response -------------------------------------------------------- # Let's start it at t=0.5s for U = zeros(5000) # Define an array of all zeros U[500:] = 1 # Make all elements of this array index>50 = 1 (all after 0.5s) # run the simulation - utilize the built-in initial condition response function [T_un,yout_un,xout_un] = control.forced_response(sys,t,U) # Make the figure pretty, then plot the results # "pretty" parameters selected based on pdf output, not screen output # Many of these setting could also be made default by the .matplotlibrc file fig = figure(figsize=(6,4)) ax = gca() subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96) setp(ax.get_ymajorticklabels(),family='CMU Serif',fontsize=18) setp(ax.get_xmajorticklabels(),family='CMU Serif',fontsize=18) ax.spines['right'].set_color('none') ax.spines['top'].set_color('none') ax.xaxis.set_ticks_position('bottom') ax.yaxis.set_ticks_position('left') ax.grid(True,linestyle=':',color='0.75') ax.set_axisbelow(True)
ylabel('Force (N)',family='CMU Serif',fontsize=22,weight='bold',labelpad=10) xlim(0,4) ylim(-1.25*fmax,1.25*fmax) plot(t,F,color='blue',linewidth=2,label='Response') # uncomment below save the figure as a high-res pdf in the current folder savefig('BangBang_Force_Flexible.png') #show the figure #show() # run the simulation - utilize the built-in forced_response function [T,yout,xout] = control.forced_response(sys,t,F) # Make the figure pretty, then plot the results # "pretty" parameters selected based on pdf output, not screen output # Many of these setting could also be made default by the .matplotlibrc file fig = figure(figsize=(6,4)) ax = gca() subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96) setp(ax.get_ymajorticklabels(),family='CMU Serif',fontsize=18) setp(ax.get_xmajorticklabels(),family='CMU Serif',fontsize=18) ax.spines['right'].set_color('none') ax.spines['top'].set_color('none') ax.xaxis.set_ticks_position('bottom') ax.yaxis.set_ticks_position('left') ax.grid(True,linestyle=':',color='0.75') ax.set_axisbelow(True)
# # The next thing that may be of interest is the step response of the system. I know that the system is unstable but the step response can possibly reveal other information. I will use the control toolbox's `forced_response` function so that we can control the magnitude of the step input. We will simulate the system for 5 seconds and set a step input of 2 degrees. # In[5]: time = np.linspace(0.0, 5.0, num=1001) # In[6]: delta = np.deg2rad(2.0) * np.ones_like(time) # In[7]: time, theta, state = cn.forced_response(theta_delta, T=time, U=delta) # Now, I'll create a reusable function for plotting a SISO input/output time history. # In[8]: def plot_siso_response(time, input, output, title='Time Response', x_lab='Time [s]', x_lim=None, input_y_lab='Input', input_y_lim=None, output_y_lab='Output', output_y_lim=None, subplots=True): """Plots a time history of the input and output of a SISO system.""" xaxis = gr.XAxis(title=x_lab, range=x_lim)
plot(T,yout,color="blue",linewidth=2) # save the figure as a high-res pdf in the current folder savefig('Step_Resp.pdf') # show the figure show() #----- We could also manually input the step response ----- # Let's start it at t=0.5s for U = np.zeros(500) # Define an array of all zeros U[50:] = 1 # Make all elements of this array index>50 = 1 (all after 0.5s) # run the simulation - utilize the built-in initial condition response function [T_man,yout_man,xout_man] = control.forced_response(sys,t,U) # Make the figure pretty, then plot the results # "pretty" parameters selected based on pdf output, not screen output # Many of these setting could also be made default by the .matplotlibrc file fig = figure(figsize=(6,4)) ax = gca() subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96) setp(ax.get_ymajorticklabels(),family='CMU Serif',fontsize=18) setp(ax.get_xmajorticklabels(),family='CMU Serif',fontsize=18) ax.spines['right'].set_color('none') ax.spines['top'].set_color('none') ax.xaxis.set_ticks_position('bottom') ax.yaxis.set_ticks_position('left') ax.grid(True,linestyle=':',color='0.75') ax.set_axisbelow(True)