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 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 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