def test_calc_flow_from_vectorfield(self):
a, b = sp.symbols("a, b", nonzero=True)
t, x1, x2, x3, x4 = sp.symbols("t, x1, x2, x3, x4")
xx = x1, x2, x3, x4
vf1 = sp.Matrix([0, 1, x3])
vf2 = sp.Matrix([0, 1, x3, sin(a * x2)])
res1, fp, iv1 = st.calc_flow_from_vectorfield(vf1,
xx[:-1],
flow_parameter=t)
vf1_sol = vf1.subs(lzip(xx[:-1], res1))
self.assertEqual(fp, t)
self.assertEqual(res1.diff(t), vf1_sol)
res2, fp, iv2 = st.calc_flow_from_vectorfield(vf2,
xx,
flow_parameter=t)
vf2_sol = vf2.subs(lzip(xx[:-1], res2))
self.assertEqual(fp, t)
self.assertEqual(res2.diff(t), vf2_sol)
res3, fp, iv3 = st.calc_flow_from_vectorfield(sp.Matrix([x1, 1, x1]),
xx[:-1])
t = fp
x1_0, x2_0, x3_0 = iv3
ref3 = sp.Matrix([[x1_0 * sp.exp(t)], [t + x2_0],
[x1_0 * sp.exp(t) - x1_0 + x3_0]])
self.assertEqual(res3, ref3)
def test_subz(self):
x1, x2, x3 = xx = sp.Matrix(sp.symbols("x1, x2, x3"))
y1, y2, y3 = yy = sp.symbols("y1, y2, y3")
a = x1 + 7 * x2 * x3
M1 = sp.Matrix([x2, x1 * x2, x3**2])
M2 = sp.ImmutableDenseMatrix(M1)
self.assertEqual(x1.subs(lzip(xx, yy)), x1.subz(xx, yy))
self.assertEqual(a.subs(lzip(xx, yy)), a.subz(xx, yy))
self.assertEqual(M1.subs(lzip(xx, yy)), M1.subz(xx, yy))
self.assertEqual(M2.subs(lzip(xx, yy)), M2.subz(xx, yy))
def point_velocity(point, coord_symbs, velo_symbs, t):
coord_funcs = []
for c in coord_symbs:
coord_funcs.append(sp.Function(c.name + "f")(t))
coord_funcs = sp.Matrix(coord_funcs)
p1 = point.subs(lzip(coord_symbs, coord_funcs))
v1_f = p1.diff(t)
backsubs = lzip(coord_funcs, coord_symbs) + lzip(coord_funcs.diff(t),
velo_symbs)
return v1_f.subs(backsubs)
def point_velocity(point, coord_symbs, velo_symbs, t):
coord_funcs = []
for c in coord_symbs:
coord_funcs.append(sp.Function(c.name + "f")(t))
coord_funcs = sp.Matrix(coord_funcs)
p1 = point.subs(lzip(coord_symbs, coord_funcs))
v1_f = p1.diff(t)
backsubs = lzip(coord_funcs, coord_symbs) + lzip(coord_funcs.diff(t),
velo_symbs)
return v1_f.subs(backsubs)
def test_rnd_number_tuples(self):
x1, x2, x3 = xx = sp.symbols('x1:4')
s = sum(xx)
res_a1 = st.rnd_number_subs_tuples(s)
self.assertTrue(isinstance(res_a1, list))
self.assertEqual(len(res_a1), len(xx))
c1 = [
len(e) == 2 and e[0].is_Symbol and st.is_number(e[1])
for e in res_a1
]
self.assertTrue(all(c1))
t = sp.Symbol('t')
f = sp.Function('f')(t)
fdot = f.diff(t)
fddot = f.diff(t, 2)
ff = sp.Matrix([f, fdot, fddot, x1 * x2])
for i in range(100):
res_b1 = st.rnd_number_subs_tuples(ff, seed=i)
expct_b1_set = set([f, fdot, fddot, t, x1, x2])
res_b1_atom_set = set(lzip(*res_b1)[0])
self.assertEqual(expct_b1_set, res_b1_atom_set)
# highest order has to be returned first
self.assertEqual(res_b1[0][0], fddot)
self.assertEqual(res_b1[1][0], fdot)
self.assertTrue(all([st.is_number(e[1]) for e in res_b1]))
def make_all_symbols_noncommutative(expr, appendix='_n'):
"""
:param expr:
:return: expr (with all symbols noncommutative) and
a subs_tuple_list [(s1_n, s1_c), ... ]
"""
if isinstance(expr, (list, tuple, set)):
expr = sp.Matrix(list(expr))
symbs = st.atoms(expr, sp.Symbol)
c_symbols = [s for s in symbs if s.is_commutative]
new_symbols = [sp.Symbol(s.name+appendix, commutative=False)
for s in c_symbols]
# preserve difforder attributes
st.copy_custom_attributes(c_symbols, new_symbols)
tup_list = lzip(new_symbols, c_symbols)
return expr.subs(lzip(c_symbols, new_symbols)), tup_list
def make_all_symbols_noncommutative(expr, appendix='_n'):
"""
:param expr:
:return: expr (with all symbols noncommutative) and
a subs_tuple_list [(s1_n, s1_c), ... ]
"""
if isinstance(expr, (list, tuple, set)):
expr = sp.Matrix(list(expr))
symbs = st.atoms(expr, sp.Symbol)
c_symbols = [s for s in symbs if s.is_commutative]
new_symbols = [
sp.Symbol(s.name + appendix, commutative=False) for s in c_symbols
]
# preserve difforder attributes
st.copy_custom_attributes(c_symbols, new_symbols)
tup_list = lzip(new_symbols, c_symbols)
return expr.subs(lzip(c_symbols, new_symbols)), tup_list
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 substitute_ext_forces(self, old_F, new_F, new_F_symbols):
"""
Backround: sometimes modeling is easier with forces which are not
the real control inputs. Then it is necessary to introduce the real
forces later.
"""
# nothing fancy has be done yet with the model
assert self.eqns != None
assert (self.f, self.solved_eq, self.state_eq) == (None,) * 3
subslist = lzip(old_F, new_F)
self.eqns = self.eqns.subs(subslist)
self.extforce_list = new_F_symbols
def substitute_ext_forces(self, old_F, new_F, new_F_symbols):
"""
Background: sometimes modeling is easier with forces which are not
the real control inputs. Then it is necessary to introduce the real
forces later.
"""
# nothing fancy has be done yet with the model
assert self.eqns != None
assert (self.f, self.solved_eq, self.state_eq) == (None,) * 3
subslist = lzip(old_F, new_F)
self.eqns = self.eqns.subs(subslist)
self.extforce_list = new_F_symbols
def nonzero_tuples(self, srn=False, eps=1e-30):
"""
returns a list of tuples (coeff, idcs) for each index-tuple idcs,
where the corresponding coeff != 0
"""
Z = lzip(self.coeff, self.indices)
if srn == "prime":
res = [(c, idcs) for (c, idcs) in Z
if abs(st.subs_random_numbers(c, prime=True)) > eps]
elif srn:
res = [(c, idcs) for (c, idcs) in Z
if abs(st.subs_random_numbers(c)) > eps]
else:
res = [(c, idcs) for (c, idcs) in Z if c != 0]
return res
def ord(self):
"""returns the highest differential order of the highest nonzero coefficient
example: w=(dx1^dxdot1) + (dx2^dxdddot1)
w.ord -> 3
"""
base_length = self._calc_base_length()
nzt = self.nonzero_tuples(srn=True)
if len(nzt) == 0:
return 0
# get the highest scalar index (basis index) which occurs in any nonzero index tuple
nz_idx_array = np.array(lzip(*nzt)[1])
highest_base_index = np.max(nz_idx_array)
res = int(highest_base_index / base_length)
return res
def test_sorted_eigenvectors(self):
V1 = st.sorted_eigenvector_matrix(self.M1)
ev1 = st.sorted_eigenvalues(self.M1)
self.assertEqual(len(ev1), V1.shape[1])
for val, vect in lzip(ev1, st.col_split(V1)):
res_vect = self.M1 * vect - val * vect
res = (res_vect.T * res_vect)[0]
self.assertTrue(res < 1e-15)
self.assertAlmostEqual((vect.T * vect)[0] - 1, 0)
V2 = st.sorted_eigenvector_matrix(self.M1, numpy=True)
V3 = st.sorted_eigenvector_matrix(self.M1, numpy=True, increase=True)
# quotients should be +-1
res1 = np.abs(st.to_np(V1) / st.to_np(V2)) - np.ones_like(V1)
res2 = np.abs(st.to_np(V1) / st.to_np(V3[:, ::-1])) - np.ones_like(V1)
self.assertTrue(np.max(np.abs(res1)) < 1e-5)
self.assertTrue(np.max(np.abs(res2)) < 1e-5)
def pull_back(phi, args, omega):
"""
computes the pullback phi^* omega for a given mapping phi between
manifolds (assumed to be given as a 1-column matrix
(however, phi is not a vector field))
"""
assert isinstance(phi, sp.Matrix)
assert phi.shape[1] == 1
assert phi.shape[0] == omega.dim_basis
assert isinstance(omega, DifferentialForm)
if omega.grad > 1:
raise NotImplementedError("Not yet implemented")
# the arguments of phi are the new basis symbols
subs_trafo = lzip(omega.basis, phi)
new_coeffs = omega.coeff.T.subs(subs_trafo) * phi.jacobian(args)
res = DifferentialForm(omega.grad, args, coeff=new_coeffs)
return res
def contraction(vf, form):
"""
performs the interior product of a vector field v and a p-form A
(v˩A)(x,y,z,...) = A(v, x, y, z, ...)
expects vf (v) as a column Matrix (coefficients of suitable basis vectors)
"""
assert isinstance(vf, sp.Matrix)
n1, n2 = vf.shape
assert n1 == form.dim_basis and n2 == 1
if form.grad == 0:
return 0 # by definition
# A can be considered as a sum of decomposable basis-forms
# contraction can be performed for each of the basis components
# A.indices contains information about the basis-Vectors
# we only need those index-tuples with a nonzero coefficient
nzt = form.nonzero_tuples()
# example: A = c1*dx0^dx1 + c2*dx2^dx4 + ...
coeffs, index_tuples = lzip(
*nzt) # -> [(c1, c2, ...), ((0,1), (2, 4), ...)]
# our result will go there
result = DifferentialForm(form.grad - 1, form.basis)
for coeff, idx_tup in nzt:
part_res = _contract_vf_with_basis_form(vf, idx_tup)
for c, rest_idcs in part_res:
# each rest-index-tuple can occur multiple times -> sum entries
tmp = result.__getitem__(rest_idcs) + c * coeff
result.__setitem__(rest_idcs, tmp)
return result
def unimod_inv(M,
s=None,
t=None,
time_dep_symbs=[],
simplify_nsm=True,
max_deg=None):
""" Assumes that M(s) is an unimodular polynomial matrix and calculates its inverse
which is again unimodular
:param M: Matrix to be inverted
:param s: Derivative Symbol
:param time_dep_symbs: sequence of time dependent symbols
:param max_deg: maximum polynomial degree w.r.t. s of the ansatz
:return: Minv
"""
assert isinstance(M, sp.MatrixBase)
assert M.is_square
n = M.shape[0]
degree_m = nc_degree(M, s)
if max_deg is None:
# upper bound according to
# Levine 2011, On necessary and sufficient conditions for differential flatness, p. 73
max_deg = (n - 1) * degree_m
assert int(max_deg) == max_deg
assert max_deg >= 0
C = M * 0
free_params = []
for i in range(max_deg + 1):
prefix = 'c{0}_'.format(i)
c_part = st.symbMatrix(n, n, prefix, commutative=False)
C += nc_mul(c_part, s**i)
free_params.extend(list(c_part))
P = nc_mul(C, M) - sp.eye(n)
P2 = right_shift_all(P, s, t, time_dep_symbs).reshape(n * n, 1)
deg_P = nc_degree(P2, s)
part_eqns = []
for i in range(deg_P + 1):
# omit the highest order (because it behaves like in the commutative case)
res = P2.diff(s, i).subs(s, 0) #/sp.factorial(i)
part_eqns.append(res)
eqns = st.row_stack(*part_eqns) # equations for all degrees of s
# now non-commutativity is inferring
eqns2, st_c_nc = make_all_symbols_commutative(eqns)
free_params_c, st_c_nc_free_params = make_all_symbols_commutative(
free_params)
# find out which of the equations are (in)homogeneous
eqns2_0 = eqns2.subs(st.zip0(free_params_c))
assert eqns2_0.atoms() in ({0, -1}, {-1}, set())
inhom_idcs = st.np.where(st.to_np(eqns2_0) != 0)[0]
hom_idcs = st.np.where(st.to_np(eqns2_0) == 0)[0]
eqns_hom = sp.Matrix(st.np.array(eqns2)[hom_idcs])
eqns_inh = sp.Matrix(st.np.array(eqns2)[inhom_idcs])
assert len(eqns_inh) == n
# find a solution for the homogeneous equations
# if this is not possible, M was not unimodular
Jh = eqns_hom.jacobian(free_params_c).expand()
nsm = st.nullspaceMatrix(Jh, simplify=simplify_nsm, sort_rows=True)
na = nsm.shape[1]
if na < n:
msg = 'Could not determine sufficiently large nullspace. '\
'Either M is not unimodular or the expressions are to complicated.'
# TODO: decide which of the two cases occurs, via substitution of
# random numbers and singular value decomposition
# (or application of st.generic_rank)
raise ValueError(msg)
# parameterize the inhomogenous equations with the solution of the homogeneous equations
# new free parameters:
aa = st.symb_vector('_a1:{0}'.format(na + 1))
nsm_a = nsm * aa
eqns_inh2 = eqns_inh.subs(lzip(free_params_c, nsm_a))
# now solve the remaining equations
# solve the linear system
Jinh = eqns_inh2.jacobian(aa)
rhs_inh = -eqns_inh2.subs(st.zip0(aa))
assert rhs_inh == sp.ones(n, 1)
sol_vect = Jinh.solve(rhs_inh)
sol = lzip(aa, sol_vect)
# get the values for all free_params (now they are not free anymore)
free_params_sol_c = nsm_a.subs(sol)
# replace the commutative symbols with the original non_commutative symbols (of M)
free_params_sol = free_params_sol_c.subs(st_c_nc)
Minv = C.subs(lzip(free_params, free_params_sol))
return Minv
def calc_lbi_nf_state_eq(self, simplify=False):
"""
calc vectorfields fz, and gz of the Lagrange-Byrnes-Isidori-Normalform
instead of the state xx
"""
n = len(self.tt)
nq = len(self.tau)
np = n - nq
nx = 2*n
# make sure that the system has the desired structure
B = self.eqns.jacobian(self.tau)
cond1 = B[:np, :] == sp.zeros(np, nq)
cond2 = B[np:, :] == -sp.eye(nq)
if not cond1 and cond2:
msg = "The jacobian of the equations of motion do not have the expected structure: %s"
raise NotImplementedError(msg % str(B))
pp = self.tt[:np,:]
qq = self.tt[np:,:]
uu = self.ttd[:np,:]
vv = self.ttd[np:,:]
ww = st.symb_vector('w1:{0}'.format(np+1))
assert len(vv) == nq
# state w.r.t normal form
self.zz = st.row_stack(qq, vv, pp, ww)
self.ww = ww
# set the actuated accelearations as new inputs
self.aa = self.ttdd[-nq:, :]
# input vectorfield
self.gz = sp.zeros(nx, nq)
self.gz[nq:2*nq, :] = sp.eye(nq) # identity matrix for the active coordinates
# drift vectorfield (will be completed below)
self.fz = sp.zeros(nx, 1)
self.fz[:nq, :] = vv
self.calc_mass_matrix()
if simplify:
self.M.simplify()
M11 = self.M[:np, :np]
M12 = self.M[:np, np:]
d = M11.berkowitz_det()
adj = M11.adjugate()
if simplify:
d = d.simplify()
adj.simplify()
M11inv = adj/d
# defining equation for ww: ww := uu + M11inv*M12*vv
uu_expr = ww - M11inv*M12*vv
# setting input tau and acceleration to 0 in the equations of motion
C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau))
N = st.time_deriv(M11inv*M12, self.tt)
ww_dot = -M11inv*C1K1.subs(lzip(uu, uu_expr)) + N.subs(lzip(uu, uu_expr))*vv
self.fz[2*nq:2*nq+np, :] = uu_expr
self.fz[2*nq+np:, :] = ww_dot
# how the new coordinates are defined:
self.ww_def = uu + M11inv*M12*vv
if simplify:
self.fz.simplify()
self.gz.simplify()
self.ww_def.simplify()
def generate_model(T, U, qq, F, **kwargs):
raise DeprecationWarning('generate_symbolic_model should be used')
"""
T kinetic energy
U potential energy
q independend deflection variables
F external forces
D dissipation function
"""
n = len(qq)
# time variable
t = sp.symbols('t')
# symbolic Variables (to prevent Functions where we not want them)
qs = []
qds = []
qdds = []
if not kwargs:
# assumptions for the symbols (facilitating the postprocessing)
kwargs ={"real": True}
for qi in qq:
# ensure that the same Symbol for t is used
assert qi.is_Function and qi.args == (t,)
s = str(qi.func)
qs.append(sp.Symbol(s, **kwargs))
qds.append(sp.Symbol(s + "_d", **kwargs))
qdds.append(sp.Symbol(s + "_dd", **kwargs))
qs, qds, qdds = sp.Matrix(qs), sp.Matrix(qds), sp.Matrix(qdds)
#derivative of configuration variables
qd = qq.diff(t)
qdd = qd.diff(t)
#lagrange function
L = T - U
#substitute functions with symbols for partial derivatives
#highest derivative first
subslist = lzip(qdd, qdds) + lzip(qd, qds) + lzip(qq, qs)
L = L.subs(subslist)
# partial derivatives of L
Ldq = st.jac(L, qs)
Ldqd = st.jac(L, qds)
# generalised external force
f = sp.Matrix(F)
# substitute symbols with functions for time derivative
subslistrev = st.rev_tuple(subslist)
Ldq = Ldq.subs(subslistrev)
Ldqd = Ldqd.subs(subslistrev)
Ldqd = Ldqd.diff(t)
#lagrange equation 2nd kind
model_eq = qq * 0
for i in range(n):
model_eq[i] = Ldqd[i] - Ldq[i] - f[i]
# model equations with symbols
model_eq = model_eq.subs(subslist)
# create object of model
model1 = SymbolicModel() # model_eq, qs, f, D)
model1.eqns = model_eq
model1.qs = qs
model1.extforce_list = f
model1.tau = f
model1.qds = qds
model1.qdds = qdds
# also store kinetic and potential energy
model1.T = T
model1.U = U
# analyse the model
return model1
def calc_lbi_nf_state_eq(self, simplify=False):
"""
calc vectorfields fz, and gz of the Lagrange-Byrnes-Isidori-Normalform
instead of the state xx
"""
n = len(self.tt)
nq = len(self.tau)
np = n - nq
nx = 2 * n
# make sure that the system has the desired structure
B = self.eqns.jacobian(self.tau)
cond1 = B[:np, :] == sp.zeros(np, nq)
cond2 = B[np:, :] == -sp.eye(nq)
if not cond1 and cond2:
msg = "The jacobian of the equations of motion do not have the expected structure: %s"
raise NotImplementedError(msg % str(B))
pp = self.tt[:np, :]
qq = self.tt[np:, :]
uu = self.ttd[:np, :]
vv = self.ttd[np:, :]
ww = st.symb_vector('w1:{0}'.format(np + 1))
assert len(vv) == nq
# state w.r.t normal form
self.zz = st.row_stack(qq, vv, pp, ww)
self.ww = ww
# set the actuated accelearations as new inputs
self.aa = self.ttdd[-nq:, :]
# input vectorfield
self.gz = sp.zeros(nx, nq)
self.gz[nq:2 * nq, :] = sp.eye(nq) # identity matrix for the active coordinates
# drift vectorfield (will be completed below)
self.fz = sp.zeros(nx, 1)
self.fz[:nq, :] = vv
self.calc_mass_matrix()
if simplify:
self.M.simplify()
M11 = self.M[:np, :np]
M12 = self.M[:np, np:]
d = M11.berkowitz_det()
adj = M11.adjugate()
if simplify:
d = d.simplify()
adj.simplify()
M11inv = adj / d
# defining equation for ww: ww := uu + M11inv*M12*vv
uu_expr = ww - M11inv * M12 * vv
# setting input tau and acceleration to 0 in the equations of motion
C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau))
N = st.time_deriv(M11inv * M12, self.tt)
ww_dot = -M11inv * C1K1.subs(lzip(uu, uu_expr)) + N.subs(lzip(uu, uu_expr)) * vv
self.fz[2 * nq:2 * nq + np, :] = uu_expr
self.fz[2 * nq + np:, :] = ww_dot
# how the new coordinates are defined:
self.ww_def = uu + M11inv * M12 * vv
if simplify:
self.fz.simplify()
self.gz.simplify()
self.ww_def.simplify()
Lf2h = st.lie_deriv(h, f_sys, xx, 2)
Lf3h = st.lie_deriv(h, f_sys, xx, 3)
print(Lf3h)
Lgh = st.lie_deriv(h, g_sys, xx)
LgLfh = st.lie_deriv(Lfh, g_sys, xx)
LgLf2h=st.lie_deriv(Lf2h, g_sys, xx)
print("LgLf2h",LgLf2h)
Trans=sp.Matrix([h,Lfh,Lf2h])
print(Trans)
zz = st.symb_vector("z1:4")
res = sp.solve(Trans-zz, xx)[0]
x_z_beziehung = st.lzip(xx, res)
print("res",res)
print("x_z_beziehung",x_z_beziehung)
z_dot_tmp = Trans.jacobian(xx)*(f_sys + g_sys*u)
z_dot = z_dot_tmp.subs(x_z_beziehung)
z_dot.simplify()
z_dot
print(z_dot)
a0,a1,a2,a3=alpha=sp.symbols('alpha0:4')
u_input=sp.Symbol('u_{input}')
w=sp.Symbol('w') #Führungsgröße
print(alpha,u_input)
def generate_model(T, U, qq, F, **kwargs):
raise DeprecationWarning('generate_symbolic_model should be used')
"""
T kinetic energy
U potential energy
q independend deflection variables
F external forces
D dissipation function
"""
n = len(qq)
# time variable
t = sp.symbols('t')
# symbolic Variables (to prevent Functions where we not want them)
qs = []
qds = []
qdds = []
if not kwargs:
# assumptions for the symbols (facilitating the postprocessing)
kwargs = {"real": True}
for qi in qq:
# ensure that the same Symbol for t is used
assert qi.is_Function and qi.args == (t, )
s = str(qi.func)
qs.append(sp.Symbol(s, **kwargs))
qds.append(sp.Symbol(s + "_d", **kwargs))
qdds.append(sp.Symbol(s + "_dd", **kwargs))
qs, qds, qdds = sp.Matrix(qs), sp.Matrix(qds), sp.Matrix(qdds)
#derivative of configuration variables
qd = qq.diff(t)
qdd = qd.diff(t)
#lagrange function
L = T - U
#substitute functions with symbols for partial derivatives
#highest derivative first
subslist = lzip(qdd, qdds) + lzip(qd, qds) + lzip(qq, qs)
L = L.subs(subslist)
# partial derivatives of L
Ldq = st.jac(L, qs)
Ldqd = st.jac(L, qds)
# generalised external force
f = sp.Matrix(F)
# substitute symbols with functions for time derivative
subslistrev = st.rev_tuple(subslist)
Ldq = Ldq.subs(subslistrev)
Ldqd = Ldqd.subs(subslistrev)
Ldqd = Ldqd.diff(t)
#lagrange equation 2nd kind
model_eq = qq * 0
for i in range(n):
model_eq[i] = Ldqd[i] - Ldq[i] - f[i]
# model equations with symbols
model_eq = model_eq.subs(subslist)
# create object of model
model1 = SymbolicModel() # model_eq, qs, f, D)
model1.eqns = model_eq
model1.qs = qs
model1.extforce_list = f
model1.tau = f
model1.qds = qds
model1.qdds = qdds
# also store kinetic and potential energy
model1.T = T
model1.U = U
# analyse the model
return model1
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 dot(self, additional_symbols=None):
"""
returns the time derivative of this n-form:
self = a*dx + 0*dy + 0*dxdot + 0*dydot
self.dot() == adot*dx + a*dxdot
currently only supported for 1- and 2-forms
additional_symbols is an optional list of time_dependent symbols
"""
if not self.degree <= 2:
raise NotImplementedError(
".dot only tested for degree <= 2, might work however")
base_length = self._calc_base_length()
if additional_symbols is None:
additional_symbols = []
additional_symbols = list(additional_symbols)
res = DifferentialForm(self.degree, self.basis) # create a zero form
# get nonzero coeffs and their indices
nz_tups = [(idx_tup, c) for idx_tup, c in zip(self.indices, self.coeff)
if c != 0]
if len(nz_tups) == 0:
# this form is already zero
# -> return a copy
return 0 * self
idx_tups, coeffs = lzip(*nz_tups)
# idx_tups = e.g. [(1, 4), ...] (2-Form) or [(0,), (2,), ....] (1-Form)
# nested list comprehension http://stackoverflow.com/a/952952/333403
flat_nonzero_idcs = [i for idx_tup in idx_tups for i in idx_tup]
# reduce duplicates
flat_nonzero_idcs = list(set(flat_nonzero_idcs))
flat_nonzero_idcs.sort()
# get corresponding coords
nz_coords = [self.basis[i] for i in flat_nonzero_idcs]
nz_coords_diff = [st.time_deriv(c, self.basis) for c in nz_coords]
# difference set
ds = set(nz_coords_diff).difference(self.basis)
if ds:
msg = "The time derivative of this form cannot be calculated. "\
"The following necessary coordinates are not part of self.basis: %s" % ds
raise ValueError(msg)
# get new indices:
basis_list = list(self.basis)
#diff_idcs = [basis_list.index(c) for c in nz_coords_diff]
# assume self = a*dx1^dx3
# result: self.dot = adot*dx1^dx3 + a*dxdot1^dx3 + a*dx1^dxdot3
# replace the original coeff with its time derivative (adot*dx1^dx3)
for idcs, c in zip(idx_tups, coeffs):
res[idcs] = st.time_deriv(c, basis_list + additional_symbols)
# now for every coordinate find the basis-index of its derivative, e.g.
# if basis is x1, x2, x3 the index of xdot2 is 4
# set the original coeff to the corresponding place (a*dxdot1^dx3 + a*dx1^dxdot3)
for idx_tup, c in zip(idx_tups, coeffs):
for j, idx in enumerate(idx_tup):
# convert to mutable data type (list instead of tuple)
idx_list = list(idx_tup)
idx_list[j] += base_length # convert x3 to xdot3
# add the coeff
res[idx_list] += c
return res
def unimod_inv(M, s=None, t=None, time_dep_symbs=[], simplify_nsm=True, max_deg=None):
""" Assumes that M(s) is an unimodular polynomial matrix and calculates its inverse
which is again unimodular
:param M: Matrix to be inverted
:param s: Derivative Symbol
:param time_dep_symbs: sequence of time dependent symbols
:param max_deg: maximum polynomial degree w.r.t. s of the ansatz
:return: Minv
"""
assert isinstance(M, sp.MatrixBase)
assert M.is_square
n = M.shape[0]
degree_m = nc_degree(M, s)
if max_deg is None:
# upper bound according to
# Levine 2011, On necessary and sufficient conditions for differential flatness, p. 73
max_deg = (n - 1)*degree_m
assert int(max_deg) == max_deg
assert max_deg >= 0
C = M*0
free_params = []
for i in range(max_deg+1):
prefix = 'c{0}_'.format(i)
c_part = st.symbMatrix(n, n, prefix, commutative=False)
C += nc_mul(c_part, s**i)
free_params.extend(list(c_part))
P = nc_mul(C, M) - sp.eye(n)
P2 = right_shift_all(P, s, t, time_dep_symbs).reshape(n*n, 1)
deg_P = nc_degree(P2, s)
part_eqns = []
for i in range(deg_P + 1):
# omit the highest order (because it behaves like in the commutative case)
res = P2.diff(s, i).subs(s, 0)#/sp.factorial(i)
part_eqns.append(res)
eqns = st.row_stack(*part_eqns) # equations for all degrees of s
# now non-commutativity is inferring
eqns2, st_c_nc = make_all_symbols_commutative(eqns)
free_params_c, st_c_nc_free_params = make_all_symbols_commutative(free_params)
# find out which of the equations are (in)homogeneous
eqns2_0 = eqns2.subs(st.zip0(free_params_c))
assert eqns2_0.atoms() in ({0, -1}, {-1}, set())
inhom_idcs = st.np.where(st.to_np(eqns2_0) != 0)[0]
hom_idcs = st.np.where(st.to_np(eqns2_0) == 0)[0]
eqns_hom = sp.Matrix(st.np.array(eqns2)[hom_idcs])
eqns_inh = sp.Matrix(st.np.array(eqns2)[inhom_idcs])
assert len(eqns_inh) == n
# find a solution for the homogeneous equations
# if this is not possible, M was not unimodular
Jh = eqns_hom.jacobian(free_params_c).expand()
nsm = st.nullspaceMatrix(Jh, simplify=simplify_nsm, sort_rows=True)
na = nsm.shape[1]
if na < n:
msg = 'Could not determine sufficiently large nullspace. '\
'Either M is not unimodular or the expressions are to complicated.'
# TODO: decide which of the two cases occurs, via substitution of
# random numbers and singular value decomposition
# (or application of st.generic_rank)
raise ValueError(msg)
# parameterize the inhomogenous equations with the solution of the homogeneous equations
# new free parameters:
aa = st.symb_vector('_a1:{0}'.format(na+1))
nsm_a = nsm*aa
eqns_inh2 = eqns_inh.subs(lzip(free_params_c, nsm_a))
# now solve the remaining equations
# solve the linear system
Jinh = eqns_inh2.jacobian(aa)
rhs_inh = -eqns_inh2.subs(st.zip0(aa))
assert rhs_inh == sp.ones(n, 1)
sol_vect = Jinh.solve(rhs_inh)
sol = lzip(aa, sol_vect)
# get the values for all free_params (now they are not free anymore)
free_params_sol_c = nsm_a.subs(sol)
# replace the commutative symbols with the original non_commutative symbols (of M)
free_params_sol = free_params_sol_c.subs(st_c_nc)
Minv = C.subs(lzip(free_params, free_params_sol))
return Minv
