

In [ ]:

    
import numpy as np
import control as con
from numpy import linalg as LA

import cvxpy
import optim_tools #own file with helper
import boris_tools

import matplotlib.pyplot as plt
%matplotlib inline



In [ ]:

    
print cvxpy.installed_solvers()

import itertools as it def getAllBoundaries(x): X = [] for i, elem in enumerate(x): for i2, elem2 in enumerate(x): if i == i2: pass else: print elem2 getAllBoundaries([0.9, 2.8])

############################## # Boris Diss Reactor # ############################## A = np.matrix([[1, 0], [0, -0.5]]) b = np.matrix([[1],[-0.5]]) c = np.matrix([1, 0]) d = np.matrix([0]) u_max = 1.0 n = 2 X0 = [np.matrix([-0.9, -2.8]).T, np.matrix([-0.9, 2.8]).T, np.matrix([0.9, -2.8]).T, np.matrix([0.9, 2.8]).T] x_max = [0.9, 2.8] #print "A:\n", A #print "a:\n", a #print "b:\n", b #print "c:\n", c ##### Entwurf parameter ##### #beta = 2 # beta >=1 !



In [ ]:

    
#############################
# Roboter pt2 (ohne delay)  #
#############################

A = np.matrix([[  0.,         -55.99932527],
               [ 10.,         -43.64272128]])

b = np.matrix([[-5.58731344],
               [ 0.        ]])

c = np.matrix([ 0.,  -10.])
              
d = np.matrix([0])
u_max = 0.5
n = len(b)

delay = 0.032

X00 = [np.matrix([-0.5, -0.025]).T,
       np.matrix([-0.5,  0.025]).T,
       np.matrix([ 0.5, -0.025]).T,
       np.matrix([ 0.5,  0.025]).T,
      ]

X0 = X00



In [ ]:

    
A0 = A
b0 = b
c0 = c
d0 = d



In [ ]:

    
###########################
# Roboter pt2d            #
###########################

A = np.matrix([[  0,    0,   35        ],
               [ 10,    0,   32.8766333],
               [  0, -100, -106.142721 ]])

#A = np.matrix([[2.84217094e-14,  1.71379179e+01],
#                [-1.00000000e+02,-1.85369064e+02]])
#a = -A[-1,:].T ### !!!!!
#print a
#b = np.matrix([[ 17.34396868], [  9.87641374]])
b = np.matrix([[34.92070901], [-5.58731344], [ 0.]])

#c = np.matrix([0, -1])
c = np.matrix([ 0.,  0., -1.])
              
d = np.matrix([0])
u_max = 0.5
n = len(b)

X00 = [np.matrix([1.0,  -0.5, -0.025]).T,
         np.matrix([1.0,  -0.5,  0.025]).T,
         np.matrix([1.0,   0.5, -0.025]).T,
         np.matrix([1.0,   0.5,  0.025]).T,
         np.matrix([-1.0, -0.5, -0.025]).T,
         np.matrix([-1.0, -0.5,  0.025]).T,
         np.matrix([-1.0,  0.5, -0.025]).T,
         np.matrix([-1.0,  0.5,  0.025]).T,
      ]

X0 = X00

########################### # Roboter pt1d + pos # ########################### pt2_smooth_2 A0 = np.matrix([[0, 1734.396868, -9.87641374], [0, 0, 1.0 ], [0, -1713.8, -185.369064]]) b0 = np.matrix([[ 0. ], [ 0. ], [ 1. ]]) c0 = np.matrix([1, 0, 0]) d0 = np.matrix([0]) u_max = 0.3 n = 3 X00 = [np.matrix([-2.0, -1.0, -0.25]).T, np.matrix([-2.0, -1.0, 0.25]).T, np.matrix([-2.0, 1.0, -0.25]).T, np.matrix([-2.0, 1.0, 0.25]).T, np.matrix([ 2.0, -1.0, -0.25]).T, np.matrix([ 2.0, -1.0, 0.25]).T, np.matrix([ 2.0, 1.0, -0.25]).T, np.matrix([ 2.0, 1.0, 0.25]).T, ] (A,b,c,d), T0, Q = optim_tools.get_Steuerungsnormalform(A,b,c.T,d) X0 = [T0.dot(x0) for x0 in X00]



