Beispiel #1
0
    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)
Beispiel #2
0
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()
Beispiel #3
0
    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)
Beispiel #4
0
    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
Beispiel #6
0
    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)
Beispiel #7
0
 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
Beispiel #8
0
    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)
Beispiel #9
0
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()
Beispiel #10
0
 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]
Beispiel #12
0
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
Beispiel #13
0
    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)
Beispiel #14
0
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
Beispiel #17
0
    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)
Beispiel #18
0
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
Beispiel #20
0
    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")
Beispiel #21
0
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)
Beispiel #23
0
    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")
Beispiel #24
0
    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")
Beispiel #25
0
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
Beispiel #26
0
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
Beispiel #27
0
 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)       
Beispiel #28
0
    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)
Beispiel #29
0
    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")
Beispiel #30
0
    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()
Beispiel #31
0
 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()
Beispiel #32
0
 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)