In [ ]:

    

import numpy as np
from __future__ import division


    

In [ ]:

    

import picos as pic
import cvxopt as cvx
import control as con

from numpy import linalg as LA
from scipy.special import comb as nchoosek # n Choose k (n ueber k)
import optim_tools as optim_tools#own file with helper


    

In [ ]:

    

###########################
# Hydraulischer Aktor     #
###########################

A0 = np.matrix([[0,   1,       0],
                [-10, -1.167, 25],
                [0,   0,    -0.8]])
print "Eigenvalues: {}".format(LA.eigvals(A0))
#a = -A[-1,:].T ### !!!!!
#print a
b0 = np.matrix([[0],[0],[2.4]])
c0 = np.matrix([1, 0, 0])
d0 = np.matrix([0])
u_max = 10.5
n = 3

X00 = [np.matrix([-20.0, -10.0, -10.0]).T,
       np.matrix([-20.0, -10.0,  10.0]).T,
       np.matrix([-20.0,  10.0, -10.0]).T,
       np.matrix([-20.0,  10.0,  10.0]).T,
       np.matrix([20.0,  -10.0, -10.0]).T,
       np.matrix([20.0,  -10.0,  10.0]).T,
       np.matrix([20.0,   10.0, -10.0]).T,
       np.matrix([20.0,   10.0,  10.0]).T,
      ]

#print "A:\n", A
#print "a:\n", a
#print "b:\n", b
#print "c:\n", c

# Convert to Normalform
(A1, b1, c1, d1), T1, Q1 = optim_tools.get_Steuerungsnormalform(A0, b0, c0.T, d0)
a1 = -A1[-1][:].T #!!!!
print "T1:\n", T1

# Convert to Normalform
ss, T2 = con.canonical_form(con.ss(A0, b0, c0, d0), form='reachable')

assert np.allclose(T1*X00[1],
                   optim_tools.reverse_x_order(T2*X00[1])),\
"own Steuerungsnormalform Transformation not equal python control version"
#print "x_r1:\n", T1*X00[1]
#print "x_r2(backwards):\n", optim_tools.reverse_x_order(T2*X00[1])

A = optim_tools.reverse_x_order(np.matrix(ss.A))
a = -A[-1][:].T #!!!!

b = optim_tools.reverse_x_order(np.matrix(ss.B))
c = optim_tools.reverse_x_order(np.matrix(ss.C))
d = optim_tools.reverse_x_order(np.matrix(ss.D)) # == 0!

print "A:\n", A
assert np.allclose(A, A1)
print "a:\n", a
assert np.allclose(a, a1)
print "b:\n", b
assert np.allclose(b, b1)
print "c:\n", c
assert np.allclose(c, c1.T)

X0 = [T1.dot(x0) for x0 in X00]
#print "X0:\n"
#for x0 in X0:
#    print x0
#print "A1:\n", A1
#print "a1:\n", a1
#print "b1:\n", b1
#print "c1:\n", c1


    

In [ ]:

    

pmin = 0.1


    

In [ ]:

    

import picos;picos.tools.available_solvers()


    

In [ ]:

    

###########################################
# Objective variant (4.66) -> max(det(Q)) #
# Volumenmaximierung -> geomean(Q)        #
###########################################
# Problem
prob_451 = pic.Problem()

# Variables
QQ = prob_451.add_variable('Q', (n, n), vtype='symmetric')
zz = prob_451.add_variable('z', n)


# Constants
XX0 = pic.new_param('X0', X0)
N = cvx.matrix(optim_tools._N(n))

# Constraints
#(4.59)
prob_451.add_constraint(QQ >> 0)

#(4.60)
prob_451.add_constraint(QQ*(A.T+a*b.T) + (A+b*a.T)*QQ - zz*b.T - b*zz.T << 0)

#(4.61)
prob_451.add_constraint(QQ*N+N*QQ << 0) 

 #(4.62)
prob_451.add_list_of_constraints([((1          & XX0[i].T) //
                                   (XX0[i]     & QQ      )) >> 0
                                       for i in range(0, len(X0))])

#(4.63)
prob_451.add_constraint(((u_max**2 - a.T*QQ*a + 2*a.T*zz & zz.T) //
                         (zz                             & QQ  )) >> 0)


    

In [ ]:

    

###########################################
# Constraint variant (4.64)               #
###########################################

m = n

## BEWARE OF SECOND RANGE ####
prob_451.add_list_of_constraints(
                    [pic.sum(
                        [nchoosek(i, k) * a.T * optim_tools._P(0, i-k, n) * QQ * N * optim_tools._P(0, k, n) * a - zz.T * N * optim_tools._P(n, i, n) * a
                             for k in range(0,i)], 'k', '[0,1,..,i]')>=0
                                   for i in range(1, m+1)])


    

