def bang_bang_problem(stage_method):
ocp = Ocp(T=FreeTime(1))
p = ocp.state()
v = ocp.state()
u = ocp.control()
ocp.set_der(p, v)
ocp.set_der(v, u)
ocp.subject_to(u <= 1)
ocp.subject_to(-1 <= u)
ocp.add_objective(ocp.T)
ocp.subject_to(ocp.at_t0(p) == 0)
ocp.subject_to(ocp.at_t0(v) == 0)
ocp.subject_to(ocp.at_tf(p) == 1)
ocp.subject_to(ocp.at_tf(v) == 0)
ocp.solver('ipopt')
ocp.method(stage_method)
return (ocp, ocp.solve(), p, v, u)
})
obs_spline_r = interpolant('r', 'bspline', [tunnel_s1], 1, {
"algorithm": "smooth_linear",
"smooth_linear_frac": 0.49
})
ocp.subject_to(x > 0)
ocp.subject_to((x - obs_spline_x(s_obs, bx))**2 +
(y - obs_spline_y(s_obs, by))**2 -
(obs_spline_r(s_obs, br))**2 < 0)
# -------------------------------------- Objective function
ocp.add_objective(1 * ocp.integral(
(x - end_goal_x)**2 +
(y - end_goal_y)**2)) # integral = cause of extra state in acados
# ----------------- Solver
options = {"ipopt": {"print_level": 5}}
options["expand"] = False
options["print_time"] = True
ocp.solver('ipopt', options)
# qp_solvers = ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP')
# integrator_types = ('ERK', 'IRK', 'GNSF', 'DISCRETE')
# SOLVER_TYPE_values = ['SQP', 'SQP_RTI']
# HESS_APPROX_values = ['GAUSS_NEWTON', 'EXACT']
# REGULARIZATION_values = ['NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY']
# ocp.set_initial(x, np.zeros(N+1))
# ocp.set_initial(y, np.zeros(N+1))
# ocp.set_initial(theta, np.zeros(N+1))
# ocp.set_initial(v , np.zeros(N+1))
# ocp.set_initial(w , np.zeros(N+1))
#----------- end point constraint
ocp.subject_to(ocp.at_tf(x) == 1)
# -------------------------------------- Objective function
ocp.add_objective(
1 * ocp.integral((x)**2 +
(y)**2)) # integral = cause of extra state in acados
# ----------------- Solver
options = {"ipopt": {"print_level": 5}}
options["expand"] = False
options["print_time"] = True
ocp.solver('ipopt', options)
method = external_method('acados',
N=N,
intg='rk',
qp_solver='FULL_CONDENSING_HPIPM',
expand=False,
nlp_solver_max_iter=1000,