In [ ]:
import numpy as np
import sympy as sp
In [ ]:
import numpy.linalg as la
A = np.matrix([[ 0, 0, 0, -1 ],
[ 0, 0, 0, 35 ],
[ 0, 10, 0, 32.8766333],
[ 0, 0, -100, -106.142721 ]])
eigval, eigvec = la.eig(A)
eigval
ws = []
w = []
s = []
p = []
for val in eigval:
if np.iscomplex(val):
if not np.conjugate(val) in ws:
ws.append(val)
w.append(np.real(val))
s.append(np.imag(val))
else:
p.append(np.real(val))
print ws
print tuple(w)
print tuple(s)
print tuple(p)
In [ ]:
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])
######
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]])
eigval, eigvec = la.eig(A)
print eigval
print eigvec
#print
ws = []
ws_vec = None
w = []
s = []
p = []
p_vec = None
for val, vec in zip(eigval, eigvec.T):
if np.iscomplex(val):
if not np.conjugate(val) in ws:
ws.append(val)
w.append(np.real(val))
s.append(np.imag(val))
if ws_vec is None:
ws_vec = vec.T
else:
ws_vec = np.hstack((ws_vec, vec.T))
else:
p.append(np.real(val))
if p_vec is None:
p_vec = vec.T
else:
p_vec = np.hstack((p_vec, vec.T))
print "real"
print val
print vec.T
print
print p
print p_vec
#print
#print tuple(ws) + tuple(p)
eigval_sorted = np.append(ws, p)
#print eigval_sorted
#print
#print
print "ws_vec"
print ws_vec
print "p_vec"
print p_vec
eigvec_sorted = np.hstack((ws_vec, p_vec))
#print eigvec_sorted
# Keep track of complex conjugates (need only one)
lst_conjugates = []
Tzx = None
for val, vec in zip(eigval_sorted, eigvec_sorted.T):
if np.iscomplex(val):
if val not in lst_conjugates:
lst_conjugates.append(val.conjugate())
if Tzx is not None:
Tzx = np.hstack((Tzx, np.hstack((vec.real.T, vec.imag.T))))
else:
Tzx = np.hstack((vec.real.T, vec.imag.T))
else:
# if conjugate has already been seen, skip this eigenvalue
lst_conjugates.remove(val)
else:
if Tzx is not None:
Tzx = np.hstack((Tzx, vec.real.T))
else:
Tzx = vec.real.T
# Generate the system matrices for the desired canonical form
A_modal = la.solve(Tzx, A).dot(Tzx)
B_modal = la.solve(Tzx, B)
C_modal = C.dot(Tzx)
print A_modal
print B_modal
print C_modal
In [ ]:
import control as pc
import matplotlib.pyplot as plt
print "poles:"
print pc.pole(pc.ss(A,B,C,D))
print pc.pole(pc.ss(A_modal, B_modal, C_modal, D))
print "zeros:"
print pc.zero(pc.ss(A,B,C,D))
print pc.zero(pc.ss(A_modal, B_modal, C_modal, D))
print
plt.plot(*pc.step_response(pc.ss(A,B,C,D)))
plt.plot(*pc.step_response(pc.ss(A_modal, B_modal, C_modal, D)))
print "observable"
sys_check0, T_check = pc.canonical_form(pc.ss(A, B, C, D), "observable")
print pc.pole(sys_check0)
print pc.zero(sys_check0)
plt.plot(*pc.step_response(sys_check0))
print
print "reachable"
sys_check1, T_check = pc.canonical_form(pc.ss(A, B, C, D), "reachable")
print pc.pole(sys_check1)
print pc.zero(sys_check1)
print
print "modal"
sys_check, T_check = pc.canonical_form(pc.ss(A, B, C, D), "modal")
print pc.pole(sys_check)
print pc.zero(sys_check)
plt.plot(*pc.step_response(sys_check))
In [ ]:
import optimization.optim_tools as optim_tools
kwargs = optim_tools.bisect_max(0, 1, None, None, None,
bisection_tol=1e-3, solver=None, bisect_verbose=False, max_iters=1100, warm_start=True)
kwargs['max_iters'] = 2000
kwargs
In [ ]:
def svd_inverse(A):
""" Inverts a matrix using singular value decomposition (SVD).
The matrix doesn't have to be square or of full rank.
"""
# A = U*(I*s)*V => is easier to invert (see literature)
U, s, V = np.linalg.svd(A, full_matrices=False) # SVD decomposition of A
for i in xrange(0, len(s)):
if s[i] > 0.001:
s[i] = 1/s[i] # ToDo: damping near singularities
return np.dot(np.dot(V.T, np.diag(s)), U.T)
In [ ]:
def svd_solve(A, b):
U, s, V = np.linalg.svd(A, full_matrices=False) # SVD decomposition of A
for i in xrange(0, len(s)):
if s[i] > 0.001:
s[i] = 1/s[i] # ToDo: damping near singularities
return V.T.dot(np.diag(s).dot(U.T.dot(b)))
In [ ]:
def svd_solve2(A, b):
U, s, V = np.linalg.svd(A, full_matrices=False) # SVD decomposition of A
s[s > 0.001] = 1/s[s > 0.001]
return V.T.dot(np.diag(s).dot(U.T.dot(b)))
In [ ]:
n=6
m=7
A_m = np.random.rand(n,m)
y_m = np.random.rand(n)
np.random.seed(0)
In [ ]:
%%timeit
A_m_inv = svd_inverse(A_m)
dq = np.dot(A_m_inv, y_m)
In [ ]:
%%timeit
dq = svd_solve(A_m, y_m)
In [ ]:
%%timeit
dq = svd_solve2(A_m, y_m)
In [ ]:
#import numpy as np
#import control as con
#from numpy import linalg as LA
import cvxpy
#import optimization.optim_tools as optim_tools#own file with helper
In [ ]:
import rospy
from tf import transformations as trans
R0 = trans.quaternion_matrix([0.500, -0.500, -0.500, -0.500])
t0 = trans.translation_matrix((-0.187, 0.359, 0.403))
T0 = trans.concatenate_matrices(t0, R0)
R1 = trans.quaternion_matrix([-0.323, 0.323, 0.629, 0.629])
t1 = trans.translation_matrix((-0.187, 0.330, 0.393))
T1 = trans.concatenate_matrices(t1, R1)
R2 = trans.quaternion_matrix([-0.208, 0.406, 0.792, 0.406])
t2 = trans.translation_matrix((-0.158, 0.335, 0.386))
T2 = trans.concatenate_matrices(t2, R2)
print T0
print T1
print T2
In [ ]:
import numpy as np
import cvxpy
n = 4
T_tool = cvxpy.Semidef(n, n) # Implys (459) (semidefinite for numerical reasons?)
T_obj = cvxpy.Semidef(n, n) # Implys (459) (semidefinite for numerical reasons?)
T_err0 = cvxpy.Semidef(n, n) # Implys (459) (semidefinite for numerical reasons?)
T_err1 = cvxpy.Semidef(n, n) # Implys (459) (semidefinite for numerical reasons?)
T_err2 = cvxpy.Semidef(n, n) # Implys (459) (semidefinite for numerical reasons?)
constraints = [T0*T_tool == T_obj,
T1*T_tool == T_obj,
T2*T_tool == T_obj,
]
In [ ]:
# Objective representation
obj = cvxpy.Minimize(cvxpy.trace(T_obj)) # Identical to geo_mean (in term of convexity and result)
#obj = cvxpy.Maximize(cvxpy.log_det(Q)) # Identical to geo_mean (in term of convexity and result)
#obj = cvxpy.Minimize(-cvxpy.geo_mean(Q)) # Not yet implemented
prob = cvxpy.Problem(obj, constraints)
prob.solve(verbose=True)
In [ ]:
In [ ]:
In [ ]:
In [ ]:
import sympy as sp
import numpy as np
import numpy.linalg as la
w = sp.symbols('w_1')
s = sp.symbols('s_1')
b = sp.symbols('b_1:3')
d = sp.symbols('d_1')
x = sp.symbols('x_1:3')
c = sp.symbols('c_1:3')
u = sp.symbols('u_1')
X = sp.Matrix([x]).T
U = sp.Matrix([u])
A = sp.Matrix([[s, w],[-w,s]])
B = sp.Matrix([b]).T
C = sp.Matrix([c])
D = sp.Matrix([d])
AA = sp.lambdify((s, w), A)
BB = sp.lambdify(b, B)
CC = sp.lambdify(c, C)
DD = sp.lambdify(d, D)
z = X.T.row_join(sp.Matrix([s])).row_join(sp.Matrix([w])).row_join(B.T).row_join(C).row_join(D).T
#print z
fx = A*X+B*U
f = sp.Matrix([0 for _ in range(len(z)-len(x))])
f = fx.col_join(f)
print "f="; sp.pprint(f)
ff = sp.lambdify(z.T.row_join(U), f)
print "F="; sp.pprint(f.jacobian(z))#; print len(f.jacobian(z))
FF = sp.lambdify(z.T.row_join(U), f.jacobian(z))
h = C*X+D*U
print "h="; sp.pprint(h)
hh = sp.lambdify(z.T.row_join(U), h)
print "H="; sp.pprint(h.jacobian(z))#; print len(h.jacobian(z))
HH = sp.lambdify(z.T.row_join(U), h.jacobian(z))
#sp.pprint((h.jacobian(z)*h.jacobian(z).T))
In [ ]:
z_test = [0, 0, -1, 1, -2, 1, 0.1, 0.2, 0]
zu_test = np.matrix([0, 0, -1, 1, -2, 1, 0.1, 0.2, 0, 2]).T
print zu_test
#print ff(*(zu_test.tolist()[0]))
def _c(M):
return M.tolist()[0]
print ff(*_c(zu_test))
In [ ]:
rho = 0.1
#R = np.eye(len(z))
R = 1
#print np.diag([sym for sym in z])
print R
q = np.array([])
for sym in z:
if sym in X.T.row_join(C):
q = np.hstack((q, 0))
else:
q = np.hstack((q, 1))
Q = rho * np.matrix(np.diag(q))
print Q
In [ ]:
### Testsystem ####
def calc_TestSystem(x, u):
Af = np.matrix([[ 2.84217094e-14, 1.71379179e+01],
[ -1.00000000e+02, -1.85369064e+02]])
Bf = np.matrix([[ 17.34396868],
[ 9.87641374]])
Cf = np.matrix([[ 0., -1.]])
Df = np.matrix([[ 0.]])
#print Af
#print x
#print "res"
#print Af.dot(x)
#print Bf.dot(u)
#print "res end"
x_dot = Af.dot(x) + Bf.dot(u)
y = Cf.dot(x) + Df.dot(u)
#print x_dot
return y, x_dot
In [ ]:
#%time
# initial
# A
s_k0 = np.matrix([-1]) # real Teil
w_k0 = np.matrix([1]) # (+/-) imag Teil
# b
b_k0 = np.matrix([-2, 1])
# c
c_k0 = np.matrix([0.1, 0.2])
# d
d_k0 = np.matrix([0])
P_k0 = np.eye(len(z))
x_k0 = np.matrix([0, 0]).T
#print x_k0
u_k0 = np.matrix([1])
T = 0.001
# First values
x_k = x_k0
u_k = u_k0
z_k = np.hstack((x_k0.T, s_k0, w_k0, b_k0, c_k0, d_k0)).T
#print "z_k:", z_k
P_k = P_k0
# Loop start
for c, t in enumerate(np.arange(0, 10, T)):
#from IPython.core.debugger import Tracer; Tracer()()
#print "x_k:\n", x_k
y_k, x_dot = calc_TestSystem(x_k, u_k)
#print "y_k:\n", y_k
##### Reconstruction from z
#print "z_k:", z_k
x_k = np.matrix(z_k.T[0, 0:2], dtype=np.float).T
#print "x_k:\n",x_k
s_k = np.matrix(z_k.T[0, 2], dtype=np.float)
#print s_k
w_k = np.matrix(z_k.T[0, 3], dtype=np.float)
#print w_k
b_k = np.matrix(z_k.T[0, 4:6], dtype=np.float)
#print b_k
c_k = np.matrix(z_k.T[0, 6:8], dtype=np.float)
d_k = np.matrix(z_k.T[0, 8], dtype=np.float)
if c%1000==1:
print "Loop", t
#print "s_k:\n", s_k
#print "w_k:\n", w_k
#print "b_k:\n", b_k
#print "c_k:\n", c_k
#print "d_k:\n", d_k
## System
A_k = AA(s_k, w_k)
B_k = BB(*_c(b_k))
C_k = CC(*_c(c_k))
D_k = DD(*_c(d_k))
##### Evaluation
# State space
#dx_k = A_k.dot(x_k) + B_k.dot(u_k)
#y_k = C_k.dot(x_k) + D_k.dot(u_k)
# Concate values for lambdafied Jacobians
zu_k = np.hstack((z_k.T, u_k))
#print "zu_k:\n", zu_k
h_k = np.matrix(hh(*_c(zu_k))) # h = y_k (predicted)
#print "h_k:\n", h_k
H_k = np.matrix(HH(*_c(zu_k))) # h.Jacobian
#print "H_k:\n", H_k
f_k = np.matrix(ff(*_c(zu_k))) # f
#print "f_k:\n", f_k
F_k = np.matrix(FF(*_c(zu_k))) #f.Jacobian
#print "F_k:\n", F_k
##### Kalman Filter
# Prediction
#print "P_k:\n", P_k
#print "H_k.T:\n", H_k.T
K_k = P_k.dot(H_k.T).dot(la.inv(H_k.dot(P_k).dot(H_k.T) + R))
#print "K_k:\n", K_k
Ps_k = (np.eye(len(z)) - K_k.dot(H_k)).dot(P_k) # P*_k
#print "Ps_k:\n", Ps_k
# Correction
P_k1 = Ps_k + T*(F_k.dot(Ps_k) + Ps_k.dot(F_k.T) + Q)
#print "P_k1:\n", P_k1
z_k1 = (z_k + T*f_k + K_k.dot((y_k - h_k)))
#print "z_k1:", z_k1
# State Propagation
x_k = x_k + x_dot*T
#print "x_k:\n",x_k
z_k = np.matrix(z_k1)
#print "z_k:\n", z_k
P_k = np.matrix(P_k1).astype(np.float)
#print "P_k:\n", P_k
In [ ]:
print A_k.squeeze()
print b_k
In [ ]:
import control as con
ss = con.matlab.ss(A_k.squeeze(), b_k.T, c_k, d_k)
ss.pole()
In [ ]:
Af = np.matrix([[ 2.84217094e-14, 1.71379179e+01],
[ -1.00000000e+02, -1.85369064e+02]])
Bf = np.matrix([[ 17.34396868],
[ 9.87641374]])
Cf = np.matrix([[ 0., -1.]])
Df = np.matrix([[ 0.]])
ss2 = con.matlab.ss(Af, Bf, Cf, Df)
ss2.pole()
In [ ]:
l = [('p{}_r'.format(i), 'p{}_i'.format(i)) for i in range(0,5)]
flat_list = [item for sublist in l for item in sublist]
flat_list
In [ ]:
import numpy as np
config = {'x': np.matrix(np.zeros(3)).T,
's' : np.matrix(-1*(1.0+np.random.rand(3)))}
print config['x']
np.save("~\test_narf", config)
In [ ]:
print np.load("test_narf.npy")
In [ ]:
filepath = "/home/lth/20180417-135357_cplx.2_real.1.npy"
#config = dict(np.ndenumerate(np.load(filepath)))
config = np.load(filepath).item()
print config['s'].shape[1]
print config['w'].shape[1]
print config['p'].shape[1]
x_k0 = np.matrix(np.zeros(len(config['c']))).T
z_k0 = np.hstack((x_k0.T,
config['w'],
config['s'],
config['p'],
config['b'],
config['c'],
config['d'])).T
#print z_k0
print len(z_k0)
print z_k0.shape[0]
P_k0 = np.eye(len(z_k0))
P_k = P_k0
z_k = z_k0
In [ ]:
import control as pc
from control import StateSpace
import numpy as np
from numpy.linalg import solve, inv
import matplotlib.pyplot as plt
from numpy import zeros, shape, poly, diag
from numpy.linalg import solve, matrix_rank, eig
#Af = np.matrix([[ 0, 1, 0],
# [ 0, 0, 1],
# [ 1, 2, 3]])
#Bf = np.matrix([[ 17.34396868],
# [ 9.87641374],
# [1]])
Cf = np.matrix([[ 1., 1., 2]])
#Af = np.random.rand(4,4)
#Bf = np.random.rand(4,1)
#Cf = np.random.rand(1,4)
Af = np.matrix([[ 3, -2],
[ 1, 1]])
Bf = np.matrix([[ 17.34396868],
[ 9.87641374]])
Cf = np.matrix([[ 1., 1.]])
Df = np.matrix([[ 0.]])
xsys = pc.ss(Af, Bf, Cf, Df)
print xsys.pole()
#print xsys
print "########################"
print
# Create a new system, starting with a copy of the old one
zsys = StateSpace(xsys)
# Calculate eigenvalues and matrix of eigenvectors Tzx,
# which is our transformation matrix
eigval, eigvec = eig(xsys.A)
print eigval
print eigvec
# Eigenvalues and according eigenvectors are not sorted,
# thus modal transformation not unique
# Sorting eigenvalues and respective vectors by largest to smallest eigenvalue
idx = eigval.argsort()[::-1]
eigval = eigval[idx]
eigvec = eigvec[:,idx]
lst_conjugates = []
P = None
for val, vec in zip(eigval, eigvec.T):
if np.iscomplex(val).any():
#print "val", val
#print "lst", lst_conjugates
if val not in lst_conjugates:
lst_conjugates.append(val.conjugate())
print "->complex", val
print "vec", vec
if P is not None:
P = np.hstack((P, np.hstack((vec.real.T, vec.imag.T))))
else:
P = np.hstack((vec.real.T, vec.imag.T))
#print "lst", lst_conjugates
print "->P", P
else:
lst_conjugates.remove(val)
else:
print "->real", val
print "vec", vec.T
if P is not None:
P = np.hstack((P, vec.real.T))
else:
P = vec.real.T
print "->P", P
print "P", P
print lst_conjugates
#print P
# Generate the system matrices for the desired canonical form
zsys.A = solve(P, xsys.A).dot(P)
zsys.B = solve(P, xsys.B)
zsys.C = xsys.C.dot(P)
print
print "#######################"
print zsys
#ss, T = pc.canonical_form(ss2, form='modal')
#print T
#print ss
print zsys.pole()
In [ ]:
import control as pc
from control import StateSpace
import numpy as np
from numpy.linalg import solve, inv
import matplotlib.pyplot as plt
import numpy as np
from numpy import zeros, shape, poly, diag
from numpy.linalg import solve, matrix_rank, eig
Af = np.matrix([[ 0, 1, 0],
[ 0, 0, 1],
[ 1, 2, 3]])
Bf = np.matrix([[ 17.34396868],
[ 9.87641374],
[1]])
Cf = np.matrix([[ 1., 1., 2]])
#Af = np.random.rand(4,4)
#Bf = np.random.rand(4,1)
#Cf = np.random.rand(1,4)
#Af = np.matrix([[ 1, 0],
# [ 3, 4]])
#Bf = np.matrix([[ 17.34396868],
# [ 9.87641374]])
#Cf = np.matrix([[ 1., 1.]])
Df = np.matrix([[ 0.]])
xsys = pc.ss(Af, Bf, Cf, Df)
print xsys.pole()
ss, T = pc.canonical_form(xsys, form='modal')
#print T
print ss
print ss.pole()
In [ ]:
a = np.array([[1,2],[3,4]])
In [ ]:
[[ 1.71837656e+00 0 0 0]
[ 0 -1.90864218e-01 1.92690569e-01 0]
[ 0 -1.92690569e-01 -1.90864218e-01 0]
[ 0 0 0 1.06306180e-01]]
In [ ]:
# Create a system in the modal canonical form
A_true = np.diag([3.0, 3.0, 2.0, 1.0]) # order from the largest to the smallest
B_true = np.matrix("1.1 2.2 3.3 4.4").T
C_true = np.matrix("1.3 1.4 1.5 1.6")
D_true = 42.0
# Perform a coordinate transform with a random invertible matrix
T_true = np.matrix([[-0.27144004, -0.39933167, 0.75634684, 0.44135471],
[-0.74855725, -0.39136285, -0.18142339, -0.50356997],
[-0.40688007, 0.81416369, 0.38002113, -0.16483334],
[-0.44769516, 0.15654653, -0.50060858, 0.72419146]])
A = np.linalg.solve(T_true, A_true)*T_true
B = np.linalg.solve(T_true, B_true)
C = C_true*T_true
D = D_true
# Create a state space system and convert it to the observable canonical form
sys_check, T_check = pc.canonical_form(pc.ss(A, B, C, D), "modal")
print sys_check
# Check against the true values
#TODO: Test in respect to non unique transformation (system characteristics?)
np.testing.assert_array_almost_equal(sys_check.A, A_true)
#np.testing.assert_array_almost_equal(sys_check.B, B_true)
#np.testing.assert_array_almost_equal(sys_check.C, C_true)
np.testing.assert_array_almost_equal(sys_check.D, D_true)
#np.testing.assert_array_almost_equal(T_check, T_true)
In [ ]:
u_all = np.array([])
np.append(u_all, 0.1)
In [ ]:
u_a = []
u_a + [1]
In [ ]:
import numpy as np
###########################
# another set of #
# Roboter pt2d + pos #
###########################
A = np.matrix([[ 0., 0., 0., -1. ],
[ 0., 0., 0, 22.7272727],
[ 0., 10, 0, 24.4318182],
[ 0., 0, -100, -86.3636364]])
B = np.matrix([[0],
[22.72727273],
[-6.25 ],
[ 0. ]])
C = np.matrix([[ 1., 0., 0., 0.]])
D = np.matrix([[ 0. ]])
u_max = 0.5
U_max = [u_max]
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
## 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)
import os
from cvxpy import *
import cvxpy
import sdpt3glue.solve as slv
import sdpt3glue
import random
n = B.shape[0] # get dim of system
n_u = B.shape[1]
# Variables
X = cvxpy.Semidef(n)
Y = cvxpy.Semidef(n)
W = cvxpy.Variable(n_u)
Ak_h = cvxpy.Semidef(*A.shape)
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(sign='positive')
# Pole restriction
ro = cvxpy.Parameter(sign='positive') # Real <=-ro
ni = cvxpy.Parameter(sign='positive') # |Imag| <= ni*Real
ro.value = 100
ni.value = 1
# Define Constraints
# (6.39a)
const_639a = cvxpy.bmat([[X, I],
[I, Y]]) >> 0
#[I, Y]]) == cvxpy.Semidef(2*n)
# (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.Semidef(2*n)
# (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.Semidef(2*n) 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.Semidef(2*n+n_u)
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.Semidef(2*n)
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.Semidef(4*n)
# 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)
ran = random.randint(0, 100)
folder ='/tmp/'
g.value = 1
# Generate filenames
try:
print "Solution CVX: ", prob_66.solve(solver=cvxpy.CVXOPT)
print "Solution MOSEK: ", prob_66.solve(solver=cvxpy.MOSEK)
except:
pass
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
In [ ]:
g.value = 1
result = sdpt3glue.sdpt3_solve_problem(prob_66, sdpt3glue.OCTAVE, matfile_target,
output_target=output_target)
In [ ]:
import optim_tools
[[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)
In [ ]:
print result
print result['Xvars'][1]
In [ ]:
OCTAVE_CMD = ("docker run --rm -it -v {workdir}:/data "
"jkawamoto/octave-sdpt3 octave").format(workdir=os.path.abspath("."))
result = sdpt3glue.sdpt3_solve_problem(problem, sdpt3glue.OCTAVE, matfile_target,
output_target=output_target, cmd=OCTAVE_CMD)
In [ ]:
result = sdpt3glue.sdpt3_solve_problem(problem, sdpt3glue.NEOS, matfile_target,
output_target=output_target)
In [ ]:
## Save and load matrizes
import numpy as np
A = np.matrix([[ 0., 0., 0., -1. ],
[ 0., 0., 0, 22.7272727],
[ 0., 10, 0, 24.4318182],
[ 0., 0, -100, -86.3636364]])
B = np.matrix([[0],
[22.72727273],
[-6.25 ],
[ 0. ]])
C = np.matrix([[ 1., 0., 0., 0.]])
D = np.matrix([[ 0. ]])
u_max = 0.5
import os
# Generic save and load
# Appends and updates values if already existing
def saveNPY(fname, **nargs):
data = nargs
try:
res, fname = loadNPY(fname)
data.update(res)
except IOError:
filename, file_extension = os.path.splitext(fname)
if file_extension is '':
file_extension = '.npy'
fname = filename + file_extension
print "Creating new file: {}".format(fname)
pass
np.save(fname, data)
# if not file_extension is given '.npy' is appended
def loadNPY(fname):
filename, file_extension = os.path.splitext(fname)
if file_extension is '':
file_extension = '.npy'
fname = filename + file_extension
res = np.load(fname).item()
return res, fname
# Takes and save SS Model
def saveModel(fname, A, B, C, D):
saveNPY(fname, A=A, B=B, C=C, D=D)
# Extracts SS Model
def getModel(fname):
res, _ = loadNPY(fname)
if all (k in res for k in ('A', 'B', 'C', 'D')):
return res['A'], res['B'], res['C'], res['D']
else:
raise KeyError('Not all matrizes (A, B, C, D) where found in loaded dictionary')
# Takes and saves Dynamical Control System Matrizes
def saveControl(fname, Ak, Bk, Ck, Dk, Ek, u_max):
saveNPY(fname, Ak=Ak, Bk=Bk, Ck=Ck, Dk=Dk, Ek=Ek, u_max=u_max)
# Extracts Dynamical Control System Matrizes
def getControl(fname):
res, _ = loadNPY(fname)
if all (k in res for k in ('Ak', 'Bk', 'Ck', 'Dk', 'Ek', 'u_max')):
return res['Ak'], res['Bk'], res['Ck'], res['Dk'], res['Ek'], res['u_max']
else:
raise KeyError('Not all matrizes (Ak, Bk, Ck, Dk, Ek, u_max) where found in loaded dictionary')
# Takes and save SS Model iekf style
def saveIEKF(fname, w, s, p, b, c, d):
iekf = {
'w' : w,
's' : s,
'p' : p,
'b' : b,
'c' : c,
'd' : d
}
saveNPY(fname, iekf_data=iekf)
# Extracts SS Model iekf style
def getIEKF(fname):
res, _ = loadNPY(fname)
if 'iekf_data' in res:
return res['iekf_data']
else:
raise KeyError('IEKF data not found in loaded dictionary')
In [ ]:
#saveModel('test', A=A, B=B, C=C, D=D)
saveControl('test', Ak=A, Bk=B, Ck=C, Dk=D, Ek=A, u_max=0.3)
In [ ]:
data = loadNPY('test.npy')
print data['A']
In [ ]:
getModel('test.npy')
In [ ]:
getControl('test.npy')
In [2]:
from pprint import pprint as pprint
import numpy as np
import matplotlib.pyplot as plt
import control as pc
%pylab inline
import pandas as pd
from sysident import loadtools
import scipy as sp
folder = "/home/lth/jupyter_nb/optimization/models/"
#fname = folder+"ss1_20180724-082630_poles2_ident_pade1_0:036_control_20180724-092309.npy"
#fname = folder+"ss2_20180724-082631_poles3_ident_pade2_0:036_control_20180724-092409.npy"
#fname = folder+"ss3_position_20180717-104106_poles3_ident_pade1_0:032_control_20180724-092445.npy"
fname = folder+"ss5_20180724-082628_poles3_ident_pade1_0:032_control_20180724-092522.npy"
#fname = folder+"ss6_20180724-082629_poles4_ident_pade2_0:032_control_20180724-092618.npy"
#/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/iekf_microssim_midnoise/ss_20181128-103659_poles3_powell_ident_pade1_0.0421005122207.npy
#/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/iekf_microssim_nonoise/ss_20181128-103832_poles3_powell_ident_pade1_0.0314547475844.npy
res, _ = loadtools.loadNPY('/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/iekf_microssim_midnoise/ss_20181128-103659_poles3_powell_ident_pade1_0.0421005122207.npy')
pprint(res.keys())
In [ ]: