def smooth_time_scaling(Tend, N, phase_fraction=.5):
"""
:param Tend:
:param N:
:param phase_fraction: fraction of Tend for smooth initial and end phase
"""
T0 = 0
T1 = Tend * phase_fraction
y0 = 0
y1 = 1
# for initial phase
poly1 = st.condition_poly(t, (T0, y0, 0, 0), (T1, y1, 0, 0))
# for end phase
poly2 = poly1.subs(t, Tend - t)
# there should be a phase in the middle with constant slope
deriv_transition = st.piece_wise((y0, t < T0), (poly1, t < T1), (y1, t < Tend - T1),
(poly2, t < Tend), (y0, True))
scaling = sp.integrate(deriv_transition, (t, T0, Tend))
time_transition = sp.integrate(deriv_transition * N / scaling, t)
# deriv_transition_func = st.expr_to_func(t, full_transition)
time_transition_func = st.expr_to_func(t, time_transition)
deriv_func = st.expr_to_func(t, deriv_transition * N / scaling)
deriv_func2 = st.expr_to_func(t, deriv_transition.diff(t) * N / scaling)
C = ipydex.Container(fetch_locals=True)
return C
def calc_trajectory(self):
"""
calculate the polynomial trajectory
:return: polynomial trajectory and it's derivatives
"""
# state transition
poly = st.condition_poly(self.t, (self.t0, *self.YA),
(self.tf, *self.YB))
# full transition within the given period
full_transition = st.piece_wise((self.YA[0], self.t < self.t0),
(poly, self.t < self.tf),
(self.YB[0], True))
poly_d = []
for i in range(len(self.YA)):
poly_d.append(full_transition.diff(self.t, i))
return poly_d
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)
def test_expr_to_func(self):
x1, x2 = xx = sp.Matrix(sp.symbols("x1, x2"))
t, = sp.symbols("t,")
r_ = np.r_
f1 = st.expr_to_func(x1, 2 * x1)
self.assertEqual(f1(5.1), 10.2)
XX1 = np.r_[1, 2, 3.7]
res1 = f1(XX1) == 2 * XX1
self.assertTrue(res1.all)
f2 = st.expr_to_func(x1, sp.Matrix([x1 * 2, x1 + 5, 4]))
res2 = f2(3) == r_[6, 8, 4]
self.assertTrue(res2.all())
res2b = f2(r_[3, 10, 0]) == np.array([[6, 8, 4], [20, 15, 4],
[0, 5, 4]])
self.assertTrue(res2b.all())
f3 = st.expr_to_func(xx, sp.Matrix([x1 * 2, x2 + 5, 4]))
res3 = np.allclose(f3(-3.1, 4), r_[-6.2, 9, 4])
self.assertTrue(res3)
# test compatibility with Piecewise Expressions
des_input = st.piece_wise((0, t <= 1), (t, t < 2), (0.5, t < 3),
(1, True))
f4s = st.expr_to_func(t, des_input)
f4v = st.expr_to_func(t, sp.Matrix([des_input, des_input]))
self.assertEqual(f4s(2.7), 0.5)
sol = r_[0, 1.6, 0.5, 1, 1]
res4a = f4s(r_[0.3, 1.6, 2.2, 3.1, 500]) == sol
self.assertTrue(res4a.all())
res4b = f4v(r_[0.3, 1.6, 2.2, 3.1, 500])
col1, col2 = res4b.T
self.assertTrue(np.array_equal(col1, sol))
self.assertTrue(np.array_equal(col2, sol))
spmatrix = sp.Matrix([[x1, x1 * x2], [0, x2**2]])
fnc1 = st.expr_to_func(xx, spmatrix, keep_shape=False)
fnc2 = st.expr_to_func(xx, spmatrix, keep_shape=True)
res1 = fnc1(1.0, 2.0)
res2 = fnc2(1.0, 2.0)
self.assertEqual(res1.shape, (4, ))
self.assertEqual(res2.shape, (2, 2))
# noinspection PyTypeChecker
self.assertTrue(np.all(res1 == [1, 2, 0, 4]))
# noinspection PyTypeChecker
self.assertTrue(np.all(res1 == res2.flatten()))
fnc = st.expr_to_func(xx, x1 + x2)
self.assertEqual(fnc(1, 3), 4)
xx_res = np.array([1, 3, 1.1, 3, 1.2, 3.0]).reshape(3, -1)
self.assertTrue(np.allclose(fnc(*xx_res.T), np.array([4, 4.1, 4.2])))
fnc1 = st.expr_to_func(xx, 3 * xx)
fnc2 = st.expr_to_func(xx, 3 * xx, allow_kwargs=True)
self.assertTrue(np.allclose(fnc1(10, 100), fnc2(x2=100, x1=10)))
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))