In [ ]:

    
### Design Sättigungsregler mittels konvexer Hülle (A.3)

# Variables (Convex)
#  Name in A.3 | Name in Program
#    Q  = P^⁻1 |   = Q
#    z  = Ql   |   = z0
#    z* = Ql*  |   = z1

# Variables (Quasi convex)
#    gamma     |   = g

# Parameter
#    mu        |   = mu (Designparameter)
#    X0        |   = X0 = [x0,...]
#    A         |   = A
#    b         |   = b

# Initialize
n = len(b) # get dim of system

# Define Variables
Q  = cvxpy.Semidef(n, n) #symmetric and positive semidefinite
#Q  = cvxpy.Variable(n, n) # Semidef could go as an additional constraint as well, thereby no need fo Q to be symmetric
#const_sdQ = Q >> 0 # semidef but not (necessarily) symmetric

z0 = cvxpy.Variable(n)
z1 = cvxpy.Variable(n)

# Bisection parameter
g = cvxpy.Parameter(sign='positive')
#g.value = g_val # TODO set by bisection loop (function?)

mu = cvxpy.Parameter(sign='positive')
#m.value = 1 # TODO: mu* >=1

# Define Constraints
# (A.10)
const_A10 = [cvxpy.bmat([[Q,       X0[i]],
                         [X0[i].T, 1    ]]) >> 0
                            for i in range(0, len(X0))]

# (A.11)
const_A11 = cvxpy.bmat([[Q,    z0],
                        [z0.T, 1 ]]) >> 0

# (A.12)
const_A12 = cvxpy.bmat([[Q,    z1   ],
                        [z1.T, mu**2]]) >> 0

# (A.13)
const_A13 = Q*A + A.T*Q - b*z0.T - z0*b.T << 0 # This constraint is strict definit

# (A.14)
const_A14 = Q*A + A.T*Q - b*z1.T - z1*b.T << -2*g*Q # This constraint is strict definit
#const_A14 = Q*A + A.T*Q - b*z1.T - z1*b.T << cvxpy.bmat([[-2*g, 0, 0],
#                                                         [0, -2*g, 0],
#                                                         [0, 0, -2*g]])*Q # This constraint is strict definit


# Collect all constraints
constraints_A15 = []
constraints_A15.extend(const_A10) ##!! Beware of the "extend" if input is array
constraints_A15.append(const_A11)
constraints_A15.append(const_A12)
constraints_A15.append(const_A13)
constraints_A15.append(const_A14)


# Feasibility for bisection:
obj_A15 = cvxpy.Minimize(0)

# Stronger requirements for bisection? -> probably resulting in higher iterations, thus better results: TODO: Fix bisect!
# obj_alt = cvxpy.Maximize(cvxpy.log_det(Q)) # Identical to geo_mean (in term of convexity and result)

# Form and solve problem.
prob_A15 = cvxpy.Problem(obj_A15, constraints_A15)



In [ ]:

    
%%time
# bisection
mu.value = 5 # TODO: mu* >=1

[[Q_o, z0_o, z1_o], g_o] = optim_tools.bisect_max(0, None, prob_A15, g, [Q, z0, z1], bisect_verbose=True,
                                                      solver=cvxpy.CVXOPT)
                                                      #solver=cvxpy.SCS, max_iters=500000)

print
print "Sättigungsregler mittels konvexer Hülle -> Bisection(Max. Abklingrate)"
print "Bisection Param:", g_o

# Output
print "Status:", prob_A15.status

