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()
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 [ ]:
#############################
# Roboter pt2+pos (ohne delay) #
#############################
A = np.matrix([[ 0, 0., -10],
[ 0, 0., -55.99932527],
[ 0, 10., -43.64272128]])
b = np.matrix([[0],
[-5.58731344],
[ 0. ]])
c = np.matrix([ 1., 0., 0.])
d = np.matrix([0])
u_max = 0.5
n = len(b)
delay = 0.032
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
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
In [ ]:
###########################
# Roboter pt2d + pos #
###########################
A = np.matrix([[ 0, 0, 0, -1 ],
[ 0, 0, 0, 35 ],
[ 0, 10, 0, 32.8766333],
[ 0, 0, -100, -106.142721 ]])
b = np.matrix([[0], [34.92070901], [-5.58731344], [ 0.]])
c = np.matrix([ 1., 0., 0., 0.])
d = np.matrix([0])
u_max = 0.5
n = len(b)
X00 = [np.matrix([1.0, 1.0, -0.5, -0.025]).T,
np.matrix([1.0, 1.0, -0.5, 0.025]).T,
np.matrix([1.0, 1.0, 0.5, -0.025]).T,
np.matrix([1.0, 1.0, 0.5, 0.025]).T,
np.matrix([1.0, -1.0, -0.5, -0.025]).T,
np.matrix([1.0, -1.0, -0.5, 0.025]).T,
np.matrix([1.0, -1.0, 0.5, -0.025]).T,
np.matrix([1.0, -1.0, 0.5, 0.025]).T,
np.matrix([-1.0, 1.0, -0.5, -0.025]).T,
np.matrix([-1.0, 1.0, -0.5, 0.025]).T,
np.matrix([-1.0, 1.0, 0.5, -0.025]).T,
np.matrix([-1.0, 1.0, 0.5, 0.025]).T,
np.matrix([-1.0, -1.0, -0.5, -0.025]).T,
np.matrix([-1.0, -1.0, -0.5, 0.025]).T,
np.matrix([-1.0, -1.0, 0.5, -0.025]).T,
np.matrix([-1.0, -1.0, 0.5, 0.025]).T,
]
X0 = X00
In [ ]:
%%time
func_g_eps = {}
# Lens 5.1.2 Entwurf mittels Anpassung der Stellgrößenbeschränkung (S.97)
# Schritt 1: Reglerentwurf nach Optimierungsproblem 3.4, mit (3.19d) = eps*u_max
# Initialize
n = len(b) # get dim of system
n_u = 1
# Define Variables
Q = cvxpy.Semidef(n, n) #symmetric and positive semidefinite
Y = cvxpy.Variable(n) #symmetric and positive semidefinite
W = cvxpy.Variable(n_u, n_u) #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
# Bisection parameter
g = cvxpy.Parameter(sign='positive')
#g.value = g_val # TODO set by bisection loop (function?)
eps = cvxpy.Parameter(sign='positive')
eps.value = 0.5
# Define Constraints
# (3.26)
const_326 = Q*A.T + A*Q - Y*b.T - b*Y.T + 2*g*Q == -cvxpy.Semidef(n)# This constraint is strict definit
# (3.19a)
const_319a = Q == cvxpy.Semidef(n)
# (3.19c)
const_319c = cvxpy.bmat([[Q, Y],
[Y.T, W ]]) == cvxpy.Semidef(n+1)
# (3.19d)
const_319d = [W[i][i]<=eps**2*u_max**2 for i in range(0, n_u)]
# (3.19e)
const_319e = [cvxpy.bmat([[Q, X0[i]],
[X0[i].T, 1 ]]) == cvxpy.Semidef(n+1)
for i in range(0, len(X0))]
# Collect all constraints
constraints_34 = []
constraints_34.append(const_326)
constraints_34.append(const_319c)
constraints_34.extend(const_319d)
constraints_34.extend(const_319e)
# Form problem.
prob_34 = cvxpy.Problem(cvxpy.Minimize(0), constraints_34)
# bisection
[[Q_o, Y_o, W_o], g_o] = optim_tools.bisect_max(0, None, prob_34, g, [Q, Y, W], bisect_verbose=True,
bisection_tol=0.1,
solver=cvxpy.CVXOPT, verbose=False)
#solver=cvxpy.MOSEK, verbose=False)
#solver=cvxpy.SCS, max_iters=50000)
#prob_34.solve(solver=cvxpy.CVXOPT, verbose=True)
K_zeta = LA.solve(Q_o, Y_o)
print K_zeta
In [ ]:
In [ ]:
%%time
# Schritt 2: Falls Schritt 1 erfolgreich, Optimierungsproblem 5.2 mit K_zeta (S.94)
# Define Variables
P11 = cvxpy.Semidef(n, n) #symmetric and positive semidefinite
P12 = np.zeros((n,n))
P22 = cvxpy.Semidef(n, n) #symmetric and positive semidefinite
P21 = np.zeros((n,n))
V = cvxpy.Variable(n) #symmetric and positive semidefinite
# Define Constraints
P = cvxpy.bmat([[P11, P12],
[P21, P22]])
# (5.7a)
const_57a = [P11 == cvxpy.Semidef(n), P22 == cvxpy.Semidef(n)]
# (5.7b)
const_57b = cvxpy.bmat([[(A-b*K_zeta.T).T*P11 + P11*(A-b*K_zeta.T), P11*b*K_zeta.T ],
[K_zeta*b.T*P11, A.T*P22 + P22*A - V*c - c.T*V.T]]) + 2*g*P == -cvxpy.Semidef(2*n)
# (5.7c)
const_57c = cvxpy.bmat([[P11, np.zeros((n,n)), K_zeta],
[np.zeros((n,n)), P22, -K_zeta],
[K_zeta.T, -K_zeta.T, W]]) == cvxpy.Semidef(2*n+1)
# (5.7d)
const_57d = [W[i][i]<=u_max**2 for i in range(0, n_u)]
# (5.7e)
# e0 = x0
const_57e = [cvxpy.bmat([[X0[i]],
[X0[i]]]).T*P* \
cvxpy.bmat([[X0[i]],
[X0[i]]]) < 1 for i in range(0, len(X0))]
# Collect all constraints
constraints_52 = []
constraints_52.extend(const_57a)
constraints_52.append(const_57b)
constraints_52.append(const_57c)
constraints_52.extend(const_57d)
constraints_52.extend(const_57e)
# Form problem.
prob_52 = cvxpy.Problem(cvxpy.Minimize(0), constraints_52)
# bisection
[[P11_o, P22_o, W_o, V_o], g_o] = optim_tools.bisect_max(0, None, prob_52, g, [P11, P22, W, V], bisect_verbose=True,
bisection_tol=0.1,
#solver=cvxpy.CVXOPT)
solver=cvxpy.MOSEK)
#solver=cvxpy.SCS, max_iters=50000)
L_zeta = LA.solve(P22_o, V_o)
print L_zeta
func_g_eps[eps.value] = g_o
func_g_eps[eps.value]
In [ ]:
In [ ]:
## Suche den besten Startwert
# (hier der eine den ich berechnet habe)
K = K_zeta
L = L_zeta
PP = P.value
In [ ]:
Q = cvxpy.Semidef(3)
prob = cvxpy.Problem(cvxpy.Minimize(0), [Q==cvxpy.Semidef(3)])
prob.solve(solver=cvxpy.MOSEK)
prob.status
In [ ]:
# Lens 5.2.1: Sättigender lin. Regler mit Beobachterentwurf
#Schritt 1: Initialisierung
alpha = 0.2
H = K
#Schritt 2: Linerarisierung
# max(gamma_tri) s.t. (5.16)
#n = len(b) # get dim of system
# Define Variables
P_tri = cvxpy.Semidef(2*n) #symmetric and positive semidefinite
H_tri = cvxpy.Variable(n)
K_tri = cvxpy.Variable(n)
L_tri = cvxpy.Variable(n)
gamma_tri = cvxpy.Variable(1)
# Define Constraints
# (5.16a)
const_516a = PP + P_tri >> 0
const_516c = cvxpy.bmat([[(A-b*K_zeta.T).T*P11 + P11*(A-b*K_zeta.T), P11*b*K_zeta.T ],
[K_zeta*b.T*P11, A.T*P22 + P22*A - V*c - c.T*V.T]]) << -2*g*P
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, 10.0, 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 [ ]:
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_zeta, u_zeta, u_sat_zeta = simulate(A, b, c, d,
control_func_luenberger(k=K_zeta, l=L_zeta, A=A, b=b, c=c, umax=u_max).regulate,
s, T, delay=None, umax=u_max, x0=np.zeros((len(b),1)))
y_zeta_2, u_zeta_2, u_sat_zeta_2 = simulate(A0, b0, c0, d0,
control_func_luenberger(k=K_zeta, l=L_zeta, A=A, b=b, c=c, umax=u_max).regulate,
s, T, delay=delay, umax=u_max, x0=np.zeros((len(b0),1)))
#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)))
#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 [ ]:
%pylab inline
pylab.rcParams['figure.figsize'] = (10, 6)
import matplotlib.pyplot as plt
line0, = plt.plot(T[:], np.array(y_zeta[0,:].T), 'r', label='zeta')
line0, = plt.plot(T[:], np.array(y_zeta_2[0,:].T), 'b', label='zeta_2')
line0, = plt.plot(T[:], np.array(y0[0,:].T), 'g--', label='open')
#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_zeta, 'b--', label='u1')
line2, = plt.plot(T, u_sat_zeta, 'b', label='u_sat1')
#>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 [ ]:
#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 [ ]: