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)