Q_A15 = Q_o
P_A15 = LA.inv(Q_o)
l0_A15 = LA.solve(Q_o, z0_o)
l1_A15 = LA.solve(Q_o, z1_o)

print "Q:\n", Q_A15
print "P:\n", P_A15
print "z:\n", z0_o
print "z*:\n", z1_o
print "l:\n", l0_A15
print "l*:\n", l1_A15

print

import cvxopt as cvx import picos as pic import numpy as np from numpy import linalg as LA ##LYAPUNOV STABILITY def lin_lyap(A, solv): # matrix A in nxn n=np.shape(A) print n F = pic.Problem() A=pic.new_param("A",A) P = F.add_variable('P',n,'symmetric') #objetive maximize the trace of P # if the objective is just to find a solution, then comment the next line F.set_objective('max','I'|P) #('I' | Z.real) works as well F.add_constraint(A.H*P+P*A<<0 ) F.add_constraint(P>>0) print F F.solve(solver=solv,verbose = 0) #print 'fidelity: F(P,Q) = {0:.4f}'.format(F.obj_value()) print 'optimal matrix P:' P= np.array(P.value) A=np.array(A.value) print P print "\n the Eigenvalues of P are:" print LA.eigvals(P) print "\nSolution test" print LA.eigvals(np.transpose(A)*P+P*A) return P ## LINEAR LUENBERGER OBSERVER def lin_obsv(A, C, solv): ## A, C are knwon matrices of the LTI system #solv: choose the solver for example mosek, sdpa, cvxopt # The solvers need to be installed separatelly # http://picos.zib.de/intro.html#solvers #size of matrices A and B [n,n] =np.shape(A) [p,n] =np.shape(C) #starting the optimization problem F = pic.Problem() #Add parameters and variables A=pic.new_param('A',A) C=pic.new_param('C',C) P = F.add_variable('P',(n,n),'symmetric') Q = F.add_variable('Q',(n,p)) F.add_constraint(A.T*P-C.T*Q.T+P*A-Q*C<<0 ) F.add_constraint(P>>0) #optimzation objective #by default the objetive is to maximize the trace of P #comment the next line if the objetive is just to find any solution F.set_objective('max','I'|P) print(F) #solving the LMI, with selected solver F.solve(solver=solv, verbose = 0) P= np.matrix(P.value) Q= np.matrix(Q.value) print('optimal matrix P:') print(P) print('matrix Q:') print(Q) print("\n the Eigenvalues of P are:") print(LA.eigvals(P)) print("\nSolution test") #print(LA.eigvals(A.T*P+P*A)) L=P.I*Q print('The gain matrix L is:') print(L) return L ## LINEAR FEEDBACK CONTROLLER def lin_fbcon(A,B,solv): ## A, B are knwon matrices of the LTI system #solv: choose the solver for example mosek, sdpa, cvxopt # The solvers need to be installed separatelly # http://picos.zib.de/intro.html#solvers #size of matrices A and B [n,n] =np.shape(A) [n,m] =np.shape(B) #starting the optimization problem F = pic.Problem() #Add parameters and variables A = pic.new_param('A',A) B = pic.new_param('B',B) P = F.add_variable('P',(n,n),'symmetric') Q = F.add_variable('Q',(m,n)) F.add_constraint(P*A.T-Q.T*B.T+ A*P-B*Q<<0 ) F.add_constraint(P>>0) #optimzation objective #by default the objetive is to maximize the trace of P #comment the next line if the objetive is just to find any solution F.set_objective('max','I'|P) print(F) #solving the LMI, with selected solver F.solve(solver=solv, verbose = 0) P= np.matrix(P.value) Q= np.matrix(Q.value) print('optimal matrix P:') print(P) print('matrix Q:') print(Q) print("\n the Eigenvalues of P are:") print(LA.eigvals(P)) print("\nSolution test") #print(LA.eigvals(A.T*P+P*A)) K=Q*P.I print('The gain matrix K is:') print(K) return K



