def test_sample_system(self, tsys): # Make sure we can convert various types of systems for sysc in (tsys.siso_tf1, tsys.siso_tf1c, tsys.siso_ss1, tsys.siso_ss1c, tsys.mimo_ss1, tsys.mimo_ss1c): for method in ("zoh", "bilinear", "euler", "backward_diff"): sysd = sample_system(sysc, 1, method=method) assert sysd.dt == 1 # Check "matched", defined only for SISO transfer functions for sysc in (tsys.siso_tf1, tsys.siso_tf1c): sysd = sample_system(sysc, 1, method="matched") assert sysd.dt == 1
def initialize(self, param_value_dict): self.k1 = param_value_dict["k1"] self.k2 = param_value_dict["k2"] self.luenberger_poles = param_value_dict["Luenberger poles"] AB = np.array([[0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 0]]) BB = np.array([[0, 0], [0, 0], [0, 0], [1, 0], [0, 0], [0, 1]]) CB = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]) L = place(AB.T, CB.T, self.luenberger_poles).T cont_observer_system = ss(AB - L @ CB, np.hstack((BB, L)), np.zeros((1, 6)), np.zeros((1, 6))) # TODO: Remove hardcoded sample time discrete_observer_system = sample_system(cont_observer_system, 1/60) self.luenberger_Ad, self.luenberger_Bd, _, _ = ssdata(discrete_observer_system) self.run_once = False
def make_statespace(self): jm = self.parameters["DC-Motor"]["Jm"] bm = self.parameters["DC-Motor"]["Bm"] kme = self.parameters["DC-Motor"]["Kme"] kmt = self.parameters["DC-Motor"]["Kmt"] rm = self.parameters["DC-Motor"]["Rm"] lm = self.parameters["DC-Motor"]["Lm"] kdm = self.parameters["DC-Motor"]["Kdm"] kpm = self.parameters["DC-Motor"]["Kpm"] kim = self.parameters["DC-Motor"]["Kim"] nm = self.parameters["DC-Motor"]["Nm"] dc = control.TransferFunction( [0, kmt], [jm * lm, bm * lm + jm * rm, bm * rm + kme * kmt]) pidm = control.TransferFunction( [kpm + kdm * nm, kpm * nm + kim, kim * nm], [1, nm, 0]) ii = control.TransferFunction([1], [1, 0, 0]) agv = ii * control.feedback(dc * pidm, sign=-1) # Laplace --> Z agvz = control.sample_system(agv, lib.pt, method='zoh') # Transferfunction --> StateSpace ss = control.tf2ss(agvz) lib.set_statespace(ss)
def __init__(self, plant, dt): self.plant, self.dt = plant, dt Ac, Bc = hcu.num_jacobian([0, 0, 0, 0], [0], plant.dyn_cont) #print Ac #print Bc Cc, Dc = np.array([[0, 1, 0, 0]]), np.array([[0]]) # regulate theta self.sysc = control.ss(Ac, Bc, Cc, Dc) self.sysd = control.sample_system(self.sysc, dt) plant_tfd = control.tf(self.sysd) ctl_tfd = control.tf([-4.945, 8.862, -3.967], [1.000, -1.481, 0.4812], dt) gt = control.feedback(ctl_tfd * plant_tfd, 1) cl_polesd = gt.pole() cl_polesc = np.log(cl_polesd) / dt #y, t = control.step(gt, np.arange(0, 5, dt)) #plt.plot(t, y[0]) #plt.show() #self.il_ctl = self.synth_pid(0.5, 0.01, 0.05) #pdb.set_trace() if 1: #self.il_ctl = DiscTimeFilter([-4.945, 8.862, -3.967], [1.000, -1.481, 0.4812], 1.05) self.il_ctl = DiscTimeFilter([-4.945, 8.862, -3.967], [1.000, -1.481, 0.4812], 1.25) else: self.il_ctl = self.synth_pid(0.4, 0.04, 0.1) self.il_ctl.num = [-c for c in self.il_ctl.num] self.il_ctl.gain = 1. #self.ol_ctl = DiscTimeFilter([0.18856, -0.37209, 0.18354], [1.00000, -1.86046, 0.86046], 0.9) self.ol_ctl = DiscTimeFilter([0.18856, -0.37209, 0.18354], [1.00000, -1.86046, 0.86046], 0.4)
def update_discrete_model(self, dt): ''' creates the discrete system due to non-constant dt ''' self.sysd = control.sample_system(self.sysc, dt) # create a discrete time model self.F = self.sysd.A self.G = self.sysd.B
def test_sample_system(self): # Make sure we can convert various types of systems for sysc in (self.siso_tf1, self.siso_tf1c, self.siso_ss1, self.siso_ss1c, self.mimo_ss1, self.mimo_ss1c): for method in ("zoh", "bilinear", "euler", "backward_diff"): sysd = sample_system(sysc, 1, method=method) self.assertEqual(sysd.dt, 1) # Check "matched", defined only for SISO transfer functions for sysc in (self.siso_tf1, self.siso_tf1c): sysd = sample_system(sysc, 1, method="matched") self.assertEqual(sysd.dt, 1) # Check errors self.assertRaises(ValueError, sample_system, self.siso_ss1d, 1) self.assertRaises(ValueError, sample_system, self.siso_tf1d, 1) self.assertRaises(ValueError, sample_system, self.siso_ss1, 1, 'unknown')
def test_printing(self): """Print SISO""" sys = ss2tf(rss(4, 1, 1)) assert isinstance(str(sys), str) assert isinstance(sys._repr_latex_(), str) # SISO, discrete time sys = sample_system(sys, 1) assert isinstance(str(sys), str) assert isinstance(sys._repr_latex_(), str)
def get_model(self): Acont = np.array([[-1 / (self.cr * self.R), 1 / (self.cr * self.R)], [1 / (self.cw * self.R), -2 / (self.cw * self.R)]]) Bcont = np.array([[1 / self.cr, 0], [0, 1 / (self.cw * self.R)]]) C = np.array([[1, 0]]) D = np.array([[0, 0]]) sys = control.ss(Acont, Bcont, C, D) sysd = control.sample_system(sys, SIMULATION_TIME_STEP) Adisc, Bdisc, Cdisc, Ddisc = control.ssdata(sysd) return Adisc, Bdisc[:, 0], Bdisc[:, 1]
def _get_longi_lti_ssr(dm, trim_args, dt=0.01, report=False): Xe, Ue = dm.trim(trim_args, report=report) Ac, Bc = dm.get_jacobian(Xe, Ue) s = p3_fr.SixDOFAeroEuler A1c = np.array([[Ac[s.sv_theta, s.sv_theta], Ac[s.sv_theta, s.sv_q]], [Ac[s.sv_q, s.sv_theta], Ac[s.sv_q, s.sv_q]]]) B1c = np.array([[Bc[s.sv_theta, dm.iv_de()]], [Bc[s.sv_q, dm.iv_de()]]]) ct_ssr = control.ss(A1c, B1c, [[1, 0]], [[0]]) dt_ssr = control.sample_system(ct_ssr, dt, method='zoh') # ‘matched’, ‘tustin’, ‘zoh’ return A1c, B1c, dt_ssr.A, dt_ssr.B
def get_PI_controller(delta_seconds): ''' Effects: create a discrete state-space PI controller ''' num_pi = [KP, KI] # numerator of the PI transfer function (KP*s + KI) den_pi = [1.0, 0.01*KI/KP] # denominator of PI transfer function (s + 0.01*KI/KP) sys = control.tf(num_pi,den_pi) # get transfer function for PI controller (since the denominator has a small term 0.01*KI/KP, it is actually a lag-compensator) sys = control.sample_system(sys, delta_seconds) # discretize the transfer function (from s-domain which is continuous to z-domain) #since our simulation is discrete sys = control.tf2ss(sys) # transform transfer function into state space. return sys
def test_sample_tf(self, tsys): # double integrator sys = TransferFunction(1, [1,0,0]) for h in (0.1, 0.5, 1, 2): numd_expected = 0.5 * h**2 * np.array([1.,1.]) dend_expected = np.array([1.,-2.,1.]) sysd = sample_system(sys, h, method='zoh') assert sysd.dt == h numd = sysd.num[0][0] dend = sysd.den[0][0] np.testing.assert_array_almost_equal(numd, numd_expected) np.testing.assert_array_almost_equal(dend, dend_expected)
def test_output_of_difference_equation_against_discrete_tf_high_order( self): """ This test checks the step response of the DifferenceEquation class vs the discrete transfer function for a system where the system has high order """ sample_time = 0.10 # [seconds] end_time = 10 coefficient_1 = 0.01 coefficient_2 = 0.001 coefficient_3 = 1 # Construct the Laplace operator s = ct.tf([1, 0], 1) step_input = 1 continuous_tf = (coefficient_3 * s) / (coefficient_1 * s**3 + coefficient_2 * s + 1.5) discrete_tf = ct.sample_system(continuous_tf, sample_time, "zoh") difference_equation = de.DifferenceEquation(discrete_tf) num_points = int(end_time / sample_time) + 1 T = np.linspace(0, end_time, num_points) T, yout = ct.step_response(discrete_tf, T) # Set up circular buffers for input/output management inputs = collections.deque( maxlen=difference_equation.get_system_order()) outputs = collections.deque( maxlen=difference_equation.get_system_order()) full_output_history = [] for i in range(0, difference_equation.get_system_order()): inputs.append(0) for i in range(0, difference_equation.get_system_order()): outputs.append(0) for i in range(0, num_points): inputs.appendleft(step_input) output = difference_equation.calculate_output(outputs, inputs) outputs.appendleft(output) full_output_history.append(output) for i in range(0, len(T)): self.assertAlmostEqual(yout[i], full_output_history[i], 1)
def test_sample_system_errors(self, tsys): # Check errors with pytest.raises(ValueError): sample_system(tsys.siso_ss1d, 1) with pytest.raises(ValueError): sample_system(tsys.siso_tf1d, 1) with pytest.raises(ValueError): sample_system(tsys.siso_ss1, 1, 'unknown')
def test_sample_ss(self, tsys): # double integrators, two different ways sys1 = StateSpace([[0.,1.],[0.,0.]], [[0.],[1.]], [[1.,0.]], 0.) sys2 = StateSpace([[0.,0.],[1.,0.]], [[1.],[0.]], [[0.,1.]], 0.) I = np.eye(2) for sys in (sys1, sys2): for h in (0.1, 0.5, 1, 2): Ad = I + h * sys.A Bd = h * sys.B + 0.5 * h**2 * sys.A @ sys.B sysd = sample_system(sys, h, method='zoh') np.testing.assert_array_almost_equal(sysd.A, Ad) np.testing.assert_array_almost_equal(sysd.B, Bd) np.testing.assert_array_almost_equal(sysd.C, sys.C) np.testing.assert_array_almost_equal(sysd.D, sys.D) assert sysd.dt == h
def test_call_siso(self, dt, omega, resp): """Evaluate the frequency response of a SISO system at one frequency.""" sys = TransferFunction([1., 3., 5], [1., 6., 2., -1]) if dt: sys = sample_system(sys, dt) s = np.exp(omega * 1j * dt) else: s = omega * 1j # Correct versions of the call np.testing.assert_allclose(evalfr(sys, s), resp, atol=1e-3) np.testing.assert_allclose(sys(s), resp, atol=1e-3) # Deprecated version of the call (should generate exception) with pytest.raises(AttributeError): np.testing.assert_allclose(sys.evalfr(omega), resp, atol=1e-3)
def synth2(self, plant, dt=0.01): Ac, Bc = hcu.num_jacobian([0, 0, 0, 0], [0], plant.dyn_cont) print('Ac\n{}\nBc\n{}'.format(Ac, Bc)) ct_ss = control.ss(Ac, Bc, [[1, 0, 0, 0]], [[0]]) print(ct_ss) dt_ss = control.sample_system( ct_ss, dt, method='zoh') # ‘matched’, ‘tustin’, ‘zoh’ print(dt_ss) dt_tf = control.tf(dt_ss) print(dt_tf) desired_cl_poles_c = np.array([ -18.36312687 + 0.j, -7.08908759 + 0.j, -6.64111168 + 1.52140048j, -6.64111168 - 1.52140048j ]) desired_cl_poles_d = np.exp(desired_cl_poles_c * dt) print('desired poles\n{}\n{}'.format(desired_cl_poles_c, desired_cl_poles_d)) def compute_cl_poles(ctl_num, ctl_den): ctl_tf = control.tf(ctl_num, ctl_den, dt) #print(ctl_tf) cl_tf = control.feedback(dt_tf, ctl_tf, sign=-1) #print control.damp(cl_tf) #print(cl_tf) cl_poles_d = control.pole(cl_tf) #print(cl_polesd) cl_poles_c = np.sort_complex(np.log(cl_poles_d) / dt) print('cl_poles_c\n{}'.format(cl_poles_c)) return cl_poles_d compute_cl_poles([-4.945, 8.862, -3.967], [1.000, -1.481, 0.4812]) compute_cl_poles([-4.945, 8.862, -3.967], [1.000, -1.2, 0.4812]) def err_fun(p): ctl_num, ctl_den = p[:3], [1, p[3], p[4]] err = np.sum( np.abs( compute_cl_poles(ctl_num, ctl_den)[:4] - desired_cl_poles_d)) print ctl_num, ctl_den, err return err p0 = [-4.945, 8.862, -3.967, -1.481, 0.4812] res = scipy.optimize.minimize(err_fun, p0) pdb.set_trace()
def ss_init(dt, delay=0, method='zoh'): # Continuous state-space matrices A_cont = np.array([[0., 1.], [0., 0.]]) B_cont = np.array([[0.], [1.]]) C_cont = np.array([[1., 0.], [0., 1.]]) D_cont = np.array([[0.], [0.]]) numState = A_cont.shape[0] # Continuous system SS_cont = ctrl.ss(A_cont, B_cont, C_cont, D_cont) # Discrete system SS_disc_nodelay = ctrl.sample_system(SS_cont, dt, method=method) if delay == 0: return SS_disc_nodelay # Initialize discrete delayed state-space matrices dumA = np.zeros((numState * (delay + 1), numState * (delay + 1))) dumB = np.zeros((numState * (delay + 1), 1)) dumC = np.zeros((SS_disc_nodelay.C.shape[0], numState * (delay + 1))) dumD = np.zeros((SS_disc_nodelay.D.shape[0], SS_disc_nodelay.B.shape[1])) dumvec = np.ones((numState * delay)) # Populate ss-matrices such that the input is delayed by delay samples dumA[0:numState, 0:numState] = SS_disc_nodelay.A for k in range(numState): dumA[k, numState + k * delay] = 1. dumB[numState - 1 + (k + 1) * delay, 0] = SS_disc_nodelay.B[k] dumvec[(k + 1) * delay - 1] = 0. dumvec = dumvec[:-1] dumA[numState:, numState:] = np.diag(dumvec, k=1) dumC[:SS_disc_nodelay.C.shape[0], :SS_disc_nodelay.C. shape[1]] = SS_disc_nodelay.C dumD[:SS_disc_nodelay.D.shape[0], :SS_disc_nodelay.D. shape[1]] = SS_disc_nodelay.D # Create delayed discrete state-space system SS_disc_delay = ctrl.ss(dumA, dumB, dumC, dumD, dt) return SS_disc_delay
def report_plant_id(plant, Xe, Ue, plant_ann, dt=0.005): Ac, Bc = plant.get_jacobian(Xe, Ue) s = p3_fr.SixDOFAeroEuler A1c = np.array([[Ac[s.sv_theta, s.sv_theta], Ac[s.sv_theta, s.sv_q]], [Ac[s.sv_q, s.sv_theta], Ac[s.sv_q, s.sv_q]]]) B1c = np.array([[Bc[s.sv_theta, plant.iv_de()]], [Bc[s.sv_q, plant.iv_de()]]]) ct_ss = control.ss(A1c, B1c, [[1, 0]], [[0]]) dt_ss = control.sample_system(ct_ss, dt, method='zoh') # ‘matched’, ‘tustin’, ‘zoh’ fmt_arg = {'precision':6, 'suppress_small':True} LOG.info('Real plant jacobian:\n{}\n{}'.format(np.array2string(dt_ss.A, **fmt_arg), np.array2string(dt_ss.B, **fmt_arg))) LOG.info(' modes: {}'.format(np.linalg.eig(dt_ss.A)[0])) w_identified = plant_ann.get_layer(name="plant").get_weights()[0] #print('Identified weights: \n{}'.format(w_identified)) A_id = np.matrix(w_identified[:2,:].T, dtype=np.float64) B_id = np.matrix(w_identified[2:,:].T, dtype=np.float64) LOG.info('Identified plant:\n{}\n{}'.format(np.array2string(A_id, **fmt_arg), np.array2string(B_id, **fmt_arg))) LOG.info(' modes: {}'.format(np.linalg.eig(A_id)[0]))
def test_div(self): # Make sure that sampling times work correctly sys1 = TransferFunction([1., 3., 5], [1., 6., 2., -1], None) sys2 = TransferFunction([[[-1., 3.]]], [[[1., 0., -1.]]], True) sys3 = sys1 / sys2 assert sys3.dt is True sys2 = TransferFunction([[[-1., 3.]]], [[[1., 0., -1.]]], 0.5) sys3 = sys1 / sys2 assert sys3.dt == 0.5 sys1 = TransferFunction([1., 3., 5], [1., 6., 2., -1], 0.1) with pytest.raises(ValueError): TransferFunction.__truediv__(sys1, sys2) sys1 = sample_system(rss(4, 1, 1), 0.5) sys3 = TransferFunction.__rtruediv__(sys2, sys1) assert sys3.dt == 0.5
def __init__(self, m=100., b=20., F=50., ts=0.05, tf=50): # sys parameters self.m = m self.b = b self.F_norm = F # Time parameters self.ts = ts self.t0 = 0 self.t1 = 5 self.t2 = 25 self.t3 = 30 self.tf = tf # Noise parameters self.sigma_pos_meas = 0.001 # m^2 self.Q = self.sigma_pos_meas self.sigma_vel = 0.01 # m^2/s^2 self.sigma_pos = 0.0001 # m^2 self.R = np.diagflat([self.sigma_pos, self.sigma_vel]) self.mu = np.array([[0.], [0.]]) self.SIG = np.eye(2) # SS parameters self.F = np.array([[0., 1.], [0., -self.b / self.m]]) self.G = np.array([[0.], [1. / self.m]]) self.H = np.array([1., 0.]) self.J = np.array([0.]) self.sys = mt.ss(self.F, self.G, self.H, self.J) self.sysd = ctl.sample_system(self.sys, self.ts, 'tustin') self.A = self.sysd.A self.B = self.sysd.B self.C = self.sysd.C self.D = self.sysd.D # States self.X_1 = np.array([[0.], [0.]]) self.X = np.array([[0.], [0.]]) # Measurements self.z = 0. + np.random.randn() * np.sqrt(self.Q)
def synth(self, plant, dt=0.01): lqr = planar_control.CtlPlaceFullLQR(plant, dt) B, C, D = [[0], [1], [0], [0]], [[0, 1, 0, 0]], [[0]] #H = hcu.get_precommand(lqr.A, lqr.B, C, lqr.K) #pdb.set_trace() cl_ss = control.ss(lqr.Acl, B, C, D) dt_ss = control.sample_system( cl_ss, dt, method='zoh') # ‘matched’, ‘tustin’, ‘zoh’ print(dt_ss) dt_tf = control.tf(dt_ss) print('desired closed loop tf') print(dt_tf) def err_fun(p): ctl_num, ctl_den = p[:3], [1, p[3], p[4]] ctl_tf = control.tf(ctl_num, ctl_den, dt) cl_tf = control.feedback(dt_tf, ctl_tf, sign=-1) pdb.set_trace() p0 = [-4.945, 8.862, -3.967, -1.481, 0.4812] err_fun(p0)
def test_sample_system_prewarp(self, tsys, plantname): """bilinear approximation with prewarping test""" wwarp = 50 Ts = 0.025 # test state space version plant = getattr(tsys, plantname) plant_fr = plant(wwarp * 1j) plant_d_warped = plant.sample(Ts, 'bilinear', prewarp_frequency=wwarp) dt = plant_d_warped.dt plant_d_fr = plant_d_warped(np.exp(wwarp * 1.j * dt)) np.testing.assert_array_almost_equal(plant_fr, plant_d_fr) plant_d_warped = sample_system(plant, Ts, 'bilinear', prewarp_frequency=wwarp) plant_d_fr = plant_d_warped(np.exp(wwarp * 1.j * dt)) np.testing.assert_array_almost_equal(plant_fr, plant_d_fr) plant_d_warped = c2d(plant, Ts, 'bilinear', prewarp_frequency=wwarp) plant_d_fr = plant_d_warped(np.exp(wwarp * 1.j * dt)) np.testing.assert_array_almost_equal(plant_fr, plant_d_fr)
def _get_distance_controller(self, delta_seconds): ''' Effects: create distance controller ''' KP_1 = 1.0 #1.0 KI_1 = 1.0 #1.0 num_pi = [-KP_1, -KI_1] # numerator of the PI transfer function (KP*s + KI) den_pi = [1.0, 0.01 * KI_1 / KP_1 ] # denominator of PI transfer function (s + 0.01*KI/KP) sys = control.tf( num_pi, den_pi ) # get transfer function for PI controller (since the denominator has a small term 0.01*KI/KP, it is actually a lag-compensator) sys = control.sample_system( sys, delta_seconds ) # discretize the transfer function (from s-domain which is continuous to z-domain) #since our simulation is discrete sys = control.tf2ss( sys) # transform transfer function into state space. self.distance_sys = sys
def test_simulator_run_for_ticks(self): """ This test checks the step response of the DifferenceEquation class vs the discrete transfer function for a specified number of ticks """ sample_time = 0.01 # [seconds] end_time = 20 coefficient_1 = 0.01 coefficient_2 = 0.001 coefficient_3 = 1 # Construct the Laplace operator s = ct.tf([1, 0], 1) step_input = 1 continuous_tf = (coefficient_3 * s**2 + coefficient_3 * coefficient_2 * s) / (coefficient_1 * s**2 + coefficient_2 * s + 1.5) discrete_tf = ct.sample_system(continuous_tf, sample_time, "zoh") difference_equation = de.DifferenceEquation(discrete_tf) num_points = int(end_time / sample_time) + 1 T = np.linspace(0, end_time, num_points) T, yout = ct.step_response(discrete_tf, T) # Create and run the difference equation simulator for the same number # of ticks as the control toolbox discrete controller deSim = de.DifferenceEquationSimulator(difference_equation) deSim.run_for_ticks(num_points, step_input) for i in range(0, len(T)): self.assertAlmostEqual(yout[i], deSim.full_output_history[i], 4)
V_max = 1.25 # Maximum velocity (m/s) theta_max = np.deg2rad(5) # maximum deflection (rad.) # Now, define the state-space form of the equations of motion # For derivation of these, you can see the Jupyter notebook at: # https://git.io/vxDsz A = np.array([[0, 1, 0, 0], [-g / l, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 0]]) B = np.array([[0], [1 / l], [0], [1]]) C = np.eye(4) # Output all states D = np.zeros((4, 1)) sys = control.ss(A, B, C, D) # Convert the system to digital. We need to use the discrete version of the # system for the MPC solution digital_sys = control.sample_system(sys, dt) # Get the number of states and inputs - for use in setting up the optimization # problem num_states = np.shape(A)[0] # Number of states num_inputs = np.shape(B)[1] # Number of inputs # Define the desired states to track. Here, it's just a desired final value # in the each of the states XD = 1.0 # desired position (m) XD_dot = 0.0 # desired velocity (m/s) thetaD = 0.0 # desired cable angle (rad) thetaD_dot = 0.0 # desired cable angular velocity (rad/s) # Define the weights on the system states and input q11 = 100 # The weight on error in angle from desired
# Form the continuous time version of the system A_cont = np.array([[0, 1], [-wn**2, -2*zeta*wn]]) B_cont = np.array([[0], [1/m]]) C_cont = np.array([[1, 0]]) #np.eye(2) D_cont = np.zeros((np.shape(C_cont)[0],np.shape(B_cont)[1])) sys = control.ss(A_cont, B_cont, C_cont, D_cont) # Now, digitize it digital_sys = control.sample_system(sys, dt) # And extract the state space components A = digital_sys.A B = digital_sys.B C = digital_sys.C D = digital_sys.D # Example arrays from the link in the preamble. Were used to check this code. # A = np.array([[1, 1],[0, 1]]) # B = np.array([[0.5],[1]]) # C = np.array([[1, 0]]) # D = np.zeros((np.shape(C)[0],np.shape(B)[1])) # Determine the number of states and inputs from the matrix shapes num_states = len(A)
#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Thu Nov 28 21:47:43 2019 Todos estos códigos requieren comentarios y una limpieza Limpio=NO Comentarios=NO @author: carlos """ import control as ct import sympy as sym import numpy as np import matplotlib.pyplot as plt from scipy import signal from matplotlib.ticker import (MultipleLocator) s = sym.symbols('s') num=[1] den= ( s*(s+1)*(s+5) ) ## pon esto en la terminal para obtener coeficientes sym.Poly(den, s).all_coeffs() G = ct.tf([1],[1, 6, 5, 0]) Gz = ct.sample_system(G,0.2) #zoh zero order hold es el predeterminado, retenedor de orden cero print(Gz) ## si pones Gz directamente en el terminal, sale más 'bonito'
bo = compute_block_ouptputs(states) # #### additional code for comparision A, B, C, D = get_linear_ct_model(theStateAdmin, sys_output) # create a control system with the python-control-toolbox, see # https://github.com/python-control/python-control import control cs = control.StateSpace(A, B, C, D) # use default method (zero oder hold (zoh)) cs_dt = control.sample_system(cs, 0.05) # calculate step-response for first input __, yy_dt = control.step_response(cs_dt, T=tt, input=0) # in the time-discrete case the result is shortened by one value. -> repeat the last one. yy_dt = np.column_stack((yy_dt, yy_dt[:, -1])) # __, yy_dt = control.step_response(cs,T=tt, input=0) # #### end of additional code
#%% dt = 0.25 InputValue = 1. #%% # Continuous state-space matrices A_cont = np.array([[0., 1.], [0., 0]]) B_cont = np.array([[0.], [1.]]) C_cont = np.array([[1., 0.], [0., 1.]]) D_cont = np.array([[0.], [0.]]) # Continuous system SS_cont = ctrl.ss(A_cont, B_cont, C_cont, D_cont) # Discrete system SS_disc_nodelay = ctrl.sample_system(SS_cont, dt, method='tustin') #%% T = np.arange(0, 100, dt) yout, T, xout = ctrl.lsim(SS_disc_nodelay, U=np.ones(T.shape) * InputValue, T=T) #%% fig = plt.figure() plt.plot(T, yout[0, :], label='Position') plt.plot(T, yout[1, :], label='Velocity') plt.plot(T, yout[1, :] / yout[0, :], label='Divergence') plt.axhline(y=dt, color='black', linestyle='--', label='dt') plt.ylim((-100, 100)) plt.legend()
import fem3d_2ss, weld1.filter1 # fem2ss = fem3d_2ss.Fem3d_fenics('data/v5/16x8x2/') # fem2ss = fem3d_2ss.Fem3d_fenics('data/v5/32x16x4/1500/') # fem2ss = fem3d_2ss.Fem3d_fenics('data/v5/40x20x5/') # fem2ss_2 = fem3d_2ss.Fem3d_fenics('data/v5/32x16x4/1500/k30/') # fem2ss = fem3d_2ss.Fem3d_fenics('data/v5/irregular/') fem2ss = fem3d_2ss.Fem3d_fenics('data/v5/irregular2/') fem2ss_2 = fem3d_2ss.Fem3d_fenics('data/v5/irregular2/k30/') # fem2ss.A.shape inp = np.array((.02,.02,0)) p1 = (.01,0,0) p2 = (0,0.005,0) plantp_d = control.sample_system(fem2ss.get_ss(inp + p1), .001) plantv_d = control.sample_system(fem2ss.get_ss_v(inp + p2), .001) # plant_d = plantp_d T = fem2ss.Teq control_on = True perturb_on = False # Controllers ctrl1_d = control.sample_system(control.tf(1, (1, 0)), .001) ctrl1 = weld1.filter1.Filter1((0,ctrl1_d.num[0][0][0]),ctrl1_d.den[0][0]) ctrl2_d = control.sample_system(control.tf(-0.0002, (1, 0)), .001) ctrl2 = weld1.filter1.Filter1((0,ctrl2_d.num[0][0][0]),ctrl2_d.den[0][0]) # Set points
from matplotlib import pyplot as plt __author__ = 'javier' import control import numpy as np steps = 500 Ts = 1.84e-3 time = steps * Ts setpoints = np.concatenate((np.ones(steps / 2), np.ones(steps / 2) * -1)) G = control.tf([52.1], [1.21, 1, 0]) C = control.tf([0.525, 5.022, 4.4], [0.005, 1, 0]) C_discrete = control.sample_system(C, Ts, method='tustin') G_discrete = control.sample_system(G, Ts, method='zoh') def factory(ctrl_in, sys): global fyk1, fyk2, fuk1, fuk2 # Simulate controller's transfer function den = sys.den[0][0] num = sys.num[0][0] # Transfer function parameters a1 = den[1] a2 = den[2] b0 = num[0] b1 = num[1]
Created on Wed Nov 27 00:49:43 2019 Todos estos códigos requieren comentarios y una limpieza Limpio=NO Comentarios=NO @author: carlos """ import control as ct import sympy as sym import numpy as np import matplotlib.pyplot as plt from scipy import signal from matplotlib.ticker import (MultipleLocator) Gp = ct.tf(100, [1, 0, 100]) ts = 0.05 Gz = ct.sample_system(Gp, ts) sym.pprint(Gz) t = np.arange(0.0, 50 * ts, ts) (num, den) = ct.tfdata(Gp) ## por que GP???? lsim no funciona con Gz num = np.array(num) ##np.asarray hace lo mismo convierte tuple/list a array den = np.array(den) num = num.flatten() den = den.flatten() do = ct.damp( Gz, doprint=True ) ##damp me devuelve en vez de un vector, un tuple (parecido a una matriz) que contiene todos los datos sys = signal.lti( num, den) ##transformo Gp en lti que es lo que acepta la syntax de lsim wn = do[ 0] ## 0 contiene wn, 1 contiene damping, 2 contiene eigen de los dos polos