In [ ]:

    

###########################################
# Objective disabled because problem is not feasible with the objective anymore :-/
###########################################

#ss = prob_451.add_variable('s', 1)
#prob_451.add_constraint(pic.geomean(QQ) > ss) 
#prob_451.add_constraint(pic.detrootn(QQ) > ss) 

#prob_451.set_objective('max', ss)


    

In [ ]:

    

print "Objective: {}\n".format(prob_451.objective)
prob_451.solve(verbose=1, solver='cvxopt', solve_via_dual = None)


    

In [ ]:

    

# Output
print "Status:", prob_451.status
print prob_451

Q_451 = QQ.value
R1_451 = LA.inv(QQ.value)
a_hat_451 = R1_451.dot(zz.value)

print "Q:\n", Q_451
print "R1:\n", R1_451
print "z:\n", zz.value
print "a_hat:\n", a_hat_451


    

In [ ]:

    

import numpy as np
print "Checking constraints (no output is ok!)"
eig_459 = np.linalg.eigvals(QQ.value)
if np.any(eig_459 <= 0): print("Q not pos definit: {}".format(eig_459))

eig_460 = np.linalg.eigvals((QQ*(A.T+a*b.T)+(A+b*a.T)*QQ-zz*b.T-b*zz.T).value)
if np.any(eig_460 >= 0): print("4.60 not neg definit: {}".format(eig_460))

eig_461 = np.linalg.eigvals((QQ*N+N*QQ).value)
if np.any(eig_461 >= 0): print("4.61 not neg definit: {}".format(eig_461))
    
