bool PDFullSpaceSolver::Solve(Number alpha,
                                Number beta,
                                const IteratesVector& rhs,
                                IteratesVector& res,
                                bool allow_inexact,
                                bool improve_solution /* = false */)
  {
    DBG_START_METH("PDFullSpaceSolver::Solve",dbg_verbosity);
    DBG_ASSERT(!allow_inexact || !improve_solution);
    DBG_ASSERT(!improve_solution || beta==0.);

    // Timing of PDSystem solver starts here
    IpData().TimingStats().PDSystemSolverTotal().Start();

    DBG_PRINT_VECTOR(2, "rhs_x", *rhs.x());
    DBG_PRINT_VECTOR(2, "rhs_s", *rhs.s());
    DBG_PRINT_VECTOR(2, "rhs_c", *rhs.y_c());
    DBG_PRINT_VECTOR(2, "rhs_d", *rhs.y_d());
    DBG_PRINT_VECTOR(2, "rhs_zL", *rhs.z_L());
    DBG_PRINT_VECTOR(2, "rhs_zU", *rhs.z_U());
    DBG_PRINT_VECTOR(2, "rhs_vL", *rhs.v_L());
    DBG_PRINT_VECTOR(2, "rhs_vU", *rhs.v_U());
    DBG_PRINT_VECTOR(2, "res_x in", *res.x());
    DBG_PRINT_VECTOR(2, "res_s in", *res.s());
    DBG_PRINT_VECTOR(2, "res_c in", *res.y_c());
    DBG_PRINT_VECTOR(2, "res_d in", *res.y_d());
    DBG_PRINT_VECTOR(2, "res_zL in", *res.z_L());
    DBG_PRINT_VECTOR(2, "res_zU in", *res.z_U());
    DBG_PRINT_VECTOR(2, "res_vL in", *res.v_L());
    DBG_PRINT_VECTOR(2, "res_vU in", *res.v_U());

    // if beta is nonzero, keep a copy of the incoming values in res_ */
    SmartPtr<IteratesVector> copy_res;
    if (beta != 0.) {
      copy_res = res.MakeNewIteratesVectorCopy();
    }

    // Receive data about matrix
    SmartPtr<const Vector> x = IpData().curr()->x();
    SmartPtr<const Vector> s = IpData().curr()->s();
    SmartPtr<const SymMatrix> W = IpData().W();
    SmartPtr<const Matrix> J_c = IpCq().curr_jac_c();
    SmartPtr<const Matrix> J_d = IpCq().curr_jac_d();
    SmartPtr<const Matrix> Px_L = IpNLP().Px_L();
    SmartPtr<const Matrix> Px_U = IpNLP().Px_U();
    SmartPtr<const Matrix> Pd_L = IpNLP().Pd_L();
    SmartPtr<const Matrix> Pd_U = IpNLP().Pd_U();
    SmartPtr<const Vector> z_L = IpData().curr()->z_L();
    SmartPtr<const Vector> z_U = IpData().curr()->z_U();
    SmartPtr<const Vector> v_L = IpData().curr()->v_L();
    SmartPtr<const Vector> v_U = IpData().curr()->v_U();
    SmartPtr<const Vector> slack_x_L = IpCq().curr_slack_x_L();
    SmartPtr<const Vector> slack_x_U = IpCq().curr_slack_x_U();
    SmartPtr<const Vector> slack_s_L = IpCq().curr_slack_s_L();
    SmartPtr<const Vector> slack_s_U = IpCq().curr_slack_s_U();
    SmartPtr<const Vector> sigma_x = IpCq().curr_sigma_x();
    SmartPtr<const Vector> sigma_s = IpCq().curr_sigma_s();
    DBG_PRINT_VECTOR(2, "Sigma_x", *sigma_x);
    DBG_PRINT_VECTOR(2, "Sigma_s", *sigma_s);

    bool done = false;
    // The following flag is set to true, if we asked the linear
    // solver to improve the quality of the solution in
    // the next solve
    bool resolve_with_better_quality = false;
    // the following flag is set to true, if iterative refinement
    // failed and we want to try if a modified system is able to
    // remedy that problem by pretending the matrix is singular
    bool pretend_singular = false;
    bool pretend_singular_last_time = false;

    // Beginning of loop for solving the system (including all
    // modifications for the linear system to ensure good solution
    // quality)
    while (!done) {

      // if improve_solution is true, we are given already a solution
      // from the calling function, so we can skip the first solve
      bool solve_retval = true;
      if (!improve_solution) {
        solve_retval =
          SolveOnce(resolve_with_better_quality, pretend_singular,
                    *W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U, *z_L, *z_U,
                    *v_L, *v_U, *slack_x_L, *slack_x_U, *slack_s_L, *slack_s_U,
                    *sigma_x, *sigma_s, 1., 0., rhs, res);
        resolve_with_better_quality = false;
        pretend_singular = false;
      }
      improve_solution = false;

      if (!solve_retval) {
        // If system seems not to be solvable, we return with false
        // and let the calling routine deal with it.
        IpData().TimingStats().PDSystemSolverTotal().End();
        return false;
      }

      if (allow_inexact) {
        // no safety checks required
        if (Jnlst().ProduceOutput(J_MOREDETAILED, J_LINEAR_ALGEBRA)) {
          SmartPtr<IteratesVector> resid = res.MakeNewIteratesVector(true);
          ComputeResiduals(*W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U,
                           *z_L, *z_U, *v_L, *v_U, *slack_x_L, *slack_x_U,
                           *slack_s_L, *slack_s_U, *sigma_x, *sigma_s,
                           alpha, beta, rhs, res, *resid);
        }
        break;
      }

      // Get space for the residual
      SmartPtr<IteratesVector> resid = res.MakeNewIteratesVector(true);

      // ToDo don't to that after max refinement?
      ComputeResiduals(*W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U,
                       *z_L, *z_U, *v_L, *v_U, *slack_x_L, *slack_x_U,
                       *slack_s_L, *slack_s_U, *sigma_x, *sigma_s,
                       alpha, beta, rhs, res, *resid);

      Number residual_ratio =
        ComputeResidualRatio(rhs, res, *resid);
      Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                     "residual_ratio = %e\n", residual_ratio);
      Number residual_ratio_old = residual_ratio;

      // Beginning of loop for iterative refinement
      Index num_iter_ref = 0;
      bool quit_refinement = false;
      while (!allow_inexact && !quit_refinement &&
             (num_iter_ref < min_refinement_steps_ ||
              residual_ratio > residual_ratio_max_) ) {

        // To the next back solve
        solve_retval =
          SolveOnce(resolve_with_better_quality, false,
                    *W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U, *z_L, *z_U,
                    *v_L, *v_U, *slack_x_L, *slack_x_U, *slack_s_L, *slack_s_U,
                    *sigma_x, *sigma_s, -1., 1., *resid, res);
        ASSERT_EXCEPTION(solve_retval, INTERNAL_ABORT,
                         "SolveOnce returns false during iterative refinement.");

        ComputeResiduals(*W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U,
                         *z_L, *z_U, *v_L, *v_U, *slack_x_L, *slack_x_U,
                         *slack_s_L, *slack_s_U, *sigma_x, *sigma_s,
                         alpha, beta, rhs, res, *resid);

        residual_ratio =
          ComputeResidualRatio(rhs, res, *resid);
        Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                       "residual_ratio = %e\n", residual_ratio);

        num_iter_ref++;
        // Check if we have to give up on iterative refinement
        if (residual_ratio > residual_ratio_max_ &&
            num_iter_ref>min_refinement_steps_ &&
            (num_iter_ref>max_refinement_steps_ ||
             residual_ratio>residual_improvement_factor_*residual_ratio_old)) {

          Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                         "Iterative refinement failed with residual_ratio = %e\n", residual_ratio);
          quit_refinement = true;

          // Pretend singularity only once - if it didn't help, we
          // have to live with what we got so far
          resolve_with_better_quality = false;
          DBG_PRINT((1, "pretend_singular = %d\n", pretend_singular));
          if (!pretend_singular_last_time) {
            // First try if we can ask the augmented system solver to
            // improve the quality of the solution (only if that hasn't
            // been done before for this linear system)
            if (!augsys_improved_) {
              Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                             "Asking augmented system solver to improve quality of its solutions.\n");
              augsys_improved_ = augSysSolver_->IncreaseQuality();
              if (augsys_improved_) {
                IpData().Append_info_string("q");
                resolve_with_better_quality = true;
              }
              else {
                // solver said it cannot improve quality, so let
                // possibly conclude that the current modification is
                // singular
                pretend_singular = true;
              }
            }
            else {
              // we had already asked the solver before to improve the
              // quality of the solution, so let's now pretend that the
              // modification is possibly singular
              pretend_singular = true;
            }
            pretend_singular_last_time = pretend_singular;
            if (pretend_singular) {
              // let's only conclude that the current linear system
              // including modifications is singular, if the residual is
              // quite bad
              if (residual_ratio < residual_ratio_singular_) {
                pretend_singular = false;
                IpData().Append_info_string("S");
                Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                               "Just accept current solution.\n");
              }
              else {
                IpData().Append_info_string("s");
                Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                               "Pretend that the current system (including modifications) is singular.\n");
              }
            }
          }
          else {
            pretend_singular = false;
            DBG_PRINT((1,"Resetting pretend_singular to false.\n"));
          }
        }

        residual_ratio_old = residual_ratio;
      } // End of loop for iterative refinement

      done = !(resolve_with_better_quality) && !(pretend_singular);

    } // End of loop for solving the linear system (incl. modifications)

    // Finally let's assemble the res result vectors
    if (alpha != 0.) {
      res.Scal(alpha);
    }

    if (beta != 0.) {
      res.Axpy(beta, *copy_res);
    }

    DBG_PRINT_VECTOR(2, "res_x", *res.x());
    DBG_PRINT_VECTOR(2, "res_s", *res.s());
    DBG_PRINT_VECTOR(2, "res_c", *res.y_c());
    DBG_PRINT_VECTOR(2, "res_d", *res.y_d());
    DBG_PRINT_VECTOR(2, "res_zL", *res.z_L());
    DBG_PRINT_VECTOR(2, "res_zU", *res.z_U());
    DBG_PRINT_VECTOR(2, "res_vL", *res.v_L());
    DBG_PRINT_VECTOR(2, "res_vU", *res.v_U());

    IpData().TimingStats().PDSystemSolverTotal().End();

    return true;
  }
