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
Beispiel #2
0
    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
Beispiel #3
0
    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)
Beispiel #4
0
    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)
Beispiel #5
0
 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
Beispiel #6
0
    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]
Beispiel #9
0
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
Beispiel #11
0
 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)
Beispiel #13
0
 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')
Beispiel #14
0
 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)
Beispiel #16
0
    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
Beispiel #18
0
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
Beispiel #20
0
    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)
Beispiel #21
0
    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)
Beispiel #22
0
    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)
Beispiel #25
0
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)
Beispiel #27
0
#!/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

Beispiel #29
0
#%%
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
Beispiel #31
0
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]
Beispiel #32
0
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