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_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_time_deriv2(self):
"""
test matrix compatibility
"""
a, b, t = sp.symbols("a, b, t")
# not time dependent
x, y = sp.symbols("x, y")
adot, bdot = st.time_deriv(sp.Matrix([a, b]), (a, b))
# has no associated function -> return the symbol itself
self.assertEqual(x.ddt_func, x)
# due to time_deriv now the .ddt_func attribute is set for a, b, adot, bdot
self.assertTrue(isinstance(adot.ddt_func, sp.Derivative))
self.assertEqual(type(type(a.ddt_func)), sp.function.UndefinedFunction)
self.assertEqual(type(a.ddt_func).__name__, a.name)
self.assertEqual(a.ddt_child, adot)
self.assertEqual(bdot.ddt_parent, b)
addot1 = st.time_deriv(adot, [a, b])
addot2 = st.time_deriv(a, [a, b], order=2)
self.assertEqual(addot1, addot2)
A = sp.Matrix([sin(a), exp(a * b), -t**2 * 7 * 0, x + y]).reshape(2, 2)
A_dot = st.time_deriv(A, (a, b))
A_dot_manual = \
sp.Matrix([adot * cos(a), b * adot * exp(a * b) + a * bdot * exp(a * b),
-t ** 2 * 7 * 0, 0]).reshape(2, 2)
self.assertEqual(A_dot.expand(), A_dot_manual)
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_depends_on_t1(self):
a, b, t = sp.symbols("a, b, t")
A = sp.Function("A")
res1 = st.depends_on_t(a + b, t, [])
self.assertEqual(res1, False)
res2 = st.depends_on_t(a + b, t, [
a,
])
self.assertEqual(res2, True)
res3 = st.depends_on_t(A(t) + b, t, [])
self.assertEqual(res3, True)
res4 = st.depends_on_t(A(t) + b, t, [b])
self.assertEqual(res4, True)
res5 = st.depends_on_t(t, t, [])
self.assertEqual(res5, True)
adot = st.time_deriv(a, [a])
res5 = st.depends_on_t(adot, t, [])
self.assertEqual(res5, True)
x1, x2 = xx = sp.symbols("x1, x2")
x1dot = st.time_deriv(x1, xx)
self.assertTrue(st.depends_on_t(x1dot, t))
y1, y2 = yy = sp.Matrix(sp.symbols("y1, y2", commutative=False))
yydot = st.time_deriv(yy, yy, order=1, commutative=False)
self.assertTrue(st.depends_on_t(yydot, t))
def test_triple_pendulum(self):
np = 1
nq = 2
pp = sp.Matrix( sp.symbols("p1:{0}".format(np+1) ) )
qq = sp.Matrix( sp.symbols("q1:{0}".format(nq+1) ) )
ttheta = st.row_stack(pp, qq)
Q1, Q2 = sp.symbols('Q1, Q2')
p1_d, q1_d, q2_d = mu = st.time_deriv(ttheta, ttheta)
p1_dd, q1_dd, q2_dd = mu_d = st.time_deriv(ttheta, ttheta, order=2)
p1, q1, q2 = ttheta
# reordering according to chain
kk = sp.Matrix([q1, q2, p1])
kd1, kd2, kd3 = q1_d, q2_d, p1_d
params = sp.symbols('l1, l2, l3, l4, s1, s2, s3, s4, J1, J2, J3, J4, m1, m2, m3, m4, g')
l1, l2, l3, l4, s1, s2, s3, s4, J1, J2, J3, J4, m1, m2, m3, m4, g = params
# geometry
mey = -Matrix([0,1])
# coordinates for centers of inertia and joints
S1 = mt.Rz(kk[0])*mey*s1
G1 = mt.Rz(kk[0])*mey*l1
S2 = G1 + mt.Rz(sum(kk[:2]))*mey*s2
G2 = G1 + mt.Rz(sum(kk[:2]))*mey*l2
S3 = G2 + mt.Rz(sum(kk[:3]))*mey*s3
G3 = G2 + mt.Rz(sum(kk[:3]))*mey*l3
# velocities of joints and center of inertia
Sd1 = st.time_deriv(S1, ttheta)
Sd2 = st.time_deriv(S2, ttheta)
Sd3 = st.time_deriv(S3, ttheta)
# energy
T_rot = ( J1*kd1**2 + J2*(kd1 + kd2)**2 + J3*(kd1 + kd2 + kd3)**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, Q1, Q2]
mod = mt.generate_symbolic_model(T, V, ttheta, external_forces, simplify=False)
eqns_ref = Matrix([[J3*(2*p1_dd + 2*q1_dd + 2*q2_dd)/2 + g*m3*s3*sin(p1 + q1 + q2) + m3*s3*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - l2*(q1_d + q2_d)**2*sin(q1 + q2) + l2*(q1_dd + q2_dd)*cos(q1 + q2) - s3*(p1_d + q1_d + q2_d)**2*sin(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*cos(p1 + q1 + q2))*cos(p1 + q1 + q2) + m3*s3*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + l2*(q1_d + q2_d)**2*cos(q1 + q2) + l2*(q1_dd + q2_dd)*sin(q1 + q2) + s3*(p1_d + q1_d + q2_d)**2*cos(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*sin(p1 + q1 + q2))*sin(p1 + q1 + q2)], [J1*q1_dd + J2*(2*q1_dd + 2*q2_dd)/2 + J3*(2*p1_dd + 2*q1_dd + 2*q2_dd)/2 - Q1 + g*m1*s1*sin(q1) + \
g*m2*(l1*sin(q1) + s2*sin(q1 + q2)) + g*m3*(l1*sin(q1) + l2*sin(q1 + q2) + s3*sin(p1 + q1 + q2)) + m1*q1_dd*s1**2*sin(q1)**2 + m1*q1_dd*s1**2*cos(q1)**2 + m2*(2*l1*sin(q1) + 2*s2*sin(q1 + q2))*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + s2*(q1_d + q2_d)**2*cos(q1 + q2) + s2*(q1_dd + q2_dd)*sin(q1 + q2))/2 + m2*(2*l1*cos(q1) + 2*s2*cos(q1 + q2))*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - s2*(q1_d + q2_d)**2*sin(q1 + q2) + s2*(q1_dd + q2_dd)*cos(q1 + q2))/2 + m3*(2*l1*sin(q1) + 2*l2*sin(q1 + q2) + 2*s3*sin(p1 + q1 + q2))*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + l2*(q1_d + q2_d)**2*cos(q1 + q2) + l2*(q1_dd + q2_dd)*sin(q1 + q2) + s3*(p1_d + q1_d + q2_d)**2*cos(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*sin(p1 + q1 + q2))/2 + m3*(2*l1*cos(q1) + 2*l2*cos(q1 + q2) + 2*s3*cos(p1 + q1 + q2))*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - l2*(q1_d + q2_d)**2*sin(q1 + q2) + l2*(q1_dd + q2_dd)*cos(q1 + q2) - \
s3*(p1_d + q1_d + q2_d)**2*sin(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*cos(p1 + q1 + q2))/2], [J2*(2*q1_dd + 2*q2_dd)/2 + J3*(2*p1_dd + 2*q1_dd + 2*q2_dd)/2 - Q2 + g*m2*s2*sin(q1 + q2) + g*m3*(l2*sin(q1 + q2) + s3*sin(p1 + q1 + q2)) + m2*s2*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - s2*(q1_d + q2_d)**2*sin(q1 + q2) + s2*(q1_dd + q2_dd)*cos(q1 + q2))*cos(q1 + q2) + \
m2*s2*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + s2*(q1_d + q2_d)**2*cos(q1 + q2) + s2*(q1_dd + q2_dd)*sin(q1 + q2))*sin(q1 + q2) + m3*(2*l2*sin(q1 + q2) + 2*s3*sin(p1 + q1 + q2))*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + l2*(q1_d + q2_d)**2*cos(q1 + q2) + l2*(q1_dd + q2_dd)*sin(q1 + q2) + s3*(p1_d + q1_d + q2_d)**2*cos(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*sin(p1 + q1 + q2))/2 + m3*(2*l2*cos(q1 + q2) + 2*s3*cos(p1 + q1 + q2))*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - l2*(q1_d + q2_d)**2*sin(q1 + q2) + l2*(q1_dd + q2_dd)*cos(q1 + q2) - s3*(p1_d + q1_d + q2_d)**2*sin(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*cos(p1 + q1 + q2))/2]])
self.assertEqual(eqns_ref, mod.eqns)
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 generate_constraints_funcs(self):
"""
Generate callable functions which represent the algebraic constraints a(tt) and its first two derivatives
adot(tt, ttdot) and addot(tt, ttdot, ttddot).
:return: None
"""
if self.constraints_func is not None:
return
actual_symbs = self.constraints.atoms(sp.Symbol)
expected_symbs = set(self.mod.tt)
if not actual_symbs == expected_symbs:
msg = "Constraints can only converted to numerical func if all parameters are passed for substitution. " \
"Unexpected symbols: {}".format(actual_symbs.difference(expected_symbs))
raise ValueError(msg)
self.constraints_func = st.expr_to_func(self.mod.tt, self.constraints)
# now we need also the differentiated constraints (e.g. to calculate consistent velocities and accelerations)
self.constraints_d = st.time_deriv(self.constraints, self.mod.tt)
self.constraints_dd = st.time_deriv(self.constraints_d, self.mod.tt)
xx = st.row_stack(self.mod.tt, self.mod.ttd)
# this function depends on coordinates ttheta and velocities ttheta_dot
self.constraints_d_func = st.expr_to_func(xx, self.constraints_d)
zz = st.row_stack(self.mod.tt, self.mod.ttd, self.mod.ttdd)
# this function depends on coordinates ttheta and velocities ttheta_dot and accel
self.constraints_dd_func = st.expr_to_func(zz, self.constraints_dd)
def test_apply_deriv2(self):
y1, y2 = yy = sp.Matrix( sp.symbols('y1, y2', commutative=False) )
ydot1 = st.time_deriv(y1, yy)
ydot2 = st.time_deriv(y2, yy)
yddot1 = st.time_deriv(y1, yy, order=2)
ydddot1 = st.time_deriv(y1, yy, order=3)
res1 = nct.apply_deriv(y1, 1, s, t, func_symbols=yy)
self.assertEqual(res1, ydot1 + y1*s)
res3 = nct.apply_deriv(y1*s, 1, s, t, func_symbols=yy)
self.assertEqual(res3, ydot1*s + y1*s**2)
res4 = nct.apply_deriv(y2 + y1, 1, s, t, func_symbols=yy)
self.assertEqual(res4, ydot1 + ydot2 + y1*s + y2*s)
res5 = nct.apply_deriv(ydot1 + y1*s, 1, s, t, func_symbols=yy)
self.assertEqual(res5, yddot1 + 2*ydot1*s + y1*s**2)
res6 = nct.apply_deriv(y1, 2, s, t, func_symbols=yy)
self.assertEqual(res5, res6)
res2 = nct.apply_deriv(y1, 3, s, t, func_symbols=yy)
self.assertEqual(res2, ydddot1 + 3*yddot1*s + 3*ydot1*s**2 + y1*s**3)
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_cart_pole_with_dissi(self):
p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1'))
F1, tau_dissi = sp.Matrix(sp.symbols('F1, tau_dissi'))
params = sp.symbols('m0, m1, l1, g, d1')
m0, m1, l1, g, d1 = params
pdot1 = st.time_deriv(p1, ttheta)
# q1dd = st.time_deriv(q1, ttheta, order=2)
ex = Matrix([1, 0])
ey = Matrix([0, 1])
S0 = ex * q1 # joint of the pendulum
S1 = S0 - mt.Rz(p1) * ey * l1 # center of mass
# velocity
S0d = st.time_deriv(S0, ttheta)
S1d = st.time_deriv(S1, ttheta)
T_rot = 0 # no moment of inertia (mathematical pendulum)
T_trans = (m0 * S0d.T * S0d + m1 * S1d.T * S1d) / 2
T = T_rot + T_trans[0]
V = m1 * g * S1[1]
QQ = sp.Matrix([tau_dissi, F1])
mod = mt.generate_symbolic_model(T, V, ttheta, QQ)
mod.substitute_ext_forces(QQ, [d1 * pdot1, F1], [F1])
self.assertEqual(len(mod.tau), 1)
self.assertEqual(len(mod.extforce_list), 1)
self.assertEqual(mod.tau[0], F1)
mod.calc_coll_part_lin_state_eq()
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_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_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_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_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_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_time_deriv1(self):
a, b, t = sp.symbols("a, b, t")
f1 = a**2 + 10 * sin(b)
f1_dot = st.time_deriv(f1, (a, b))
self.assertEqual(str(f1_dot), "2*a*adot + 10*bdot*cos(b)")
f2 = sin(a)
f2_dot2 = st.time_deriv(f2, [a, b], order=2)
f2_dot = st.time_deriv(f2, [a])
f2_ddot = st.time_deriv(f2_dot, [a])
self.assertEqual(f2_ddot, f2_dot2)
def test_lie_deriv_cartan(self):
x1, x2, x3 = xx = sp.symbols('x1:4')
u1, u2 = uu = sp.Matrix(sp.symbols('u1:3'))
# ordinary lie_derivative
# source: inspired by the script of Prof. Kugi (TU-Wien)
f = sp.Matrix([-x1**3, cos(x1) * cos(x2), x2])
g = sp.Matrix([cos(x2), 1, exp(x1)])
h = x3
Lfh = x2
Lf2h = f[1]
Lgh = exp(x1)
res1 = st.lie_deriv_cartan(h, f, xx)
res2 = st.lie_deriv_cartan(h, f, xx, order=2)
self.assertEqual(res1, Lfh)
self.assertEqual(res2, Lf2h)
# incorporating the input
h2 = u1
udot1, udot2 = uudot = st.time_deriv(uu, uu, order=1)
uddot1, uddot2 = st.time_deriv(uu, uu, order=2)
res_a1 = st.lie_deriv_cartan(h2, f, xx, uu, order=1)
res_a2 = st.lie_deriv_cartan(h2, f, xx, uu, order=2)
self.assertEqual(res_a1, udot1)
self.assertEqual(res_a2, uddot1)
res_a3 = st.lie_deriv_cartan(udot1, f, xx, [uu, uudot], order=1)
self.assertEqual(res_a3, uddot1)
# more complex examples
h3 = x3 + u1
fg = f + g * u2
res_b1 = st.lie_deriv_cartan(h3, fg, xx, uu, order=1)
res_b2 = st.lie_deriv_cartan(h3, fg, xx, uu, order=2)
res_b3 = st.lie_deriv_cartan(res_b1, fg, xx, [uu, uudot], order=1)
self.assertEqual(res_b1, Lfh + Lgh * u2 + udot1)
self.assertEqual(sp.expand(res_b2 - res_b3), 0)
h4 = x3 * sin(x2)
fg = f + g * u2
res_c1 = st.lie_deriv_cartan(h4, fg, xx, uu, order=1)
res_c2 = st.lie_deriv_cartan(res_c1, fg, xx, uu, order=1)
res_c3 = st.lie_deriv_cartan(h4, fg, xx, uu, order=2)
self.assertEqual(sp.expand(res_c2 - res_c3), 0)
def test_perform_time_deriv5f(self):
# test numbered symbols
x1, x2 = xx = sp.symbols("x1, x_2")
# TODO: These two assertions should pass
# Then the above FIXME-issues can be resolved and this test is obsolete
res_a5 = st.time_deriv(x1, xx, order=5)
self.assertEqual(str(res_a5), 'x1_d5')
res_b5 = st.time_deriv(x2, xx, order=5)
self.assertEqual(str(res_b5), 'x_2_d5')
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_two_link_manipulator(self):
p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1'))
pdot1, qdot1 = st.time_deriv(ttheta, ttheta)
tau1, = ttau = sp.Matrix(sp.symbols('F1,'))
params = sp.symbols('m1, m2, l1, l2, s1, s2, J1, J2')
m1, m2, l1, l2, s1, s2, J1, J2 = params
ex = Matrix([1,0])
ey = Matrix([0,1])
S1 = mt.Rz(q1)*ex*s1 # center of mass (first link)
G1 = mt.Rz(q1)*ex*l1 # first joint
S2 = G1 + mt.Rz(q1+p1)*ex*s2
#velocity
S1d = st.time_deriv(S1, ttheta)
S2d = st.time_deriv(S2, ttheta)
T_rot = (J1*qdot1**2 + J2*(qdot1 + pdot1)**2) / 2
T_trans = ( m1*S1d.T*S1d + m2*S2d.T*S2d )/2
T = T_rot + T_trans[0]
V = 0
mod = mt.generate_symbolic_model(T, V, ttheta, [tau1, 0])
#mod.eqns.simplify()
#mod.calc_coll_part_lin_state_eq(simplify=True)
mod.calc_lbi_nf_state_eq(simplify=True)
w1 = mod.ww[0]
kappa = l1*m2*s2 / (J2 + m2*s2**2)
fz4 = - kappa * qdot1*sin(p1) *(w1 - kappa*qdot1*cos(p1))
fzref = sp.Matrix([[ qdot1],
[ 0],
[ (-qdot1*(1 + kappa*cos(p1) ) ) + w1],
[ fz4]])
w_def_ref = pdot1 + qdot1*(1+kappa*cos(p1))
self.assertEqual(mod.gz, sp.Matrix([0, 1, 0, 0]))
fz_diff = mod.fz - fzref
fz_diff.simplify()
self.assertEqual(fz_diff, sp.Matrix([0, 0, 0, 0]))
diff = mod.ww_def[0,0] - w_def_ref
self.assertEqual(diff.simplify(), 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)
def test_integrate_with_time_derivs(self):
t = sp.Symbol("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)
z = xdot1 - 4 * xdot2
res = st.smart_integrate(z, t)
self.assertEqual(res, x1 - 4 * x2)
z = xdot1 - 4 * xdot2 + 5 * xddot2 + t + 3 * x1
res = st.smart_integrate(z, t)
eres = x1 - 4 * x2 + t**2 / 2 + 5 * xdot2 + 3 * sp.integrate(
x1.ddt_func, t)
self.assertEqual(res, eres)
def test_two_link_manipulator(self):
p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1'))
pdot1, qdot1 = st.time_deriv(ttheta, ttheta)
tau1, = ttau = sp.Matrix(sp.symbols('F1,'))
params = sp.symbols('m1, m2, l1, l2, s1, s2, J1, J2')
m1, m2, l1, l2, s1, s2, J1, J2 = params
ex = Matrix([1, 0])
ey = Matrix([0, 1])
S1 = mt.Rz(q1) * ex * s1 # center of mass (first link)
G1 = mt.Rz(q1) * ex * l1 # first joint
S2 = G1 + mt.Rz(q1 + p1) * ex * s2
#velocity
S1d = st.time_deriv(S1, ttheta)
S2d = st.time_deriv(S2, ttheta)
T_rot = (J1 * qdot1**2 + J2 * (qdot1 + pdot1)**2) / 2
T_trans = (m1 * S1d.T * S1d + m2 * S2d.T * S2d) / 2
T = T_rot + T_trans[0]
V = 0
mod = mt.generate_symbolic_model(T, V, ttheta, [tau1, 0])
#mod.eqns.simplify()
#mod.calc_coll_part_lin_state_eq(simplify=True)
mod.calc_lbi_nf_state_eq(simplify=True)
w1 = mod.ww[0]
kappa = l1 * m2 * s2 / (J2 + m2 * s2**2)
fz4 = -kappa * qdot1 * sin(p1) * (w1 - kappa * qdot1 * cos(p1))
fzref = sp.Matrix([[qdot1], [0],
[(-qdot1 * (1 + kappa * cos(p1))) + w1], [fz4]])
w_def_ref = pdot1 + qdot1 * (1 + kappa * cos(p1))
self.assertEqual(mod.gz, sp.Matrix([0, 1, 0, 0]))
fz_diff = mod.fz - fzref
fz_diff.simplify()
self.assertEqual(fz_diff, sp.Matrix([0, 0, 0, 0]))
diff = mod.ww_def[0, 0] - w_def_ref
self.assertEqual(diff.simplify(), 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
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
# noinspection PyUnusedLocal
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])
self.assertEqual(mod.fz, None)
self.assertEqual(mod.gz, None)
mod.calc_lbi_nf_state_eq()
self.assertEqual(mod.fz.shape, (6, 1))
self.assertEqual(mod.gz, sp.eye(6)[:, 2:4])
def get_orig_ref_replm(self, maxorder=None):
"""
:return:
"""
if maxorder is None:
maxorder = np.inf
u_ref_list = []
u_orig_list = []
for u in self.uu:
ur = sp.Symbol("{}_ref".format(u.name))
u_orig_list.append(u)
u_ref_list.append(ur)
tmp1 = u
tmp2 = ur
while tmp1.ddt_child is not None:
if tmp1.difforder >= maxorder:
break
tmp1 = tmp1.ddt_child
tmp2 = st.time_deriv(tmp2, [ur])
u_orig_list.append(tmp1)
u_ref_list.append(tmp2)
res = list(zip(self.xx, self.xx_ref)) + list(
zip(u_orig_list, u_ref_list))
return res
def transform_2nd_to_1st_order_matrices(P0, P1, P2, xx):
"""Transforms an implicit second order (tangential) representation of a mechanical system to
an first order representation (needed to apply the "Franke-approach")
:param P0: eqns.jacobian(theta)
:param P1: eqns.jacobian(theta_d)
:param P2: eqns.jacobian(theta_dd)
:param xx: vector of state variables
:return: P0_bar, P1_bar
with P0_bar = implicit_state_equations.jacobian(x)
and P1_bar = implicit_state_equations.jacobian(x_d)
"""
assert P0.shape == P1.shape == P2.shape
assert xx.shape == (P0.shape[1]*2, 1)
xxd = st.time_deriv(xx, xx)
N = int(xx.shape[0]/2)
# definitional equations like xdot1 - x3 = 0 (for N=2)
eqns_def = xx[N:, :] - xxd[:N, :]
eqns_mech = P2*xxd[N:, :] + P1*xxd[:N, :] + P0*xx[:N, :]
F = st.row_stack(eqns_def, eqns_mech)
P0_bar = F.jacobian(xx)
P1_bar = F.jacobian(xxd)
return P0_bar, P1_bar
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 reduction(iter_stack):
P1i = iter_stack.P1
P0i = iter_stack.P0
P1i_roc = roc_hint(
"P1", iter_stack.i,
P1i) if mode == "manual" else al.right_ortho_complement(P1i)
P1i_rpinv = rpinv_hint(
"P1", iter_stack.i,
P1i) if mode == "manual" else al.right_pseudo_inverse(P1i)
P1i_dot = st.time_deriv(P1i, myStack.diffvec_x)
Ai = al.custom_simplify((P0i - P1i_dot) * P1i_rpinv)
Bi = al.custom_simplify((P0i - P1i_dot) * P1i_roc)
if myStack.calc_G:
Bi_lpinv = al.left_pseudo_inverse(Bi)
else:
Bi_lpinv = None
# store
iter_stack.store_reduction_matrices(Ai, Bi, Bi_lpinv, P1i_roc, P1i_rpinv,
P1i_dot)
return Bi
def transform_2nd_to_1st_order_matrices(P0, P1, P2, xx):
"""Transforms an implicit second order (tangential) representation of a mechanical system to
an first order representation (needed to apply the "Franke-approach")
:param P0: eqns.jacobian(theta)
:param P1: eqns.jacobian(theta_d)
:param P2: eqns.jacobian(theta_dd)
:param xx: vector of state variables
:return: P0_bar, P1_bar
with P0_bar = implicit_state_equations.jacobian(x)
and P1_bar = implicit_state_equations.jacobian(x_d)
"""
assert P0.shape == P1.shape == P2.shape
assert xx.shape == (P0.shape[1] * 2, 1)
xxd = st.time_deriv(xx, xx)
N = int(xx.shape[0] / 2)
# definitional equations like xdot1 - x3 = 0 (for N=2)
eqns_def = xx[N:, :] - xxd[:N, :]
eqns_mech = P2 * xxd[N:, :] + P1 * xxd[:N, :] + P0 * xx[:N, :]
F = st.row_stack(eqns_def, eqns_mech)
P0_bar = F.jacobian(xx)
P1_bar = F.jacobian(xxd)
return P0_bar, P1_bar
def test_vector_form_dot(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)
Q = sp.Matrix([[x3 / sin(x1), 1, 0], [1, 0, x3]])
Q_ = st.col_stack(Q, sp.zeros(2, 6))
# vector 1-forms
omega = pc.VectorDifferentialForm(1, XX, coeff=Q_)
with self.assertRaises(ValueError) as cm:
# coordinates are not part of basis
omega.dot().dot().dot()
def test_right_shift_all2(self):
a, b = sp.symbols("a, b", commutative=False)
ab = sp.Matrix([a, b])
adot, bdot = ab_dot = st.time_deriv(ab, ab)
sab = sp.Matrix([s*a, s*b])
res1 = nct.right_shift_all(sab, func_symbols=ab)
res2 = ab_dot + nct.nc_mul(ab, s)
self.assertEqual(res1, res2)
res = nct.right_shift_all(s, s, t)
self.assertEqual(res, s)
res = nct.right_shift_all(s**2, s, t)
self.assertEqual(res, s**2)
res = nct.right_shift_all(a, s, t)
self.assertEqual(res, a)
res = nct.right_shift_all(a**(sp.S(1)/2), s, t)
self.assertEqual(res, a**(sp.S(1)/2))
res = nct.right_shift_all(s*1/a, s, func_symbols=ab)
self.assertEqual(res, 1/a*s -1/a**2*adot)
res = nct.right_shift_all(s + sp.sin(a), s, func_symbols=ab)
self.assertEqual(res, s + sp.sin(a))
self.assertRaises( ValueError, nct.right_shift_all, s**(sp.S(1)/2), s, t)
self.assertRaises( ValueError, nct.right_shift_all, sp.sin(s), s, t)
def vec_x(self, value):
self._vec_x = value
# calculate vec_xdot
self.vec_xdot = st.time_deriv(self.vec_x, self.vec_x)
# vector for differentiation
self.diffvec_x = st.concat_rows(self.vec_x, self.vec_xdot, self.diff_symbols)
def test_right_shift4(self):
y1, y2 = yy = sp.Matrix( sp.symbols('y1, y2', commutative=False) )
ydot1, ydot2 = st.time_deriv(yy, yy)
res1 = nct.right_shift(s*y1, s, t, yy)
self.assertEqual(res1, ydot1 + y1*s)
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 new_model_from_equations_of_motion(eqns, theta, tau):
"""
:param eqns: vector of equations of motion
:param tt: generalized coordinates
:param tau: input
:return: SymbolicModel instance
"""
mod = SymbolicModel()
mod.eqns = eqns
mod.tt = theta
mod.ttd = st.time_deriv(theta, theta)
mod.ttdd = st.time_deriv(theta, theta, order=2)
mod.extforce_list = tau
mod.tau = tau
return mod
def test_right_shift_all_naive(self):
a, b = sp.symbols("a, b", commutative=False)
ab = sp.Matrix([a, b])
adot, bdot = ab_dot = st.time_deriv(ab, ab)
addot, bddot = ab_ddot = st.time_deriv(ab, ab, order=2)
sab = sp.Matrix([s*a, s*b])
abs = sp.Matrix([a*s, b*s])
res1 = nct.right_shift_all(sab, func_symbols=None)
self.assertEqual(res1, abs)
# normally derivatives are recognized as time dependent automatically
res2 = nct.right_shift_all(s*adot)
self.assertEqual(res2, addot + adot*s)
# if func_symbols=None derivatives are like constants
res3 = nct.right_shift_all(s*adot, func_symbols=None)
self.assertEqual(res3, adot*s)
def test_simple1(self):
q1, = qq = sp.Matrix(sp.symbols('q1,'))
F1, = FF = sp.Matrix(sp.symbols('F1,'))
m = sp.Symbol('m')
q1d = st.time_deriv(q1, qq)
q1dd = st.time_deriv(q1, qq, order=2)
T = q1d**2*m/2
V = 0
mod = mt.generate_symbolic_model(T, V, qq, FF)
eq = m*q1dd - F1
self.assertEqual(mod.eqns[0], eq)
# test the application of the @property
M = mod.MM
self.assertEqual(M[0], m)
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_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_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 apply_deriv(term, power, s, t, func_symbols=[]):
res = 0
assert int(power) == power
if power == 0:
return term
if isinstance(term, sp.Add):
res = sum(apply_deriv(a, power, s, t, func_symbols) for a in term.args)
return res
tmp = st.time_deriv(term, func_symbols) + term*s
res = apply_deriv(tmp, power-1, s, t, func_symbols).expand()
return res
def test_make_all_symbols_noncommutative(self):
a, b, c = abc = sp.symbols("a, b, c", commutative=True)
x, y = xy = sp.symbols("x, y", commutative=False)
adddot = st.time_deriv(a, abc, order=3)
exp1 = a*b*x + b*c*y
exp1_nc, subs_tuples = nct.make_all_symbols_noncommutative(exp1)
self.assertTrue( all([ not r.is_commutative for r in exp1_nc.atoms()]) )
adddot_nc = nct.make_all_symbols_noncommutative(adddot)[0]
self.assertEqual(adddot.difforder, adddot_nc.difforder)
def reduction(iter_stack):
P1i = iter_stack.P1
P0i = iter_stack.P0
P1i_roc = roc_hint("P1", iter_stack.i, P1i) if mode=="manual" else al.right_ortho_complement(P1i)
P1i_rpinv = rpinv_hint("P1", iter_stack.i, P1i) if mode=="manual" else al.right_pseudo_inverse(P1i)
P1i_dot = st.time_deriv(P1i, myStack.diffvec_x)
Ai = al.custom_simplify( (P0i - P1i_dot)*P1i_rpinv )
Bi = al.custom_simplify( (P0i - P1i_dot)*P1i_roc )
if myStack.calc_G:
Bi_lpinv = al.left_pseudo_inverse(Bi)
else:
Bi_lpinv = None
# store
iter_stack.store_reduction_matrices( Ai, Bi, Bi_lpinv, P1i_roc, P1i_rpinv, P1i_dot )
return Bi
def generate_basis(self):
# TODO: not the most elegant way?
# check highest order of derivatives
highest_order = 0
for n in self._myStack.transformation.H.atoms():
if hasattr(n, "difforder"):
if n.difforder>highest_order:
highest_order = n.difforder
# generate vector with vec_x and its derivatives up to highest_order
new_vector = self._myStack.vec_x
for index in xrange(1, highest_order+1):
vec_x_ndot = st.time_deriv(self._myStack.vec_x, self._myStack.vec_x, order=index)
new_vector = st.concat_rows( new_vector, vec_x_ndot )
# generate basis_1form up to this order
basis, basis_1form = ct.diffgeo_setup(new_vector)
# store
self._myStack.basis = basis
self._myStack.basis_1form = basis_1form
def test_right_shift_all(self):
a, b = sp.symbols("a, b", commutative=False)
f1 = sp.Function('f1', commutative=False)(t)
f2 = sp.Function('f2', commutative=False)(t)
f1d = f1.diff(t)
f2d = f2.diff(t)
p1 = s*(f1 + f2)
ab = sp.Matrix([a, b])
adot, bdot = st.time_deriv(ab, ab)
res1 = nct.right_shift_all(p1)
self.assertEqual(res1, f1d + f1*s + f2d + f2*s)
res2 = nct.right_shift_all(f1**-1, s, t)
self.assertEqual(res2, 1/f1)
res3 = nct.right_shift_all(s*a + s*a*b, s, t, [])
self.assertEqual(res3, a*s + a*b*s)
res4 = nct.right_shift_all(s*a + s*a*b, s, t, [a, b])
self.assertEqual(res4, a*s + a*b*s + adot + a*bdot + adot*b)
# -*- coding: utf-8 -*-
# enable true divison
from __future__ import division
import sympy as sp
import symbtools as st
x1, x2, x3 = sp.symbols("x1, x2, x3")
vec_x = sp.Matrix([x1, x2, x3])
vec_xdot = st.time_deriv(vec_x, vec_x)
xdot1, xdot2, xdot3 = vec_xdot
F_eq = sp.Matrix([xdot1*x3 - x2*x3 - x1*xdot3])
#P1i = sp.Matrix([ [x3, 0, -x1] ])
#P0i = sp.Matrix([ [-xdot3, -x3, -x2+xdot1] ])
def calc_lbi_nf_state_eq(self, simplify=False):
"""
calc vectorfields fz, and gz of the Lagrange-Byrnes-Isidori-Normalform
instead of the state xx
"""
n = len(self.tt)
nq = len(self.tau)
np = n - nq
nx = 2*n
# make sure that the system has the desired structure
B = self.eqns.jacobian(self.tau)
cond1 = B[:np, :] == sp.zeros(np, nq)
cond2 = B[np:, :] == -sp.eye(nq)
if not cond1 and cond2:
msg = "The jacobian of the equations of motion do not have the expected structure: %s"
raise NotImplementedError(msg % str(B))
pp = self.tt[:np,:]
qq = self.tt[np:,:]
uu = self.ttd[:np,:]
vv = self.ttd[np:,:]
ww = st.symb_vector('w1:{0}'.format(np+1))
assert len(vv) == nq
# state w.r.t normal form
self.zz = st.row_stack(qq, vv, pp, ww)
self.ww = ww
# set the actuated accelearations as new inputs
self.aa = self.ttdd[-nq:, :]
# input vectorfield
self.gz = sp.zeros(nx, nq)
self.gz[nq:2*nq, :] = sp.eye(nq) # identity matrix for the active coordinates
# drift vectorfield (will be completed below)
self.fz = sp.zeros(nx, 1)
self.fz[:nq, :] = vv
self.calc_mass_matrix()
if simplify:
self.M.simplify()
M11 = self.M[:np, :np]
M12 = self.M[:np, np:]
d = M11.berkowitz_det()
adj = M11.adjugate()
if simplify:
d = d.simplify()
adj.simplify()
M11inv = adj/d
# defining equation for ww: ww := uu + M11inv*M12*vv
uu_expr = ww - M11inv*M12*vv
# setting input tau and acceleration to 0 in the equations of motion
C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau))
N = st.time_deriv(M11inv*M12, self.tt)
ww_dot = -M11inv*C1K1.subs(lzip(uu, uu_expr)) + N.subs(lzip(uu, uu_expr))*vv
self.fz[2*nq:2*nq+np, :] = uu_expr
self.fz[2*nq+np:, :] = ww_dot
# how the new coordinates are defined:
self.ww_def = uu + M11inv*M12*vv
if simplify:
self.fz.simplify()
self.gz.simplify()
self.ww_def.simplify()
def generate_symbolic_model(T, U, tt, F, simplify=True, **kwargs):
"""
T: kinetic energy
U: potential energy
tt: sequence of independent deflection variables ("theta")
F: external forces
simplify: determines whether the equations of motion should be simplified
(default=True)
kwargs: optional assumptions like 'real=True'
"""
n = len(tt)
for theta_i in tt:
assert isinstance(theta_i, sp.Symbol)
F = sp.Matrix(F)
if F.shape[0] == 1:
# convert to column vector
F = F.T
if not F.shape == (n, 1):
msg = "Vector of external forces has the wrong length. Should be " + \
str(n) + " but is %i!" % F.shape[0]
raise ValueError(msg)
# introducing symbols for the derivatives
tt = sp.Matrix(tt)
ttd = st.time_deriv(tt, tt, **kwargs)
ttdd = st.time_deriv(tt, tt, order=2, **kwargs)
#Lagrange function
L = T - U
if not T.atoms().intersection(ttd) == set(ttd):
raise ValueError('Not all velocity symbols do occur in T')
# partial derivatives of L
L_diff_tt = st.jac(L, tt)
L_diff_ttd = st.jac(L, ttd)
#prov_deriv_symbols = [ttd, ttdd]
# time-depended_symbols
tds = list(tt) + list(ttd)
L_diff_ttd_dt = st.time_deriv(L_diff_ttd, tds, **kwargs)
#lagrange equation 2nd kind
model_eq = sp.zeros(n, 1)
for i in range(n):
model_eq[i] = L_diff_ttd_dt[i] - L_diff_tt[i] - F[i]
# create object of model
mod = SymbolicModel() # model_eq, qs, f, D)
mod.eqns = model_eq
mod.extforce_list = F
reduced_F = sp.Matrix([s for s in F if st.is_symbol(s)])
mod.F = reduced_F
mod.tau = reduced_F
# coordinates velocities and accelerations
mod.tt = tt
mod.ttd = ttd
mod.ttdd = ttdd
mod.qs = tt
mod.qds = ttd
mod.qdds = ttdd
# also store kinetic and potential energy
mod.T = T
mod.U = U
if simplify:
mod.eqns.simplify()
return mod
eq_coeffsC.append(coeffsC)
all_coeffs.extend(coeffs)
#x1, x2, x3, x4, x5, x6 = sp.symbols("x1, x2, x3, x4, x5, x6")
vec_x = sp.Matrix( sp.symbols("x1:%i" % (n*2 + 1)) )
theta = vec_x[:n, :]
mu = vec_x[n:, :]
vec_xdot = st.time_deriv(vec_x, vec_x)
thetadot = st.time_deriv(theta, vec_x)
mudot = st.time_deriv(mu, vec_x)
AA = sp.Matrix(eq_coeffsA) # P0
BB = sp.Matrix(eq_coeffsB) # P1
CC = sp.Matrix(eq_coeffsC) # P2
# Informationen über das konkrete System:
diff_symbols = list(AA[:,:2]) + list(AA[:, -1]) + list(BB[:, 2])
diff_symbols = sp.Matrix(sorted(diff_symbols, key=str))
def test_cart_pole(self):
p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1'))
F1, = FF = sp.Matrix(sp.symbols('F1,'))
params = sp.symbols('m0, m1, l1, g')
m0, m1, l1, g = params
pdot1 = st.time_deriv(p1, ttheta)
# q1dd = st.time_deriv(q1, ttheta, order=2)
ex = Matrix([1,0])
ey = Matrix([0,1])
S0 = ex*q1 # joint of the pendulum
S1 = S0 - mt.Rz(p1)*ey*l1 # center of mass
#velocity
S0d = st.time_deriv(S0, ttheta)
S1d = st.time_deriv(S1, ttheta)
T_rot = 0 # no moment of inertia (mathematical pendulum)
T_trans = ( m0*S0d.T*S0d + m1*S1d.T*S1d )/2
T = T_rot + T_trans[0]
V = m1*g*S1[1]
with self.assertRaises(ValueError) as cm:
# wrong length of external forces
mt.generate_symbolic_model(T, V, ttheta, [F1])
with self.assertRaises(ValueError) as cm:
# wrong length of external forces
mt.generate_symbolic_model(T, V, ttheta, [F1, 0, 0])
QQ = sp.Matrix([0, F1])
mod = mt.generate_symbolic_model(T, V, ttheta, [0, F1])
mod_1 = mt.generate_symbolic_model(T, V, ttheta, QQ)
mod_2 = mt.generate_symbolic_model(T, V, ttheta, QQ.T)
self.assertEqual(mod.eqns, mod_1.eqns)
self.assertEqual(mod.eqns, mod_2.eqns)
mod.eqns.simplify()
M = mod.MM
M.simplify()
M_ref = Matrix([[l1**2*m1, l1*m1*cos(p1)], [l1*m1*cos(p1), m1 + m0]])
self.assertEqual(M, M_ref)
rest = mod.eqns.subs(st.zip0((mod.ttdd)))
rest_ref = Matrix([[g*l1*m1*sin(p1)], [-F1 - l1*m1*pdot1**2*sin(p1)]])
self.assertEqual(M, M_ref)
mod.calc_state_eq(simplify=True)
mod.calc_coll_part_lin_state_eq(simplify=True)
pdot1, qdot1 = mod.ttd
ff_ref = sp.Matrix([[pdot1],[qdot1], [-g*sin(p1)/l1], [0]])
gg_ref = sp.Matrix([[0], [0], [-cos(p1)/l1], [1]])
self.assertEqual(mod.ff, ff_ref)
self.assertEqual(mod.gg, gg_ref)
