def test_rnd_number_tuples2(self):
x1, x2, x3 = xx = st.symb_vector('x1:4')
yy = st.symb_vector('y1:4')
s = sum(xx)
res_a1 = st.rnd_number_subs_tuples(s, seed=1)
res_a2 = st.rnd_number_subs_tuples(s, seed=2)
self.assertNotEqual(res_a1, res_a2)
res_b1 = st.rnd_number_subs_tuples(s, seed=2)
self.assertEqual(res_b1, res_a2)
xxyy = xx + yy
rnst1 = st.rnd_number_subs_tuples(xxyy)
rnst2 = st.rnd_number_subs_tuples(xxyy, exclude=x1)
rnst3 = st.rnd_number_subs_tuples(xxyy, exclude=[x1, x2])
rnst4 = st.rnd_number_subs_tuples(xxyy, exclude=xx)
symbols1 = xxyy.subs(rnst1).atoms(sp.Symbol)
symbols2 = xxyy.subs(rnst2).atoms(sp.Symbol)
symbols3 = xxyy.subs(rnst3).atoms(sp.Symbol)
symbols4 = xxyy.subs(rnst4).atoms(sp.Symbol)
self.assertEqual(symbols1, set())
self.assertEqual(symbols2, set([x1]))
self.assertEqual(symbols3, set([x1, x2]))
self.assertEqual(symbols4, set([x1, x2, x3]))
# this was a bug:
rnst = st.rnd_number_subs_tuples(xxyy, prime=True, exclude=[x1, x2])
self.assertEqual(xxyy.subs(rnst).atoms(sp.Symbol), set([x1, x2]))
def test_pickle_full_dump_and_load3(self):
"""
Test for correct handling of assumptions
"""
xx = st.symb_vector("x1, x2, x3")
xdot1, xdot2, xdot3 = xxd = st.time_deriv(xx, xx)
y1, y2, y3 = yy = st.symb_vector("y1, y2, y3")
yyd = st.time_deriv(yy, yy)
yydd = st.time_deriv(yy, yy, order=2)
s_nc = sp.Symbol('s', commutative=False)
sk_nc = sp.Symbol('sk', commutative=False)
s_c = sp.Symbol('s')
pdata1 = st.Container()
pdata1.s1 = sk_nc # different names
pdata1.s2 = s_c
pdata1.xx = xx
pdata2 = st.Container()
pdata2.s1 = s_nc # same names
pdata2.s2 = s_c
pdata2.xx = xx
pfname = "tmp_dump_test.pcl"
# this should pass
st.pickle_full_dump(pdata1, pfname)
with self.assertRaises(ValueError) as cm:
st.pickle_full_dump(pdata2, pfname)
os.remove(pfname)
def test_conversion_all_funcs(self):
x1, x2, x3 = xx = st.symb_vector("x1:4")
u1, u2 = uu = st.symb_vector("u1:3")
xxuusum = sum(xx) + sum(uu)
arg = sp.tanh(xxuusum) # limit the argument to (-1, 1)*0.99
# see mpc.CassadiPrinter.__init__ for exlanation
sp_func_names = mpc.CassadiPrinter().cs_func_keys.keys()
blacklist = ["atan2", ]
flist = [getattr(sp, name) for name in sp_func_names if name not in blacklist]
# create the test_matrix
expr_list = []
for func in flist:
if func is sp.acosh:
# only defined for values > 1
expr_list.append(func(1/arg))
else:
expr_list.append(func(arg))
expr_sp = sp.Matrix(expr_list + [arg, xxuusum])
func_cs = mpc.create_casadi_func(expr_sp, xx, uu)
xxuu = list(xx) + list(uu)
func_np = st.expr_to_func(xxuu, expr_sp)
argvals = np.random.rand(len(xxuu))
argvals_cs = (argvals[:len(xx)], argvals[len(xx):])
res_np = func_np(*argvals)
res_cs = func_cs(*argvals_cs).full().squeeze()
self.assertTrue(np.allclose(res_np, res_cs))
def test_dynamic_time_deriv1(self):
x1, x2 = xx = st.symb_vector("x1, x2")
u1, u2 = uu = st.symb_vector("u1, u2")
uu_dot = st.time_deriv(uu, uu)
uu_ddot = st.time_deriv(uu, uu, order=2)
ff = sp.Matrix([x2 + sp.exp(3 * x1), x1**2])
GG = sp.Matrix([[x1 - x1**2 * x2, sin(x1 / x2)], [1, x1**2 + x2]])
FF = ff + GG * uu
h = x1 * cos(x2)
h_dot_v1 = st.dynamic_time_deriv(h, FF, xx, uu)
h_dot_v2 = st.lie_deriv(h, FF, xx)
self.assertEqual(h_dot_v1, h_dot_v2)
h_dddot_v1 = st.dynamic_time_deriv(h, FF, xx, uu, order=3)
h_ddot_v2 = st.dynamic_time_deriv(h_dot_v1, FF, xx, uu)
h_dddot_v2 = st.dynamic_time_deriv(h_ddot_v2, FF, xx, uu)
self.assertEqual(h_dddot_v1, h_dddot_v2)
self.assertTrue(uu[0] in h_dot_v1.atoms())
self.assertTrue(uu_dot[0] in h_ddot_v2.atoms())
self.assertTrue(uu_dot[0] in h_dddot_v1.atoms())
def test_simple_pendulum_with_actuated_mountpoint(self):
np = 1
nq = 2
n = np + nq
pp = st.symb_vector("p1:{0}".format(np + 1))
qq = st.symb_vector("q1:{0}".format(nq + 1))
p1, q1, q2 = ttheta = st.row_stack(pp, qq)
pdot1, qdot1, qdot2 = tthetad = st.time_deriv(ttheta, ttheta)
mud = st.time_deriv(ttheta, ttheta, order=2)
params = sp.symbols('l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g')
l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g = params
tau1, tau2 = ttau = st.symb_vector("tau1, tau2")
## Geometry
ex = sp.Matrix([1, 0])
ey = sp.Matrix([0, 1])
# Koordinaten der Schwerpunkte und Gelenke
S1 = ex * q1
S2 = ex * q1 + ey * q2
G3 = S2 # Gelenk
# Schwerpunkt des Pendels #zeigt nach oben
S3 = G3 + mt.Rz(p1) * ey * s3
# Zeitableitungen der Schwerpunktskoordinaten
Sd1, Sd2, Sd3 = st.col_split(
st.time_deriv(st.col_stack(S1, S2, S3), ttheta)) ##
# Energy
T_rot = (J3 * pdot1**2) / 2
T_trans = (m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1]
external_forces = [0, tau1, tau2]
assert not any(external_forces[:np])
mod = mt.generate_symbolic_model(T, V, ttheta, external_forces)
mod.calc_coll_part_lin_state_eq(simplify=True)
#pdot1, qdot1, qdot2 = mod.ttd
ff_ref = sp.Matrix([[pdot1], [qdot1], [qdot2],
[g * m3 * s3 * sin(p1) / (J3 + m3 * s3**2)], [0],
[0]])
gg_ref_part = sp.Matrix([
m3 * s3 * cos(p1) / (J3 + m3 * s3**2),
m3 * s3 * sin(p1) / (J3 + m3 * s3**2)
]).T
self.assertEqual(mod.ff, ff_ref)
self.assertEqual(mod.gg[-3, :], gg_ref_part)
def test_simple_pendulum_with_actuated_mountpoint(self):
np = 1
nq = 2
n = np + nq
pp = st.symb_vector("p1:{0}".format(np + 1))
qq = st.symb_vector("q1:{0}".format(nq + 1))
p1, q1, q2 = ttheta = st.row_stack(pp, qq)
pdot1, qdot1, qdot2 = st.time_deriv(ttheta, ttheta)
params = sp.symbols('l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g')
l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g = params
tau1, tau2 = st.symb_vector("tau1, tau2")
# Geometry
ex = sp.Matrix([1, 0])
ey = sp.Matrix([0, 1])
# Coordinates of centers of masses (com) and joints
S1 = ex * q1
S2 = ex * q1 + ey * q2
G3 = S2 # Joints
# com of pendulum (points upwards)
S3 = G3 + mt.Rz(p1) * ey * s3
# timederivatives
Sd1, Sd2, Sd3 = st.col_split(
st.time_deriv(st.col_stack(S1, S2, S3), ttheta)) ##
# Energy
T_rot = (J3 * pdot1**2) / 2
T_trans = (m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1]
external_forces = [0, tau1, tau2]
assert not any(external_forces[:np])
mod = mt.generate_symbolic_model(T, V, ttheta, external_forces)
mod.calc_coll_part_lin_state_eq(simplify=True)
# Note: pdot1, qdot1, qdot2 = mod.ttd
ff_ref = sp.Matrix([[pdot1], [qdot1], [qdot2],
[g * m3 * s3 * sin(p1) / (J3 + m3 * s3**2)], [0],
[0]])
gg_ref_part = sp.Matrix([
m3 * s3 * cos(p1) / (J3 + m3 * s3**2),
m3 * s3 * sin(p1) / (J3 + m3 * s3**2)
]).T
self.assertEqual(mod.ff, ff_ref)
self.assertEqual(mod.gg[-3, :], gg_ref_part)
def _test_make_global(self):
xx = st.symb_vector('x1:4')
yy = st.symb_vector('y1:4')
st.make_global(xx)
self.assertEqual(x1 + x2, xx[0] + xx[1])
# test if set is accepted
st.make_global(yy.atoms(sp.Symbol))
self.assertEqual(y1 + y2, yy[0] + yy[1])
with self.assertRaises(TypeError) as cm:
st.make_global(dict())
def test_simple_pendulum_with_actuated_mountpoint(self):
np = 1
nq = 2
n = np + nq
pp = st.symb_vector("p1:{0}".format(np+1))
qq = st.symb_vector("q1:{0}".format(nq+1))
p1, q1, q2 = ttheta = st.row_stack(pp, qq)
pdot1, qdot1, qdot2 = tthetad = st.time_deriv(ttheta, ttheta)
mud = st.time_deriv(ttheta, ttheta, order=2)
params = sp.symbols('l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g')
l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g = params
tau1, tau2 = ttau= st.symb_vector("tau1, tau2")
## Geometry
ex = sp.Matrix([1,0])
ey = sp.Matrix([0,1])
# Koordinaten der Schwerpunkte und Gelenke
S1 = ex*q1
S2 = ex*q1 + ey*q2
G3 = S2 # Gelenk
# Schwerpunkt des Pendels #zeigt nach oben
S3 = G3 + mt.Rz(p1)*ey*s3
# Zeitableitungen der Schwerpunktskoordinaten
Sd1, Sd2, Sd3 = st.col_split(st.time_deriv(st.col_stack(S1, S2, S3), ttheta)) ##
# Energy
T_rot = ( J3*pdot1**2 )/2
T_trans = ( m1*Sd1.T*Sd1 + m2*Sd2.T*Sd2 + m3*Sd3.T*Sd3 )/2
T = T_rot + T_trans[0]
V = m1*g*S1[1] + m2*g*S2[1] + m3*g*S3[1]
external_forces = [0, tau1, tau2]
assert not any(external_forces[:np])
mod = mt.generate_symbolic_model(T, V, ttheta, external_forces)
mod.calc_coll_part_lin_state_eq(simplify=True)
#pdot1, qdot1, qdot2 = mod.ttd
ff_ref = sp.Matrix([[pdot1], [qdot1], [qdot2], [g*m3*s3*sin(p1)/(J3 + m3*s3**2)], [0], [0]])
gg_ref_part = sp.Matrix([m3*s3*cos(p1)/(J3 + m3*s3**2), m3*s3*sin(p1)/(J3 + m3*s3**2)]).T
self.assertEqual(mod.ff, ff_ref)
self.assertEqual(mod.gg[-3, :], gg_ref_part)
def __init__(self, inverted=True, calc_coll_part_lin=True):
self.inverted = inverted
self.calc_coll_part_lin = calc_coll_part_lin
# -----------------------------------------
# Pendel-Wagen System mit hängendem Pendel
# -----------------------------------------
pp = st.symb_vector(("varphi", ))
qq = st.symb_vector(("q", ))
ttheta = st.row_stack(pp, qq)
st.make_global(ttheta)
params = sp.symbols('m1, m2, l, g, q_r, t, T')
st.make_global(params)
ex = sp.Matrix([1, 0])
ey = sp.Matrix([0, 1])
# Koordinaten der Schwerpunkte und Gelenke
S1 = ex * q # Schwerpunkt Wagen
G2 = S1 # Pendel-Gelenk
# Schwerpunkt des Pendels (Pendel zeigt für kleine Winkel nach oben)
if inverted:
S2 = G2 + mt.Rz(varphi) * ey * l
else:
S2 = G2 + mt.Rz(varphi) * -ey * l
# Zeitableitungen der Schwerpunktskoordinaten
S1d, S2d = st.col_split(st.time_deriv(st.col_stack(S1, S2), ttheta))
# Energie
E_rot = 0 # (Punktmassenmodell)
E_trans = (m1 * S1d.T * S1d + m2 * S2d.T * S2d) / 2
E = E_rot + E_trans[0]
V = m2 * g * S2[1]
# Partiell linearisiertes Model
mod = mt.generate_symbolic_model(E, V, ttheta, [0, sp.Symbol("u")])
mod.calc_state_eq()
mod.calc_coll_part_lin_state_eq()
self.mod = mod
def test_casadify(self):
x1, x2, x3 = xx = st.symb_vector("x1:4")
u1, u2 = uu = st.symb_vector("u1:3")
lmd1, lmd2 = llmd = st.symb_vector("lmd1:3")
xxuullmd_sp = list(xx) + list(uu) + list(llmd)
expr1_sp = sp.Matrix([x1 + x2 + x3, sp.sin(x1)*x2**x3, 1.23, 0, u1*sp.exp(u2), x1*lmd1 + lmd2 ** 4])
expr2_sp = sp.Matrix([x1**2 - x3**2, sp.cos(x1)*x2**x1, -0.123, 0, u2*sp.exp(-u2), x2*lmd1 + lmd2 ** -4])
expr1_cs, cs_symbols1 = mpc.casidify(expr1_sp, xxuullmd_sp)
expr2_cs, cs_symbols2 = mpc.casidify(expr2_sp, xxuullmd_sp, cs_vars=cs_symbols1)
self.assertTrue(mpc.cs.is_equal(cs_symbols1, cs_symbols2))
def test_commutative_simplification(self):
x1, x2 = xx = st.symb_vector('x1, x2', commutative=False)
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s, z, t = sz = st.symb_vector('s, z, t', commutative=False)
a, b = ab = st.symb_vector('a, b', commutative=True)
F = sp.Function('F')(t)
e1 = x1*y1 - y1*x1
e2 = e1*s + x2
e3 = e1*s + x2*s
M1 = sp.Matrix([[e1, 1], [e2, e3]])
r1 = nct.commutative_simplification(e1, s)
self.assertEqual(r1, 0)
r2 = nct.commutative_simplification(e2, s)
self.assertEqual(r2, x2)
r3 = nct.commutative_simplification(e3, s)
self.assertEqual(r3, x2*s)
r4 = nct.commutative_simplification(M1, s)
r4_expected = sp.Matrix([[0, 1], [x2, x2*s]])
self.assertEqual(r4, r4_expected)
f1 = x1*s*x2*s
f2 = s**2*x1*x2
f3 = a*x1*s**2
f4 = F*s
with self.assertRaises(ValueError) as cm:
nct.commutative_simplification(f1, s)
with self.assertRaises(ValueError) as cm:
nct.commutative_simplification(f2, s)
with self.assertRaises(ValueError) as cm:
nct.commutative_simplification(e1, [s, z])
with self.assertRaises(ValueError) as cm:
nct.commutative_simplification(f3, s)
with self.assertRaises(NotImplementedError) as cm:
nct.commutative_simplification(f4, s)
def test_left_mul_by_2(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
C = sp.Symbol('C', commutative=False)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
# matrix independent of s
M2 = sp.Matrix([[1, 0], [-C, 1]])
# 1-forms
w1 = pc.DifferentialForm(1, XX, coeff=Q_[0, :])
w2 = pc.DifferentialForm(1, XX, coeff=Q_[1, :])
# vector 1-form
w = pc.VectorDifferentialForm(1, XX, coeff=Q_)
t = w.left_mul_by(M2, additional_symbols=[C])
# object to compare with:
t2 = -C * w1 + w2
self.assertEqual(t2.coeff, t.coeff.row(1).T)
def test_left_mul_by_1(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
s = sp.Symbol('s', commutative=False)
C = sp.Symbol('C', commutative=False)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
# s-dependent matrix
M1 = sp.Matrix([[1, 0], [-C * s, 1]])
# 1-forms
w1 = pc.DifferentialForm(1, XX, coeff=Q_[0, :])
w2 = pc.DifferentialForm(1, XX, coeff=Q_[1, :])
# vector 1-form
w = pc.VectorDifferentialForm(1, XX, coeff=Q_)
t = w.left_mul_by(M1, s, [C])
t2 = -C * w1.dot() + w2
self.assertEqual(t2.coeff, t.coeff.row(1).T)
def test_mul(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
s = sp.Symbol('s', commutative=False)
C = sp.Symbol('C', commutative=False)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
W = pc.VectorDifferentialForm(1, xx, coeff=Q)
W1 = s * W
W2 = W * C
self.assertEqual(W1.coeff, nct.nc_mul(s, W.coeff))
self.assertNotEqual(W1.coeff, nct.nc_mul(W.coeff, s))
self.assertEqual(W2.coeff, nct.nc_mul(W.coeff, C))
self.assertNotEqual(W2.coeff, nct.nc_mul(C, W.coeff))
alpha = pc.DifferentialForm(1, xx)
with self.assertRaises(TypeError) as cm:
alpha * W1
with self.assertRaises(TypeError) as cm:
W1 * alpha
M = sp.eye(2)
with self.assertRaises(sp.SympifyError) as cm:
M * W1
with self.assertRaises(TypeError) as cm:
W1 * M
def test_vector_form_append_2(self):
x1, x2, x3 = xx = st.symb_vector('x1:4', commutative=False)
xdot1, xdot2, xdot3 = xxdot = st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxddot = st.time_deriv(xxdot, xxdot)
XX = st.row_stack(xx, xxdot, xxddot)
s = sp.Symbol('s', commutative=False)
C = sp.Symbol('C', commutative=False)
# vector 1-form
Q1 = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]])
Q1_ = st.col_stack(Q1, sp.zeros(2, 6))
w1 = pc.VectorDifferentialForm(1, XX, coeff=Q1_)
# 1-forms
Q2 = sp.Matrix([[x1, x2, x3], [x3, x1, x2]])
Q2_ = st.col_stack(Q2, sp.zeros(2, 6))
w2 = pc.VectorDifferentialForm(1, XX, coeff=Q2_)
w1.append(w2)
# vector form to compare with:
B = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3], [x1, x2, x3],
[x3, x1, x2]])
B_ = st.col_stack(B, sp.zeros(4, 6))
self.assertEqual(w1.coeff, B_)
def test_get_baseform_from_plain_index(self):
x1, x2, x3 = xx = st.symb_vector("x1, x2, x3")
xx, dxx = pc.setup_objects(xx)
dx1, dx2, dx3 = dxx
W = 7 * (dx1 ^ dx2) + 3 * x2 * (dx1 ^ dx3)
res = W.get_baseform_from_plain_index(0)
self.assertEqual(res, dx1 ^ dx2)
res = W.get_baseform_from_plain_index(2)
self.assertEqual(res, dx2 ^ dx3)
res = W.get_baseform_from_plain_index(-1)
self.assertEqual(res, dx2 ^ dx3)
res = W.get_baseform_from_plain_index(-2)
self.assertEqual(res, dx1 ^ dx3)
res = W.get_baseform_from_plain_index(-3)
self.assertEqual(res, dx1 ^ dx2)
with self.assertRaises(ValueError) as cm:
res = W.get_baseform_from_plain_index(3)
with self.assertRaises(ValueError) as cm:
res = W.get_baseform_from_plain_index(-4)
def test_get_baseform(self):
x1, x2, x3 = xx = st.symb_vector("x1, x2, x3")
xx, dxx = pc.setup_objects(xx)
dx1, dx2, dx3 = dxx
W = 7 * (dx1 ^ dx2) + 3 * x2 * (dx1 ^ dx3)
res1 = W.get_baseform_from_idcs((0, 1))
self.assertEqual(res1, dx1 ^ dx2)
idcs_matrix = sp.Matrix([0, 2])
res2 = W.get_baseform_from_idcs(idcs_matrix)
self.assertEqual(res2, dx1 ^ dx3)
idcs_array = st.np.array([0, 2])
res2b = W.get_baseform_from_idcs(idcs_array)
self.assertEqual(res2b, dx1 ^ dx3)
res3 = W.get_baseform_from_idcs((1, 2))
self.assertEqual(res3, dx2 ^ dx3)
Z = dx1 + x3**2 * dx2
res4 = Z.get_baseform_from_idcs((1, ))
self.assertEqual(res4, dx2)
res5 = Z.get_baseform_from_idcs(2)
self.assertEqual(res5, dx3)
with self.assertRaises(TypeError) as cm:
res = W.get_baseform_from_idcs(dx1)
with self.assertRaises(ValueError) as cm:
res = W.get_baseform_from_idcs((0, 0))
def test_ord(self):
x1, x2, x3 = xx = st.symb_vector("x1, x2, x3")
xdot1, xdot2, xdot3 = xxd = pc.st.time_deriv(xx, xx)
xxdd = pc.st.time_deriv(xx, xx, order=2)
XX = st.concat_rows(xx, xxd, xxdd)
XX, dXX = pc.setup_objects(XX)
dx1, dx2, dx3, dxdot1, dxdot2, dxdot3, dxddot1, dxddot2, dxddot3 = dXX
w0 = 0 * dx1
w1 = dx1 + dxdot3
w2 = 4 * x2 * dx1 - sp.sin(x3) * xdot1 * dx2
self.assertEqual(w0.ord, 0)
self.assertEqual(dx1.ord, 0)
self.assertEqual(dxdot1.ord, 1)
self.assertEqual(dxddot3.ord, 2)
self.assertEqual(w1.ord, 1)
self.assertEqual(w2.ord, 0)
self.assertEqual(w2.d.ord, 1)
w3 = w1 ^ w2
self.assertEqual(w3.ord, 1)
self.assertEqual(w3.dot().ord, 2)
def test_jet_extend_basis1(self):
x1, x2, x3 = xx = st.symb_vector("x1, x2, x3")
xx_tmp, ddx = pc.setup_objects(xx)
self.assertTrue(xx is xx_tmp)
# get the individual forms
dx1, dx2, dx3 = ddx
dx1.jet_extend_basis()
xdot1, xdot2, xdot3 = xxd = pc.st.time_deriv(xx, xx)
xddot1, xddot2, xddot3 = xxdd = pc.st.time_deriv(xx, xx, order=2)
full_basis = st.row_stack(xx, xxd, xxdd)
foo, ddX = pc.setup_objects(full_basis)
dx1.jet_extend_basis()
self.assertEqual(ddX[0].basis, dx1.basis)
self.assertEqual(ddX[0].coeff, dx1.coeff)
half_basis = st.row_stack(xx, xxd)
foo, ddY = pc.setup_objects(half_basis)
dx2.jet_extend_basis()
self.assertEqual(ddY[1].basis, dx2.basis)
self.assertEqual(ddY[1].coeff, dx2.coeff)
def test_unimod_inv3(self):
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s = sp.Symbol('s', commutative=False)
ydot1, ydot2 = yyd1 = st.time_deriv(yy, yy, order=1, commutative=False)
yddot1, yddot2 = yyd2 = st.time_deriv(yy,
yy,
order=2,
commutative=False)
yyd3 = st.time_deriv(yy, yy, order=3, commutative=False)
yyd4 = st.time_deriv(yy, yy, order=4, commutative=False)
yya = st.row_stack(yy, yyd1, yyd2, yyd3, yyd4)
M3 = sp.Matrix([[ydot2, y1 * s],
[
y2 * yddot2 + y2 * ydot2 * s, y1 * yddot2 +
y2 * y1 * s**2 + y2 * ydot1 * s + ydot2 * ydot1
]])
M3inv = nct.unimod_inv(M3, s, time_dep_symbs=yya)
product3a = nct.right_shift_all(nct.nc_mul(M3, M3inv),
s,
func_symbols=yya)
product3b = nct.right_shift_all(nct.nc_mul(M3inv, M3),
s,
func_symbols=yya)
res3a = nct.make_all_symbols_commutative(product3a)[0]
res3b = nct.make_all_symbols_commutative(product3b)[0]
res3a.simplify()
res3b.simplify()
self.assertEqual(res3a, sp.eye(2))
self.assertEqual(res3b, sp.eye(2))
def test_unimod_inv(self):
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s = sp.Symbol('s', commutative=False)
ydot1, ydot2 = yyd1 = st.time_deriv(yy, yy, order=1, commutative=False)
yddot1, yddot2 = yyd2 = st.time_deriv(yy,
yy,
order=2,
commutative=False)
yyd3 = st.time_deriv(yy, yy, order=3, commutative=False)
yyd4 = st.time_deriv(yy, yy, order=4, commutative=False)
yya = st.row_stack(yy, yyd1, yyd2, yyd3, yyd4)
M1 = sp.Matrix([yy[0]])
M1inv = nct.unimod_inv(M1, s, time_dep_symbs=yy)
self.assertEqual(M1inv, M1.inv())
M2 = sp.Matrix([[y1, y1 * s], [0, y2]])
M2inv = nct.unimod_inv(M2, s, time_dep_symbs=yy)
product2a = nct.right_shift_all(nct.nc_mul(M2, M2inv),
s,
func_symbols=yya)
product2b = nct.right_shift_all(nct.nc_mul(M2inv, M2),
s,
func_symbols=yya)
res2a = nct.make_all_symbols_commutative(product2a)[0]
res2b = nct.make_all_symbols_commutative(product2b)[0]
self.assertEqual(res2a, sp.eye(2))
self.assertEqual(res2b, sp.eye(2))
def test_system_prolongation1(self):
x1, x2, x3 = xx = st.symb_vector('x1:4')
z1, z2, z3 = zz = st.symb_vector('z1:4')
a1, a2, a3 = aa = st.symb_vector('a1:4')
f = sp.Matrix([x2, 0, 0])
gg = sp.Matrix([[0, 0], [a1, a2], [0, a3]])
fnew, ggnew, xxnew = st.system_pronlongation(f, gg, xx, [(0, 2),
(1, 1)])
fnew_ref = sp.Matrix([x2, a1 * z1 + a2 * z3, a3 * z3, z2, 0, 0])
ggnew_ref = sp.eye(6)[:, -2:]
self.assertEqual(fnew, fnew_ref)
self.assertEqual(ggnew, ggnew_ref)
def test_lie_deriv(self):
xx = st.symb_vector('x1:4')
st.make_global(xx)
f = sp.Matrix([x1 + x3 * x2, 7 * exp(x1), cos(x2)])
h1 = x1**2 + sin(x3) * x2
res1 = st.lie_deriv(h1, f, xx)
eres1 = 2 * x1**2 + 2 * x1 * x2 * x3 + 7 * exp(x1) * sin(
x3) + x2 * cos(x2) * cos(x3)
self.assertEqual(res1.expand(), eres1)
res2a = st.lie_deriv(h1, f, xx, order=2).expand()
res2b = st.lie_deriv(h1, f, xx, 2).expand()
eres2 = st.lie_deriv(eres1, f, xx).expand()
self.assertEqual(res2a, eres2)
self.assertEqual(res2b, eres2)
res2c = st.lie_deriv(h1, f, f, xx).expand()
res2d = st.lie_deriv(h1, f, f, xx=xx).expand()
self.assertEqual(res2c, eres2)
self.assertEqual(res2d, eres2)
F = f[:-1, :]
with self.assertRaises(ValueError) as cm:
# different lengths of vectorfields:
res1 = st.lie_deriv(h1, F, f, xx)
def test_do_laplace_deriv(self):
t, s = sp.symbols('t, s')
x1, x2, x3 = xx = st.symb_vector('x1:4')
x1dot, x2dot, x3dot = st.time_deriv(xx, xx)
x1ddot, x2ddot, x3ddot = st.time_deriv(xx, xx, order=2)
expr1 = 5
expr2 = 5 * s * t**2 - 7 * t + 2
expr3 = 1 * s**2 * x1 - 7 * s * x2 * t + 2
res = st.do_laplace_deriv(expr1, s, t)
ex_res = 5
self.assertEqual(res, ex_res)
res = st.do_laplace_deriv(expr2, s, t)
ex_res = 10 * t - 7 * t + 2
self.assertEqual(res, ex_res)
res = st.do_laplace_deriv(expr3, s, t)
ex_res = -7 * x2 + 2
self.assertEqual(res, ex_res)
res = st.do_laplace_deriv(expr3, s, t, tds=xx)
ex_res = x1ddot - 7 * x2 + -7 * x2dot * t + 2
self.assertEqual(res, ex_res)
def test_convert_functions_to_symbols(self):
x1, x2, x3 = xx = st.symb_vector("x1, x2, x3")
f1a = sp.Function("f1", commutative=True)(st.t)
f1b = sp.Function("f1", commutative=False)(x1, x2)
f2a = sp.Function("f2")(x1)
f2b = sp.Function("f2")(x1 + x2)
funcs = {f1a, f1b, f2a, f2b}
rplmts, function_data = pt.convert_functions_to_symbols(funcs)
self.assertEqual(set(list(zip(*rplmts))[0]), funcs)
symb_keys = dict(rplmts)
fd1a = function_data[symb_keys[f1a]]
fd1b = function_data[symb_keys[f1b]]
self.assertEqual(fd1a.args, (st.t, ))
self.assertEqual(fd1b.args, (
x1,
x2,
))
self.assertEqual(fd1a.assumptions["commutative"], True)
self.assertEqual(fd1b.assumptions["commutative"], False)
def test_get_custom_attr_map(self):
t = st.t
x1, x2 = xx = st.symb_vector("x1, x2")
xdot1, xdot2 = xxd = st.time_deriv(xx, xx)
xddot1, xddot2 = xxdd = st.time_deriv(xx, xx, order=2)
m1 = st.get_custom_attr_map("ddt_child")
em1 = [(x1, xdot1), (x2, xdot2), (xdot1, xddot1), (xdot2, xddot2)]
# convert to set because sorting might depend on plattform
self.assertEqual(set(m1), set(em1))
m2 = st.get_custom_attr_map("ddt_parent")
em2 = [(xdot1, x1), (xdot2, x2), (xddot1, xdot1), (xddot2, xdot2)]
self.assertEqual(set(m2), set(em2))
m3 = st.get_custom_attr_map("ddt_func")
# ensure unique sorting
m3.sort(key=lambda x: "{}_{}".format(x[0].difforder, str(x[0])))
self.assertEqual(len(m3), 6)
x2_func = sp.Function(x2.name)(t)
self.assertEqual(type(type(m3[0][1])), sp.function.UndefinedFunction)
self.assertEqual(m3[-1][1], x2_func.diff(t, t))
def test_equilibrium_one(self):
k1, k2, k3, m1, m2 = sp.symbols('k1, k2, k3, m1, m2')
F = sp.Symbol('F')
params_values = [(k1, 10), (k2, 40), (k3, 8), (m1, 0.5), (m2, 1)]
Np = 2 # number of active coordinates
pp = st.symb_vector("p1:{0}".format(Np + 1)) # active coordinates
ttheta = pp # system states
tthetad = st.time_deriv(ttheta, ttheta)
# kinetic energy of the bodies
T = 1 / 2 * (m1 * tthetad[0]**2 + m2 * tthetad[1]**2
) # total kinetic energy
# total potential energy
V = 1 / 2 * (k1 * ttheta[0]**2 + k2 * ttheta[1]**2 + k3 *
(ttheta[1] - ttheta[0])**2)
external_forces = [0, F]
# symbolic model of the system
mod = mt.generate_symbolic_model(T, V, ttheta, external_forces)
# calculate equilibrium point for external force = 2N
mod.calc_eqlbr(params_values, etype='one_ep', uu=[2])
eqrt_point = list(zip(mod.tt,
mod.eqlbr)) # equilibrium points of states
all_vars = params_values + eqrt_point + list(zip(mod.tau, [2]))
state_eqns = mod.eqns.subz0(mod.ttd, mod.ttdd).subs(all_vars)
for i in range(len(ttheta)):
self.assertAlmostEqual(0, state_eqns[i], delta=6)
def test_lie_deriv_covf(self):
xx = st.symb_vector('x1:4')
st.make_global(xx)
# we test this by building the observability matrix with two different but equivalent approaches
f = sp.Matrix([x1 + x3 * x2, 7 * exp(x1), cos(x2)])
y = x1**2 + sin(x3) * x2
ydot = st.lie_deriv(y, f, xx)
yddot = st.lie_deriv(ydot, f, xx)
cvf1 = st.gradient(y, xx)
cvf2 = st.gradient(ydot, xx)
cvf3 = st.gradient(yddot, xx)
# these are the rows of the observability matrix
# second approach
dh0 = cvf1
dh1 = st.lie_deriv_covf(dh0, f, xx)
dh2a = st.lie_deriv_covf(dh1, f, xx)
dh2b = st.lie_deriv_covf(dh0, f, xx, order=2)
zero = dh0 * 0
self.assertEqual((dh1 - cvf2).expand(), zero)
self.assertEqual((dh2a - cvf3).expand(), zero)
self.assertEqual((dh2b - cvf3).expand(), zero)
def test_conversion1(self):
x1, x2, x3 = xx = st.symb_vector("x1:4")
u1, u2 = uu = st.symb_vector("u1:3")
expr_sp = sp.Matrix([x1 + x2 + x3, sp.sin(x1)*x2**x3, 1.23, 0, u1*sp.exp(u2)])
func_cs = mpc.create_casadi_func(expr_sp, xx, uu)
xxuu = list(xx) + list(uu)
func_np = st.expr_to_func(xxuu, expr_sp)
argvals = np.random.rand(len(xxuu))
argvals_cs = (argvals[:len(xx)], argvals[len(xx):])
res_np = func_np(*argvals)
res_cs = func_cs(*argvals_cs).full().squeeze()
self.assertTrue(np.allclose(res_np, res_cs))
def test_involutivity_test(self):
x1, x2, x3 = xx = st.symb_vector('x1:4')
st.make_global(xx)
# not involutive
f1 = sp.Matrix([x2 * x3 + x1**2, 3 * x1, 4 + x2 * x3])
f2 = sp.Matrix([x3 - 2 * x1 * x3, x2 - 5, 3 + x1 * x2])
dist1 = st.col_stack(f1, f2)
# involutive
f3 = sp.Matrix([-x2, x1, 0])
f4 = sp.Matrix([0, -x3, x2])
dist2 = st.col_stack(f3, f4)
res, fail = st.involutivity_test(dist1, xx)
self.assertFalse(res)
self.assertEqual(fail, (0, 1))
res2, fail2 = st.involutivity_test(dist2, xx)
self.assertTrue(res2)
self.assertEqual(fail2, [])
def test_get_all_deriv_childs_and_parents(self):
x1, x2 = xx = st.symb_vector("x1, x2")
xdot1, xot2 = xxd = st.time_deriv(xx, xx)
xddot1, xdot2 = xxdd = st.time_deriv(xx, xx, order=2)
expr = x1 * x2
E2 = st.time_deriv(expr, xx, order=2)
dc = st.get_all_deriv_childs(xx)
self.assertEqual(len(dc), 4)
xdot1, xdot2 = st.time_deriv(xx, xx)
xddot1, xddot2 = st.time_deriv(xx, xx, order=2)
self.assertTrue(xdot1 in dc)
self.assertTrue(xddot1 in dc)
self.assertTrue(xdot2 in dc)
self.assertTrue(xddot2 in dc)
dp1 = st.get_all_deriv_parents(xdot1)
dp2 = st.get_all_deriv_parents(xddot2)
self.assertEqual(dp1, sp.Matrix([x1]))
self.assertEqual(dp2, sp.Matrix([xdot2, x2]))
def test_make_all_symbols_commutative3(self):
x1, x2, x3 = xx = st.symb_vector('x1, x2, x3', commutative=False)
xxd = st.time_deriv(xx, xx)
xxd_c = nct.make_all_symbols_commutative(xxd)[0]
self.assertEqual(xxd_c[0].difforder, 1)
def intern_test(np, nq):
N = np + nq
xx = st.symb_vector('x1:{0}'.format(N*2+1))
P0 = st.symbMatrix(np, N, s='A')
P1 = st.symbMatrix(np, N, s='B')
P2 = st.symbMatrix(np, N, s='C')
P0bar, P1bar = mt.transform_2nd_to_1st_order_matrices(P0, P1, P2, xx)
self.assertEqual(P0bar[:N, N:], sp.eye(N))
self.assertEqual(P1bar[:N, :N], -sp.eye(N))
self.assertEqual(P0bar[N:, :N], P0)
self.assertEqual(P1bar[N:, :], P1.row_join(P2))
def test_unimod_inv2(self):
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s = sp.Symbol('s', commutative=False)
ydot1, ydot2 = yyd1 = st.time_deriv(yy, yy, order=1, commutative=False)
yddot1, yddot2 = yyd2 = st.time_deriv(yy, yy, order=2, commutative=False)
yyd3 = st.time_deriv(yy, yy, order=3, commutative=False)
yyd4 = st.time_deriv(yy, yy, order=4, commutative=False)
yya = st.row_stack(yy, yyd1, yyd2, yyd3, yyd4)
# this Matrix is not unimodular due to factor 13 (should be 1)
M3 = sp.Matrix([[ydot2, 13*y1*s],
[y2*yddot2 + y2*ydot2*s, y1*yddot2 + y2*y1*s**2 + y2*ydot1*s + ydot2*ydot1]])
with self.assertRaises(ValueError) as cm:
res = nct.unimod_inv(M3, s, time_dep_symbs=yya)
def test_unicycle(self):
# test the generation of Lagrange-Byrnes-Isidori-Normal form
theta = st.symb_vector("p1, q1, q2")
p1, q1, q2 = theta
theta
params = sp.symbols('l1, l2, s1, s2, delta0, delta1, delta2, J0, J1, J2, m0, m1, m2, r, g')
l1, l2, s1, s2, delta0, delta1, delta2, J0, J1, J2, m0, m1, m2, r, g = params
QQ = sp.symbols("Q0, Q1, Q2")
Q0, Q1, Q2 = QQ
mu = st.time_deriv(theta, theta)
p1d, q1d, q2d = mu
# Geometry
ex = Matrix([1,0])
ey = Matrix([0,1])
M0 = Matrix([-r*p1, r])
S1 = M0 + mt.Rz(p1+q1)*ey*s1
S2 = M0 + mt.Rz(p1+q1)*ey*l1+mt.Rz(p1+q1+q2)*ey*s2
M0d = st.time_deriv(M0, theta)
S1d = st.time_deriv(S1, theta)
S2d = st.time_deriv(S2, theta)
# Energy
T_rot = ( J0*p1d**2 + J1*(p1d+q1d)**2 + J2*(p1d+ q1d+q2d)**2 )/2
T_trans = ( m0*M0d.T*M0d + m1*S1d.T*S1d + m2*S2d.T*S2d )/2
T = T_rot + T_trans[0]
V = m1*g*S1[1] + m2*g*S2[1]
mod = mt.generate_symbolic_model(T, V, theta, [0, Q1, Q2])
mod.calc_lbi_nf_state_eq()
def test_unimod_inv3(self):
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s = sp.Symbol('s', commutative=False)
ydot1, ydot2 = yyd1 = st.time_deriv(yy, yy, order=1, commutative=False)
yddot1, yddot2 = yyd2 = st.time_deriv(yy, yy, order=2, commutative=False)
yyd3 = st.time_deriv(yy, yy, order=3, commutative=False)
yyd4 = st.time_deriv(yy, yy, order=4, commutative=False)
yya = st.row_stack(yy, yyd1, yyd2, yyd3, yyd4)
M3 = sp.Matrix([[ydot2, y1*s],
[y2*yddot2 + y2*ydot2*s, y1*yddot2 + y2*y1*s**2 + y2*ydot1*s + ydot2*ydot1]])
M3inv = nct.unimod_inv(M3, s, time_dep_symbs=yya)
product3a = nct.right_shift_all( nct.nc_mul(M3, M3inv), s, func_symbols=yya)
product3b = nct.right_shift_all( nct.nc_mul(M3inv, M3), s, func_symbols=yya)
res3a = nct.make_all_symbols_commutative(product3a)[0]
res3b = nct.make_all_symbols_commutative(product3b)[0]
res3a.simplify()
res3b.simplify()
self.assertEqual(res3a, sp.eye(2))
self.assertEqual(res3b, sp.eye(2))
def test_unimod_inv(self):
y1, y2 = yy = st.symb_vector('y1, y2', commutative=False)
s = sp.Symbol('s', commutative=False)
ydot1, ydot2 = yyd1 = st.time_deriv(yy, yy, order=1, commutative=False)
yddot1, yddot2 = yyd2 = st.time_deriv(yy, yy, order=2, commutative=False)
yyd3 = st.time_deriv(yy, yy, order=3, commutative=False)
yyd4 = st.time_deriv(yy, yy, order=4, commutative=False)
yya = st.row_stack(yy, yyd1, yyd2, yyd3, yyd4)
M1 = sp.Matrix([yy[0]])
M1inv = nct.unimod_inv(M1, s, time_dep_symbs=yy)
self.assertEqual(M1inv, M1.inv())
M2 = sp.Matrix([[y1, y1*s], [0, y2]])
M2inv = nct.unimod_inv(M2, s, time_dep_symbs=yy)
product2a = nct.right_shift_all( nct.nc_mul(M2, M2inv), s, func_symbols=yya)
product2b = nct.right_shift_all( nct.nc_mul(M2inv, M2), s, func_symbols=yya)
res2a = nct.make_all_symbols_commutative( product2a)[0]
res2b = nct.make_all_symbols_commutative( product2b)[0]
self.assertEqual(res2a, sp.eye(2))
self.assertEqual(res2b, sp.eye(2))
# -*- coding: utf-8 -*-
# enable true divison
from __future__ import division
import sympy as sp
import symbtools as st
C,L,Ld,Rd,Ll,Rl,Ul,U = sp.symbols("C,L,Ld,Rd,Ll,Rl,Ul,U", commutative=True)
vec_x = st.symb_vector('x1:13', commutative=True)
vec_xdot = st.time_deriv(vec_x, vec_x)
st.make_global(vec_x, 1)
st.make_global(vec_xdot, 1)
F_eq = sp.Matrix([
#~ [L*xdot5 + x9*x1 + x10*x2 + L*xdot6 - U + Ld*xdot5 + Ld*xdot7 - Rd*x5 - Rd*x7],
[ xdot8 - (1-Rd/Rl)*(x5+x7) ],
[L*xdot7 + x11*x3 + x12*x4 + L*xdot8 - U + Ld*xdot5 + Ld*xdot7 + Rd*x5 + Rd*x7],
[L*xdot5 - L*xdot7 - x11*x3 + x9*x1 + Ll*xdot5 - Ll*xdot6 + Rl*x5 + Rl*x6 + Ul],
[L*xdot6 - L*xdot8 + x10*x2 - x12*x4 - Ll*xdot5 + Ll*xdot6 - Rl*x5 + Rl*x6 - Ul],
[C*xdot1 - x5*x9],
[C*xdot2 - x6*x10],
[C*xdot3 - x7*x11],
[C*xdot4 - x8*x12],
[ xdot6 - (Rd/Rl)*(xdot5 + xdot7) ]
#~ [x5 + x7 - x6 - x8]
])
# algebraische Substitutionsgleichungen mit Ableitungen:
x6subs = -Rd*(x5 + x7)/Rl
# -*- coding: utf-8 -*-
# enable true divison
from __future__ import division
import sympy as sp
import symbtools as st
vec_x = st.symb_vector('x1:6')
vec_xdot = st.time_deriv(vec_x, vec_x)
st.make_global(vec_x, 1)
st.make_global(vec_xdot, 1)
F_eq = sp.Matrix([
[-x3*x4 + xdot1],
[-x4 + xdot2],
[-x5 + xdot3]])
# -*- coding: utf-8 -*-
import sympy as sp
import symbtools as st
from sympy import sin, cos, tan
# Number of state variables
n = 6
vec_x = st.symb_vector('x1:%i' % (n+1))
vec_xdot = st.time_deriv(vec_x, vec_x)
st.make_global(vec_x, 1)
st.make_global(vec_xdot, 1)
# Additional symbols
g, l = sp.symbols("g, l")
# Time-dependent symbols
diff_symbols = sp.Matrix([l])
# Nonlinear system in state space representation 0 = F_eq
F_eq = sp.Matrix([
[ xdot1 - x4 ],
[ xdot2 - x5 ],
[ xdot3 - x6 ],
[ g*sin(x1) + xdot4*x3 + 2*x4*x6 + xdot5*cos(x1) ]])
# Container carrying additional information about the example
# (will be stored in pickle-file)
data = st.Container()
data.F_eq = F_eq
data.time_dep_symbols = diff_symbols
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
# -*- coding: utf-8 -*-
# enable true divison
from __future__ import division
import sympy as sp
from sympy import sin,cos,tan
import symbtools as st
L1, L2 = sp.symbols("L1,L2", commutative=False)
vec_x = st.symb_vector('x1:7', commutative=False)
vec_xdot = st.time_deriv(vec_x, vec_x)
st.make_global(vec_x, 1)
st.make_global(vec_xdot, 1)
# zur kontrolle
#~ P1 = sp.Matrix([
#~ [ -tan(x4), 1, 0, 0, 0, 0],
#~ [ -tan(x3)*(L1*cos(x4))**(-1), 0, 0, 1, 0, 0],
#~ [-sin(x5 + x6)*(L2*cos(x5)*cos(x4))**(-1), 0, 0, -1, 0, 1]
#~ ])
#~ P0 = sp.Matrix([
#~ [0, 0, 0, -xdot1 - xdot1*tan(x4)**2, 0, 0],
#~ [0, 0, -xdot1*(1 + tan(x3)**2)*(L1*cos(x4))**(-1), -xdot1*tan(x3)*(L1*cos(x4))**(-1)*L1*sin(x4)*(L1*cos(x4))**(-1), 0, 0],
#~ [0, 0, 0, -xdot1*sin(x5 + x6)*(L2*cos(x5)*cos(x4))**(-1)*L2*cos(x5)*sin(x4)*(L2*cos(x5)*cos(x4))**(-1), -xdot1*sin(x5 + x6)*(L2*cos(x5)*cos(x4))**(-1)*L2*sin(x5)*cos(x4)*(L2*cos(x5)*cos(x4))**(-1) - xdot1*cos(x5 + x6)*(L2*cos(x5)*cos(x4))**(-1), -xdot1*cos(x5 + x6)*(L2*cos(x5)*cos(x4))**(-1)]
#~ ])
F_eq = sp.Matrix([
[xdot2 - xdot1*sp.tan(x4)],
# -*- coding: utf-8 -*-
import sympy as sp
import symbtools as st
from sympy import sin, cos, tan
vec_x = st.symb_vector('x1:7')
vec_xdot = st.time_deriv(vec_x, vec_x)
st.make_global(vec_x)
st.make_global(vec_xdot)
g = sp.symbols("g")
diff_symbols = sp.Matrix([])
F_eq = sp.Matrix([
[ xdot1 - x4 ],
[ xdot2 - x5 ],
[ xdot3 - x6 ],
[ g*sin(x1) + xdot4*x3 + 2*x4*x6 + xdot5*cos(x1) ]])
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()
