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) ]])