In [ ]:

    
# Lens 5.1.2 Entwurf mittels Anpassung der Stellgrößenbeschränkung

# Schritt 1: Reglerentwurf nach Optimierungsproblem 3.4, mit (3.19d) = eps*u_max

# Initialize
n = len(b) # get dim of system

# Define Variables
Q  = cvxpy.Semidef(n, n) #symmetric and positive semidefinite
#Q  = cvxpy.Variable(n, n) # Semidef could go as an additional constraint as well, thereby no need fo Q to be symmetric
#const_sdQ = Q >> 0 # semidef but not (necessarily) symmetric

z0 = cvxpy.Variable(n)
z1 = cvxpy.Variable(n)

# Bisection parameter
g = cvxpy.Parameter(sign='positive')
#g.value = g_val # TODO set by bisection loop (function?)

mu = cvxpy.Parameter(sign='positive')
#m.value = 1 # TODO: mu* >=1

# Define Constraints
# (A.10)
const_A10 = [cvxpy.bmat([[Q,       X0[i]],
                         [X0[i].T, 1    ]]) >> 0
                            for i in range(0, len(X0))]

# (A.11)
const_A11 = cvxpy.bmat([[Q,    z0],
                        [z0.T, 1 ]]) >> 0

# (A.12)
const_A12 = cvxpy.bmat([[Q,    z1   ],
                        [z1.T, mu**2]]) >> 0

# (A.13)
const_A13 = Q*A + A.T*Q - b*z0.T - z0*b.T << 0 # This constraint is strict definit

# (A.14)
const_A14 = Q*A + A.T*Q - b*z1.T - z1*b.T << -2*g*Q # This constraint is strict definit
#const_A14 = Q*A + A.T*Q - b*z1.T - z1*b.T << cvxpy.bmat([[-2*g, 0, 0],
#                                                         [0, -2*g, 0],
#                                                         [0, 0, -2*g]])*Q # This constraint is strict definit


# Collect all constraints
constraints_A15 = []
constraints_A15.extend(const_A10) ##!! Beware of the "extend" if input is array
constraints_A15.append(const_A11)
constraints_A15.append(const_A12)
constraints_A15.append(const_A13)
constraints_A15.append(const_A14)



In [ ]:

    
# Lens 5.2.1: Sättigender lin. Regler mit Beobachterentwurf

#Schritt 1: Initialisierung
# max(gamma_tri) s.t. (5.16)

n = len(b) # get dim of system

# Define Variables
P_tri = cvxpy.Semidef(n, n) #symmetric and positive semidefinite
H_tri = cvxpy.Semidef(n, n)
K_tri = cvxpy.Semidef(n, n)
L_tri = cvxpy.Semidef(n, n)

gamma_tri = cvxpy.Variable(1)

print pic.tools.available_solvers() l_opt = lin_obsv(A, c, 'cvxopt') k_opt = lin_fbcon(A, b, 'cvxopt') print lin_lyap(A, 'cvxopt')



In [ ]:

    
from numpy.linalg import solve

def control_func(y, s, x, k, A, b, c):
    #v = -np.linalg.inv(c.dot(np.linalg.inv(A-b.dot(k.T))).dot(b))
    #u = v.dot(s)-k.T.dot(x)
    N = -c.dot(solve(A-b.dot(k.T), b))
    u = solve(N, np.array([s]))-k.T.dot(x)
    return u

