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