def load_existing(linearized=True):
if linearized:
lin = '_lin'
else:
lin = ''
path = os.path.dirname(os.path.abspath(__file__))
x1, x2, x3, x4, x5, x6, u = sp.symbols("x1, x2, x3, x4, x5, x6, u")
with open(path+'/c_files/cart_pole_double_serial' + lin + '_ode.p', 'rb') as opened_file:
dx_t_sym = pickle.load(opened_file)
print('Model loaded')
with open(path+'/c_files/cart_pole_double_serial' + lin + '_A.p', 'rb') as opened_file:
Asym = pickle.load(opened_file)
print('A matrix loaded')
with open(path+'/c_files/cart_pole_double_serial' + lin + '_B.p', 'rb') as opened_file:
Bsym = pickle.load(opened_file)
print('B matrix loaded')
params = sp.symbols('m0, m1, m2, J1, J2, a1, a2, l1, l2, g, d0, d1, d2') # system parameters
m0, m1, m2, J1, J2, a1, a2, l1, l2, g, d0, d1, d2 = params
params_values = [(m0, 3.34), (m1, 0.876), (m2, 0.938), (J1, 0.013), (J2, 0.024),
(a1, 0.215), (a2, 0.269), (l1, 0.323), (l2, 0.419), (g, 9.81),
(d0, 0.1), (d1, 0.215), (d2, 0.002)]
dx_t_sym = dx_t_sym.subs(params_values)
Asym = Asym.subs(params_values)
Bsym = Bsym.subs(params_values)
try:
dx_c_func = sp2c.convert_to_c((x1, x2, x3, x4, x5, x6, u), dx_t_sym,
cfilepath=path+'/c_files/cart_pole_double_serial' + lin + '.c',
use_exisiting_so=False)
A_c_func = sp2c.convert_to_c((x1, x2, x3, x4, x5, x6, u), Asym,
cfilepath=path+'/c_files/cart_pole_double_serial' + lin + '_A.c',
use_exisiting_so=False)
B_c_func = sp2c.convert_to_c((x1, x2, x3, x4, x5, x6, u), Bsym,
cfilepath=path+'/c_files/cart_pole_double_serial' + lin + '_B.c',
use_exisiting_so=False)
A = lambda x, u: A_c_func(*x, *u)
B = lambda x, u: B_c_func(*x, *u)
dxdt = lambda t, x, u: dx_c_func(*x, *u).T[0]
assert(any(dxdt(0, [0, 0, 0, 1., 1., 1.], [0]) != [0., 0., 0., 0., 0., 0.]))
print('Using C-function')
except:
A_func = sp.lambdify((x1, x2, x3, x4, x5, x6, u), Asym, modules="numpy")
B_func = sp.lambdify((x1, x2, x3, x4, x5, x6, u), Bsym, modules="numpy")
A = lambda x, u: A_func(*x, *u)
B = lambda x, u: B_func(*x, *u)
dx_func = sp.lambdify((x1, x2, x3, x4, x5, x6, u), dx_t_sym[:], modules="numpy") # creating a callable python function
dxdt = lambda t, x, u: np.array(dx_func(*x, *u))
assert(any(dxdt(0, [0, 0, 0, 1., 1., 1.], [0]) != [0., 0., 0., 0., 0., 0.]))
print('Using lambdify')
return dxdt, A, B
def load_existing(linearized=True):
if linearized:
lin = '_lin'
else:
lin = ''
path = os.path.dirname(os.path.abspath(__file__))
x1, x2, x3, x4, u = sp.symbols("x1, x2, x3, x4, u")
with open(path + '/c_files/cart_pole' + lin + '_ode.p',
'rb') as opened_file:
dx_t_sym = pickle.load(opened_file)
print('Model loaded')
with open(path + '/c_files/cart_pole' + lin + '_A.p', 'rb') as opened_file:
Asym = pickle.load(opened_file)
print('A matrix loaded')
with open(path + '/c_files/cart_pole' + lin + '_B.p', 'rb') as opened_file:
Bsym = pickle.load(opened_file)
print('B matrix loaded')
try:
A_c_func = sp2c.convert_to_c(
(x1, x2, x3, x4, u),
Asym,
cfilepath=path + '/c_files/cart_pole' + lin + '_A.c',
use_exisiting_so=False)
B_c_func = sp2c.convert_to_c(
(x1, x2, x3, x4, u),
Bsym,
cfilepath=path + '/c_files/cart_pole' + lin + '_B.c',
use_exisiting_so=False)
A = lambda x, u: A_c_func(*x, *u)
B = lambda x, u: B_c_func(*x, *u)
dx_c_func = sp2c.convert_to_c(
(x1, x2, x3, x4, u),
dx_t_sym,
cfilepath=path + '/c_files/cart_pole' + lin + '_ode.c',
use_exisiting_so=False)
dxdt = lambda t, x, u: dx_c_func(*x, *u).T[0]
assert (any(dxdt(0, [0, 0, 1., 1.], [0]) != [0., 0., 0., 0.]))
print('Using C-function')
except:
A_func = sp.lambdify((x1, x2, x3, x4, u), Asym, modules="numpy")
B_func = sp.lambdify((x1, x2, x3, x4, u), Bsym, modules="numpy")
A = lambda x, u: A_func(*x, *u)
B = lambda x, u: B_func(*x, *u)
dx_func = sp.lambdify(
(x1, x2, x3, x4, u), dx_t_sym[:],
modules="numpy") # creating a callable python function
dxdt = lambda t, x, u: np.array(dx_func(*x, *u))
assert (any(dxdt(0, [0, 0, 1., 1.], [0]) != [0., 0., 0., 0.]))
print('Using lambdify')
return dxdt, A, B
def feedback_factory(vf_f, vf_g, xx, clcp_coeffs):
n = len(xx)
assert len(clcp_coeffs) == n + 1
assert len(vf_f) == n
assert len(vf_g) == n
assert clcp_coeffs[-1] == 1
# prevent datatype problems:
clcp_coeffs = st.to_np(clcp_coeffs)
# calculate the relevant covector_fields
# 1. extended nonlinear controllability matrix
Qext = st.nl_cont_matrix(vf_f, vf_g, xx, n_extra_cols=0)
QQinv = Qext.inverse_ADJ()
w_i = QQinv[-1, :]
omega_symb_list = [w_i]
t0 = time.time()
for i in range(1, n + 1):
w_i = st.lie_deriv_covf(w_i, vf_f, xx)
omega_symb_list.append(w_i)
print(i, t0 - time.time())
# dieser schritt dauert ca. 1 min
# ggf. sinnvoll: Konvertierung in c-code
# stack all 1-forms together
omega_matrix = sp.Matrix(omega_symb_list)
IPS()
omega_matrix_func = sp2c.convert_to_c(xx,
omega_matrix,
cfilepath="omega.c")
# noinspection PyPep8Naming
def feedback(xx_ref):
omega_matrix = omega_matrix_func(*xx_ref)
# iterate row-wise over the matrix
feedback_gain = st.to_np(
sum([rho_i * w for (rho_i, w) in zip(clcp_coeffs, omega_matrix)]))
return feedback_gain
# now return that fabricated function
return feedback
def cost_init(self):
""" Computes second order expansion of the """
# 2nd order taylor expansion of the cost function along a trajectory
xx = sp.symbols('x1:' + str(self.xDim + 1))
uu = sp.symbols('u1:' + str(self.uDim + 1))
t = sp.Symbol('t')
c = self.cost_fnc(xx, uu, t, sp)
cc = sp.Matrix([[c]])
cx = cc.jacobian(xx)
cu = cc.jacobian(uu)
Cxx = cx.jacobian(xx)
Cuu = cu.jacobian(uu)
Cxu = cx.jacobian(uu)
# final cost
cf = self.fcost_fnc(xx, sp)
ccf = sp.Matrix([[cf]])
cfx = ccf.jacobian(xx)
Cfxx = cfx.jacobian(xx)
try:
cx_func = sp2c.convert_to_c((*xx, *uu, t), cx.T, cfilepath=self.path + 'c_files/cx.c', use_exisiting_so=False)
self.cx = lambda x, u, t: cx_func(*x, *u, t)
cu_func = sp2c.convert_to_c((*xx, *uu, t), cu.T, cfilepath=self.path + 'c_files/cu.c', use_exisiting_so=False)
self.cu = lambda x, u, t: cu_func(*x, *u, t)
Cxx_func = sp2c.convert_to_c((*xx, *uu, t), Cxx, cfilepath=self.path + 'c_files/Cxx.c', use_exisiting_so=False)
self.Cxx = lambda x, u, t: Cxx_func(*x, *u, t)
Cuu_func = sp2c.convert_to_c((*xx, *uu, t), Cuu, cfilepath=self.path + 'c_files/Cuu.c', use_exisiting_so=False)
self.Cuu = lambda x, u, t: Cuu_func(*x, *u, t)
Cxu_func = sp2c.convert_to_c((*xx, *uu, t), Cxu, cfilepath=self.path + 'c_files/Cxu.c', use_exisiting_so=False)
self.Cxu = lambda x, u, t: Cxu_func(*x, *u, t)
cfx_func = sp2c.convert_to_c((*xx,), cfx.T, cfilepath=self.path + 'c_files/cfx.c', use_exisiting_so=False)
self.cfx = lambda x: cfx_func(*x,)
Cfxx_func = sp2c.convert_to_c((*xx,), Cfxx, cfilepath=self.path + 'c_files/Cfxx.c', use_exisiting_so=False)
self.Cfxx = lambda x: Cfxx_func(*x,)
print('Using C functions for taylor expansion of the cost function!')
except:
print('Could not use C functions for taylor expansion of the cost function!')
self.cx = sp.lambdify((xx, uu, t), cx.T)
self.cu = sp.lambdify((xx, uu, t), cu.T)
self.Cxx = sp.lambdify((xx, uu, t), Cxx)
self.Cuu = sp.lambdify((xx, uu, t), Cuu)
self.Cxu = sp.lambdify((xx, uu, t), Cxu)
self.cfx = sp.lambdify((xx,), cfx.T)
self.Cfxx = sp.lambdify((xx,), Cfxx)
pass
def tv_feedback_factory(ff, gg, xx, uu, clcp_coeffs, use_exisiting_so="smart"):
"""
:param ff:
:param gg:
:param xx:
:param uu:
:param clcp_coeffs:
:param use_exisiting_so:
:return:
"""
n = len(ff)
assert len(xx) == n
clcp_coeffs = st.to_np(clcp_coeffs)
assert len(clcp_coeffs) == n + 1
cfilepath = "k_timev.c"
# if we reuse the compiled code for the controller,
# we can skip the complete caluclation
input_data_hash = sp2c.reproducible_fast_hash([ff, gg, xx, uu])
print(input_data_hash)
if use_exisiting_so == "smart":
try:
lib_meta_data = sp2c.get_meta_data(cfilepath, reload_lib=True)
except FileNotFoundError as ferr:
use_exisiting_so = False
else:
if lib_meta_data.get("input_data_hash") == input_data_hash:
pass
use_exisiting_so = True
else:
use_exisiting_so = False
assert use_exisiting_so in (True, False)
if use_exisiting_so is True:
try:
nargs = sp2c.get_meta_data(cfilepath, reload_lib=True)["nargs"]
except FileNotFoundError as ferr:
print("File not found. Unable to use exisiting shared libray.")
# noinspection PyTypeChecker
return tv_feedback_factory(ff,
gg,
xx,
uu,
clcp_coeffs,
use_exisiting_so=False)
# now load the existing function
sopath = sp2c._get_so_path(cfilepath)
L_matrix_func = sp2c.load_func(sopath)
nu = nargs - n
else:
K1 = time_variant_controllability_matrix(ff, gg, xx, uu)
kappa = K1.det()
# K1_inv = K1.inverse_ADJ()
K1_adj = K1.adjugate()
lmd = K1_adj[-1, :]
diffop = DiffOpTimeVarSys(ff, gg, xx, uu)
ll = [
diffop.MA_vect(lmd, order=i, subs_xref=False) for i in range(n + 1)
]
# append a vector which contains kappa as first entries and 0s elsewhere
kappa_vector = sp.Matrix([kappa] + [0] * (n - 1)).T
ll.append(kappa_vector)
L_matrix = st.row_stack(*ll)
maxorder = max([0] + [symb.difforder for symb in L_matrix.s])
rplm = diffop.get_orig_ref_replm(maxorder)
L_matrix_r = L_matrix.subs(rplm)
xxuu = list(zip(*rplm))[1]
nu = len(xxuu) - len(xx)
# additional metadata
amd = dict(input_data_hash=input_data_hash, variables=xxuu)
L_matrix_func = sp2c.convert_to_c(xxuu,
L_matrix_r,
cfilepath=cfilepath,
use_exisiting_so=False,
additional_metadata=amd)
# noinspection PyShadowingNames
def tv_feedback_gain(xref, uuref=None):
if uuref is None:
args = list(xref) + [0] * nu
else:
args = list(xref) + list(uuref)
ll_num_ext = L_matrix_func(*args)
ll_num = ll_num_ext[:n + 1, :]
# extract kappa which we inserted into the L_matrix for convenience
kappa = ll_num_ext[n + 1, 0]
k = np.dot(clcp_coeffs, ll_num) / kappa
return k
# ship the information and internal functions to the outside
tv_feedback_gain.nu = nu
tv_feedback_gain.n = n
tv_feedback_gain.L_matrix_func = L_matrix_func
# return the fucntion
return tv_feedback_gain
def load_existing(linearized=True):
if linearized:
lin = '_lin'
else:
lin = ''
path = os.path.dirname(os.path.abspath(__file__))
x1, x2, x3, x4, x5, x6, x7, x8, u = sp.symbols(
"x1, x2, x3, x4, x5, x6, x7, x8, u")
with open(path + '/c_files/cart_pole_triple' + lin + '_ode.p',
'rb') as opened_file:
dx_t_sym = pickle.load(opened_file)
print('Model loaded')
with open(path + '/c_files/cart_pole_triple' + lin + '_A.p',
'rb') as opened_file:
Asym = pickle.load(opened_file)
print('A matrix loaded')
with open(path + '/c_files/cart_pole_triple' + lin + '_B.p',
'rb') as opened_file:
Bsym = pickle.load(opened_file)
print('B matrix loaded')
params = sp.symbols(
'm0, m1, m2, m3, J1, J2, J3, a1, a2, a3, l1, l2, l3, g, d0, d1, d2, d3'
) # system parameters
m0, m1, m2, m3, J1, J2, J3, a1, a2, a3, l1, l2, l3, g, d0, d1, d2, d3 = params
params_values = [(m0, 3.34), (m1, 0.876), (m2, 0.938), (m3, 0.553),
(J1, 0.013), (J2, 0.024), (J3, 0.018), (a1, 0.215),
(a2, 0.269), (a3, 0.226), (l1, 0.323), (l2, 0.419),
(l3, 0.484), (g, 9.81), (d0, 0.1), (d1, 0.215),
(d2, 0.002), (d3, 0.002)]
# parameters of the test bench at the Institute of Control Theory, TU Dresden
params_values = [(m0, 3.34), (m1, 0.8512), (m2, 0.8973), (m3, 0.5519),
(J1, 0.01980194), (J2, 0.02105375), (J3, 0.01818537),
(a1, 0.20001517), (a2, 0.26890449), (a3, 0.21666087),
(l1, 0.32), (l2, 0.419), (l3, 0.485), (g, 9.81),
(d0, 0.1), (d1, 0.00715294), (d2, 1.9497e-06),
(d3, 0.00164642)]
dx_t_sym = dx_t_sym.subs(params_values)
Asym = Asym.subs(params_values)
Bsym = Bsym.subs(params_values)
try:
A_c_func = sp2c.convert_to_c(
(x1, x2, x3, x4, x5, x6, x7, x8, u),
Asym,
cfilepath=path + '/c_files/cart_pole_triple' + lin + '_A.c',
use_exisiting_so=False)
B_c_func = sp2c.convert_to_c(
(x1, x2, x3, x4, x5, x6, x7, x8, u),
Bsym,
cfilepath=path + '/c_files/cart_pole_triple' + lin + '_B.c',
use_exisiting_so=False)
A = lambda x, u: A_c_func(*x, *u)
B = lambda x, u: B_c_func(*x, *u)
dx_c_func = sp2c.convert_to_c(
(x1, x2, x3, x4, x5, x6, x7, x8, u),
dx_t_sym,
cfilepath=path + '/c_files/cart_pole_triple' + lin + '_ode.c',
use_exisiting_so=False)
dxdt = lambda t, x, u: dx_c_func(*x, *u).T[0]
assert (any(
dxdt(0, [0, 0, 0, 0, 1., 1., 1., 1.], [0]) !=
[0., 0., 0., 0., 0., 0., 0., 0.]))
print('Using C-function')
except:
A_func = sp.lambdify((x1, x2, x3, x4, x5, x6, x7, x8, u),
Asym,
modules="numpy")
B_func = sp.lambdify((x1, x2, x3, x4, x5, x6, x7, x8, u),
Bsym,
modules="numpy")
A = lambda x, u: A_func(*x, *u)
B = lambda x, u: B_func(*x, *u)
dx_func = sp.lambdify(
(x1, x2, x3, x4, x5, x6, x7, x8, u), dx_t_sym[:],
modules="numpy") # creating a callable python function
dxdt = lambda t, x, u: np.array(dx_func(*x, *u))
assert (any(
dxdt(0, [0, 0, 0, 0, 1., 1., 1., 1.], [0]) !=
[0., 0., 0., 0., 0., 0., 0., 0.]))
print('Using lambdify')
return dxdt, A, B
