def get_sim_model(self, model_parameters):
if self.calc_coll_part_lin:
return st.SimulationModel(self.mod.ff, self.mod.gg, self.mod.xx,
model_parameters)
else:
return st.SimulationModel(self.mod.f, self.mod.g, self.mod.xx,
model_parameters)
def get_sim_model(self, model_parameters):
if self.calc_coll_part_lin:
sim_mod = st.SimulationModel(self.mod.ff, self.mod.gg, self.mod.xx, model_parameters)
else:
sim_mod = st.SimulationModel(self.mod.f, self.mod.g, self.mod.xx, model_parameters)
sim_mod.convert_to_c()
return sim_mod
def rhs_for_simulation(f, g, xx, controller_func):
"""
# calculate right hand side equation for simulation of the nonlinear system
:param f: vector field
:param g: input matrix
:param xx: states of the system
:param controller_func: input equation (trajectory)
:return: rhs: equation that is solved
"""
# call the class 'SimulationModel' to build the
# 'right hand side'equation for ode
sim_mod = st.SimulationModel(f, g, xx)
rhs_eq = sim_mod.create_simfunction(controller_function=controller_func)
return rhs_eq
def test_create_simfunction2(self):
x1, x2, x3, x4 = xx = sp.Matrix(sp.symbols("x1, x2, x3, x4"))
u1, u2 = uu = sp.Matrix(sp.symbols("u1, u2")) # inputs
p1, p2, p3, p4 = pp = sp.Matrix(
sp.symbols("p1, p2, p3, p4")) # parameter
t = sp.Symbol('t')
A = A0 = sp.randMatrix(len(xx), len(xx), -10, 10, seed=704)
B = B0 = sp.randMatrix(len(xx), len(uu), -10, 10, seed=705)
v1 = A[0, 0]
A[0, 0] = p1
v2 = A[2, -1]
A[2, -1] = p2
v3 = B[3, 0]
B[3, 0] = p3
v4 = B[2, 1]
B[2, 1] = p4
par_vals = lzip(pp, [v1, v2, v3, v4])
f = A * xx
G = B
fxu = (f + G * uu).subs(par_vals)
# some random initial values
x0 = st.to_np(sp.randMatrix(len(xx), 1, -10, 10, seed=706)).squeeze()
u0 = st.to_np(sp.randMatrix(len(uu), 1, -10, 10, seed=2257)).squeeze()
# create the model and the rhs-function
mod = st.SimulationModel(f, G, xx, par_vals)
rhs_xx_uu = mod.create_simfunction(free_input_args=True)
res0_1 = rhs_xx_uu(x0, u0, 0)
dres0_1 = st.to_np(fxu.subs(lzip(xx, x0) + lzip(uu, u0))).squeeze()
bin_res01 = np.isclose(res0_1, dres0_1) # binary array
self.assertTrue(np.all(bin_res01))
def __init__(self,
f,
g,
xx,
x0,
t0,
T,
input_func=None,
model_parameters=(),
func_parameters=None,
search_space=None,
step_size=0.1e-3,
seed=42):
self.system = st.SimulationModel(f, g, xx, model_parameters)
self.input_func = input_func
self.func_parameters = func_parameters
self.x0 = x0
self.t0 = t0
self.T = T
self.t = np.arange(self.t0, self.t0 + self.T, step_size)
self.search_space = search_space
np.random.seed(seed)
def test_num_trajectory_compatibility_test(self):
x1, x2, x3, x4 = xx = sp.Matrix(sp.symbols("x1, x2, x3, x4"))
u1, u2 = uu = sp.Matrix(sp.symbols("u1, u2")) # inputs
t = sp.Symbol('t')
# we want to create a random but stable matrix
np.random.seed(2805)
diag = np.diag(np.random.random(len(xx)) * -10)
T = sp.randMatrix(len(xx), len(xx), -10, 10, seed=704)
Tinv = T.inv()
A = Tinv * diag * T
B = B0 = sp.randMatrix(len(xx), len(uu), -10, 10, seed=705)
x0 = st.to_np(sp.randMatrix(len(xx), 1, -10, 10, seed=706)).squeeze()
tt = np.linspace(0, 5, 2000)
des_input = st.piece_wise((2 - t, t <= 1), (t, t < 2),
(2 * t - 2, t < 3), (4, True))
des_input_func_vec = st.expr_to_func(t,
sp.Matrix([des_input, des_input]))
mod2 = st.SimulationModel(A * xx, B, xx)
rhs3 = mod2.create_simfunction(input_function=des_input_func_vec)
XX = sc.integrate.odeint(rhs3, x0, tt)
UU = des_input_func_vec(tt)
res1 = mod2.num_trajectory_compatibility_test(tt, XX, UU)
self.assertTrue(res1)
# slightly different input signal -> other results
res2 = mod2.num_trajectory_compatibility_test(tt, XX, UU * 1.1)
self.assertFalse(res2)
gg = ggl
else:
ff = ff_o
gg = gg_o
clcp_coeffs = st.coeffs((x1 + 2)**4)[::-1]
tv_feedback_gain = tv_feedback_factory(ff, gg, xx, uu, clcp_coeffs)
if 0:
ff2 = st.multi_taylor_matrix(ff, xx, x0=[0] * 4, order=2)
gg2 = st.multi_taylor_matrix(gg, xx, x0=[0] * 4, order=2)
feedback_gain_func = feedback_factory(ff, gg, xx, clcp_coeffs)
# feedback_gain_func(xx0)
mod1 = st.SimulationModel(ff, gg, xx)
detQc = st.nl_cont_matrix(ff, gg, xx, n_extra_cols=0).berkowitz_det()
detQc_func = sp.lambdify(xx, detQc, modules="numpy")
# calculate ref-input for swingup:
ffl = sp.lambdify(list(xx) + list(uu), list(ff + gg * u1), modules=["sympy"])
def pytraj_f(xx, uu, uuref, t, pp):
args = list(xx) + list(uu)
return ffl(*args)
def test_create_simfunction(self):
x1, x2, x3, x4 = xx = sp.Matrix(sp.symbols("x1, x2, x3, x4"))
u1, u2 = uu = sp.Matrix(sp.symbols("u1, u2")) # inputs
p1, p2, p3, p4 = pp = sp.Matrix(
sp.symbols("p1, p2, p3, p4")) # parameter
t = sp.Symbol('t')
A = A0 = sp.randMatrix(len(xx), len(xx), -10, 10, seed=704)
B = B0 = sp.randMatrix(len(xx), len(uu), -10, 10, seed=705)
v1 = A[0, 0]
A[0, 0] = p1
v2 = A[2, -1]
A[2, -1] = p2
v3 = B[3, 0]
B[3, 0] = p3
v4 = B[2, 1]
B[2, 1] = p4
par_vals = lzip(pp, [v1, v2, v3, v4])
f = A * xx
G = B
fxu = (f + G * uu).subs(par_vals)
# some random initial values
x0 = st.to_np(sp.randMatrix(len(xx), 1, -10, 10, seed=706)).squeeze()
# Test handling of unsubstituted parameters
mod = st.SimulationModel(f, G, xx, model_parameters=par_vals[1:])
with self.assertRaises(ValueError) as cm:
rhs0 = mod.create_simfunction()
self.assertTrue("unexpected symbols" in cm.exception.args[0])
# create the model and the rhs-function
mod = st.SimulationModel(f, G, xx, par_vals)
rhs0 = mod.create_simfunction()
self.assertFalse(mod.compiler_called)
self.assertFalse(mod.use_sp2c)
res0_1 = rhs0(x0, 0)
dres0_1 = st.to_np(fxu.subs(lzip(xx, x0) + st.zip0(uu))).squeeze()
bin_res01 = np.isclose(res0_1, dres0_1) # binary array
self.assertTrue(np.all(bin_res01))
# difference should be [0, 0, ..., 0]
self.assertFalse(np.any(rhs0(x0, 0) - rhs0(x0, 3.7)))
# simulate
tt = np.linspace(0, 0.5,
100) # simulation should be short due to instability
res1 = sc.integrate.odeint(rhs0, x0, tt)
# create and try sympy_to_c bridge (currently only works on linux
# and if sympy_to_c is installed (e.g. with `pip install sympy_to_c`))
# until it is not available for windows we do not want it as a requirement
# see also https://stackoverflow.com/a/10572833/333403
try:
import sympy_to_c
except ImportError:
# noinspection PyUnusedLocal
sympy_to_c = None
sp2c_available = False
else:
sp2c_available = True
if sp2c_available:
rhs0_c = mod.create_simfunction(use_sp2c=True)
self.assertTrue(mod.compiler_called)
res1_c = sc.integrate.odeint(rhs0_c, x0, tt)
self.assertTrue(np.all(np.isclose(res1_c, res1)))
mod.compiler_called = None
rhs0_c = mod.create_simfunction(use_sp2c=True)
self.assertTrue(mod.compiler_called is None)
# proof calculation
# x(t) = x0*exp(A*t)
Anum = st.to_np(A.subs(par_vals))
Bnum = st.to_np(G.subs(par_vals))
# noinspection PyUnresolvedReferences
xt = [np.dot(sc.linalg.expm(Anum * T), x0) for T in tt]
xt = np.array(xt)
# test whether numeric results are close within given tolerance
bin_res1 = np.isclose(res1, xt, rtol=2e-5) # binary array
self.assertTrue(np.all(bin_res1))
# test handling of parameter free models:
mod2 = st.SimulationModel(Anum * xx, Bnum, xx)
rhs2 = mod2.create_simfunction()
res2 = sc.integrate.odeint(rhs2, x0, tt)
self.assertTrue(np.allclose(res1, res2))
# test input functions
des_input = st.piece_wise((0, t <= 1), (t, t < 2), (0.5, t < 3),
(1, True))
des_input_func_scalar = st.expr_to_func(t, des_input)
des_input_func_vec = st.expr_to_func(t,
sp.Matrix([des_input, des_input]))
# noinspection PyUnusedLocal
with self.assertRaises(TypeError) as cm:
mod2.create_simfunction(input_function=des_input_func_scalar)
rhs3 = mod2.create_simfunction(input_function=des_input_func_vec)
# noinspection PyUnusedLocal
res3_0 = rhs3(x0, 0)
rhs4 = mod2.create_simfunction(input_function=des_input_func_vec,
time_direction=-1)
res4_0 = rhs4(x0, 0)
self.assertTrue(np.allclose(res3_0, np.array([119., -18., -36.,
-51.])))
self.assertTrue(np.allclose(res4_0, -res3_0))
def get_sim_model(self, model_parameters):
self.model_parameters = model_parameters
sim_mod = st.SimulationModel(self.mod.f, self.mod.g, self.mod.xx, model_parameters)
sim_mod.convert_to_c()
return sim_mod