class control_func_luenberger():
    def __init__(self, k, l, A, b, c, umax=None):
        self.k = k
        self.l = l
        self.A = A
        self.b = b
        self.c = c
        self.N = -c.dot(solve(A-b.dot(k.T), b))
        self.umax = umax
        
        self.x = np.zeros((len(b),1))

    def estimate(self, y, u, x):
        #print "OBS: u:", u
        #print "OBS: y:", y
        #print "OBS: self.x(n):", self.x
        
        x_dot = (np.identity(len(self.b))-self.l.dot(self.c)).dot(self.A.dot(self.x) + self.b.dot(u)) + self.l.dot(y)
        #print "OBS: x_dot(n)", x_dot
        
        #x_dot = (self.A-self.l.dot(self.c)).dot(self.x) + self.b.dot(s) + self.l.dot(self.c).dot(x)
        #print "x_dot (Luen mit x)", x_dot

        return self.x + x_dot*1e-3
        
    def regulate(self, y, s, x):
        #print "CON: x(n):", x
        #print "CON: self.x(n):", self.x
        #print (self.A - self.l.dot(self.c)).dot(x - self.x)
        u = solve(self.N, np.array([s]))-self.k.T.dot(self.x)
        
        # Saturate 
        if self.umax is not None:
            u = optim_tools.sat(u, self.umax)
            
        self.x = self.estimate(y, u, x)
        #print "CON: self.x(n+1):", self.x

        
        return u

