notebook.community

Edit and run 





In [ ]:



    


import numpy as np
#from numpy import linalg as LA
from scipy import linalg as LA


import cvxpy
import optim_tools #own file with helper

import control as pc
import matplotlib.pyplot as plt
import itertools

from sysident import loadtools
%pylab inline
import shutil
import time



    











In [ ]:



    


print cvxpy.installed_solvers()
print cvxpy.__version__



    











In [ ]:



    


# Append integrator after velocity model
def add_position_state(A0, B0, C0):
    A = np.vstack((C0, A0))
    A = np.hstack((np.zeros((A.shape[0],1)), A))
    
    B = np.vstack((np.zeros((1, B0.shape[1])), B0))
    
    C = np.matrix(np.zeros(B.shape).T)
    C[0,0] = 1
    return A, B, C

#add_position_state(A, B, C)

# Gets all permutations (pos, neg) for boundary conditions
def getX00(x0):
    arrays = []
    X00 = []
    for x in x0:
        arrays.append((x, -x))
    for per in list(itertools.product(*arrays)):
        X00.append(np.matrix(per).T)
    return X00
    
#getX00([1.0, 0.5, 0.025])



## Helper functions for optimization problem
def Tri(i, n_u):
    if n_u>1:
        print "not implemented"
        print "alle permutationen mit einsen auf der hauptdiagonalen!"
        raise Error()
        
    if i==1:
        return np.eye(n_u)
    else:
        # only true for n_u == 1
        return np.zeros((n_u, n_u))

def negTri(i, n_u):
    return np.eye(n_u) - Tri(i, n_u)



    











In [ ]:



    


## Simulation functions
from numpy.linalg import solve, inv

class control_func_dyn_out():
    def __init__(self, Ak, Bk, Ck, Dk, Ek, A, B, C, D, umax=None, dT=1e-3):
        self.Ak = Ak
        self.Bk = Bk
        self.Ck = Ck
        self.Dk = Dk
        self.Ek = Ek
        self.umax = umax
        self.dT = dT

        self.z = np.zeros(B.shape)
                
        # Construct Prefilter
        C2 = np.hstack((-C, np.zeros(Ck.shape)))

        A2_u = np.hstack(((A+B.dot(Dk).dot(C)), B.dot(Ck)))
        A2_d = np.hstack((Bk.dot(C), Ak))

        A2 = np.vstack((A2_u, A2_d))
        B2 = np.vstack((B, np.zeros(Bk.shape)))

        self.N = inv(C2.dot(inv(A2)).dot(B2))
        #print self.N
        
    def estimate(self, y, u):
        # already saturated in this case
        #if self.umax is not None:
        #    u = optim_tools.sat(u, self.umax)
        
        z_dot = self.Ak.dot(self.z) + self.Bk.dot(y) + self.Ek.dot(u)
        return self.z + z_dot*self.dT
        
    def regulate(self, y, s, x):
        u = self.N.dot(s)+self.Ck.dot(self.z) + self.Dk.dot(y)

        # Saturate 
        if self.umax is not None:
            u = optim_tools.sat(u, self.umax)
            
        self.z = self.estimate(y, u)
        return u
##### Only a specific test ##### # List of files to be used as synthesis model fileX = "/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/acc_10/iekf_microssim_nonoise/20181203-200402_cplx.0_real.3.npy" fileY = "/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/acc_10/iekf_microssim_nonoise/20181205-121831_cplx.0_real.3.npy" x = loadtools.getIEKF(fileX) y = loadtools.getIEKF(fileY) assert y.keys() == x.keys() assert x.keys() == y.keys() for v in x.keys(): assert(np.array_equal(y[v], x[v])) print "-> Identical IEKF Data" 




In [ ]:



    


folder_tony_models = "/home/lth/jupyter_nb/optimization/models/"

folder_acc2 = '/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/acc_2/'
folder_acc10 = '/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/acc_10/'

fnames_iekf = [
    folder_acc2+"iekf_microssim_highnoise/20181206-090206_cplx.0_real.3.npy",
    folder_acc2+"iekf_microssim_midnoise/20181206-090250_cplx.0_real.3.npy",
    folder_acc2+"iekf_microssim_midnoise_nodelay/20181206-090325_cplx.0_real.3.npy",
    folder_acc2+"iekf_microssim_nonoise/20181206-085840_cplx.0_real.3.npy",
    folder_acc2+"iekf_microssim_nonoise_nodelay/20181206-090511_cplx.0_real.3.npy",
    folder_acc2+"iekf_preload_microssim_highnoise/20181206-090554_cplx.0_real.3.npy",
    folder_acc2+"iekf_preload_microssim_highnoise/20181206-090554_cplx.0_real.3.npy",
    folder_acc2+"iekf_preload_microssim_nonoise/20181206-090717_cplx.0_real.3.npy",
    folder_acc10+"iekf_microssim_highnoise/20181206-091132_cplx.0_real.3.npy",
    folder_acc10+"iekf_microssim_midnoise/20181206-091201_cplx.0_real.3.npy",
    folder_acc10+"iekf_microssim_midnoise_nodelay/20181206-091227_cplx.0_real.3.npy",
    folder_acc10+"iekf_microssim_nonoise/20181205-121831_cplx.0_real.3.npy",
    folder_acc10+"iekf_microssim_nonoise_nodelay/20181206-091253_cplx.0_real.3.npy",
    folder_acc10+"iekf_preload_microssim_highnoise/20181206-091336_cplx.0_real.3.npy",
    folder_acc10+"iekf_preload_microssim_midnoise/20181206-091416_cplx.0_real.3.npy",
    folder_acc10+"iekf_preload_microssim_nonoise/20181206-091437_cplx.0_real.3.npy",
]    

fnames_powell = [
    folder_acc2+"iekf_microssim_nonoise/ss_20181129-111322_poles3_powell_ident_pade1_0.0314547475844.npy",
    folder_acc2+"iekf_microssim_nonoise/ss_20181130-104001_poles3_powell_ident_pade1_0.0309673208056.npy",
]
### TODO: Iterate over list
#fname = fnames_iekf[0]
###


#_Ax, _Bx, _Cx, D = loadtools.getModel('/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/acc_2/iekf_microssim_nonoise/ss_20181130-104001_poles3_powell_ident_pade1_0.0309673208056.npy')

# Get one sample
#_Ax, _Bx, _Cx, D = loadtools.getIEKFModel(fnames_iekf[0])
#A, B, C = add_position_state(_Ax, _Bx, _Cx)



    











In [ ]:



    


# Eigenvalue restrictions
eig_ro = 2000 # Real <=-ro
eig_ni = 0.1  # |Imag| <= ni*Real

# U_max
u_max = 2.0
U_max = [u_max]

# *not* transformed Limitations [pos, vel, acc, jerk]
xx0 = [1.0, 2.0, 5, 1]

store_files = False



    











In [ ]:



    


#%%time

for fname in fnames_powell:
    try:
        ####################################
        # Load the model                   #
        ####################################
        try:
            _Ax, _Bx, _Cx, D = loadtools.getIEKFModel(fname)
        except:
            _Ax, _Bx, _Cx, D = loadtools.getModel(fname)
        A, B, C = add_position_state(_Ax, _Bx, _Cx)

        print A, B, C, D
        ####################################
        # Define and transform limits      #
        ####################################
        print 'Limits:', xx0[0:len(B)] # Limit is cut to appropriate length
        X00 = getX00(xx0[0:len(B)])

        # Transform into reachable form to specify boundaries useful
        (A1, B1, C1, D1), T1, Q1 = optim_tools.get_Steuerungsnormalform(A, B, C.T, D)
        X0 = [T1.dot(x0) for x0 in X00]


        ####################################
        # Initialize Optimisation Problem  #
        ####################################
        #### Satz 6.6

        # Init
        n = B.shape[0] # get dim of system
        n_u = B.shape[1] 

        # Variables
        X = cvxpy.Variable((n, n), PSD=True)
        Y = cvxpy.Variable((n, n), PSD=True)
        W = cvxpy.Variable((n_u, n_u))

        Ak_h = cvxpy.Variable(A.shape, PSD=True)
        Bk_h = cvxpy.Variable(B.shape)
        Ck_h = cvxpy.Variable(C.shape)
        Dk_h = cvxpy.Variable(D.shape)

        Ch = cvxpy.Variable(C.shape)
        Dh = cvxpy.Variable(D.shape)

        # Substitutionen
        C_hat = lambda i: Tri(i, n_u)*Ck_h + negTri(i, n_u)*Ch
        D_hat = lambda i: Tri(i, n_u)*Dk_h + negTri(i, n_u)*Dh

        Xi = cvxpy.bmat([[ A*X + B*Ck_h, A + B*Dk_h*C ],
                         [ Ak_h,         Y*A + Bk_h*C ]])

        I = np.eye(n)

        # Bisection parameter
        g = cvxpy.Parameter(nonneg=True)

        # Pole restriction
        ro = cvxpy.Parameter(nonneg=True) # Real <=-ro
        ni = cvxpy.Parameter(nonneg=True) # |Imag| <= ni*Real

        ro.value = eig_ro
        ni.value = eig_ni


        poles = pc.pole(pc.ss(A, B, C, D))
        if any(poles.real < -ro.value) or any(poles.imag < ni.value*poles.real):
            print "WARNING: Pole restriction too tight for model?"
            print "Poles:", poles

            #raw_input("Press Enter to continue...")


        # Define Constraints
        # (6.39a)
        const_639a = cvxpy.bmat([[X, I],
                                 [I, Y]]) >> 0
                                 #[I, Y]]) == cvxpy.Variable((2*n, 2*n), PSD=True)



        # (6.39b)
        const_639b = cvxpy.bmat([[ X*A.T + A*X + B*Ck_h + (B*Ck_h).T, Ak_h.T + A + B*Dk_h*C            ],
                                 [ cvxpy.Variable((n, n)),          A.T*Y + Y*A + Bk_h*C + (Bk_h*C).T]]) + \
                     2*g*cvxpy.bmat([[X, I],
                                     [I, Y]]) << 0
                                     #[I, Y]]) == -cvxpy.Variable((2*n, 2*n), PSD=True)

        # (6.39c)
        const_639c = [cvxpy.bmat([[ X*A.T + A*X + B*C_hat(i) + (B*C_hat(i)).T, Ak_h.T + A + B*D_hat(i)*C            ],
                                  [ cvxpy.Variable((n, n)),                      A.T*Y + Y*A + Bk_h*C + (Bk_h*C).T]]) << 0 for i in range(2, (2**n_u)+1)]
                                  #[ cvxpy.Variable((n, n)),                      A.T*Y + Y*A + Bk_h*C + (Bk_h*C).T]]) == -cvxpy.Variable((2*n, 2*n), PSD=True) for i in range(2, (2**n_u)+1)]


        # (6.39d)
        const_639d = cvxpy.bmat([[ X,  I,    Ch.T     ],
                                 [ I,  Y,    (Dh*C).T ],
                                 [ Ch, Dh*C, W        ]]) >> 0
                                 #[ Ch, Dh*C, W        ]]) == cvxpy.Variable((2*n+n_u, 2*n+n_u), PSD=True)

        const_639e = [W[j,j] <= U_max[j]**2 for j in range(0, n_u)]

        const_639f = [ X0[k].T*Y*X0[k] <= 1.0
                                    for k in range(0, len(X0))]

        const_621a = Xi.T + Xi + ro*cvxpy.bmat([[X, I],
                                                [I, Y]]) >> 0
                                                #[I, Y]]) == cvxpy.Variable((2*n, 2*n), PSD=True)

        const_621b = cvxpy.bmat([[ ni*(Xi.T + Xi), Xi.T - Xi ],
                                 [ -Xi.T + Xi,     ni*(Xi.T + Xi) ]]) << 0
                                 #[ -Xi.T + Xi,     ni*(Xi.T + Xi) ]]) == -cvxpy.Variable((4*n, 4*n), PSD=True)


        # Collect all constraints
        constraints_639 = []
        constraints_639.append(const_639a)
        constraints_639.append(const_639b)
        constraints_639.extend(const_639c)
        constraints_639.append(const_639d)
        constraints_639.extend(const_639e)
        constraints_639.extend(const_639f)

        constraints_639.append(const_621a)
        constraints_639.append(const_621b)


        # Form problem.
        prob_66 = cvxpy.Problem(cvxpy.Minimize(0), constraints_639)

        # bisection with one solver
        #[[X_o, Y_o, W_o,
        #  Ak_h_o, Bk_h_o, Ck_h_o, Dk_h_o,
        #  Ch_o, Dh_o], g_o] = optim_tools.bisect_max(0, None, prob_66, g, [X, Y, W, Ak_h, Bk_h, Ck_h, Dk_h, Ch, Dh], bisect_verbose=True,
        #                                                      bisection_tol=0.01,
        #                                                      #solver=cvxpy.CVXOPT, verbose=False,  max_iters=50000)
        #                                                      solver=cvxpy.MOSEK, verbose=False)
        #                                                      #solver=cvxpy.SCS, max_iters=50000)


        ####################################
        # Solve the problem                #
        ####################################
        # bisection alternative with list of multiple solvers
        [[X_o, Y_o, W_o,
          Ak_h_o, Bk_h_o, Ck_h_o, Dk_h_o,
          Ch_o, Dh_o], g_o] = optim_tools.bisect_max2(0, None, prob_66, g, [X, Y, W, Ak_h, Bk_h, Ck_h, Dk_h, Ch, Dh],
                                                              bisect_verbose=True,
                                                              bisection_tol=0.1,
                                                              solvers=[
                                                                       (cvxpy.CVXOPT, {'verbose':False}),
                                                                       (cvxpy.MOSEK, {'verbose':False}),
                                                                       #(cvxpy.SUPER_SCS, {'max_iters':50000, 'verbose':True}),
                                                                       #(cvxpy.SCS, {'max_iters':50000, 'warm_start':True, 'verbose':True})
                                                                      ])
        print "g:", g_o


        ####################################
        # Reconstruct the parameters       #
        ####################################
        # QR Decomposition for M and N
        M, NT = LA.qr(I - X_o.dot(Y_o))
        N = NT.T
        #assert(np.allclose(I - X_o.dot(Y_o), M.dot(N.T)))

        # LU Decomposition for M and N
        #M, L, U = LA.lu(I - X_o.dot(Y_o))
        #N = L.dot(U).T

        # Reconstruction
        Ek = -LA.solve(N, Y_o.dot(B))
        Dk = np.matrix(Dk_h_o)

        Ck = LA.solve(M, (Ck_h_o - Dk.dot(C).dot(X_o)).T).T
        #Ck_1 = (Ck_h_o - Dk.dot(C).dot(X_o)).dot(LA.inv(M).T) #Check
        #assert(np.allclose(Ck_1, Ck))

        Bk = LA.solve(N, Bk_h_o)

        temp_alpha = LA.solve(N, (Ak_h_o-Y_o.dot(A).dot(X_o)))
        temp_beta = Bk.dot(C).dot(X_o)
        Ak = (LA.solve(M, temp_alpha.T) - LA.solve(M, temp_beta.T)).T

        #Ak_1 = LA.solve(N, (Ak_h_o-Y_o.dot(A).dot(X_o))).dot(LA.inv(M).T) - Bk.dot(C).dot(X_o).dot(LA.inv(M).T) #Check
        #assert(np.allclose(Ak_1, Ak))

        ####################################
        # Output the results               #
        ####################################
        print Ak
        print Bk
        print Ck
        print Dk
        print Ek
        print u_max
        #print dT
        print
        print A
        print B
        print C
        print D



        ####################################
        # Simulation of the step response  #
        ####################################
        # Timeline
        T = np.arange(0, 2, 1e-3) 

        #s: input, e.g., step function with amplitude of 0.2
        #s = np.zeros(len(T));
        s = np.ones(len(T))*1;


        # Initial state
        x0 = np.zeros(B.shape)


        y1, u1, u1_sat = optim_tools.simulate(A, B, C, D, 
                                           control_func_dyn_out(Ak, Bk, Ck, Dk, Ek,
                                                                A, B, C, D,
                                                                umax=u_max).regulate,
                                           s, T, delay=None, umax=u_max, x0=x0)



        ####################################
        # Plot the step response           #
        ####################################
        pylab.rcParams['figure.figsize'] = (10, 6)

        line_s, = plt.plot(T[:], s, 'b', label='reference')
        line0, = plt.plot(T[:], np.array(y1[0,:].T), 'r', label='dyn_out')


        #first_legend = plt.legend(handles=[line1], loc=1)
        plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
        plt.xlabel('T')
        plt.ylabel('y')
        plt.title('Closed Loop Step Response')
        plt.show()


        line0, = plt.plot(T, u1, 'r--', label='u1')


        #>first_legend = plt.legend(handles=[line1, line2, line1b, line2b], loc=1)
        plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
        plt.xlabel('T')
        plt.ylabel('u')
        plt.title('Output values')
        plt.show()


        if store_files:
            ####################################
            # Store in npy file                #
            ####################################
            poles = pc.pole(pc.ss(A, B, C, D))
            #print poles

            # Creates new file
            fname_new = fname[:-4]+'_control_{}.npy'.format(time.strftime("%Y%m%d-%H%M%S"),
                                                len(poles))
            # Copy to not destroy something
            shutil.copyfile(fname, fname_new)

            loadtools.saveModel(fname_new, A, B, C, D)
            loadtools.saveControl(fname_new, Ak, Bk, Ck, Dk, Ek, u_max)
            loadtools.saveNPY(fname_new, ro=ro.value, ni=ni.value, X0=X0)


            loadtools.saveNPY(fname, plot_control_step_y=(T[:], np.array(y1[0,:].T)),
                                     plot_control_step_u=(T, u1),
                                     plot_control_step_s=(T[:], s))
    except Exception as e:
        print e
        print fname


print 
print '-----------------'
import os import sdpt3glue import random g.value = 1 print g.value print prob_66.solve(solver=cvxpy.MOSEK, verbose=False)
ran = random.randint(0, 100) folder ='/tmp/' # Generate filenames matfile_target = os.path.join(folder, 'matfile_{}.mat'.format(ran)) # Where to save the .mat file to print matfile_target output_target = os.path.join(folder, 'output_{}.txt'.format(ran)) # Where to save the output log print output_target #g.value = 1 result = sdpt3glue.sdpt3_solve_problem(prob_66, sdpt3glue.OCTAVE, matfile_target, output_target=output_target) 




In [ ]:



    






    











In [ ]:



    






    











In [ ]:



    






    











In [ ]:

Content source: ipa-lth/jupyter_nb

Similar notebooks:

notebook.community | gallery | about