eig_463 = np.linalg.eigvals(((u_max**2-a.T*QQ*a+2*a.T*zz & zz.T) // (zz & QQ)).value)
if np.any(eig_463 <= 0): print("4.63 not pos definit: {}".format(eig_463))


    

In [ ]:

    

###########################################
# Objective variant (4.70)                #
# Max. Abklingrate                        #
###########################################
# Problem
prob_452 = pic.Problem()

# Variables
QQ = prob_452.add_variable('Q', (n, n), vtype='symmetric')
zz = prob_452.add_variable('z', n)


# Constants
XX0 = pic.new_param('X0', X0) #We also point out that new_param() converts lists into vectors and lists of lists into matrices (given in row major order)
N = cvx.matrix(optim_tools._N(n))

# Bisection parameter
beta = pic.new_param('beta', 0) # sign='positive'


# Constraints
#(4.59)
prob_452.add_constraint(QQ >> 0)

#(4.60)
prob_452.add_constraint(QQ*(A.T+a*b.T) + (A+b*a.T)*QQ - zz*b.T - b*zz.T << 0)

#(4.61)
prob_452.add_constraint(QQ*N+N*QQ << 0) 

 #(4.62)
prob_452.add_list_of_constraints([((1          & XX0[i].T) //
                                   (XX0[i]     & QQ      )) >> 0
                                       for i in range(0, len(X0))])

#(4.63)
prob_452.add_constraint(((u_max**2 - a.T*QQ*a + 2*a.T*zz & zz.T) //
                         (zz                             & QQ  )) >> 0)


    

In [ ]:

    

#(4.71)
constraint_471 = prob_452.add_constraint(QQ*(A.T + a*b.T) + (A + b*a.T)*QQ - zz*b.T - b*zz.T + 2*beta*QQ << 0, ret=True)


    

In [ ]:

    

###########################################
# Constraint variant (4.64)               #
###########################################

m = n

## BEWARE OF SECOND RANGE ####
prob_452.add_list_of_constraints(
                    [pic.sum(
                        [nchoosek(i, k) * a.T * optim_tools._P(0, i-k, n) * QQ * N * optim_tools._P(0, k, n) * a - zz.T * N * optim_tools._P(n, i, n) * a
                             for k in range(0,i)], 'k', '[0,1,..,i]')>=0
                                   for i in range(1, m+1)])


    

In [ ]:

    

def bisect_max(l, u, func_solve_with_param, prob,
           bisection_tol=1e-3, bisect_verbose=False):

    #if (u is not None):
    #    # cross check bound
    #    if (u < l): raise ValueError("upperBound({}) < lowerBound({})".format(u, l))
    #
    #elif (u is None) and (l is not None):
    #    # First iteration to find upper bound
    #    u = l + 1.0
    #    if bisect_verbose: print "processing upper bound: {}".format(u)
    #    
    #    parameter.value = u
    #    problem = accurate_solve(problem, solver, **kwargs_solver)
    #    uStatus = problem.status
    #   
    #    while 'optimal' in uStatus:
    #        #if u >= l: # shift upper bound if found feasible, this condition is always true
    #            #l = u
    #        u = 2.0*u
    #        if bisect_verbose:
    #            print "processing upper bound: {}".format(u)
    #        parameter.value = u
    #        problem = accurate_solve(problem, solver, **kwargs_solver)
    #        uStatus = problem.status
    #    print 'found bounds: [{}-{}]'.format(l, u)
    #else:
    #    raise ValueError("Not implemented")
    
    # check validity solution of l is optimal, solution of u is infeasible
    val_opt = l
    problem_opt, lStatus = func_solve_with_param(prob, l) # Set lower bound and solve, shall return (problem, status[True/False])

    problem, uStatus = func_solve_with_param(prob, u) # Set upper bound and solve, shall return (problem, status[True/False])

    if lStatus is False and uStatus is True:
        raise ValueError("UpperBound({})={}, LowerBound({})={}".format(u, uStatus, l, lStatus))

    while u - l >= bisection_tol:
        val = (l + u) / 2.0 # Solve for parameter in the middle of upper and lower bound
        
        ## solve the feasibility problem
        problem, status = func_solve_with_param(problem_opt, val)

        if bisect_verbose:
            print "Range: {}-{}; parameter {} -> {}".format(l, u, val, problem.status)

        if status is False:
            u = val
        else:
            l = val
            val_opt = val
            problem_opt = problem #Store last (optimal) solved problem

        # Solve it a last time with the last optimal value, to set the status of problem (since it is not a copy but a ref)
        problem_opt, _ = func_solve_with_param(problem_opt, val_opt)
    return problem_opt, val_opt


    

In [ ]:

    

def picos_solve_with_param(problem, val):
    global constraint_471
    #beta = pic.new_param('beta', val) # sign='positive'

    constraint_471.delete()
    constraint_471 = problem.add_constraint(QQ*(A.T + a*b.T) + (A + b*a.T)*QQ - zz*b.T - b*zz.T + 2*val*QQ << 0, ret=True)
    
    problem.solve(verbose=0, solver='cvxopt', solve_via_dual = None)
    return problem, problem.status == 'optimal'


    

In [ ]:

    

%%time

problem_opt, val_opt = bisect_max(0, 2, picos_solve_with_param, prob_452)
print "Objective variant (4.5.2) -> Bisection(Max. Abklingrate)"
print "Bisection Param:", val_opt

# Output
print "Status:", problem_opt.status
#print problem_opt

Q_452 = QQ.value
R1_452 = LA.inv(QQ.value)
a_hat_452 = R1_452.dot(zz.value)

print "Q:\n", Q_452
print "R1:\n", R1_452
print "z:\n", zz.value
print "a_hat:\n", a_hat_452


    

In [ ]:

    

T = np.arange(0, 5, 1e-2) 

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

p_init = pmin

# Initial state
x0 = np.matrix([[20.],[10.],[10.]])

p_t = np.zeros(len(T))


    

In [ ]:

    

# Skalierte Version! aus Yankulova

# k(v) (4.5)
def get_k(v, a, a_hat):
    try:
        v = v.squeeze() # This is weird, needed for minimize for some reason
    except:
        pass
    D_inv = optim_tools._D_inv(v, len(a))
    #print D_inv
    k = D_inv.dot(a_hat) - a
    return k

# Fixing a and a_hat for convinience
# func_k = lambda v: get_k(v, a, a_hat)

# G(v) (4.4)
def get_g(v, x, R1, u_max, a, a_hat):
    try:
        v = v.squeeze() # This is weird, needed for minimize for some reason
    except:
        pass
    D_inv = optim_tools._D_inv(v, len(x))
    R = D_inv.dot(R1).dot(D_inv) # R(v) = D^⁻1 * R1 * D^-1
    
    k = get_k(v, a, a_hat) # k(v)
    e = (u_max**(-2)) * (k.T.dot(LA.inv(R)).dot(k))
    #assert e < 1.0
    g = e*(x.T.dot(R).dot(x)) - 1.0
    #assert g <= 0, "g = {} > 0".format(g)
    # Update 2016: As of python 3.5, there is a new matrix_multiply symbol, @:
    # g = x' @ D^-1 @ R1 @ D^-1 @ x - 1.0
    return g


    

In [ ]:

    

from scipy.optimize import minimize
last_p = p_init

def contr_func(y, s, x, u_max, a, a_hat, R1, T1=None):
    global last_p, pmin
    
    # Transformation in Regelungsnormalform
    if T1 is None:
        T1 = np.eye(len(x))

    x_R = T1*x
    
    func_g = lambda p: np.absolute(get_g(p, x_R, R1, u_max, a, a_hat))
    res = minimize(func_g, last_p, method='Nelder-Mead') # find 0 -fzero
    
    # Saturate if too small
    if res.x < pmin:
        p = pmin
    elif res.x > 1.0:
        p = 1.0
        print "WARNING: p=({})>1! -> p is saturated to 1.0".format(res.x)
    else:
        p = res.x

    p_t.append(p)
    p2_t.append(res.x)

    last_p = p
    
    ## Calc K according to p
    K = get_k(p, a, a_hat)

    # Calc u
    u = s-K.T.dot(x_R)
    
    # Saturate u
    u = optim_tools.sat(u, u_max)
    #print "u", u
    
    return u


    

In [ ]:

    

a_hat_x = np.matrix([[14.345160], [14.837268], [3.598276]])

R1_x = np.matrix([[1.677977, 0.543373, 0.140299],
                  [0.543373, 0.32290,  0.062725],
                  [0.140298, 0.06272,  0.026148]])


    

In [ ]:

    

p_t = []
p2_t = []

y, u, u_sat = optim_tools.simulate(A0, b0, c0, d0, lambda y, s, x: contr_func(y, s, x, u_max, a, a_hat_451, R1_451, T1), s, T, umax=u_max, x0=x0)
y1, u1, u1_sat = optim_tools.simulate(A0, b0, c0, d0, lambda y, s, x: contr_func(y, s, x, u_max, a, a_hat_x, R1_x, T1), s, T, umax=u_max, x0=x0)
y2, u2, u2_sat = optim_tools.simulate(A0, b0, c0, d0, lambda y, s, x: contr_func(y, s, x, u_max, a, a_hat_452, R1_452, T1), s, T, umax=u_max, x0=x0)

#y1x, u1x, u1x_sat = optim_tools.simulate(A0, b0, c0, d0, lambda y, s, x: contr_func(y, s, x, u_max, a, a_hat_x, R1_x, T1), s, T, umax=u_max, x0=x0)
#y1, u1, u1_sat = optim_tools.simulate(A1, b1, c1.T, d1, lambda y, s, x: contr_func(y, s, x, u_max, a, a_hat), s, T, umax=u_max, x0=T1*x0)

#y2, u2, u2_sat = optim_tools.simulate(A0, b0, c0, d0, lambda y, s, x: s-get_k(1, a, a_hat_451).T.dot(T1*x), s, T, umax=u_max, x0=x0)
#y3, u3, u3_sat = optim_tools.simulate(A0, b0, c0, d0, lambda y, s, x: s-np.matrix([-0.0833, 0.0828, 0.76068]).dot(x), s, T, umax= u_max, x0=x0)
y4, u4, u4_sat = optim_tools.simulate(A0, b0, c0, d0, lambda y, s, x: s-np.matrix([13.25659, 7.097648, 4.0502]).dot(x), s, T, umax= u_max, x0=x0)


    

In [ ]:

    

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

#plt.figure()
line0, = plt.plot(T[:], np.array(y[0,:].T), 'b', label='WSVC 451')
line1, = plt.plot(T[:], np.array(y1[0,:].T), 'y*-', label='WSVC x')
line2, = plt.plot(T[:], np.array(y2[0,:].T), 'r-', label='WSVC 452')
#line2, = plt.plot(T[:], np.array(y2[0,:].T), 'r-', label='linear(v=1.0)')
#line3, = plt.plot(T[:], np.array(y3[0,:].T), 'g-', label='linear')
line4, = plt.plot(T[:], np.array(y4[0,:].T), 'g-', label='y4 lin, apx evo')



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

#plt.figure()
#line0, = plt.plot(T, u_sat, 'b', label='u')

#line0, = plt.plot(T, u1x_sat, 'b', label='WSVC x')

line0, = plt.plot(T, u, 'b.-', label='WSVC 451')
line1, = plt.plot(T, u1_sat, 'y*-', label='WSVC x')
line2, = plt.plot(T, u2_sat, 'r-', label='WSVC 452')
#line2b, = plt.plot(T, u2, 'r.-', label='u fixed')
#line3, = plt.plot(T, u3_sat, 'g', label='u, linear')
#line3b, = plt.plot(T, u2, 'g.-', label='u lin')
line4, = plt.plot(T, u4_sat, 'g-', label='u lin, apx evo')


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

line6, = plt.plot(p_t, 'r', label='p')
line7, = plt.plot(p2_t, 'r.', label='p')
plt.show()


    

In [ ]:

    




    

In [ ]: