void PuzzleSolver::LinearSolver(PuzzleSolution* solution) { Position nextPop = solution->NextPop(); if (nextPop.m_x < BoardWidth && nextPop.m_y < BoardHeight) { solution->CurrentPoints() += solution->Board().Pop(nextPop.m_x, nextPop.m_y); solution->m_solutionPath.push_back(Position(solution->NextPop().m_x, solution->NextPop().m_y)); } PieceMap matches; solution->Board().GetPoppableItems(matches); //if this is the end of this line of pops then add to solutions and return if (0 >= matches.size()) { PuzzleSolution* processSolution = new PuzzleSolution(*solution); m_outputQueue.Push(processSolution); m_process.notify_one(); } for (auto pos : matches) { PuzzleSolution* newSolution = new PuzzleSolution(*solution); newSolution->NextPop() = pos; newSolution->Level() += 1; LinearSolver(newSolution); } delete solution; solution = nullptr; }
void ImplicitFixedStepIntegratorInternal::init() { // Call the base class init FixedStepIntegratorInternal::init(); // Get the NLP creator function std::string implicit_function_name = getOption("implicit_solver"); // Allocate an NLP solver implicit_solver_ = ImplicitFunction(implicit_function_name, F_, Function(), LinearSolver()); implicit_solver_.setOption("name", string(getOption("name")) + "_implicit_solver"); implicit_solver_.setOption("implicit_input", DAE_Z); implicit_solver_.setOption("implicit_output", DAE_ALG); // Pass options if (hasSetOption("implicit_solver_options")) { const Dictionary& implicit_solver_options = getOption("implicit_solver_options"); implicit_solver_.setOption(implicit_solver_options); } // Initialize the solver implicit_solver_.init(); // Allocate a root-finding solver for the backward problem if (nRZ_>0) { // Get the NLP creator function std::string backward_implicit_function_name = getOption("implicit_solver"); // Allocate an NLP solver backward_implicit_solver_ = ImplicitFunction(backward_implicit_function_name, G_, Function(), LinearSolver()); backward_implicit_solver_.setOption("name", string(getOption("name")) + "_backward_implicit_solver"); backward_implicit_solver_.setOption("implicit_input", RDAE_RZ); backward_implicit_solver_.setOption("implicit_output", RDAE_ALG); // Pass options if (hasSetOption("implicit_solver_options")) { const Dictionary& backward_implicit_solver_options = getOption("implicit_solver_options"); backward_implicit_solver_.setOption(backward_implicit_solver_options); } // Initialize the solver backward_implicit_solver_.init(); } }
void QcqpToSocp::init() { // Initialize the base classes QcqpSolverInternal::init(); // Collection of sparsities that will make up SOCP_SOLVER_G std::vector<Sparsity> socp_g; // Allocate Cholesky solvers cholesky_.push_back(LinearSolver("csparsecholesky", st_[QCQP_STRUCT_H])); for (int i=0;i<nq_;++i) { cholesky_.push_back( LinearSolver("csparsecholesky", DMatrix(st_[QCQP_STRUCT_P])(range(i*n_, (i+1)*n_), ALL).sparsity())); } for (int i=0;i<nq_+1;++i) { // Initialize Cholesky solve cholesky_[i].init(); // Harvest Cholsesky sparsity patterns // Note that we add extra scalar to make room for the epigraph-reformulation variable socp_g.push_back(blkdiag( cholesky_[i].getFactorizationSparsity(false), Sparsity::dense(1, 1))); } // Create an SocpSolver instance solver_ = SocpSolver(getOption(solvername()), socpStruct("g", horzcat(socp_g), "a", horzcat(input(QCQP_SOLVER_A).sparsity(), Sparsity::sparse(nc_, 1)))); //solver_.setQCQPOptions(); if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname())); std::vector<int> ni(nq_+1); for (int i=0;i<nq_+1;++i) { ni[i] = n_+1; } solver_.setOption("ni", ni); // Initialize the SocpSolver solver_.init(); }
/** * Parametrized constructor of the factory object. * \param aF The vector-function to fit. * \param aFillJac The jacobian of the vector-function to fit. * \param aY The desired output vector of the function (to be matched by the algorithm). * \param aMaxIter The maximum number of iterations to perform. * \param aTau The initial, relative damping value to damp the Hessian matrix. * \param aEpsj The tolerance on the norm of the gradient. * \param aEpsx The tolerance on the norm of the step size. * \param aEpsy The tolerance on the norm of the residual. * \param aImposeLimits The functor that can impose simple limits on the search domain (i.e. using this boils down to a gradient projection method, for more complex constraints please use a constraint optimization method instead). * \param aLinSolver The functor that can solve a linear system. */ levenberg_marquardt_nllsq_factory(Function aF, JacobianFunction aFillJac, OutputVector aY, unsigned int aMaxIter = 100, T aTau = T(1e-3), T aEpsj = T(1e-6), T aEpsx = T(1e-6), T aEpsy = T(1e-6), LimitFunction aImposeLimits = LimitFunction(), LinearSolver aLinSolver = LinearSolver()) : f(aF), fill_jac(aFillJac), y(aY), max_iter(aMaxIter), tau(aTau), epsj(aEpsj), epsx(aEpsx), epsy(aEpsy), impose_limits(aImposeLimits), lin_solve(aLinSolver) { };
LinearSolver GslInternal::getLinearSolver() { return LinearSolver();}
void SDPSDQPInternal::init() { // Initialize the base classes SdqpSolverInternal::init(); cholesky_ = LinearSolver("csparsecholesky", st_[SDQP_STRUCT_H]); cholesky_.init(); MX g_socp = MX::sym("x", cholesky_.getFactorizationSparsity(true)); MX h_socp = MX::sym("h", n_); MX f_socp = sqrt(inner_prod(h_socp, h_socp)); MX en_socp = 0.5/f_socp; MX f_sdqp = MX::sym("f", input(SDQP_SOLVER_F).sparsity()); MX g_sdqp = MX::sym("g", input(SDQP_SOLVER_G).sparsity()); std::vector<MX> fi(n_+1); MX znp = MX::sparse(n_+1, n_+1); for (int k=0;k<n_;++k) { MX gk = vertcat(g_socp(ALL, k), DMatrix::sparse(1, 1)); MX fk = -blockcat(znp, gk, gk.T(), DMatrix::sparse(1, 1)); // TODO(Joel): replace with ALL fi.push_back(blkdiag(f_sdqp(ALL, Slice(f_sdqp.size1()*k, f_sdqp.size1()*(k+1))), fk)); } MX fin = en_socp*DMatrix::eye(n_+2); fin(n_, n_+1) = en_socp; fin(n_+1, n_) = en_socp; fi.push_back(blkdiag(DMatrix::sparse(f_sdqp.size1(), f_sdqp.size1()), -fin)); MX h0 = vertcat(h_socp, DMatrix::sparse(1, 1)); MX g = blockcat(f_socp*DMatrix::eye(n_+1), h0, h0.T(), f_socp); g = blkdiag(g_sdqp, g); IOScheme mappingIn("g_socp", "h_socp", "f_sdqp", "g_sdqp"); IOScheme mappingOut("f", "g"); mapping_ = MXFunction(mappingIn("g_socp", g_socp, "h_socp", h_socp, "f_sdqp", f_sdqp, "g_sdqp", g_sdqp), mappingOut("f", horzcat(fi), "g", g)); mapping_.init(); // Create an sdpsolver instance std::string sdpsolver_name = getOption("sdp_solver"); sdpsolver_ = SdpSolver(sdpsolver_name, sdpStruct("g", mapping_.output("g").sparsity(), "f", mapping_.output("f").sparsity(), "a", horzcat(input(SDQP_SOLVER_A).sparsity(), Sparsity::sparse(nc_, 1)))); if (hasSetOption("sdp_solver_options")) { sdpsolver_.setOption(getOption("sdp_solver_options")); } // Initialize the SDP solver sdpsolver_.init(); sdpsolver_.input(SDP_SOLVER_C).at(n_)=1; // Output arguments setNumOutputs(SDQP_SOLVER_NUM_OUT); output(SDQP_SOLVER_X) = DMatrix::zeros(n_, 1); std::vector<int> r = range(input(SDQP_SOLVER_G).size1()); output(SDQP_SOLVER_P) = sdpsolver_.output(SDP_SOLVER_P).isEmpty() ? DMatrix() : sdpsolver_.output(SDP_SOLVER_P)(r, r); output(SDQP_SOLVER_DUAL) = sdpsolver_.output(SDP_SOLVER_DUAL).isEmpty() ? DMatrix() : sdpsolver_.output(SDP_SOLVER_DUAL)(r, r); output(SDQP_SOLVER_COST) = 0.0; output(SDQP_SOLVER_DUAL_COST) = 0.0; output(SDQP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1); output(SDQP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1); }
::Spacy::LinearSolver LinearOperator::solver() const { return LinearSolver( get() ); }
Function implicitRK(Function& f, const std::string& impl, const Dictionary& impl_options, const MX& tf, int order, const std::string& scheme, int ne) { casadi_assert_message(ne>=1, "Parameter ne (number of elements must be at least 1), " "but got " << ne << "."); casadi_assert_message(order==4, "Only RK order 4 is supported now."); casadi_assert_message(f.getNumInputs()==DAE_NUM_IN && f.getNumOutputs()==DAE_NUM_OUT, "Supplied function must adhere to dae scheme."); casadi_assert_message(f.output(DAE_QUAD).isEmpty(), "Supplied function cannot have quadrature states."); // Obtain collocation points std::vector<double> tau_root = collocationPoints(order, "legendre"); // Retrieve collocation interpolating matrices std::vector < std::vector <double> > C; std::vector < double > D; collocationInterpolators(tau_root, C, D); // Retrieve problem dimensions int nx = f.input(DAE_X).size(); int nz = f.input(DAE_Z).size(); int np = f.input(DAE_P).size(); //Variables for one finite element MX X = MX::sym("X", nx); MX P = MX::sym("P", np); MX V = MX::sym("V", order*(nx+nz)); // Unknowns MX X0 = X; // Components of the unknowns that correspond to states at collocation points std::vector<MX> Xc;Xc.reserve(order); Xc.push_back(X0); // Components of the unknowns that correspond to algebraic states at collocation points std::vector<MX> Zc;Zc.reserve(order); // Splitting the unknowns std::vector<int> splitPositions = range(0, order*nx, nx); if (nz>0) { std::vector<int> Zc_pos = range(order*nx, order*nx+(order+1)*nz, nz); splitPositions.insert(splitPositions.end(), Zc_pos.begin(), Zc_pos.end()); } else { splitPositions.push_back(order*nx); } std::vector<MX> Vs = vertsplit(V, splitPositions); // Extracting unknowns from Z for (int i=0;i<order;++i) { Xc.push_back(X0+Vs[i]); } if (nz>0) { for (int i=0;i<order;++i) { Zc.push_back(Vs[order+i]); } } // Get the collocation Equations (that define V) std::vector<MX> V_eq; // Local start time MX t0_l=MX::sym("t0"); MX h = MX::sym("h"); for (int j=1;j<order+1;++j) { // Expression for the state derivative at the collocation point MX xp_j = 0; for (int r=0;r<order+1;++r) { xp_j+= C[j][r]*Xc[r]; } // Append collocation equations & algebraic constraints std::vector<MX> f_out; MX t_l = t0_l+tau_root[j]*h; if (nz>0) { f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P, "z", Zc[j-1])); } else { f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P)); } V_eq.push_back(h*f_out[DAE_ODE]-xp_j); V_eq.push_back(f_out[DAE_ALG]); } // Root-finding function, implicitly defines V as a function of X0 and P std::vector<MX> vfcn_inputs; vfcn_inputs.push_back(V); vfcn_inputs.push_back(X); vfcn_inputs.push_back(P); vfcn_inputs.push_back(t0_l); vfcn_inputs.push_back(h); Function vfcn = MXFunction(vfcn_inputs, vertcat(V_eq)); vfcn.init(); try { // Attempt to convert to SXFunction to decrease overhead vfcn = SXFunction(vfcn); vfcn.init(); } catch(CasadiException & e) { // } // Create a implicit function instance to solve the system of equations ImplicitFunction ifcn(impl, vfcn, Function(), LinearSolver()); ifcn.setOption(impl_options); ifcn.init(); // Get an expression for the state at the end of the finite element std::vector<MX> ifcn_call_in(5); ifcn_call_in[0] = MX::zeros(V.sparsity()); std::copy(vfcn_inputs.begin()+1, vfcn_inputs.end(), ifcn_call_in.begin()+1); std::vector<MX> ifcn_call_out = ifcn.call(ifcn_call_in, true); Vs = vertsplit(ifcn_call_out[0], splitPositions); MX XF = 0; for (int i=0;i<order+1;++i) { XF += D[i]*(i==0? X : X + Vs[i-1]); } // Get the discrete time dynamics ifcn_call_in.erase(ifcn_call_in.begin()); MXFunction F = MXFunction(ifcn_call_in, XF); F.init(); // Loop over all finite elements MX h_ = tf/ne; MX t0_ = 0; for (int i=0;i<ne;++i) { std::vector<MX> F_in; F_in.push_back(X); F_in.push_back(P); F_in.push_back(t0_); F_in.push_back(h_); t0_+= h_; std::vector<MX> F_out = F.call(F_in); X = F_out[0]; } // Create a ruturn function with Integrator signature MXFunction ret = MXFunction(integratorIn("x0", X0, "p", P), integratorOut("xf", X)); ret.init(); return ret; }
void SdqpToSdp::init() { // Initialize the base classes SdqpSolverInternal::init(); cholesky_ = LinearSolver("cholesky", "csparsecholesky", st_[SDQP_STRUCT_H]); MX g_socp = MX::sym("x", cholesky_.getFactorizationSparsity(true)); MX h_socp = MX::sym("h", n_); MX f_socp = sqrt(inner_prod(h_socp, h_socp)); MX en_socp = 0.5/f_socp; MX f_sdqp = MX::sym("f", input(SDQP_SOLVER_F).sparsity()); MX g_sdqp = MX::sym("g", input(SDQP_SOLVER_G).sparsity()); std::vector<MX> fi(n_+1); MX znp = MX(n_+1, n_+1); for (int k=0;k<n_;++k) { MX gk = vertcat(g_socp(ALL, k), MX(1, 1)); MX fk = -blockcat(znp, gk, gk.T(), MX(1, 1)); // TODO(Joel): replace with ALL fi.push_back(diagcat(f_sdqp(ALL, Slice(f_sdqp.size1()*k, f_sdqp.size1()*(k+1))), fk)); } MX fin = en_socp*DMatrix::eye(n_+2); fin(n_, n_+1) = en_socp; fin(n_+1, n_) = en_socp; fi.push_back(diagcat(DMatrix(f_sdqp.size1(), f_sdqp.size1()), -fin)); MX h0 = vertcat(h_socp, DMatrix(1, 1)); MX g = blockcat(f_socp*DMatrix::eye(n_+1), h0, h0.T(), f_socp); g = diagcat(g_sdqp, g); Dict opts; opts["input_scheme"] = IOScheme("g_socp", "h_socp", "f_sdqp", "g_sdqp"); opts["output_scheme"] = IOScheme("f", "g"); mapping_ = MXFunction("mapping", make_vector(g_socp, h_socp, f_sdqp, g_sdqp), make_vector(horzcat(fi), g), opts); Dict options; if (hasSetOption(optionsname())) options = getOption(optionsname()); // Create an SdpSolver instance solver_ = SdpSolver("sdpsolver", getOption(solvername()), make_map("g", mapping_.output("g").sparsity(), "f", mapping_.output("f").sparsity(), "a", horzcat(input(SDQP_SOLVER_A).sparsity(), Sparsity(nc_, 1))), options); solver_.input(SDP_SOLVER_C).at(n_)=1; // Output arguments obuf_.resize(SDQP_SOLVER_NUM_OUT); output(SDQP_SOLVER_X) = DMatrix::zeros(n_, 1); std::vector<int> r = range(input(SDQP_SOLVER_G).size1()); output(SDQP_SOLVER_P) = solver_.output(SDP_SOLVER_P).isempty() ? DMatrix() : solver_.output(SDP_SOLVER_P)(r, r); output(SDQP_SOLVER_DUAL) = solver_.output(SDP_SOLVER_DUAL).isempty() ? DMatrix() : solver_.output(SDP_SOLVER_DUAL)(r, r); output(SDQP_SOLVER_COST) = 0.0; output(SDQP_SOLVER_DUAL_COST) = 0.0; output(SDQP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1); output(SDQP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1); }
void ImplicitFunctionInternal::init() { // Initialize the residual function f_.init(false); // Which input/output correspond to the root-finding problem? iin_ = getOption("implicit_input"); iout_ = getOption("implicit_output"); // Get the number of equations and check consistency casadi_assert_message(iin_>=0 && iin_<f_.getNumInputs() && f_.getNumInputs()>0, "Implicit input not in range"); casadi_assert_message(iout_>=0 && iout_<f_.getNumOutputs() && f_.getNumOutputs()>0, "Implicit output not in range"); casadi_assert_message(f_.output(iout_).isDense() && f_.output(iout_).isVector(), "Residual must be a dense vector"); casadi_assert_message(f_.input(iin_).isDense() && f_.input(iin_).isVector(), "Unknown must be a dense vector"); n_ = f_.output(iout_).size(); casadi_assert_message(n_ == f_.input(iin_).size(), "Dimension mismatch. Input size is " << f_.input(iin_).size() << ", while output size is " << f_.output(iout_).size()); // Allocate inputs setNumInputs(f_.getNumInputs()); for (int i=0; i<getNumInputs(); ++i) { input(i) = f_.input(i); } // Allocate output setNumOutputs(f_.getNumOutputs()); for (int i=0; i<getNumOutputs(); ++i) { output(i) = f_.output(i); } // Same input and output schemes setInputScheme(f_.getInputScheme()); setOutputScheme(f_.getOutputScheme()); // Call the base class initializer FunctionInternal::init(); // Generate Jacobian if not provided if (jac_.isNull()) jac_ = f_.jacobian(iin_, iout_); jac_.init(false); // Check for structural singularity in the Jacobian casadi_assert_message( !jac_.output().sparsity().isSingular(), "ImplicitFunctionInternal::init: singularity - the jacobian is structurally rank-deficient. " "sprank(J)=" << sprank(jac_.output()) << " (instead of "<< jac_.output().size1() << ")"); // Get the linear solver creator function if (linsol_.isNull()) { if (hasSetOption("linear_solver")) { std::string linear_solver_name = getOption("linear_solver"); // Allocate an NLP solver linsol_ = LinearSolver(linear_solver_name, jac_.output().sparsity(), 1); // Pass options if (hasSetOption("linear_solver_options")) { const Dictionary& linear_solver_options = getOption("linear_solver_options"); linsol_.setOption(linear_solver_options); } // Initialize linsol_.init(); } } else { // Initialize the linear solver, if provided linsol_.init(false); casadi_assert(linsol_.input().sparsity()==jac_.output().sparsity()); } // No factorization yet; fact_up_to_date_ = false; // Constraints if (hasSetOption("constraints")) u_c_ = getOption("constraints"); casadi_assert_message(u_c_.size()==n_ || u_c_.empty(), "Constraint vector if supplied, must be of length n, but got " << u_c_.size() << " and n = " << n_); }