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 smooth_time_scaling(Tend, N, phase_fraction=.5): """ :param Tend: :param N: :param phase_fraction: fraction of Tend for smooth initial and end phase """ T0 = 0 T1 = Tend * phase_fraction y0 = 0 y1 = 1 # for initial phase poly1 = st.condition_poly(t, (T0, y0, 0, 0), (T1, y1, 0, 0)) # for end phase poly2 = poly1.subs(t, Tend - t) # there should be a phase in the middle with constant slope deriv_transition = st.piece_wise((y0, t < T0), (poly1, t < T1), (y1, t < Tend - T1), (poly2, t < Tend), (y0, True)) scaling = sp.integrate(deriv_transition, (t, T0, Tend)) time_transition = sp.integrate(deriv_transition * N / scaling, t) # deriv_transition_func = st.expr_to_func(t, full_transition) time_transition_func = st.expr_to_func(t, time_transition) deriv_func = st.expr_to_func(t, deriv_transition * N / scaling) deriv_func2 = st.expr_to_func(t, deriv_transition.diff(t) * N / scaling) C = ipydex.Container(fetch_locals=True) return C
def solve(problem_spec): t = sp.Symbol('t') planer_p2 = tp.Trajectory_Planning(problem_spec.YA_p2, problem_spec.YB_p2, problem_spec.t0, problem_spec.tf, problem_spec.tt) mod = problem_spec.rhs(problem_spec.ttheta, problem_spec.tthetad, problem_spec.u_F) planer_p2.mod = mod planer_p2.yy = problem_spec.output_func(problem_spec.ttheta, problem_spec.u_F) planer_p2.ff = mod.f # xd = f(x) + g(x)*u planer_p2.gg = mod.g yy = planer_p2.cal_li_derivative() # lie derivatives of the flat output ploy_p2 = planer_p2.calc_trajectory() # planned trajectory of CuZn-ball p2_func = st.expr_to_func(t, ploy_p2[0]) # trajectory to function # find trajectory of Fe-ball p1_p2 = planer_p2.ff[3].subs(problem_spec.ttheta[1], ploy_p2[0]) func_p1 = p1_p2 - ploy_p2[2] ploy_p1 = sp.solve(func_p1, problem_spec.ttheta[0]) p1_func = st.expr_to_func(t, ploy_p1[0]) yy_4 = yy[4].subs([(problem_spec.ttheta[0], ploy_p1[0]), (problem_spec.ttheta[1], ploy_p2[0])]) in_output_func = yy_4 - ploy_p2[4] # input force trajectory input_f_tra = sp.solve(in_output_func, problem_spec.u_F) f_func = st.expr_to_func(t, input_f_tra) # input current trajectory f_c_func = problem_spec.force_current_function(problem_spec.ttheta, problem_spec.u_i) c_tra_func = (input_f_tra[0] - f_c_func).subs([(problem_spec.ttheta[0], ploy_p1[0])]) input_c_tra = sp.solve(c_tra_func, problem_spec.u_i) current_func = st.expr_to_func(t, input_c_tra[1]) # tracking controller tracking_controller = tp.Tracking_controller(yy, mod.xx, problem_spec.u_F, problem_spec.pol, ploy_p2) control_law = tracking_controller.error_dynamics()[0] # control law # simulate the system with control law rhs = rhs_for_simulation(planer_p2.ff, planer_p2.gg, mod.xx, control_law) # original initial values : [0.0008, 0.004, 0, 0] res = odeint(rhs, problem_spec.xx0, problem_spec.tt2) solution_data = SolutionData() solution_data.res = res # output values of the system solution_data.ploy_p1 = p1_func # desired full transition of p1 solution_data.ploy_p2 = p2_func # desired full transition of p2 solution_data.f_func = f_func # required magnet force input solution_data.current_func = current_func # required current input solution_data.coefficients = tracking_controller.coefficient # coefficients of error dynamics solution_data.control_law = control_law # control law function return solution_data
def solve(problem_spec): s, t, T = sp.symbols("s, t, T") transfer_func = problem_spec.transfer_func() z_func, n_func = transfer_func.expand().as_numer_denom( ) # separate numerator and denominator z_coeffs = [float(c) for c in st.coeffs(z_func, s)] # coefficients of numerator n_coeffs = [float(c) for c in st.coeffs(n_func, s)] # coefficients of denominator b_0 = z_func.coeff(s, 0) # Boundary conditions for q and its derivative q_a = [problem_spec.YA[0] / b_0, 0, 0, 0] q_e = [problem_spec.YB[0] / b_0, 0, 0, 0] # generate trajectory of q(t) planer = tp.Trajectory_Planning(q_a, q_e, problem_spec.t0, problem_spec.tf, problem_spec.tt) planer.dem = n_func planer.num = z_func q_poly = planer.calc_trajectory() # trajectory of input and output u_poly, y_poly = planer.num_den_laplace(q_poly[0]) q_func = st.expr_to_func(t, q_poly[0]) u_func = st.expr_to_func(t, u_poly) # desired input trajectory function y_func = st.expr_to_func(t, y_poly) # desired output trajectory function # tracking controller # numerator and denominator of controller cd_res = cd.coprime_decomposition(z_func, n_func, problem_spec.pol) u1, u2, fb = inputs('u1, u2, fb') # external force and feedback SUM1 = Blockfnc(u1 - fb) Controller = TFBlock(cd_res.f_func / cd_res.h_func, SUM1.Y) SUM2 = Blockfnc(u2 + Controller.Y) System = TFBlock(z_func / n_func, SUM2.Y) loop(System.Y, fb) t1, states = blocksimulation(6, { u1: y_func, u2: u_func }) # simulate 10 seconds t1 = t1.flatten() bo = compute_block_ouptputs(states) solution_data = SolutionData() solution_data.u = u_func solution_data.q = q_func solution_data.yy = bo[System] solution_data.y_func = y_func solution_data.tt = t1 return solution_data
def test_conversion_all_funcs(self): x1, x2, x3 = xx = st.symb_vector("x1:4") u1, u2 = uu = st.symb_vector("u1:3") xxuusum = sum(xx) + sum(uu) arg = sp.tanh(xxuusum) # limit the argument to (-1, 1)*0.99 # see mpc.CassadiPrinter.__init__ for exlanation sp_func_names = mpc.CassadiPrinter().cs_func_keys.keys() blacklist = ["atan2", ] flist = [getattr(sp, name) for name in sp_func_names if name not in blacklist] # create the test_matrix expr_list = [] for func in flist: if func is sp.acosh: # only defined for values > 1 expr_list.append(func(1/arg)) else: expr_list.append(func(arg)) expr_sp = sp.Matrix(expr_list + [arg, xxuusum]) func_cs = mpc.create_casadi_func(expr_sp, xx, uu) xxuu = list(xx) + list(uu) func_np = st.expr_to_func(xxuu, expr_sp) argvals = np.random.rand(len(xxuu)) argvals_cs = (argvals[:len(xx)], argvals[len(xx):]) res_np = func_np(*argvals) res_cs = func_cs(*argvals_cs).full().squeeze() self.assertTrue(np.allclose(res_np, res_cs))
def solve(problem_spec): s, t, T = sp.symbols("s, t, T") planer = tp.Trajectory_Planning(problem_spec.YA, problem_spec.YB, problem_spec.t0, problem_spec.tf, problem_spec.tt) mod = problem_spec.rhs(problem_spec.xx, problem_spec.uu) planer.mod = mod planer.yy = problem_spec.output_func(problem_spec.xx, problem_spec.uu) planer.ff = mod.f # xd = f(x) + g(x)*u planer.gg = mod.g yy = planer.cal_li_derivative() ploy_tem = planer.calc_trajectory() tem_func = st.expr_to_func(t, ploy_tem[0]) # tracking controller tracking_controller = tp.Tracking_controller(yy, mod.xx, problem_spec.uu, problem_spec.pol, ploy_tem) control_law = tracking_controller.error_dynamics()[0] # control law rhs = rhs_for_simulation(planer.ff, planer.gg, mod.xx, control_law) res = odeint(rhs, problem_spec.xx0, problem_spec.tt2) solution_data = SolutionData() solution_data.res = res solution_data.p2_func = tem_func return solution_data
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_penalty_expression(self): x1, = xx = st.symb_vector('x1:2') pe = st.penalty_expression(x1, -2, 2) pefnc = st.expr_to_func(x1, pe) eps = 1e-2 self.assertTrue(pefnc(1) < eps) self.assertTrue(pefnc(5) - 25 < eps) self.assertTrue(pefnc(-3) - 9 < eps)
def test_conversion1(self): x1, x2, x3 = xx = st.symb_vector("x1:4") u1, u2 = uu = st.symb_vector("u1:3") expr_sp = sp.Matrix([x1 + x2 + x3, sp.sin(x1)*x2**x3, 1.23, 0, u1*sp.exp(u2)]) func_cs = mpc.create_casadi_func(expr_sp, xx, uu) xxuu = list(xx) + list(uu) func_np = st.expr_to_func(xxuu, expr_sp) argvals = np.random.rand(len(xxuu)) argvals_cs = (argvals[:len(xx)], argvals[len(xx):]) res_np = func_np(*argvals) res_cs = func_cs(*argvals_cs).full().squeeze() self.assertTrue(np.allclose(res_np, res_cs))
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 add_element(self, points, init_fun, update_fun, **kwargs): """ Add a visualiser element :param points: 2x? SymPy matrix or list of 2x1 SymPy vectors describing the defining points as symbolic expressions w.r.t the visualisers free variables :param init_fun: callable with args (matplotlib axes, 2x? numpy array of points, dict of kwargs) that returns a list of matplotlib drawables. Will get called to create all drawables needed by this element. :param update_fun: callable with args (matplotlib axes, list of drawables, 2x? numpy array of points, dict of kwargs) that returns a list of matplotlib drawables. Will get called every time the plot needs to be updated. :param kwargs: arbitrary keyword arguments that get passed to init_fun and update_fun """ if not isinstance(points, sp.Matrix): if isinstance(points, list): points = st.col_stack(*points) else: raise Exception("'points' must be a SymPy matrix or a list of column vectors") points_fun = st.expr_to_func(self.variables, points, keep_shape=True) self.elements.append(VisualiserElement(points_fun, init_fun, update_fun, kwargs))
def test_conversion2(self): x1, x2, x3 = xx = st.symb_vector("x1:4") u1, u2 = uu = st.symb_vector("u1:3") lmd1, lmd2 = llmd = st.symb_vector("lmd1:3") xxuullmd = list(xx) + list(uu) + list(llmd) expr_sp = sp.Matrix([x1 + x2 + x3, sp.sin(x1)*x2**x3, 1.23, 0, u1*sp.exp(u2), x1*lmd1 + lmd2**4]) func_cs = mpc.create_casadi_func(expr_sp, xxuullmd) func_np = st.expr_to_func(xxuullmd, expr_sp) argvals = np.random.rand(len(xxuullmd)) # unpack the array for lambdified function res_np = func_np(*argvals) # pass the whole array for casadi function res_cs = func_cs(argvals).full().squeeze() self.assertTrue(np.allclose(res_np, res_cs))
def test_num_trajectory_compatibility_test(self): x1, x2, x3, x4 = xx = sp.Matrix(sp.symbols("x1, x2, x3, x4")) u1, u2 = uu = sp.Matrix(sp.symbols("u1, u2")) # inputs t = sp.Symbol('t') # we want to create a random but stable matrix np.random.seed(2805) diag = np.diag(np.random.random(len(xx)) * -10) T = sp.randMatrix(len(xx), len(xx), -10, 10, seed=704) Tinv = T.inv() A = Tinv * diag * T B = B0 = sp.randMatrix(len(xx), len(uu), -10, 10, seed=705) x0 = st.to_np(sp.randMatrix(len(xx), 1, -10, 10, seed=706)).squeeze() tt = np.linspace(0, 5, 2000) des_input = st.piece_wise((2 - t, t <= 1), (t, t < 2), (2 * t - 2, t < 3), (4, True)) des_input_func_vec = st.expr_to_func(t, sp.Matrix([des_input, des_input])) mod2 = st.SimulationModel(A * xx, B, xx) rhs3 = mod2.create_simfunction(input_function=des_input_func_vec) XX = sc.integrate.odeint(rhs3, x0, tt) UU = des_input_func_vec(tt) res1 = mod2.num_trajectory_compatibility_test(tt, XX, UU) self.assertTrue(res1) # slightly different input signal -> other results res2 = mod2.num_trajectory_compatibility_test(tt, XX, UU * 1.1) self.assertFalse(res2)
def add_graph(self, content, subplot_pos=111, ax_kwargs=None, plot_kwargs=None, ax=None): """ Add a subplot containing an animated graph of some system variables. :param content: Can be - symbolic: SymPy expression, list of SymPy expressions or SymPy matrix, of some system variables or a combination of them - numeric: NumPy array of values to be plotted over time, rows are sample times, columns are separate plot lines. The number of rows must match the length of the time vector. :param subplot_pos: subplot position, this simply gets passed on to matplotlib's Figure.add_subplot() so every valid type of specification should work. :param ax_kwargs: keyword arguments to be passed on when creating Axes object :param plot_kwargs: keyword arguments to be passed on when calling Axes.plot function :param ax: optional, existing Axes object to be used for plotting, if omitted one will be created :return: Axes object of the subplot """ # create Axes if necessary if ax is None: if ax_kwargs is None: ax_kwargs = {} ax = self.fig.add_subplot(subplot_pos, **ax_kwargs) ax.grid() assert isinstance(content, np.ndarray) or isinstance(content, sp.Expr) or isinstance(content, sp.Matrix)\ or isinstance(content, list) if isinstance(content, np.ndarray): assert content.ndim == 1 or content.ndim == 2, "Data must be one or two-dimensional" assert content.shape[0] == self.n_sim_frames, "Data must have as many rows as there are entries in 't' vector" # convert all types of symbolic content to a SymPy vector if isinstance(content, sp.Expr): content = sp.Matrix([content]) elif isinstance(content, list): content = sp.Matrix(content) # content is now a SymPy vector or an array of values if isinstance(content, np.ndarray): # We later expect all data to be two dimensional, so a vector must be converted to a ?x1 matrix if content.ndim == 1: data = np.reshape(content, (content.shape[0], 1)) else: data = content else: # content is still symbolic, we need to generate the data vector ourselves # instantiate a function that takes one row of x_sim and returns the values to plot at one time instance expr_fun = st.expr_to_func(self.x_symb, content, keep_shape=True) # allocate memory for the data to plot data = np.zeros((self.n_sim_frames, len(content))) # use the prepared function to fill the plotting data for i in range(data.shape[0]): data[i, :] = expr_fun(*self.x_sim[i, :]).flatten() # expr_fun returns a 2D column vector --> flatten if plot_kwargs is None: plot_kwargs = {} self.axes.append((ax, data, plot_kwargs)) # adding a new subplot invalidates any cached animation we might have self._cached_anim = None return ax
fs = [140*mm*scale, 60*mm*scale] fig1 = pl.figure(2, figsize=fs) pl.plot(tt, w1fnc_v(tt), color=color_list[1], lw=1) pl.plot(tt, bo[DI], color=color_list[0], lw=2) pl.grid() pl.xticks([0, 1, 2, 3]) pl.yticks([0, .5, 1]) pl.axis([0, 2, -.1, 1.3]) poly1 = st.trans_poly(t, 3, (T0, 0, 0, 0, 0), (T1, 1, 0, 0, 0)) pw_poly = st.create_piecewise(t, (T0, T1), (0, poly1, 1)) pw_poly_fnc = st.expr_to_func(t, pw_poly) pl.plot(tt, pw_poly_fnc(tt), color=color_list[3], lw=3) pl.savefig('pid_step2.pdf') ## fs = [140*mm*scale, 60*mm*scale] fig1 = pl.figure(3, figsize=fs) pl.grid() pl.xticks([0, 1, 2, 3])
def gen_leqs_for_acc_llmd(self, parameter_values=None): """ Create a callable function which returns A, bnum of the linear eqn-system A*ww = bnum, where ww := (ttheta_dd, llmnd). :return: None, set self.leqs_acc_lmd_func """ if self.leqs_acc_lmd_func is not None and self.acc_of_lmd_func is not None: return if parameter_values is None: parameter_values = [] ntt = self.ntt nll = self.nll self.generate_constraints_funcs() # also respect those values, which have been passed to the constructor parameter_values = list(self.parameter_values) + list(parameter_values) # we use mod.eqns here because we do not want ydot-vars inside eqns = st.concat_rows(self.mod.eqns.subs(parameter_values), self.constraints_dd) ww = st.concat_rows(self.mod.ttdd, self.mod.llmd) A = eqns.jacobian(ww) b = -eqns.subz0(ww) # rhs of the leqs Ab = st.concat_cols(A, b) fvars = st.concat_rows(self.mod.tt, self.mod.ttd, self.mod.tau) actual_symbs = Ab.atoms(sp.Symbol) expected_symbs = set(fvars) unexpected_symbs = actual_symbs.difference(expected_symbs) if unexpected_symbs: msg = "Equations can only converted to numerical func if all parameters are passed for substitution. " \ "Unexpected symbols: {}".format(unexpected_symbs) raise ValueError(msg) A_fnc = st.expr_to_func(fvars, A, keep_shape=True) b_fnc = st.expr_to_func(fvars, b) nargs = len(fvars) # noinspection PyShadowingNames def leqs_acc_lmd_func(*args): """ Calculate the matrices of the linear equation system for ttheta and llmd. Assume args = (ttheta, theta_d, ttau) :param args: :return: """ assert len(args) == nargs Anum = A_fnc(*args) bnum = b_fnc(*args) # theese arrays can now be passed to a linear equation solver return Anum, bnum self.leqs_acc_lmd_func = leqs_acc_lmd_func def acc_of_lmd_func(*args): """ Calculate ttheta in dependency of args= (yy, ttau) = ((ttheta, ttheta_d, llmd), ttau) :param args: :return: """ ttheta = args[:ntt] ttheta_d = args[ntt:2 * ntt] llmd = args[2 * ntt:2 * ntt + nll] ttau = args[2 * ntt + nll:] args1 = np.concatenate((ttheta, ttheta_d, ttau)) Anum = A_fnc(*args1) A1 = Anum[:ntt, :ntt] A2 = Anum[:ntt, ntt:] b1 = b_fnc(*args1)[:ntt] ttheta_dd_res = np.linalg.solve(A1, b1 - np.dot(A2, llmd)) return ttheta_dd_res self.acc_of_lmd_func = acc_of_lmd_func
def solve(problem_spec): s, t, T = sp.symbols("s, t, T") # transfer function of system transfer_func = problem_spec.transfer_func() z_func, n_func = transfer_func.expand().as_numer_denom() # separate numerator and denominator z_coeffs = [float(c) for c in st.coeffs(z_func, s)] # coefficients of numerator n_coeffs = [float(c) for c in st.coeffs(n_func, s)] # coefficients of denominator b_0 = z_func.coeff(s, 0) # Boundary conditions for q and its derivative q_a = [problem_spec.YA[0] / b_0, 0] q_e = [problem_spec.YB[0] / b_0, 0] # generate trajectory of q(t) planer = tp.Trajectory_Planning(q_a, q_e, problem_spec.t0, problem_spec.tf, problem_spec.tt) planer.dem = n_func planer.num = z_func q_poly = planer.calc_trajectory() # trajectory of input and output u_poly, y_poly = planer.num_den_laplace(q_poly[0]) q_func = st.expr_to_func(t, q_poly[0]) u_func = st.expr_to_func(t, u_poly) # desired input trajectory function y_func = st.expr_to_func(t, y_poly) # desired output trajectory function # tracking controller # numerator and denominator of controller cd_res = cd.coprime_decomposition(z_func, n_func, problem_spec.poles) # open_loop k(s) * P(s) tf_k = (cd_res.f_func * z_func) / (cd_res.h_func * n_func) z_o, n_o = sp.simplify(tf_k).expand().as_numer_denom() # coefficients of controller z_coeffs_c = [float(c) for c in st.coeffs(cd_res.f_func, s)] # coefficients of numerator n_coeffs_c = [float(c) for c in st.coeffs(cd_res.h_func, s)] # coefficients of denominator # coefficients of open loop z_coeffs_o = [float(c) for c in st.coeffs(z_o, s)] n_coeffs_o = [float(c) for c in st.coeffs(n_o, s)] # In order to simulate the closed loop system with the controller, # the system is divided into two subsystems. one of them with the y_ref as input # and the other with u_ref close_loop_1 = control.feedback(control.tf(z_coeffs_o, n_coeffs_o)) close_loop_2 = control.feedback(control.tf(z_coeffs, n_coeffs), control.tf(z_coeffs_c, n_coeffs_c)) # subsystem 1 with y_ref y_1 = control.forced_response(close_loop_1, problem_spec.tt2, y_func(problem_spec.tt2), problem_spec.x0_1) # subsystem 2 with u_ref y_2 = control.forced_response(close_loop_2, problem_spec.tt2, u_func(problem_spec.tt2), problem_spec.x0_2) solution_data = SolutionData() solution_data.u = u_func solution_data.q = q_func solution_data.y_1 = y_1[1] solution_data.y_2 = y_2[1] solution_data.y_func = y_func return solution_data
if 0: fs = [140 * mm * scale, 60 * mm * scale] fig1 = pl.figure(2, figsize=fs) pl.plot(tt, w1fnc_v(tt), color=color_list[1], lw=1) pl.plot(tt, bo[DI], color=color_list[0], lw=2) pl.grid() pl.xticks([0, 1, 2, 3]) pl.yticks([0, .5, 1]) pl.axis([0, 2, -.1, 1.3]) poly1 = st.trans_poly(t, 3, (T0, 0, 0, 0, 0), (T1, 1, 0, 0, 0)) pw_poly = st.create_piecewise(t, (T0, T1), (0, poly1, 1)) pw_poly_fnc = st.expr_to_func(t, pw_poly) pl.plot(tt, pw_poly_fnc(tt), color=color_list[3], lw=3) pl.savefig('pid_step2.pdf') ## fs = [140 * mm * scale, 60 * mm * scale] fig1 = pl.figure(3, figsize=fs) pl.grid() pl.xticks([0, 1, 2, 3]) pl.yticks([0, .5, 1]) poly1 = st.trans_poly(t, 3, (T0, 0, 0, 0, 0), (T1, 1, 0, 0, 0))
def generate_eqns_funcs(self, parameter_values=None): """ Creates two callable functions. The first of the form F_tilde(ww) which *internally* represents the lhs of F(t, yy, yyd) = 0 with ww = (yy, yydot, ttau). Note that the input tau will later be calculated by a controller ttau = k(t, yy). The second: F itself with the signature as above. :return: None, set self.eq_func """ if self.eq_func is not None and self.model_func is not None: return if parameter_values is None: parameter_values = [] # also respect those values, which have been passed to the constructor parameter_values = list(self.parameter_values) + list(parameter_values) # create needed helper function: self.gen_leqs_for_acc_llmd(parameter_values=parameter_values) # avoid dot access in the internal function below ntt, nll, acc_of_lmd_func = self.ntt, self.nll, self.acc_of_lmd_func fvars = st.concat_rows(self.yy, self.yyd, self.mod.tau) # keep self.eqns unchanged (maybe not necessary) eqns = self.eqns.subs(parameter_values) actual_symbs = eqns.atoms(sp.Symbol) expected_symbs = set(fvars) unexpected_symbs = actual_symbs.difference(expected_symbs) if unexpected_symbs: msg = "Equations can only be converted to numerical func if all parameters are passed for substitution. " \ "Unexpected symbols: {}".format(unexpected_symbs) raise ValueError(msg) # full equations in classical formulation (currently not needed internally) self.eq_func = st.expr_to_func(fvars, eqns) # only the ode part self.deq_func = st.expr_to_func(fvars, eqns[:2 * self.ntt, :]) def model_func(t, yy, yyd): """ This function is intended to be passed to a DAE solver like IDA. The model consists of two coupled parts: ODE part F_ode(yy, yydot)=0 and algebraic C(yy)=0 part. Problem is, that for mechanical systems with constraints we have differential index=3, i.e. C and C_dot not depend on llmd. C_ddot can be formulated to depend on llmd (if F_ode is plugged in). Idea: instead fo returning just C we return C**2 + C_dot**2 + C_ddot**2 :param t: :param yy: :param yyd: :return: F(t, yy, yyd) (should be 0 with shape (2*ntt + nll,)) """ # to use a controller, this needs to be more sophisticated external_forces = self.input_func(t) args = np.concatenate((yy, yyd, external_forces)) ttheta = yy[:ntt] ttheta_d = yy[ntt:2 * ntt] # not needed, just for comprehension # llmd = yy[2*ntt:2*ntt + nll] ode_part = self.deq_func(*args) # now calculate the accelerations in depencency of yy (and thus in dependency of llmd) # Note: signature: acc_of_lmd_func(yy, ttau) ttheta_dd = acc_of_lmd_func(*np.concatenate((yy, external_forces))) c2 = self.constraints_dd_func(*np.concatenate((ttheta, ttheta_d, ttheta_dd))) c2 = np.atleast_1d(c2) res = np.concatenate((ode_part, c2)) return res self.model_func = model_func
def solve(problem_spec): # problem_spec is dummy t = sp.Symbol('t') # time variable np = 2 nq = 2 ns = 2 n = np + nq + ns p1, p2 = pp = st.symb_vector("p1:{0}".format(np + 1)) q1, q2 = qq = st.symb_vector("q1:{0}".format(nq + 1)) s1, s2 = ss = st.symb_vector("s1:{0}".format(ns + 1)) ttheta = st.row_stack(qq[0], pp[0], ss[0], qq[1], pp[1], ss[1]) qdot1, pdot1, sdot1, qdot2, pdot2, sdot2 = tthetad = st.time_deriv(ttheta, ttheta) tthetadd = st.time_deriv(ttheta, ttheta, order=2) ttheta_all = st.concat_rows(ttheta, tthetad, tthetadd) c1, c2, c3, c4, c5, c6, m1, m2, m3, m4, m5, m6, J1, J2, J3, J4, J5, J6, l1, l2, l3, l4, l5, l6, d, g = params = sp.symbols( 'c1, c2, c3, c4, c5, c6, m1, m2, m3, m4, m5, m6, J1, J2, J3, J4, J5, J6, l1, l2, l3, l4, l5, l6, d, g') tau1, tau2, tau3, tau4, tau5, tau6 = ttau = st.symb_vector("tau1, tau2, tau3, tau4, tau5, tau6 ") # unit vectors ex = sp.Matrix([1, 0]) ey = sp.Matrix([0, 1]) # coordinates of centers of mass and joints # left G0 = 0 * ex ##: C1 = G0 + Rz(q1) * ex * c1 ##: G1 = G0 + Rz(q1) * ex * l1 ##: C2 = G1 + Rz(q1 + p1) * ex * c2 ##: G2 = G1 + Rz(q1 + p1) * ex * l2 ##: C3 = G2 + Rz(q1 + p1 + s1) * ex * c3 ##: G3 = G2 + Rz(q1 + p1 + s1) * ex * l3 ##: # right G6 = d * ex ##: C6 = G6 + Rz(q2) * ex * c6 ##: G5 = G6 + Rz(q2) * ex * l6 ##: C5 = G5 + Rz(q2 + p2) * ex * c5 ##: G4 = G5 + Rz(q2 + p2) * ex * l5 ##: C4 = G4 + Rz(q2 + p2 + s2) * ex * c4 ##: G3b = G4 + Rz(q2 + p2 + s2) * ex * l4 ##: # time derivatives of centers of mass Sd1, Sd2, Sd3, Sd4, Sd5, Sd6 = st.col_split(st.time_deriv(st.col_stack(C1, C2, C3, C4, C5, C6), ttheta)) # Kinetic Energy (note that angles are relative) T_rot = (J1 * qdot1 ** 2) / 2 + (J2 * (qdot1 + pdot1) ** 2) / 2 + (J3 * (qdot1 + pdot1 + sdot1) ** 2) / 2 + \ (J4 * (qdot2 + pdot2 + sdot2) ** 2) / 2 + (J5 * (qdot2 + pdot2) ** 2) / 2 + (J6 * qdot2 ** 2) / 2 T_trans = ( m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3 + m4 * Sd4.T * Sd4 + m5 * Sd5.T * Sd5 + m6 * Sd6.T * Sd6) / 2 T = T_rot + T_trans[0] # Potential Energy V = m1 * g * C1[1] + m2 * g * C2[1] + m3 * g * C3[1] + m4 * g * C4[1] + m5 * g * C5[1] + m6 * g * C6[1] parameter_values = list(dict(c1=0.4 / 2, c2=0.42 / 2, c3=0.55 / 2, c4=0.55 / 2, c5=0.42 / 2, c6=0.4 / 2, m1=6.0, m2=12.0, m3=39.6, m4=39.6, m5=12.0, m6=6.0, J1=(6 * 0.4 ** 2) / 12, J2=(12 * 0.42 ** 2) / 12, J3=(39.6 * 0.55 ** 2) / 12, J4=(39.6 * 0.55 ** 2) / 12, J5=(12 * 0.42 ** 2) / 12, J6=(6 * 0.4 ** 2) / 12, l1=0.4, l2=0.42, l3=0.55, l4=0.55, l5=0.42, l6=0.4, d=0.26, g=9.81).items()) external_forces = [tau1, tau2, tau3, tau4, tau5, tau6] dir_of_this_file = os.path.dirname(os.path.abspath(sys.modules.get(__name__).__file__)) fpath = os.path.join(dir_of_this_file, "7L-dae-2020-07-15.pcl") if not os.path.isfile(fpath): # if model is not present it could be regenerated # however this might take long (approx. 20min) mod = mt.generate_symbolic_model(T, V, ttheta, external_forces, constraints=[G3 - G3b], simplify=False) mod.calc_state_eq(simplify=False) mod.f_sympy = mod.f.subs(parameter_values) mod.G_sympy = mod.g.subs(parameter_values) with open(fpath, "wb") as pfile: pickle.dump(mod, pfile) else: with open(fpath, "rb") as pfile: mod = pickle.load(pfile) # calculate DAE equations from symbolic model dae = mod.calc_dae_eq(parameter_values) dae.generate_eqns_funcs() torso1_unit = Rz(q1 + p1 + s1) * ex torso2_unit = Rz(q2 + p2 + s2) * ex neck_length = 0.12 head_radius = 0.1 body_width = 15 neck_width = 15 H1 = G3 + neck_length * torso1_unit H1r = G3 + (neck_length - head_radius) * torso1_unit H2 = G3b + neck_length * torso2_unit H2r = G3b + (neck_length - head_radius) * torso2_unit vis = vt.Visualiser(ttheta, xlim=(-1.5, 1.5), ylim=(-.2, 2)) # get default colors and set them explicitly # this prevents color changes in onion skin plot default_colors = plt.get_cmap("tab10") guy1_color = default_colors(0) guy1_joint_color = "darkblue" guy2_color = default_colors(1) guy2_joint_color = "red" guy1_head_fc = guy1_color # facecolor guy1_head_ec = guy1_head_fc # edgecolor guy2_head_fc = guy2_color # facecolor guy2_head_ec = guy2_head_fc # edgecolor # guy 1 body vis.add_linkage(st.col_stack(G0, G1, G2, G3).subs(parameter_values), color=guy1_color, solid_capstyle='round', lw=body_width, ms=body_width, mfc=guy1_joint_color) # guy 1 neck # vis.add_linkage(st.col_stack(G3, H1r).subs(parameter_values), color=head_color, solid_capstyle='round', lw=neck_width) # guy 1 head vis.add_disk(st.col_stack(H1, H1r).subs(parameter_values), fc=guy1_head_fc, ec=guy1_head_ec, plot_radius=False, fill=True) # guy 2 body vis.add_linkage(st.col_stack(G6, G5, G4, G3b).subs(parameter_values), color=guy2_color, solid_capstyle='round', lw=body_width, ms=body_width, mfc=guy2_joint_color) # guy 2 neck # vis.add_linkage(st.col_stack(G3b, H2r).subs(parameter_values), color=head_color, solid_capstyle='round', lw=neck_width) # guy 2 head vis.add_disk(st.col_stack(H2, H2r).subs(parameter_values), fc=guy2_head_fc, ec=guy2_head_ec, plot_radius=False, fill=True) eq_stat = mod.eqns.subz0(tthetadd).subz0(tthetad).subs(parameter_values) # vector for tau and lambda together ttau_symbols = sp.Matrix(mod.uu) ##:T mmu = st.row_stack(ttau_symbols, mod.llmd) ##:T # linear system of equations (and convert to function w.r.t. ttheta) K0_expr = eq_stat.subz0(mmu) ##:i K1_expr = eq_stat.jacobian(mmu) ##:i K0_func = st.expr_to_func(ttheta, K0_expr) K1_func = st.expr_to_func(ttheta, K1_expr, keep_shape=True) def get_mu_stat_for_theta(ttheta_arg, rho=10): # weighting matrix for mu K0 = K0_func(*ttheta_arg) K1 = K1_func(*ttheta_arg) return solve_qlp(K0, K1, rho) def solve_qlp(K0, K1, rho): R_mu = npy.diag([1, 1, 1, rho, rho, rho, .1, .1]) n1, n2 = K1.shape # construct the equation system for least squares with linear constraints M1 = npy.column_stack((R_mu, K1.T)) M2 = npy.column_stack((K1, npy.zeros((n1, n1)))) M_coeff = npy.row_stack((M1, M2)) M_rhs = npy.concatenate((npy.zeros(n2), -K0)) mmu_stat = npy.linalg.solve(M_coeff, M_rhs)[:n2] return mmu_stat ttheta_start = npy.r_[0.9, 1.5, -1.9, 2.1, -2.175799453493845, 1.7471971159642905] mmu_start = get_mu_stat_for_theta(ttheta_start) connection_point_func = st.expr_to_func(ttheta, G3.subs(parameter_values)) cs_ttau = mpc.casidify(mod.uu, mod.uu)[0] cs_llmd = mpc.casidify(mod.llmd, mod.llmd)[0] controls_sp = mmu controls_cs = cs.vertcat(cs_ttau, cs_llmd) coords_cs, _ = mpc.casidify(ttheta, ttheta) # parameters: 0: value of y_connection, 1: x_connection_last, # 2: y_connection_last, 3: delta_r_max, 4: rho (penalty factor for 2nd persons torques), # 5:11: ttheta_old[6], 11:17: ttheta:old2 # P = SX.sym('P', 5 + 12) rho = P[10] # weightning of inputs R = mpc.SX_diag_matrix((1, 1, 1, rho, rho, rho, 0.1, 0.1)) # Construction of Constraints g1 = [] # constraints vector (system dynamics) g2 = [] # inequality-constraints closed_chain_constraint, _ = mpc.casidify(mod.dae.constraints, ttheta, cs_vars=coords_cs) connection_position, _ = mpc.casidify(list(G3.subs(parameter_values)), ttheta, cs_vars=coords_cs) ##:i connection_y_value, _ = mpc.casidify([G3[1].subs(parameter_values)], ttheta, cs_vars=coords_cs) ##:i stationary_eqns, _, _ = mpc.casidify(eq_stat, ttheta, controls_sp, cs_vars=(coords_cs, controls_cs)) ##:i g1.extend(mpc.unpack(stationary_eqns)) g1.extend(mpc.unpack(closed_chain_constraint)) # force the connecting joint to a given hight (which will be provided later) g1.append(connection_y_value - P[0]) ng1 = len(g1) # squared distance from the last reference should be smaller than P[3] (delta_r_max): # this will be a restriction between -inf and 0 r = connection_position - P[1:3] g2.append(r.T @ r - P[3]) # change of angles should be smaller than a given bound (P[5:11] are the old coords) coords_old = P[5:11] coords_old2 = P[11:17] pseudo_vel = (coords_cs - coords_old) / 1 pseudo_acc = (coords_cs - 2 * coords_old + coords_old2) / 1 g2.extend(mpc.unpack(pseudo_vel)) g2.extend(mpc.unpack(pseudo_acc)) g_all = mpc.seq_to_SX_matrix(g1 + g2) ### Construction of objective Function obj = controls_cs.T @ R @ controls_cs + 1e5 * pseudo_acc.T @ pseudo_acc + 0.3e6 * pseudo_vel.T @ pseudo_vel OPT_variables = cs.vertcat(coords_cs, controls_cs) # for debugging g_all_cs_func = cs.Function("g_all_cs_func", (OPT_variables, P), (g_all,)) nlp_prob = dict(f=obj, x=OPT_variables, g=g_all, p=P) ipopt_settings = dict(max_iter=5000, print_level=0, acceptable_tol=1e-8, acceptable_obj_change_tol=1e-6) opts = dict(print_time=False, ipopt=ipopt_settings) xx_guess = npy.r_[ttheta_start, mmu_start] # note: g1 contains the equality constraints (system dynamics) (lower bound = upper bound) delta_phi = .05 d_delta_phi = .02 eps = 1e-9 lbg = npy.r_[[-eps] * ng1 + [-inf] + [-delta_phi] * n, [-d_delta_phi] * n] ubg = npy.r_[[eps] * ng1 + [0] + [delta_phi] * n, [d_delta_phi] * n] # ubx = [inf]*OPT_variables.shape[0]##: # lower and upper bounds for decision variables: # lbx = [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]*N1 + [tau1_min, tau4_min, -inf, -inf]*N # ubx = [inf, inf, inf, inf, inf, inf, inf, inf]*N1 + [tau1_max, tau4_max, inf, inf]*N rho = 3 P_init = npy.r_[connection_point_func(*ttheta_start)[1], connection_point_func(*ttheta_start), 0.01, rho, ttheta_start, ttheta_start] args = dict(lbx=-inf, ubx=inf, lbg=lbg, ubg=ubg, # unconstrained optimization p=P_init, # initial and final state x0=xx_guess # initial guess ) solver = cs.nlpsol("solver", "ipopt", nlp_prob, opts) sol = solver(**args) global_vars = ipydex.Container(old_sol=xx_guess, old_sol2=xx_guess) def get_optimal_equilibrium(y_value, rho=3): ttheta_old = global_vars.old_sol[:n] ttheta_old2 = global_vars.old_sol2[:n] opt_prob_params = npy.r_[y_value, connection_point_func(*ttheta_old), 0.01, rho, ttheta_old, ttheta_old2] args.update(dict(p=opt_prob_params, x0=global_vars.old_sol)) sol = solver(**args) stats = solver.stats() if not stats['success']: raise ValueError(stats["return_status"]) XX = sol["x"].full().squeeze() # save the last two results global_vars.old_sol2 = global_vars.old_sol global_vars.old_sol = XX return XX y_start = connection_point_func(*ttheta_start)[1] N = 100 y_end = 1.36 y_func = st.expr_to_func(t, st.condition_poly(t, (0, y_start, 0, 0), (1, y_end, 0, 0))) def get_qs_trajectory(rho): pseudo_time = npy.linspace(0, 1, N) yy_connection = y_func(pseudo_time) # reset the initial guess global_vars.old_sol = xx_guess global_vars.old_sol2 = xx_guess XX_list = [] for i, y_value in enumerate(yy_connection): # print(i, y_value) XX_list.append(get_optimal_equilibrium(y_value, rho=rho)) XX = npy.array(XX_list) return XX rho = 30 XX = get_qs_trajectory(rho=rho) def smooth_time_scaling(Tend, N, phase_fraction=.5): """ :param Tend: :param N: :param phase_fraction: fraction of Tend for smooth initial and end phase """ T0 = 0 T1 = Tend * phase_fraction y0 = 0 y1 = 1 # for initial phase poly1 = st.condition_poly(t, (T0, y0, 0, 0), (T1, y1, 0, 0)) # for end phase poly2 = poly1.subs(t, Tend - t) # there should be a phase in the middle with constant slope deriv_transition = st.piece_wise((y0, t < T0), (poly1, t < T1), (y1, t < Tend - T1), (poly2, t < Tend), (y0, True)) scaling = sp.integrate(deriv_transition, (t, T0, Tend)) time_transition = sp.integrate(deriv_transition * N / scaling, t) # deriv_transition_func = st.expr_to_func(t, full_transition) time_transition_func = st.expr_to_func(t, time_transition) deriv_func = st.expr_to_func(t, deriv_transition * N / scaling) deriv_func2 = st.expr_to_func(t, deriv_transition.diff(t) * N / scaling) C = ipydex.Container(fetch_locals=True) return C N = XX.shape[0] Tend = 4 res = smooth_time_scaling(Tend, N) def get_derivatives(XX, time_scaling, res=100): """ :param XX: Nxm array :param time_scaling: container for time scaling :param res: time resolution of the returned arrays """ N = XX.shape[0] Tend = time_scaling.Tend assert npy.isclose(time_scaling.time_transition_func([0, Tend])[-1], N) tt = npy.linspace(time_scaling.T0, time_scaling.Tend, res) NN = npy.arange(N) # high_resolution version of index arry NN2 = npy.linspace(0, N, res, endpoint=False) # time-scaled verion of index-array NN3 = time_scaling.time_transition_func(tt) NN3d = time_scaling.deriv_func(tt) NN3dd = time_scaling.deriv_func2(tt) XX_num, XXd_num, XXdd_num = [], [], [] # iterate over every column for col in XX.T: spl = splrep(NN, col) # function value and derivatives XX_num.append(splev(NN3, spl)) XXd_num.append(splev(NN3, spl, der=1)) XXdd_num.append(splev(NN3, spl, der=2)) XX_num = npy.array(XX_num).T XXd_num = npy.array(XXd_num).T XXdd_num = npy.array(XXdd_num).T NN3d_bc = npy.broadcast_to(NN3d, XX_num.T.shape).T NN3dd_bc = npy.broadcast_to(NN3dd, XX_num.T.shape).T XXd_n = XXd_num * NN3d_bc # apply chain rule XXdd_n = XXdd_num * NN3d_bc ** 2 + XXd_num * NN3dd_bc C = ipydex.Container(fetch_locals=True) return C C = XX_derivs = get_derivatives(XX[:, :], time_scaling=res) expr = mod.eqns.subz0(mod.uu, mod.llmd).subs(parameter_values) dynterm_func = st.expr_to_func(ttheta_all, expr) def get_torques(dyn_term_func, XX_derivs, static1=False, static2=False): ttheta_num_all = npy.c_[XX_derivs.XX_num[:, :n], XX_derivs.XXd_n[:, :n], XX_derivs.XXdd_n[:, :n]] ##:S if static1: # set velocities to 0 ttheta_num_all[:, n:2 * n] = 0 if static2: # set accelerations to 0 ttheta_num_all[:, 2 * n:] = 0 res = dynterm_func(*ttheta_num_all.T) return res lhs_static = get_torques(dynterm_func, XX_derivs, static1=True, static2=True) ##:i lhs_dynamic = get_torques(dynterm_func, XX_derivs, static2=False) ##:i mmu_stat_list = [] for L_k_stat, L_k_dyn, ttheta_k in zip(lhs_static, lhs_dynamic, XX_derivs.XX_num[:, :n]): K1_k = K1_func(*ttheta_k) mmu_stat_k = solve_qlp(L_k_stat, K1_k, rho) mmu_stat_list.append(mmu_stat_k) mmu_stat_all = npy.array(mmu_stat_list) solution_data = SolutionData() solution_data.tt = XX_derivs.tt solution_data.xx = XX_derivs.XX_num solution_data.mmu = mmu_stat_all solution_data.vis = vis save_plot(problem_spec, solution_data) return solution_data
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 test_expr_to_func(self): x1, x2 = xx = sp.Matrix(sp.symbols("x1, x2")) t, = sp.symbols("t,") r_ = np.r_ f1 = st.expr_to_func(x1, 2 * x1) self.assertEqual(f1(5.1), 10.2) XX1 = np.r_[1, 2, 3.7] res1 = f1(XX1) == 2 * XX1 self.assertTrue(res1.all) f2 = st.expr_to_func(x1, sp.Matrix([x1 * 2, x1 + 5, 4])) res2 = f2(3) == r_[6, 8, 4] self.assertTrue(res2.all()) res2b = f2(r_[3, 10, 0]) == np.array([[6, 8, 4], [20, 15, 4], [0, 5, 4]]) self.assertTrue(res2b.all()) f3 = st.expr_to_func(xx, sp.Matrix([x1 * 2, x2 + 5, 4])) res3 = np.allclose(f3(-3.1, 4), r_[-6.2, 9, 4]) self.assertTrue(res3) # test compatibility with Piecewise Expressions des_input = st.piece_wise((0, t <= 1), (t, t < 2), (0.5, t < 3), (1, True)) f4s = st.expr_to_func(t, des_input) f4v = st.expr_to_func(t, sp.Matrix([des_input, des_input])) self.assertEqual(f4s(2.7), 0.5) sol = r_[0, 1.6, 0.5, 1, 1] res4a = f4s(r_[0.3, 1.6, 2.2, 3.1, 500]) == sol self.assertTrue(res4a.all()) res4b = f4v(r_[0.3, 1.6, 2.2, 3.1, 500]) col1, col2 = res4b.T self.assertTrue(np.array_equal(col1, sol)) self.assertTrue(np.array_equal(col2, sol)) spmatrix = sp.Matrix([[x1, x1 * x2], [0, x2**2]]) fnc1 = st.expr_to_func(xx, spmatrix, keep_shape=False) fnc2 = st.expr_to_func(xx, spmatrix, keep_shape=True) res1 = fnc1(1.0, 2.0) res2 = fnc2(1.0, 2.0) self.assertEqual(res1.shape, (4, )) self.assertEqual(res2.shape, (2, 2)) # noinspection PyTypeChecker self.assertTrue(np.all(res1 == [1, 2, 0, 4])) # noinspection PyTypeChecker self.assertTrue(np.all(res1 == res2.flatten())) fnc = st.expr_to_func(xx, x1 + x2) self.assertEqual(fnc(1, 3), 4) xx_res = np.array([1, 3, 1.1, 3, 1.2, 3.0]).reshape(3, -1) self.assertTrue(np.allclose(fnc(*xx_res.T), np.array([4, 4.1, 4.2]))) fnc1 = st.expr_to_func(xx, 3 * xx) fnc2 = st.expr_to_func(xx, 3 * xx, allow_kwargs=True) self.assertTrue(np.allclose(fnc1(10, 100), fnc2(x2=100, x1=10)))