Ejemplo n.º 2
0
  bool StdStepCalculator::Step(DenseVector& delta_u,
			       IteratesVector& sol)
  {
    DBG_START_METH("StdStepCalculator::Step", dbg_verbosity);

    bool retval = false;

    SmartPtr<IteratesVector> delta_u_long = IpData().trial()->MakeNewIteratesVector();
    ift_data_->TransMultiply(delta_u, *delta_u_long);

    SmartPtr<IteratesVector> r_s = IpData().trial()->MakeNewIteratesVector();
    if (kkt_residuals_) {
      /* This should be almost zero... */
      r_s->Set_x_NonConst(*IpCq().curr_grad_lag_x()->MakeNewCopy());
      r_s->Set_s_NonConst(*IpCq().curr_grad_lag_s()->MakeNewCopy());
      r_s->Set_y_c_NonConst(*IpCq().curr_c()->MakeNewCopy());
      r_s->Set_y_d_NonConst(*IpCq().curr_d_minus_s()->MakeNewCopy());
      r_s->Set_z_L_NonConst(*IpCq().curr_compl_x_L()->MakeNewCopy());
      r_s->Set_z_U_NonConst(*IpCq().curr_compl_x_U()->MakeNewCopy());
      r_s->Set_v_L_NonConst(*IpCq().curr_compl_s_L()->MakeNewCopy());
      r_s->Set_v_U_NonConst(*IpCq().curr_compl_s_U()->MakeNewCopy());

      r_s->Print(Jnlst(),J_VECTOR,J_USER1,"r_s init");
      delta_u.Print(Jnlst(),J_VECTOR,J_USER1,"delta_u init");
      DBG_PRINT((dbg_verbosity,"r_s init Nrm2=%23.16e\n", r_s->Asum()));

      delta_u_long->Axpy(-1.0, *r_s);
    }

    backsolver_->Solve(&sol, ConstPtr(delta_u_long));

    SmartPtr<IteratesVector> Kr_s;
    if (Do_Boundcheck()) {
      Kr_s = sol.MakeNewIteratesVectorCopy();
    }

    sol.Axpy(1.0, *IpData().trial());

    if (Do_Boundcheck()) {
      DBG_PRINT((dbg_verbosity, "Entering boundcheck"));
      // initialize
      Index new_du_size =0;
      Number* new_du_values;
      std::vector<Index> x_bound_violations_idx;
      std::vector<Number> x_bound_violations_du;
      std::vector<Index> delta_u_sort;
      bool bounds_violated;
      SmartPtr<DenseVectorSpace> delta_u_space = new DenseVectorSpace(0);
      SmartPtr<DenseVector> old_delta_u = new DenseVector(GetRawPtr(delta_u_space));
      SmartPtr<DenseVector> new_delta_u;

      bounds_violated = BoundCheck(sol, x_bound_violations_idx, x_bound_violations_du);
      while (bounds_violated) {
	Driver()->data_A()->Print(Jnlst(),J_VECTOR,J_USER1,"data_A_init");
	Driver()->data_B()->Print(Jnlst(),J_VECTOR,J_USER1,"data_B_init");
	// write new schurdata A
	dynamic_cast<IndexSchurData*>(GetRawPtr(Driver()->data_A_nonconst()))->AddData_List(x_bound_violations_idx, delta_u_sort, new_du_size, 1);
	// write new schurdata B
	dynamic_cast<IndexSchurData*>(GetRawPtr(Driver()->data_B_nonconst()))->AddData_List(x_bound_violations_idx, delta_u_sort, new_du_size, 1);
	Driver()->data_A()->Print(Jnlst(),J_VECTOR,J_USER1,"data_A");
	Driver()->data_B()->Print(Jnlst(),J_VECTOR,J_USER1,"data_B");
	Driver()->SchurBuild();
	Driver()->SchurFactorize();

	old_delta_u->Print(Jnlst(),J_VECTOR,J_USER1,"old_delta_u");
	delta_u_space = NULL; // delete old delta_u space
	delta_u_space = new DenseVectorSpace(new_du_size); // create new delta_u space
	new_delta_u = new DenseVector(GetRawPtr(ConstPtr(delta_u_space)));
	new_du_values = new_delta_u->Values();
	IpBlasDcopy(old_delta_u->Dim(), old_delta_u->Values(), 1, new_du_values, 1);
	for (UINT i=0; i<x_bound_violations_idx.size(); ++i) {
	  //	  printf("i=%d, delta_u_sort[i]=%d, x_bound_viol_du[i]=%f\n", i, delta_u_sort[i], x_bound_violations_du[i]);
	  new_du_values[delta_u_sort[i]] = x_bound_violations_du[i];
	}
	SmartPtr<IteratesVector> new_sol = sol.MakeNewIteratesVector();
	new_delta_u->Print(Jnlst(),J_VECTOR,J_USER1,"new_delta_u");

	// solve with new data_B and delta_u
	retval = Driver()->SchurSolve(&sol, ConstPtr(delta_u_long), dynamic_cast<Vector*>(GetRawPtr(new_delta_u)), Kr_s);

	sol.Axpy(1.0, *IpData().trial());

	x_bound_violations_idx.clear();
	x_bound_violations_du.clear();
	delta_u_sort.clear();
	bounds_violated = BoundCheck(sol, x_bound_violations_idx, x_bound_violations_du);
	// copy new vector in old vector ->has to be done becpause otherwise only pointers will be copied and then it makes no sense
	old_delta_u = new_delta_u->MakeNewDenseVector();
	old_delta_u->Copy(*new_delta_u);
      }
    }

    return retval;
  }