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_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_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 __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_lie_bracket(self): xx = st.symb_vector('x1:4') st.make_global(xx) fx = sp.Matrix([[(x2 - 1)**2 + 1 / x3], [x1 + 7], [-x3**2 * (x2 - 1)]]) v = sp.Matrix([[0], [0], [-x3**2]]) dist = st.col_stack(v, st.lie_bracket(-fx, v, xx), st.lie_bracket(-fx, v, xx, order=2)) v0, v1, v2 = st.col_split(dist) self.assertEqual(v1, sp.Matrix([1, 0, 0])) self.assertEqual(v2, sp.Matrix([0, 1, 0])) self.assertEqual(st.lie_bracket(fx, fx, xx), sp.Matrix([0, 0, 0]))
def test_make_global(self): aa = tuple(st.symb_vector('a1:4')) xx = st.symb_vector('x1:4') yy = st.symb_vector('y1:4') zz = st.symb_vector('z1:11').reshape(2, 5) # tollerate if there are numbers in the sequences: zz[0] = 0 zz[1] = 10 st.make_global(xx, yy, zz, aa) res = a1 + x2 + y3 + z4 + z7 + z10 res2 = aa[0] + xx[1] + yy[2] + zz[3] + zz[6] + zz[9] self.assertEqual(res, res2)
def setup_objects(n): """ convenience function creates a coordinate basis, and a set of canonical one-forms and inserts both in the global name space """ if isinstance(n, int): xx = sp.symbols("x1:%i" % (n + 1)) elif isinstance(n, string_types): xx = sp.symbols(n) # now assume n is a sequence of symbols elif all([x.is_Symbol for x in n]): xx = n else: raise TypeError("unexpected argument-type: " + str(type(n))) bf = basis_1forms(xx) st.make_global(xx, upcount=2) st.make_global(bf, upcount=2) return xx, bf
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())
# -*- 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]])
def test_four_bar_constraints(self): # 2 passive Gelenke np = 2 nq = 1 p1, p2 = pp = st.symb_vector("p1:{0}".format(np + 1)) q1, = qq = st.symb_vector("q1:{0}".format(nq + 1)) ttheta = st.row_stack(pp, qq) pdot1, pdot2, qdot1 = tthetad = st.time_deriv(ttheta, ttheta) tthetadd = st.time_deriv(ttheta, ttheta, order=2) st.make_global(ttheta, tthetad, tthetadd) params = sp.symbols( 's1, s2, s3, m1, m2, m3, J1, J2, J3, l1, l2, l3, kappa1, kappa2, g' ) s1, s2, s3, m1, m2, m3, J1, J2, J3, l1, l2, l3, kappa1, kappa2, g = params st.make_global(params) parameter_values = dict(s1=1 / 2, s2=1 / 2, s3=1 / 2, m1=1, m2=1, m3=3, J1=1 / 12, J2=1 / 12, J3=1 / 12, l1=1, l2=1.5, l3=1.5, kappa1=3 / 2, kappa2=14.715, g=9.81).items() # ttau = sp.symbols('tau') tau1, tau2 = st.symb_vector("tau1, tau2") # unit vectors ex = sp.Matrix([1, 0]) # Basis 1 and 2 # B1 = sp.Matrix([0, 0]) B2 = sp.Matrix([2 * l1, 0]) # Koordinaten der Schwerpunkte und Gelenke S1 = Rz(q1) * ex * s1 G1 = Rz(q1) * ex * l1 S2 = G1 + Rz(q1 + p1) * ex * s2 G2 = G1 + Rz(q1 + p1) * ex * l2 # one link manioulator G2b = B2 + Rz(p2) * ex * l3 S3 = B2 + Rz(p2) * ex * s3 # timederivatives of centers of masses Sd1, Sd2, Sd3 = st.col_split( st.time_deriv(st.col_stack(S1, S2, S3), ttheta)) # kinetic energy T_rot = (J1 * qdot1**2 + J2 * (qdot1 + pdot1)**2 + J3 * (pdot2)**2) / 2 T_trans = (m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3) / 2 T = T_rot + T_trans[0] # potential energy V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1] # # Kinetische Energie mit Platzhaltersymbolen einführen (jetzt nicht): # # M1, M2, M3 = MM = st.symb_vector('M1:4') # MM_subs = [(J1 + m1*s1 ** 2 + m2*l1 ** 2, M1), (J2 + m2*s2 ** 2, M2), (m2*l1*s2, M3)] # # MM_rplm = st.rev_tuple(MM_subs) # Umkehrung der inneren Tupel -> [(M1, J1+... ), ...] external_forces = [0, 0, tau1] mod = mt.generate_symbolic_model(T, V, ttheta, external_forces, constraints=[G2 - G2b]) self.assertEqual(mod.llmd.shape, (2, 1)) self.assertEqual(mod.constraints.shape, (2, 1)) dae = mod.calc_dae_eq(parameter_values=parameter_values) nc = len(mod.llmd) self.assertEqual(dae.eqns[-nc:, :], mod.constraints) # qdot1 = 0 ttheta_1, ttheta_d_1 = dae.calc_consistent_conf_vel(q1=npy.pi / 4, _disp=False) eres_c = npy.array([-0.2285526, 1.58379902, 0.78539816]) eres_v = npy.array([0, 0, 0]) self.assertTrue(npy.allclose(ttheta_1, eres_c)) self.assertTrue(npy.allclose(ttheta_d_1, eres_v)) # in that situation there is no force acc_1, llmd_1 = dae.calc_consistent_accel_lmd((ttheta_1, ttheta_d_1)) # these values seem reasonable but have yet not been checked analytically self.assertTrue( npy.allclose(acc_1, [13.63475466, -1.54473017, -8.75145644])) self.assertTrue(npy.allclose(llmd_1, [-0.99339947, 0.58291489])) # qdot1 ≠ 0 ttheta_2, ttheta_d_2 = dae.calc_consistent_conf_vel(q1=npy.pi / 8 * 7, qdot1=3, _disp=False) eres_c = npy.array([-0.85754267, 0.89969149, 0.875]) * npy.pi eres_v = npy.array([-3.42862311, 2.39360715, 3.]) self.assertTrue(npy.allclose(ttheta_2, eres_c)) self.assertTrue(npy.allclose(ttheta_d_2, eres_v)) yy_1, yyd_1 = dae.calc_consistent_init_vals(q1=12.7, qdot1=100) dae.generate_eqns_funcs() res = dae.model_func(0, yy_1, yyd_1) self.assertEqual(res.shape, (dae.ntt * 2 + dae.nll, )) self.assertTrue(npy.allclose(res, 0))
# -*- coding: utf-8 -*- # enable true divison 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) st.make_global(vec_xdot) F_eq = sp.Matrix([[-x3 * x4 + xdot1], [-x4 + xdot2], [-x5 + xdot3]])
import sympy as sp import symbtools as st import symbtools.modeltools as mt from joblib import dump Np = 3 Nq = 1 n = Np + Nq pp = st.symb_vector("p1:{0}".format(Np+1)) qq = st.symb_vector("q1:{0}".format(Nq+1)) ttheta = st.row_stack(pp, qq) tthetad = st.time_deriv(ttheta, ttheta) tthetadd = st.time_deriv(ttheta, ttheta, order=2) st.make_global(ttheta, tthetad, tthetadd) params = sp.symbols('l1, l2, l3, s1, s2, s3, m1, m2, m3, J1, J2, J3, m0, g') st.make_global(params) tau1 = sp.Symbol("tau1") #Einheitsvektoren ex = sp.Matrix([1,0]) ey = sp.Matrix([0,1]) # Koordinaten der Schwerpunkte und Gelenke S0 = ex*q1 # Schwerpunkt Wagen G0 = S0 # Gelenk zwischen Wagen und Pendel 1 G1 = G0 + mt.Rz(p1)*ey*l1 # Gelenk zwischen Pendel 1 und 2
# -*- 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(vec_x, vec_xdot) F_eq = sp.Matrix([[xdot1 - x4], [xdot2 - x5], [xdot3 - x6], [g * sin(x1) + xdot4 * x3 + 2 * x4 * x6 + xdot5 * cos(x1)]])
# -*- 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) ]])