def test_jet_extend_basis1(self): x1, x2, x3 = xx = st.symb_vector("x1, x2, x3") xx_tmp, ddx = pc.setup_objects(xx) self.assertTrue(xx is xx_tmp) # get the individual forms dx1, dx2, dx3 = ddx dx1.jet_extend_basis() xdot1, xdot2, xdot3 = xxd = pc.st.time_deriv(xx, xx) xddot1, xddot2, xddot3 = xxdd = pc.st.time_deriv(xx, xx, order=2) full_basis = st.row_stack(xx, xxd, xxdd) foo, ddX = pc.setup_objects(full_basis) dx1.jet_extend_basis() self.assertEqual(ddX[0].basis, dx1.basis) self.assertEqual(ddX[0].coeff, dx1.coeff) half_basis = st.row_stack(xx, xxd) foo, ddY = pc.setup_objects(half_basis) dx2.jet_extend_basis() self.assertEqual(ddY[1].basis, dx2.basis) self.assertEqual(ddY[1].coeff, dx2.coeff)
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 calc_eqlbr(self, parameter_values=None, ttheta_guess=None, etype='one_ep', display=False, **kwargs): """In the simplest case(RT1 and 2) only one of the equilibrium points of a system is used for linearization and other researches. Such a equilibrium point is calculated by minimizing the target function for a certain input variable. In other case(NT) all of the equilibrium points of a system are needed, which can be calculated by using Slover in Sympy to solve the differential equations for certain input values. :param: uu: certain input value defined by user :param: parameter_values: unknown system parameters in list.(Default: all parameters are known) :param: ttheta_guess: initial values for minimizing the target function.(Default: 0) :param: etype: 'one_ep': one equilibrium point, 'all_ep': all of the equilibrium points. :param: display: display all information of the result of the 'fmin' fucntion :return: equilibrium point(s) in list """ if parameter_values is None: parameter_values = [] if kwargs.get('uu') is None: # if the system doesn't have input assert self.tau is None uu = [] all_vars = st.row_stack(self.tt) uu_para = [] else: uu = kwargs.get('uu') all_vars = st.row_stack(self.tt, self.tau) uu_para = list(zip(self.tau, uu)) class Type_equilibrium(Enum): one_ep = auto() all_ep = auto() if etype == Type_equilibrium.one_ep.name: # set all of the derivatives of the system states to zero state_eqns = self.eqns.subz0(self.ttd, self.ttdd) # target function for minimizing state_eqns_func = st.expr_to_func(all_vars, state_eqns.subs(parameter_values)) if ttheta_guess is None: ttheta_guess = st.to_np(self.tt * 0) def target(ttheta): """target function for the certain global input values uu """ all_values = np.r_[ttheta, uu] # combine arrays rhs = state_eqns_func(*all_values) return np.sum(rhs ** 2) self.eqlbr = fmin(target, ttheta_guess, disp=display) elif etype == Type_equilibrium.all_ep.name: state_eqns = self.eqns.subz0(self.ttd, self.ttdd) all_vars_value = uu_para + parameter_values self.eqlbr = sp.solve(state_eqns.subs(all_vars_value), self.tt)
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 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 regular_completion(matrix): m,n = matrix.shape r = matrix.rank() #~ IPS() assert m!=n, "There is no regular completion of a square matrix." if m<n: assert r==m, "Matrix does not have full row rank." A, B, V_pi = reshape_matrix_columns(matrix) zeros = sp.zeros(n-m,m) ones = sp.eye(n-m) S = st.col_stack(zeros,ones) completion = S*V_pi.T regular_matrix = st.row_stack(matrix,completion) assert st.rnd_number_rank(regular_matrix)==n, "Regular completion seems to be wrong." elif n<m: assert r==n, "Matrix does not have full column rank." A, B, V_pi = reshape_matrix_columns(matrix.T) zeros = sp.zeros(m-n,n) ones = sp.eye(m-n) S = st.col_stack(zeros,ones) completion = V_pi*S.T regular_matrix = st.col_stack(completion,matrix) assert st.rnd_number_rank(regular_matrix)==m, "Regular completion seems to be wrong." return completion
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_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_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_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 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_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 regular_completion(matrix): m, n = matrix.shape r = matrix.rank() #~ IPS() assert m != n, "There is no regular completion of a square matrix." if m < n: assert r == m, "Matrix does not have full row rank." A, B, V_pi = reshape_matrix_columns(matrix) zeros = sp.zeros(n - m, m) ones = sp.eye(n - m) S = st.col_stack(zeros, ones) completion = S * V_pi.T regular_matrix = st.row_stack(matrix, completion) assert st.rnd_number_rank( regular_matrix) == n, "Regular completion seems to be wrong." elif n < m: assert r == n, "Matrix does not have full column rank." A, B, V_pi = reshape_matrix_columns(matrix.T) zeros = sp.zeros(m - n, n) ones = sp.eye(m - n) S = st.col_stack(zeros, ones) completion = V_pi * S.T regular_matrix = st.col_stack(completion, matrix) assert st.rnd_number_rank( regular_matrix) == m, "Regular completion seems to be wrong." return completion
def calc_state_eq(self, simplify=True): """ reformulate the second order model to a first order statespace model xd = f(x)+g(x)*u """ self.xx = st.row_stack(self.tt, self.ttd) self.x = self.xx # xx is preferred now eq2nd_order = self.solve_for_acc(simplify=simplify) self.state_eq = st.row_stack(self.ttd, eq2nd_order) self.f = self.state_eq.subs(st.zip0(self.tau)) self.g = self.state_eq.jacobian(self.tau) if simplify: self.f.simplify() self.g.simplify()
def calc_coll_part_lin_state_eq(self, simplify=True): """ calc vector fields ff, and gg of collocated linearization. self.ff and self.gg are set. """ self.xx = st.row_stack(self.tt, self.ttd) self.x = self.xx # xx is preferred now nq = len(self.tau) np = len(self.tt) - nq 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 does not have the expected structure: %s" raise NotImplementedError(msg % str(B)) # set the actuated accelearations as new inputs self.aa = self.ttdd[-nq:, :] 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 # setting input and acceleration to 0 C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau)) # eq_passive = -M11inv*C1K1 - M11inv*M12*self.aa self.ff = st.row_stack(self.ttd, -M11inv * C1K1, self.aa * 0) self.gg = st.row_stack(sp.zeros(np + nq, nq), -M11inv * M12, sp.eye(nq)) if simplify: self.ff.simplify() self.gg.simplify()
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 calc_coll_part_lin_state_eq(self, simplify=True): """ calc vector fields ff, and gg of collocated linearization. self.ff and self.gg are set. """ self.xx = st.row_stack(self.tt, self.ttd) self.x = self.xx # xx is preferred now nq = len(self.tau) np = len(self.tt) - nq 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 does not have the expected structure: %s" raise NotImplementedError(msg % str(B)) # set the actuated accelearations as new inputs self.aa = self.ttdd[-nq:, :] 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 # setting input and acceleration to 0 C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau)) #eq_passive = -M11inv*C1K1 - M11inv*M12*self.aa self.ff = st.row_stack(self.ttd, -M11inv*C1K1, self.aa*0) self.gg = st.row_stack(sp.zeros(np + nq, nq), -M11inv*M12, sp.eye(nq)) if simplify: self.ff.simplify() self.gg.simplify()
def append(self, k_form): assert k_form.degree == self.degree, "Degrees of vector forms do not match." if isinstance(k_form, DifferentialForm): rows = k_form.coeff.T else: rows = k_form.coeff self._w.append(k_form) self.coeff = st.row_stack(self.coeff, rows) self.m = self.m + 1
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_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_) omega_dot = omega.dot() omega_1, omega_2 = omega.unpack() omega_1dot = omega_1.dot() omega_2dot = omega_2.dot() Qdot_ = st.row_stack(omega_1dot.coeff.T, omega_2dot.coeff.T) # vector form to compare with omegadot_comp = pc.VectorDifferentialForm(1, XX, coeff=Qdot_) self.assertEqual(omega_dot.coeff, omegadot_comp.coeff)
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 __init__(self, inverted=True, calc_coll_part_lin=True): self.inverted = inverted self.calc_coll_part_lin = calc_coll_part_lin # ----------------------------------------- # Pendel-Wagen System mit hängendem Pendel # ----------------------------------------- pp = st.symb_vector(("varphi", )) qq = st.symb_vector(("q", )) ttheta = st.row_stack(pp, qq) st.make_global(ttheta) params = sp.symbols('m1, m2, l, g, q_r, t, T') st.make_global(params) ex = sp.Matrix([1, 0]) ey = sp.Matrix([0, 1]) # Koordinaten der Schwerpunkte und Gelenke S1 = ex * q # Schwerpunkt Wagen G2 = S1 # Pendel-Gelenk # Schwerpunkt des Pendels (Pendel zeigt für kleine Winkel nach oben) if inverted: S2 = G2 + mt.Rz(varphi) * ey * l else: S2 = G2 + mt.Rz(varphi) * -ey * l # Zeitableitungen der Schwerpunktskoordinaten S1d, S2d = st.col_split(st.time_deriv(st.col_stack(S1, S2), ttheta)) # Energie E_rot = 0 # (Punktmassenmodell) E_trans = (m1 * S1d.T * S1d + m2 * S2d.T * S2d) / 2 E = E_rot + E_trans[0] V = m2 * g * S2[1] # Partiell linearisiertes Model mod = mt.generate_symbolic_model(E, V, ttheta, [0, sp.Symbol("u")]) mod.calc_state_eq() mod.calc_coll_part_lin_state_eq() self.mod = mod
def test_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_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 calc_eqlbr_rt1(mod, uu, sys_paras, ttheta_guess=None, display=False, debug=False): """ In the simplest case, only one of the equilibrium points of a nonlinear system is used for linearization.Such a equilibrium point is calculated by minimizing the target function for a certain input variable. :param mod: symbolic_model :param uu: system inputs :param sys_paras: system parameters :param ttheta_guess: initial guess of the equilibrium points :param display: Set to True to print convergence messages. :param debug: output control for debugging in unittest(False:normal output,True: output local variables and normal output) :return: Parameter that minimizes function """ # set all of the derivatives of the system states to zero stat_eqns = mod.eqns.subz0(mod.ttd, mod.ttdd) all_vars = st.row_stack(mod.tt, mod.uu) # target function for minimizing mod.stat_eqns_func = st.expr_to_func(all_vars, stat_eqns.subs(sys_paras)) if ttheta_guess is None: ttheta_guess = st.to_np(mod.tt * 0) def target(ttheta): """target function for the certain global input values uu """ all_vars = np.r_[ttheta, uu] # combine arrays rhs = mod.stat_eqns_func(*all_vars) return np.sum(rhs**2) res = fmin(target, ttheta_guess, disp=display) if debug: C = ipydex.Container(fetch_locals=True) return C, res return res
def test_vector_form_get_coeff_from_idcs(self): x1, x2, x3 = xx = st.symb_vector('x1:4') 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], [-tan(x1), 0, x3]]) Q_ = st.col_stack(Q, sp.zeros(2, 6)) w = pc.VectorDifferentialForm(1, XX, coeff=Q_) w_0 = w.get_coeff_from_idcs(0) Q_0 = Q_.col(0) self.assertEqual(w_0, Q_0) w_1 = w.get_coeff_from_idcs(1) Q_1 = Q_.col(1) self.assertEqual(w_1, Q_1)
def stack_to_vector_form(*arg): """ This function stacks k-forms to a vector k-form. """ # TODO: asserts if len(arg) == 1: return arg[0] else: XX = arg[0].basis k = arg[0].degree coeff_matrix = sp.Matrix([]) for i in range(0, len(arg)): coeff_matrix_i = arg[i].coeff.T coeff_matrix = st.row_stack(coeff_matrix, coeff_matrix_i) return VectorDifferentialForm(k, XX, coeff=coeff_matrix)
def test_vector_k_form(self): x1, x2, x3 = xx = st.symb_vector('x1:4') 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], [-tan(x1), 0, x3]]) Q_ = st.col_stack(Q, sp.zeros(2, 6)) w1 = pc.DifferentialForm(1, XX, coeff=Q_[0, :]) w2 = pc.DifferentialForm(1, XX, coeff=Q_[1, :]) w = pc.VectorDifferentialForm(1, XX, coeff=Q_) w1_tilde = w.get_differential_form(0) self.assertEqual(w1.coeff, w1_tilde.coeff) w2_tilde = w.get_differential_form(1) self.assertEqual(w2.coeff, w2_tilde.coeff)
def test_vector_form_subs(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], [C, 0, x3]]) Q_ = st.col_stack(Q, sp.zeros(2, 6)) B = sp.Matrix([[x3 / sin(x1), 1, 0], [-tan(x1), 0, x3]]) B_ = st.col_stack(B, sp.zeros(2, 6)) # vector 1-forms omega = pc.VectorDifferentialForm(1, XX, coeff=Q_).subs(C, -tan(x1)) self.assertEqual(B_, omega.coeff)
def test_stack_to_vector_form(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], [-tan(x1), 0, x3]]) Q_ = st.col_stack(Q, sp.zeros(2, 6)) # 1-forms w1 = pc.DifferentialForm(1, XX, coeff=Q_[0, :]) w2 = pc.DifferentialForm(1, XX, coeff=Q_[1, :]) w_stacked = pc.stack_to_vector_form(w1, w2) # vector 1-form w = pc.VectorDifferentialForm(1, XX, coeff=Q_) self.assertEqual(w.coeff, w_stacked.coeff)
def test_left_mul_by_3(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)) M3 = sp.Matrix([[1, 0], [-C * s**2, 1]]) # vector 1-forms w = pc.VectorDifferentialForm(1, XX, coeff=Q_) with self.assertRaises(Exception) as cm: # raises NotImplemented but this might change t = w.left_mul_by(M3, s, additional_symbols=[C])
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_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_difference_vector_forms(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) A = sp.Matrix([[x3 / sin(x1), 1, 0], [1, 0, x3]]) A_ = st.col_stack(A, sp.zeros(2, 6)) B = sp.Matrix([[x3 / cos(x1), 0, 1], [-tan(x1), 0, x3]]) B_ = st.col_stack(B, sp.zeros(2, 6)) # vector 1-forms omega_a = pc.VectorDifferentialForm(1, XX, coeff=A_) omega_b = pc.VectorDifferentialForm(1, XX, coeff=B_) omega_c = omega_a - omega_b # vector form to compare with Q_ = A_ - B_ omega_comp = pc.VectorDifferentialForm(1, XX, coeff=Q_) self.assertEqual(omega_c.coeff, omega_comp.coeff)
def test_minors_funcs(self): s = sp.Symbol("s") A = sp.Matrix([[-2, 0, 1], [3, 2, 0], [2, 0, -1]]) C = sp.Matrix([0, 1, 0]).T AC = st.row_stack(s * sp.eye(3) - A, C) M1 = st.col_minor(AC.T, 0, 1, 2) M2 = st.col_minor(AC.T, 0, 1, 3) M3 = st.col_minor(AC.T, 0, 2, 3) M4 = st.col_minor(AC.T, 1, 2, 3) all_row_minors = st.all_k_minors(AC, k=3) self.assertEqual(all_row_minors, [M1, M2, M3, M4]) all_row_minors_idcs = st.all_k_minors(AC, k=3, return_indices=True) eres = [(((0, 1, 2), (0, 1, 2)), s**3 + s**2 - 6 * s), (((0, 1, 3), (0, 1, 2)), 3), (((0, 2, 3), (0, 1, 2)), -s**2 - 3 * s), (((1, 2, 3), (0, 1, 2)), 3 * s + 3)] self.assertEqual(all_row_minors_idcs, eres)
def unimod_inv(M, s=None, t=None, time_dep_symbs=[], simplify_nsm=True, max_deg=None): """ Assumes that M(s) is an unimodular polynomial matrix and calculates its inverse which is again unimodular :param M: Matrix to be inverted :param s: Derivative Symbol :param time_dep_symbs: sequence of time dependent symbols :param max_deg: maximum polynomial degree w.r.t. s of the ansatz :return: Minv """ assert isinstance(M, sp.MatrixBase) assert M.is_square n = M.shape[0] degree_m = nc_degree(M, s) if max_deg is None: # upper bound according to # Levine 2011, On necessary and sufficient conditions for differential flatness, p. 73 max_deg = (n - 1)*degree_m assert int(max_deg) == max_deg assert max_deg >= 0 C = M*0 free_params = [] for i in range(max_deg+1): prefix = 'c{0}_'.format(i) c_part = st.symbMatrix(n, n, prefix, commutative=False) C += nc_mul(c_part, s**i) free_params.extend(list(c_part)) P = nc_mul(C, M) - sp.eye(n) P2 = right_shift_all(P, s, t, time_dep_symbs).reshape(n*n, 1) deg_P = nc_degree(P2, s) part_eqns = [] for i in range(deg_P + 1): # omit the highest order (because it behaves like in the commutative case) res = P2.diff(s, i).subs(s, 0)#/sp.factorial(i) part_eqns.append(res) eqns = st.row_stack(*part_eqns) # equations for all degrees of s # now non-commutativity is inferring eqns2, st_c_nc = make_all_symbols_commutative(eqns) free_params_c, st_c_nc_free_params = make_all_symbols_commutative(free_params) # find out which of the equations are (in)homogeneous eqns2_0 = eqns2.subs(st.zip0(free_params_c)) assert eqns2_0.atoms() in ({0, -1}, {-1}, set()) inhom_idcs = st.np.where(st.to_np(eqns2_0) != 0)[0] hom_idcs = st.np.where(st.to_np(eqns2_0) == 0)[0] eqns_hom = sp.Matrix(st.np.array(eqns2)[hom_idcs]) eqns_inh = sp.Matrix(st.np.array(eqns2)[inhom_idcs]) assert len(eqns_inh) == n # find a solution for the homogeneous equations # if this is not possible, M was not unimodular Jh = eqns_hom.jacobian(free_params_c).expand() nsm = st.nullspaceMatrix(Jh, simplify=simplify_nsm, sort_rows=True) na = nsm.shape[1] if na < n: msg = 'Could not determine sufficiently large nullspace. '\ 'Either M is not unimodular or the expressions are to complicated.' # TODO: decide which of the two cases occurs, via substitution of # random numbers and singular value decomposition # (or application of st.generic_rank) raise ValueError(msg) # parameterize the inhomogenous equations with the solution of the homogeneous equations # new free parameters: aa = st.symb_vector('_a1:{0}'.format(na+1)) nsm_a = nsm*aa eqns_inh2 = eqns_inh.subs(lzip(free_params_c, nsm_a)) # now solve the remaining equations # solve the linear system Jinh = eqns_inh2.jacobian(aa) rhs_inh = -eqns_inh2.subs(st.zip0(aa)) assert rhs_inh == sp.ones(n, 1) sol_vect = Jinh.solve(rhs_inh) sol = lzip(aa, sol_vect) # get the values for all free_params (now they are not free anymore) free_params_sol_c = nsm_a.subs(sol) # replace the commutative symbols with the original non_commutative symbols (of M) free_params_sol = free_params_sol_c.subs(st_c_nc) Minv = C.subs(lzip(free_params, free_params_sol)) return Minv
def unimod_inv(M, s=None, t=None, time_dep_symbs=[], simplify_nsm=True, max_deg=None): """ Assumes that M(s) is an unimodular polynomial matrix and calculates its inverse which is again unimodular :param M: Matrix to be inverted :param s: Derivative Symbol :param time_dep_symbs: sequence of time dependent symbols :param max_deg: maximum polynomial degree w.r.t. s of the ansatz :return: Minv """ assert isinstance(M, sp.MatrixBase) assert M.is_square n = M.shape[0] degree_m = nc_degree(M, s) if max_deg is None: # upper bound according to # Levine 2011, On necessary and sufficient conditions for differential flatness, p. 73 max_deg = (n - 1) * degree_m assert int(max_deg) == max_deg assert max_deg >= 0 C = M * 0 free_params = [] for i in range(max_deg + 1): prefix = 'c{0}_'.format(i) c_part = st.symbMatrix(n, n, prefix, commutative=False) C += nc_mul(c_part, s**i) free_params.extend(list(c_part)) P = nc_mul(C, M) - sp.eye(n) P2 = right_shift_all(P, s, t, time_dep_symbs).reshape(n * n, 1) deg_P = nc_degree(P2, s) part_eqns = [] for i in range(deg_P + 1): # omit the highest order (because it behaves like in the commutative case) res = P2.diff(s, i).subs(s, 0) #/sp.factorial(i) part_eqns.append(res) eqns = st.row_stack(*part_eqns) # equations for all degrees of s # now non-commutativity is inferring eqns2, st_c_nc = make_all_symbols_commutative(eqns) free_params_c, st_c_nc_free_params = make_all_symbols_commutative( free_params) # find out which of the equations are (in)homogeneous eqns2_0 = eqns2.subs(st.zip0(free_params_c)) assert eqns2_0.atoms() in ({0, -1}, {-1}, set()) inhom_idcs = st.np.where(st.to_np(eqns2_0) != 0)[0] hom_idcs = st.np.where(st.to_np(eqns2_0) == 0)[0] eqns_hom = sp.Matrix(st.np.array(eqns2)[hom_idcs]) eqns_inh = sp.Matrix(st.np.array(eqns2)[inhom_idcs]) assert len(eqns_inh) == n # find a solution for the homogeneous equations # if this is not possible, M was not unimodular Jh = eqns_hom.jacobian(free_params_c).expand() nsm = st.nullspaceMatrix(Jh, simplify=simplify_nsm, sort_rows=True) na = nsm.shape[1] if na < n: msg = 'Could not determine sufficiently large nullspace. '\ 'Either M is not unimodular or the expressions are to complicated.' # TODO: decide which of the two cases occurs, via substitution of # random numbers and singular value decomposition # (or application of st.generic_rank) raise ValueError(msg) # parameterize the inhomogenous equations with the solution of the homogeneous equations # new free parameters: aa = st.symb_vector('_a1:{0}'.format(na + 1)) nsm_a = nsm * aa eqns_inh2 = eqns_inh.subs(lzip(free_params_c, nsm_a)) # now solve the remaining equations # solve the linear system Jinh = eqns_inh2.jacobian(aa) rhs_inh = -eqns_inh2.subs(st.zip0(aa)) assert rhs_inh == sp.ones(n, 1) sol_vect = Jinh.solve(rhs_inh) sol = lzip(aa, sol_vect) # get the values for all free_params (now they are not free anymore) free_params_sol_c = nsm_a.subs(sol) # replace the commutative symbols with the original non_commutative symbols (of M) free_params_sol = free_params_sol_c.subs(st_c_nc) Minv = C.subs(lzip(free_params, free_params_sol)) return Minv
# definitorische Gleichungen eq_defin = thetadot - mu #eq_mech = AA*theta + BB*mu + CC*mudot eq_mech = AA*theta + BB*thetadot + CC*mudot # Container um zusätzliche Information über das Beispiel zu speichern data = st.Container() data.P0 = AA data.P1 = BB data.P2 = CC data.eq_mech = eq_mech data.time_dep_symbols = diff_symbols F_eq_orig = F_eq = st.row_stack(eq_defin, eq_mech) sys_name = "mechanik_ph_RTtt_seriell" #~ from ipHelp import IPS #~ IPS() #F_eq = sp.Matrix([ #[ xdot1 - x4 ], #[ xdot2 - x5 ], #[ xdot3 - x6 ], #[ a2*xdot4 + b2*xdot5 + c2*xdot6 + b1*x5 + c1*x6 + b0*x2 + c0*x3 ]])
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 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)