T = np.arange(0, .5, 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((len(b),1))

T[1]-T[0]



In [ ]:

    
import collections
import math
def sat(v, u_max):
    return np.clip(v, -u_max, u_max)

''' State Space Simulator '''
def simulate(A, B, C, D, regulator_func, s, T, delay=None, umax=None, x0=0):
    #intitialize y, u
    y = np.matrix(np.zeros((C.shape[0],len(T))))
    u = np.zeros((len(T),np.size(x0,1)))
    u_sat = np.zeros((len(T),np.size(x0,1)))
    if type(x0) is int:
        xt = np.matrix([x0]*len(A)).T
        print "x0 = \n{}".format(xt)
    else:
        xt = x0

    if delay:
        s_queue = collections.deque(maxlen=int(math.ceil(delay/(T[1]-T[0]))))
        
    for i, t in enumerate(T):
        if delay:
            s_queue.append(s[i])
            if len(s_queue) == int(math.ceil(delay/(T[1]-T[0]))):
                s_delay = s_queue[0]
            else:
                s_delay = 0
        else:
            s_delay = s[i]
        
        #print "----------------------------", i
        #print "SS: x:", x_delay
        #print "SS: y:", y[:,i]
        u[[i],:] = regulator_func(y[:,i], s_delay, xt)

        #if i >= 2:
        #    raise Exception
        
        if umax is not None:
            u_sat[[i],:] = sat(u[[i],:], umax)
        else:
            u_sat[[i],:] = u[[i],:]

        x_dot = A.dot(xt) + B.dot(u_sat[[i],:])
        #print "SS: x_dot:", x_dot
        
        y[:,i] = C.dot(xt) + D.dot(u_sat[[i],:])

        if i < len(T)-1:
            xt = xt + x_dot*(T[i+1]-T[i])
            #print "x_dot (SS)", x_dot
    return y, u, u_sat



In [ ]:

    
#l = place(A',C',p).'

#l = con.place(A.T, c.T, [-100, -110, -150]).T

y_l01, u_l01, u_l01_sat = simulate(A0, b0, c0, d0, 
                                   control_func_luenberger(k=l0_A15, l=l1_A15, A=A, b=b, c=c, umax=u_max).regulate,
                                   s, T, delay=delay, umax=u_max, x0=np.zeros((len(b0),1)))

y_l10, u_l10, u_l10_sat = simulate(A0, b0, c0, d0, 
                                   control_func_luenberger(k=l1_A15, l=l0_A15, A=A, b=b, c=c, umax=u_max).regulate,
                                   s, T, delay=delay, umax=u_max, x0=np.zeros((len(b0),1)))

y_l11, u_l11, u_l11_sat = simulate(A0, b0, c0, d0, 
                                   control_func_luenberger(k=l1_A15, l=l1_A15, A=A, b=b, c=c, umax=u_max).regulate,
                                   s, T, delay=delay, umax=u_max, x0=np.zeros((len(b0),1)))

#yx, ux, ux_sat = simulate(A, b, c, d, 
#                                   controller.regulate,
#                                   s, T, delay=None, umax=u_max, x0=np.zeros((len(b),1)))

#yx, ux, ux_sat = optim_tools.simulate(A, b, c, d, 
#                                   partial(control_func, k=l1_A15, A=A, b=b, c=c),
#                                   s, T, delay=None, umax=u_max, x0=x0)

print y_l11[0, -1]



In [ ]:

    
#l = place(A',C',p).'

#l = con.place(A.T, c.T, [-100, -110, -150]).T

y_l01, u_l01, u_l01_sat = simulate(A0, b0, c0, d0, 
                                   control_func_luenberger(k=l0_A15, l=l1_A15, A=A, b=b, c=c, umax=None).regulate,
                                   s, T, delay=delay, umax=u_max, x0=np.zeros((len(b0),1)))

y_l10, u_l10, u_l10_sat = simulate(A0, b0, c0, d0, 
                                   control_func_luenberger(k=l1_A15, l=l0_A15, A=A, b=b, c=c, umax=None).regulate,
                                   s, T, delay=delay, umax=u_max, x0=np.zeros((len(b0),1)))

y_l11, u_l11, u_l11_sat = simulate(A0, b0, c0, d0, 
                                   control_func_luenberger(k=l1_A15, l=l1_A15, A=A, b=b, c=c, umax=None).regulate,
                                   s, T, delay=delay, umax=u_max, x0=np.zeros((len(b0),1)))

#yx, ux, ux_sat = simulate(A, b, c, d, 
#                                   controller.regulate,
#                                   s, T, delay=None, umax=u_max, x0=np.zeros((len(b),1)))

#yx, ux, ux_sat = optim_tools.simulate(A, b, c, d, 
#                                   partial(control_func, k=l1_A15, A=A, b=b, c=c),
#                                   s, T, delay=None, umax=u_max, x0=x0)

print y_l11[0, -1]



In [ ]:

    
from functools import partial

y0, u0, u0_sat = simulate(A0, b0, c0, d0, 
                                       partial(optim_tools.openLoop),
                                       s, T, delay=delay, umax=u_max, x0=np.zeros((len(b0),1)))

y, u, u_sat = optim_tools.simulate(A, b, c, d, 
                                   partial(control_func, k=l1_A15, A=A, b=b, c=c),
                                   s, T, delay=None, umax=u_max, x0=x0)

y1, u1, u_sat1 = optim_tools.simulate(A, b, c, d, 
                                   partial(control_func, k=l0_A15, A=A, b=b, c=c),
                                   s, T, delay=None, umax=u_max, x0=x0)
print y[0, -1]



In [ ]:

    
%pylab inline
pylab.rcParams['figure.figsize'] = (10, 6)
import matplotlib.pyplot as plt

line0, = plt.plot(T[:], np.array(y0[0,:].T), 'r', label='open')
line0, = plt.plot(T[:], np.array(y[0,:].T), 'b', label='l1')
line1, = plt.plot(T[:], np.array(y1[0,:].T), 'g', label='l0')

linex, = plt.plot(T[:], np.array(y_l01[0,:].T), 'r--', label='l0_obs1')
linex, = plt.plot(T[:], np.array(y_l10[0,:].T), 'b--', label='l1_obs0')
linex, = plt.plot(T[:], np.array(y_l11[0,:].T), 'g--', label='l1_obs1')
#linex, = plt.plot(T[:], np.array(yx2[0,:].T), 'g.-', label='x2')


#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, u, 'b--', label='u1')
line1, = plt.plot(T, u1, 'g--', label='u0')
line2, = plt.plot(T, u_sat, 'b', label='u_sat1')
line3, = plt.plot(T, u_sat1, 'g', label='u_sat0')

line1, = plt.plot(T, u_l11, 'r--', label='ux')
line3, = plt.plot(T, u_l11_sat, 'r-', label='u_satx')


#>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()



In [ ]:



In [ ]: