8 std::shared_ptr<BCLagrangianForm> lagr_form,
9 std::shared_ptr<BCPenaltyForm> pen_form,
10 const double initial_al_weight,
12 const double max_al_weight,
14 const std::function<
void(
const Eigen::VectorXd &)> &update_barrier_stiffness)
15 : lagr_form(lagr_form),
17 initial_al_weight(initial_al_weight),
19 max_al_weight(max_al_weight),
21 update_barrier_stiffness(update_barrier_stiffness)
27 assert(sol.size() == nl_problem.
full_size());
29 const Eigen::VectorXd initial_sol = sol;
37 const int iters = nl_solver->stop_criteria().iterations;
39 const double initial_error =
pen_form->compute_error(sol);
43 while (!std::isfinite(nl_problem.
value(tmp_sol))
50 logger().debug(
"Solving AL Problem with weight {}", al_weight);
58 nl_solver->minimize(nl_problem, tmp_sol);
60 catch (
const std::runtime_error &e)
67 const double current_error =
pen_form->compute_error(sol);
68 const double eta = 1 - sqrt(current_error / initial_error);
70 logger().debug(
"Current eta = {}", eta);
74 logger().debug(
"Higher error than initial, increase weight and revert to previous solution");
84 lagr_form->update_lagrangian(sol, al_weight);
90 nl_solver->stop_criteria().iterations = iters;
95 assert(sol.size() == nl_problem.
full_size());
100 if (!std::isfinite(nl_problem.
value(tmp_sol))
103 log_and_throw_error(
"Failed to apply boundary conditions; solve with augmented lagrangian first!");
108 logger().debug(
"Successfully applied boundary conditions; solving in reduced space");
110 nl_problem.
init(sol);
114 nl_solver->minimize(nl_problem, tmp_sol);
116 catch (
const std::runtime_error &e)
const double max_al_weight
void set_al_weight(NLProblem &nl_problem, const Eigen::VectorXd &x, const double weight)
std::function< void(const Eigen::VectorXd &)> update_barrier_stiffness
ALSolver(std::shared_ptr< BCLagrangianForm > lagr_form, std::shared_ptr< BCPenaltyForm > pen_form, const double initial_al_weight, const double scaling, const double max_al_weight, const double eta_tol, const std::function< void(const Eigen::VectorXd &)> &update_barrier_stiffness)
std::shared_ptr< BCPenaltyForm > pen_form
std::shared_ptr< BCLagrangianForm > lagr_form
std::function< void(const double)> post_subsolve
const double initial_al_weight
void solve_reduced(std::shared_ptr< NLSolver > nl_solver, NLProblem &nl_problem, Eigen::MatrixXd &sol)
void solve_al(std::shared_ptr< NLSolver > nl_solver, NLProblem &nl_problem, Eigen::MatrixXd &sol)
virtual void line_search_end() override
virtual void init(const TVector &x0) override
void line_search_begin(const TVector &x0, const TVector &x1) override
virtual bool is_step_valid(const TVector &x0, const TVector &x1) override
virtual TVector full_to_reduced(const TVector &full) const
virtual bool is_step_collision_free(const TVector &x0, const TVector &x1) override
virtual TVector reduced_to_full(const TVector &reduced) const
virtual double value(const TVector &x) override
void set_apply_DBC(const TVector &x, const bool val)
spdlog::logger & logger()
Retrieves the current logger.
void log_and_throw_error(const std::string &msg)