Example #1
0
    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]))
Example #2
0
    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)
Example #3
0
    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))
Example #4
0
    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())
Example #5
0
    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)
Example #6
0
    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)
Example #7
0
    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())
Example #8
0
    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)
Example #9
0
    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
Example #10
0
    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))
Example #11
0
    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)
Example #12
0
    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)
Example #13
0
    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)
Example #14
0
    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
Example #15
0
    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_)
Example #16
0
    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)
Example #17
0
    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))
Example #18
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)
Example #19
0
    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)
Example #20
0
    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))
Example #21
0
    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))
Example #22
0
    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)
Example #23
0
    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)
Example #24
0
    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)
Example #25
0
    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)
Example #26
0
    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))
Example #27
0
    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)
Example #28
0
    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)
Example #29
0
    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))
Example #30
0
    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]))
Example #32
0
    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)
Example #33
0
        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))
Example #34
0
    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)
Example #35
0
    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()
Example #36
0
    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))
Example #37
0
    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))
Example #38
0
# -*- 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
Example #39
0
# -*- 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]])
Example #40
0
# -*- 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
Example #41
0
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
Example #42
0
# -*- 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)],
Example #43
0
# -*- 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)  ]])
Example #44
0
    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()