def linearise(model): """ linearise model at equilibrium point zero """ if model.zero_equilibrium: J = model.f.jacobian(model.x) # substitute state varibles with equilibrium point zero model.A = J.subs(st.zip0(model.x)) model.b = model.g.subs(st.zip0(model.x))
def is_zero_equilibrium(model): """ checks model for equilibrium point zero """ # substitution of state variables and their derivatives to zero # substitution of input to zero --> stand-alone system subslist = st.zip0(model.qs) + st.zip0(model.qds) + st.zip0( model.qdds) + st.zip0(model.extforce_list) eq_1 = model.eq_list.subs(subslist) model.zero_equilibrium = all(eq_1)
def is_zero_equilibrium(model): """ checks model for equilibrium point zero """ # substitution of state variables and their derivatives to zero #substitution of input to zero --> stand-alone system subslist = st.zip0(model.qs) + st.zip0(model.qds) + st.zip0( model.qdds) + st.zip0(model.extforce_list) eq_1 = model.eq_list.subs(subslist) model.zero_equilibrium = all(eq_1)
def solve(problem_spec): """ solution of full state feedback :param problem_spec: ProblemSpecification object :return: solution_data: states and output values of the stabilized system """ sys_f_body = msp.System_Property() # instance of the class System_Property sys_f_body.sys_state = problem_spec.xx # state of the system sys_f_body.tau = problem_spec.u # inputs of the system # original nonlinear system functions sys_f_body.n_state_func = problem_spec.rhs(problem_spec.xx, problem_spec.u) # original output functions sys_f_body.n_out_func = problem_spec.output_func(problem_spec.xx, problem_spec.u) sys_f_body.eqlbr = problem_spec.eqrt # equilibrium point # linearize nonlinear system around the chosen equilibrium point sys_f_body.sys_linerazition() tuple_system = (sys_f_body.aa, sys_f_body.bb, sys_f_body.cc, sys_f_body.dd ) # system tuple # call the state_feedback method to stabilize an unstable system observer_res = ofr.full_observer(tuple_system, problem_spec.poles_o, problem_spec.poles_cl, debug=False) # simulation original nonlinear system with controller f = sys_f_body.n_state_func.subs(st.zip0( sys_f_body.tau)) # x_dot = f(x) + g(x) * u g = sys_f_body.n_state_func.jacobian(sys_f_body.tau) # observer simulation function observer_sim = osf.Observer_SimModel(f, g, problem_spec.xx, tuple_system, problem_spec.eqrt, observer_res.observer_gain, observer_res.state_feedback, observer_res.pre_filter, problem_spec.yr) rhs = observer_sim.calc_observer_rhs_func() res = odeint(rhs, problem_spec.xx0, problem_spec.tt) # add equilibrium points to the estimated states from observer res[:, len(problem_spec.xx):] += np.array( [problem_spec.eqrt[i][1] for i in range(len(problem_spec.xx))]) # output function output_function = sp.lambdify(problem_spec.xx, sys_f_body.n_out_func, modules='numpy') yy = output_function(*res[:, 0:4].T) solution_data = SolutionData() solution_data.yy = yy # output of the stabilized system solution_data.res = res # states of the stabilized system solution_data.state_feedback = observer_res.state_feedback # controller gains solution_data.observer_gain = observer_res.observer_gain # observer gains solution_data.pre_filter = observer_res.pre_filter # pre-filter return solution_data
def rhs(xx, uu): """ Right hand side of the equation of motion in nonlinear state space form :param xx: system state :param uu: system input :return: nonlinear state function """ R0 = 6 # reference resistance at the reference temperature in [Ohm] Tr = 303.15 # reference temperature in [K] alpha = 3.93e-3 # temperature coefficient in [1/K] Ta = 293.15 # environment temperature in [K] sigma = 5.67e-8 # Stefan–Boltzmann constant in [W/m**2/k**4] A = 0.0025 # surface area of resistance m ** 2 c = 87 # heat capacity in [J/k] ''' heat capacity is equal to specific heat capacity * mass of resistance specific heat capacity for Cu: 394 [J/kg/K], density of Cu_resistance : 8.86[g/cm**3] volume of Cu_resistance: 0.0025 [m**2] * 0.01 [m] ''' x1 = xx[0] # state: temperature u = uu[0] p1_dot = u / (c * R0 * (1 + alpha * (x1 - Tr))) - (sigma * A * (x1**4 - Ta**4)) / c ff = sp.Matrix([p1_dot]) mod = mt.SymbolicModel() mod.xx = sp.Matrix([x1]) mod.eqns = ff mod.tau = sp.Matrix([u]) mod.f = ff.subs(st.zip0(mod.tau)) mod.g = ff.jacobian(mod.tau) return mod
def test_subz0(self): x1, x2, x3 = xx = st.symb_vector("x1, x2, x3") y1, y2, y3 = yy = st.symb_vector("y1, y2, y3") XX = (x1, x2) a = x1 + 7 * x2 * x3 M1 = sp.Matrix([x2, x1 * x2, x3**2]) M2 = sp.ImmutableDenseMatrix(M1) self.assertEqual(x1.subs(st.zip0(XX)), x1.subz0(XX)) self.assertEqual(a.subs(st.zip0(XX)), a.subz0(XX)) self.assertEqual(M1.subs(st.zip0(XX)), M1.subz0(XX)) self.assertEqual(M2.subs(st.zip0(XX)), M2.subz0(XX)) konst = sp.Matrix([1, 2, 3]) zz = konst + xx + 5 * yy self.assertEqual(zz.subz0(xx, yy), konst)
def solve(problem_spec): """ the design of a linear full observer is based on a linear system. therefore the non-linear system should first be linearized at the beginning :param problem_spec: ProblemSpecification object :return: solution_data: states and output values of the stabilized system """ sys_f_body = msp.System_Property() # instance of the class System_Property sys_f_body.sys_state = problem_spec.xx # state of the system sys_f_body.tau = problem_spec.u # inputs of the system # original nonlinear system functions sys_f_body.n_state_func = problem_spec.rhs(problem_spec.xx, problem_spec.u) # original output functions sys_f_body.n_out_func = problem_spec.output_func(problem_spec.xx, problem_spec.u) sys_f_body.eqlbr = problem_spec.eqrt # equilibrium point # linearize nonlinear system around the chosen equilibrium point sys_f_body.sys_linerazition(parameter_values=None) tuple_system = (sys_f_body.aa, sys_f_body.bb, sys_f_body.cc, sys_f_body.dd ) # system tuple # calculate controller function LQR_res = mlqr.lqr_method(tuple_system, problem_spec.q, problem_spec.r, problem_spec.xx, problem_spec.eqrt, problem_spec.yr, debug=False) # simulation original nonlinear system with controller f = sys_f_body.n_state_func.subs(st.zip0( sys_f_body.tau)) # x_dot = f(x) + g(x) * u g = sys_f_body.n_state_func.jacobian(sys_f_body.tau) rhs = rhs_for_simulation(f, g, problem_spec.xx, LQR_res.input_func) res = odeint(rhs, problem_spec.xx0, problem_spec.tt) output_function = sp.lambdify(problem_spec.xx, sys_f_body.n_out_func, modules='numpy') yy = output_function(*res.T) solution_data = SolutionData() solution_data.res = res # states of system solution_data.pre_filter = LQR_res.pre_filter # pre-filter solution_data.state_feedback = LQR_res.state_feedback # controller gain solution_data.poles = LQR_res.poles_lqr solution_data.yy = yy[0][0] return solution_data
def solve(problem_spec): """ solution of full state feedback :param problem_spec: ProblemSpecification object :return: solution_data: states and output values of the stabilized system, and controller gain """ sys_f_body = msp.System_Property() # instance of the class System_Property sys_f_body.sys_state = problem_spec.xx # state of the system sys_f_body.tau = problem_spec.u # inputs of the system # original nonlinear system functions sys_f_body.n_state_func = problem_spec.rhs(problem_spec.xx, problem_spec.u) # original output functions sys_f_body.n_out_func = problem_spec.output_func(problem_spec.xx, problem_spec.u) sys_f_body.eqlbr = problem_spec.eqrt # equilibrium point # linearize nonlinear system around the chosen equilibrium point sys_f_body.sys_linerazition() tuple_system = (sys_f_body.aa, sys_f_body.bb, sys_f_body.cc, sys_f_body.dd ) # system tuple # calculate controller function # sfb for dataclass StateFeedbackResult sfb = ftf.state_feedback(tuple_system, problem_spec.poles_cl, problem_spec.xx, problem_spec.eqrt, problem_spec.yr, debug=False) # simulation original nonlinear system with controller f = sys_f_body.n_state_func.subs(st.zip0( sys_f_body.tau)) # x_dot = f(x) + g(x) * u g = sys_f_body.n_state_func.jacobian(sys_f_body.tau) rhs = rhs_for_simulation(f, g, problem_spec.xx, sfb.input_func) res = odeint(rhs, problem_spec.xx0, problem_spec.tt) output_function = sp.lambdify(problem_spec.xx, sys_f_body.n_out_func, modules='numpy') yy = output_function(*res.T) solution_data = SolutionData() solution_data.res = res # states of system solution_data.pre_filter = sfb.pre_filter # pre-filter solution_data.ff = sfb.state_feedback # controller gain solution_data.input_fun = sfb.input_func # controller function solution_data.yy = yy[0][0] # system output return solution_data
def solve_for_acc(self, simplify=False): self.calc_mass_matrix() if simplify: self.M.simplify() M = self.M rhs = self.eqns.subs(st.zip0(self.ttdd)) * -1 d = M.berkowitz_det() adj = M.adjugate() if simplify: d = d.simplify() adj.simplify() Minv = adj/d res = Minv*rhs return res
def solve_for_acc(self, simplify=False): self.calc_mass_matrix() if simplify: self.M.simplify() M = self.M rhs = self.eqns.subs(st.zip0(self.ttdd)) * -1 d = M.berkowitz_det() adj = M.adjugate() if simplify: d = d.simplify() adj.simplify() Minv = adj / d res = Minv * rhs return res
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 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_zip0(self): aa = sp.symbols('a1:5') bb = sp.symbols('b1:4') xx = sp.symbols('x1:3') s1 = sum(aa) self.assertEqual(s1.subs(st.zip0(aa)), 0) self.assertEqual(s1.subs(st.zip0(aa, arg=3.5)), 3.5 * len(aa)) self.assertEqual(s1.subs(st.zip0(aa, arg=xx[0])), xx[0] * len(aa)) s2 = s1 + sum(bb) + sum(xx) self.assertEqual(s1.subs(st.zip0(aa, bb, xx)), 0) t2 = s2.subs(st.zip0(aa, bb, xx, arg=-2.1)) self.assertEqual(t2, -2.1 * len(aa + bb + xx)) ff = sp.Function('f1')(*xx), sp.Function('f2')(*xx) s3 = ff[0] + ff[1] + 10 # noinspection PyUnresolvedReferences self.assertEqual(s3.subs(st.zip0(ff)), 10)
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)
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') parameter_values = list(dict(m0=2.0, m1=1.0, l1=2.0, g=9.81).items()) 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] # noinspection PyUnusedLocal with self.assertRaises(ValueError) as cm: # wrong length of external forces mt.generate_symbolic_model(T, V, ttheta, [F1]) # noinspection PyUnusedLocal 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) # noinspection PyUnusedLocal rest = mod.eqns.subs(st.zip0(mod.ttdd)) # noinspection PyUnusedLocal 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) # ensure that recalculation is carried out only when we want it tmp_f, tmp_g = mod.f, mod.g mod.f, mod.g = "foo", "bar" mod.calc_state_eq(simplify=True) self.assertEqual(mod.f, "foo") mod.calc_state_eq(simplify=True, force_recalculation=True) self.assertEqual((mod.f, mod.g), (tmp_f, tmp_g)) 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) A, b = mod.calc_jac_lin(base="coll_part_lin", parameter_values=parameter_values) Ae = sp.Matrix([[0, 0, 1, 0], [0, 0, 0, 1], [-4.905, 0, 0, 0], [0, 0, 0, 0]]) be = sp.Matrix([0, 0, -0.5, 1]) self.assertEqual(A, Ae) self.assertEqual(b, be) A, b = mod.calc_jac_lin(base="force_input", parameter_values=parameter_values) Ae = sp.Matrix([[0, 0, 1, 0], [0, 0, 0, 1], [-7.3575, 0, 0, 0], [4.905, 0, 0, 0]]) be = sp.Matrix([0, 0, -.25, 0.5]) self.assertEqual(A, Ae) self.assertEqual(b, be)
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 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
def solve_bezout_eq(p1, p2, var): """ solving the bezout equation c1*p1 + c2*p2 = 1 for monovariate polynomials p1, p2 by ansatz-polynomials and equating coefficients """ g1 = st.poly_degree(p1, var) g2 = st.poly_degree(p2, var) if (not sp.gcd(p1, p2) == 1) and (not p1*p2==0): # pass errmsg = "p1, p2 need to be coprime "\ "(condition for Bezout identity to be solveble).\n"\ "p1 = {p1}\n"\ "p2 = {p2}" raise ValueError(errmsg.format(p1=p1, p2=p2)) if p1 == p2 == 0: raise ValueError("invalid: p1==p2==0") if p1 == 0 and g2 > 0: raise ValueError("invalid: p1==0 and not p2==const ") if p2 == 0 and g1 > 0: raise ValueError("invalid: p2==0 and not p1==const ") # Note: degree(0) = -sp.oo = -inf k1 = g2 - 1 k2 = g1 - 1 if k1<0 and k2 <0: if p1 == 0: k2 = 0 else: k1 = 0 if k1 == -sp.oo: k1 = -1 if k2 == -sp.oo: k2 = -1 cc1 = sp.symbols("c1_0:%i" % (k1 + 1)) cc2 = sp.symbols("c2_0:%i" % (k2 + 1)) c1 = coeff_list_to_poly(cc1, var) c2 = coeff_list_to_poly(cc2, var) # Bezout equation: eq = c1*p1 + c2*p2 - 1 # solve it w.r.t. the unknown coeffs sol = sp.solve(eq, cc1+cc2, dict=True) if len(sol) == 0: errmsg = "No solution found.\n"\ "p1 = {p1}\n"\ "p2 = {p2}" raise ValueError(errmsg.format(p1=p1, p2=p2)) sol = sol[0] sol_symbols = st.atoms(sp.Matrix(sol.values()), sp.Symbol) # there might be some superflous coeffs free_c_symbols = set(cc1+cc2).intersection(sol_symbols) if free_c_symbols: # set them to zero fcs0 = st.zip0(free_c_symbols) keys, values = zip(*sol.items()) new_values = [v.subs(fcs0) for v in values] sol = dict(zip(keys, new_values)+fcs0) return c1.subs(sol), c2.subs(sol)
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)
def test_create_simfunction(self): x1, x2, x3, x4 = xx = sp.Matrix(sp.symbols("x1, x2, x3, x4")) u1, u2 = uu = sp.Matrix(sp.symbols("u1, u2")) # inputs p1, p2, p3, p4 = pp = sp.Matrix( sp.symbols("p1, p2, p3, p4")) # parameter t = sp.Symbol('t') A = A0 = sp.randMatrix(len(xx), len(xx), -10, 10, seed=704) B = B0 = sp.randMatrix(len(xx), len(uu), -10, 10, seed=705) v1 = A[0, 0] A[0, 0] = p1 v2 = A[2, -1] A[2, -1] = p2 v3 = B[3, 0] B[3, 0] = p3 v4 = B[2, 1] B[2, 1] = p4 par_vals = lzip(pp, [v1, v2, v3, v4]) f = A * xx G = B fxu = (f + G * uu).subs(par_vals) # some random initial values x0 = st.to_np(sp.randMatrix(len(xx), 1, -10, 10, seed=706)).squeeze() # Test handling of unsubstituted parameters mod = st.SimulationModel(f, G, xx, model_parameters=par_vals[1:]) with self.assertRaises(ValueError) as cm: rhs0 = mod.create_simfunction() self.assertTrue("unexpected symbols" in cm.exception.args[0]) # create the model and the rhs-function mod = st.SimulationModel(f, G, xx, par_vals) rhs0 = mod.create_simfunction() self.assertFalse(mod.compiler_called) self.assertFalse(mod.use_sp2c) res0_1 = rhs0(x0, 0) dres0_1 = st.to_np(fxu.subs(lzip(xx, x0) + st.zip0(uu))).squeeze() bin_res01 = np.isclose(res0_1, dres0_1) # binary array self.assertTrue(np.all(bin_res01)) # difference should be [0, 0, ..., 0] self.assertFalse(np.any(rhs0(x0, 0) - rhs0(x0, 3.7))) # simulate tt = np.linspace(0, 0.5, 100) # simulation should be short due to instability res1 = sc.integrate.odeint(rhs0, x0, tt) # create and try sympy_to_c bridge (currently only works on linux # and if sympy_to_c is installed (e.g. with `pip install sympy_to_c`)) # until it is not available for windows we do not want it as a requirement # see also https://stackoverflow.com/a/10572833/333403 try: import sympy_to_c except ImportError: # noinspection PyUnusedLocal sympy_to_c = None sp2c_available = False else: sp2c_available = True if sp2c_available: rhs0_c = mod.create_simfunction(use_sp2c=True) self.assertTrue(mod.compiler_called) res1_c = sc.integrate.odeint(rhs0_c, x0, tt) self.assertTrue(np.all(np.isclose(res1_c, res1))) mod.compiler_called = None rhs0_c = mod.create_simfunction(use_sp2c=True) self.assertTrue(mod.compiler_called is None) # proof calculation # x(t) = x0*exp(A*t) Anum = st.to_np(A.subs(par_vals)) Bnum = st.to_np(G.subs(par_vals)) # noinspection PyUnresolvedReferences xt = [np.dot(sc.linalg.expm(Anum * T), x0) for T in tt] xt = np.array(xt) # test whether numeric results are close within given tolerance bin_res1 = np.isclose(res1, xt, rtol=2e-5) # binary array self.assertTrue(np.all(bin_res1)) # test handling of parameter free models: mod2 = st.SimulationModel(Anum * xx, Bnum, xx) rhs2 = mod2.create_simfunction() res2 = sc.integrate.odeint(rhs2, x0, tt) self.assertTrue(np.allclose(res1, res2)) # test input functions des_input = st.piece_wise((0, t <= 1), (t, t < 2), (0.5, t < 3), (1, True)) des_input_func_scalar = st.expr_to_func(t, des_input) des_input_func_vec = st.expr_to_func(t, sp.Matrix([des_input, des_input])) # noinspection PyUnusedLocal with self.assertRaises(TypeError) as cm: mod2.create_simfunction(input_function=des_input_func_scalar) rhs3 = mod2.create_simfunction(input_function=des_input_func_vec) # noinspection PyUnusedLocal res3_0 = rhs3(x0, 0) rhs4 = mod2.create_simfunction(input_function=des_input_func_vec, time_direction=-1) res4_0 = rhs4(x0, 0) self.assertTrue(np.allclose(res3_0, np.array([119., -18., -36., -51.]))) self.assertTrue(np.allclose(res4_0, -res3_0))
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()