//-------------------------------------------------------------------------------------- // void fourier_direct (GF_Bloc_ImTime const & Gt, GF_Bloc_ImFreq & Gw, bool time_mesh_starts_at_half_bin) { double Beta = Gw.Beta; //assert(Gw.Statistic==Fermion); // Boson not implemented. check_have_same_structure (Gw,Gt,false,true); assert (Gw.mesh.index_min==0); Gw.zero(); Array<COMPLEX,1> G_out(Range(Gw.mesh.index_min, Gw.mesh.index_max)), G_in(Range(Gt.mesh.index_min, Gt.mesh.index_max));//G_in(Gt.mesh.len()); Gw.tail = Gt.tail; int N1 = Gt.N1, N2 = Gt.N2; double t; for (int n1=1; n1<=N1;n1++) for(int n2=1; n2<=N2;n2++) { COMPLEX d= Gw.tail[1](n1,n2), A= Gw.tail[2](n1,n2),B = Gw.tail[3](n1,n2); double b1, b2, b3; COMPLEX a1, a2, a3; if (Gw.Statistic == Fermion){ b1 = 0; b2 =1; b3 =-1; a1 = d-B; a2 = (A+B)/2; a3 = (B-A)/2; } else { b1 = -0.5; b2 =-1; b3 =1; a1=4*(d-B)/3; a2=B-(d+A)/2; a3=d/6+A/2+B/3; } G_in = 0; for (int i =Gt.mesh.index_min; i <= Gt.mesh.index_max ; i++) { t = real(Gt.mesh[i]); if (Gw.Statistic == Fermion){ G_in(i) = Gt.data_const(n1,n2,i) - ( oneFermion(a1,b1,t,Beta) + oneFermion(a2,b2,t,Beta)+ oneFermion(a3,b3,t,Beta) ); G_in(i) *= exp(I*Pi*t/Beta); } else { G_in(i) = Gt.data_const(n1,n2,i) - ( oneBoson(a1,b1,t,Beta) + oneBoson(a2,b2,t,Beta)+ oneBoson(a3,b3,t,Beta) ); } } G_in *= Beta/Gt.numberTimeSlices; fourier_base(G_in, G_out, true); // const COMPLEX C(I*Pi/Gt.mesh.lenTotal()); for (int om = Gw.mesh.index_min; om<= Gw.mesh.index_max ; om++) { COMPLEX om1 = Gw.mesh[om]; if (time_mesh_starts_at_half_bin) { Gw.data(n1,n2,om) = G_out(om)*exp(I*om*Pi/Gt.numberTimeSlices) + a1/(om1 - b1) + a2 / (om1-b2) + a3/(om1-b3); } else { Gw.data(n1,n2,om) = G_out(om) + a1/(om1 - b1) + a2 / (om1-b2) + a3/(om1-b3); } } } }
void fourier_inverse (GF_Bloc_ImFreq const & Gw, GF_Bloc_ImTime & Gt, bool time_mesh_starts_at_half_bin) { if (abs(Gw.Beta - Gt.Beta)> 1.e-10) TRIQS_RUNTIME_ERROR<<"The time and frequency Green function are not at the same Beta"; double Beta = Gw.Beta; //assert(Gw.Statistic==Fermion); // Boson not implemented. check_have_same_structure (Gw,Gt,false,true); assert (Gw.mesh.index_min==0); Gt.zero(); Array<COMPLEX,1> G_out(Range(Gt.mesh.index_min, Gt.mesh.index_max)); Array<COMPLEX,1> G_in(Range(Gw.mesh.index_min, Gw.mesh.index_max)); Gt.tail = Gw.tail; int N1 = Gw.N1, N2 = Gw.N2; for (int n1=1; n1<=N1;n1++) for (int n2=1; n2<=N2;n2++) { COMPLEX d= Gt.tail[1](n1,n2), A= Gt.tail[2](n1,n2),B = Gt.tail[3](n1,n2); //double b1 = 0,b2 =1, b3 =-1; //COMPLEX a1 = d-B, a2 = (A+B)/2, a3 = (B-A)/2; double b1, b2, b3; COMPLEX a1, a2, a3; if (Gw.Statistic == Fermion){ b1 = 0; b2 =1; b3 =-1; a1 = d-B; a2 = (A+B)/2; a3 = (B-A)/2; } else { b1 = -0.5; b2 =-1; b3 =1; a1=4*(d-B)/3; a2=B-(d+A)/2; a3=d/6+A/2+B/3; } G_in = 0; for (int i = Gw.mesh.index_min; i <= Gw.mesh.index_max ; i++) { COMPLEX om1 = Gw.mesh[i]; G_in(i) = Gw.data_const(n1,n2,i) - ( a1/(om1 - b1) + a2 / (om1-b2) + a3/(om1-b3) ); // you need an extra factor if the time mesh starts at 0.5*Beta/L if (time_mesh_starts_at_half_bin) G_in(i) *= exp(-i*I*Pi/Gt.numberTimeSlices); } // for bosons GF(w=0) is divided by 2 to avoid counting it twice if (Gw.Statistic == Boson && !Green_Function_Are_Complex_in_time ) G_in(Gw.mesh.index_min) *= 0.5; G_out = 0; fourier_base(G_in, G_out, false); // If the Green function are NOT complex, then one use the symmetry property // fold the sum and get a factor 2 double fact = (Green_Function_Are_Complex_in_time ? 1 : 2),t; G_out = G_out*(fact/Beta); for (int i =Gt.mesh.index_min; i<= Gt.mesh.index_max ; i++) { t= real(Gt.mesh[i]); if(Gw.Statistic == Fermion){ Gt.data(n1,n2,i) = convert_green(G_out(i)*exp(-I*Pi*t/Beta) + oneFermion(a1,b1,t,Beta) + oneFermion(a2,b2,t,Beta)+ oneFermion(a3,b3,t,Beta) );} else { Gt.data(n1,n2,i) = convert_green(G_out(i) + oneBoson(a1,b1,t,Beta) + oneBoson(a2,b2,t,Beta)+ oneBoson(a3,b3,t,Beta) );} } } }
void fourier_direct(GF_Bloc_ReTime const & Gt, GF_Bloc_ReFreq & Gw) { assert(Gw.Statistic==Fermion); // Boson not implemented. check_have_same_structure (Gw,Gt,false,true); assert (Gw.mesh.index_min==0); assert (Gt.mesh.index_min==0); const int N(Gw.mesh.len()); const int Nt(Gt.mesh.len()); const double omega0(2*pi/(Gt.TimeMax - Gt.TimeMin)); const double tmin( Gt.TimeMin); const double Deltat((Gt.TimeMax - Gt.TimeMin)); const double fact(Deltat/N); test_conformity(Gt,Gw); Gw.zero(); Array<COMPLEX,1> G_out(N),G_in(N); double t; COMPLEX om; // check the frequency grid for (int i = 0; i <= N/2 ; i++) { om = Gw.mesh[i]; assert ( abs (om - (-N/2 + i)*omega0) < 1.e-5); } for (int i = N/2+1; i < N ; i++) { om = Gw.mesh[i]; assert ( abs (om - (-N/2 + i)*omega0) < 1.e-5); } Gw.tail = Gt.tail; int N1 = Gw.N1, N2 = Gw.N2; for (int n1=1; n1<=N1;n1++) for (int n2=1; n2<=N2;n2++) { COMPLEX t1= Gw.tail[1](n1,n2), t2= Gw.tail[2](n1,n2); // t3 = tail[3](n1,n2); COMPLEX a1 = (t1 - I * t2)/2, a2 = (t1 + I * t2)/2; G_in = 0; for (int i =0; i<N ; i++) { t= real(Gt.mesh[i]); G_in(i) = Gt.data_const(n1,n2,i) - ( a1*TH_EXPO(t,1) + a2 * TH_EXPO_NEG(t,1) ); } fourier_base(G_in, G_out, true); for (int i = 0; i < N ; i++) { om = Gw.mesh[i]; int p = (i+ N/2)%N; Gw.data(n1,n2,i) = G_out(p)*exp(I*om*(tmin+ fact/2))*fact + ( a1/(om + I) + a2 / (om - I)) ; } } }
void fourier_inverse (GF_Bloc_ReFreq const & Gw, GF_Bloc_ReTime & Gt ) { assert(Gw.Statistic==Fermion); // Boson not implemented. check_have_same_structure (Gw,Gt,false,true); assert (Gw.mesh.index_min==0); const int N(Gw.mesh.index_max - Gw.mesh.index_min+1); const int Nt(Gt.mesh.index_max - Gt.mesh.index_min+1); const double tmin( Gt.TimeMin); const double Deltat((Gt.TimeMax - Gt.TimeMin)); const double fact(Deltat/N); const double pi = acos(-1); test_conformity(Gt,Gw); Gt.zero(); Array<COMPLEX,1> G_out(N),G_in(N); double t; COMPLEX om; Gt.tail = Gw.tail; int N1 = Gt.N1, N2 = Gt.N2; for (int n1=1; n1<=N1;n1++) for (int n2=1; n2<=N2;n2++) { COMPLEX t1= Gt.tail[1](n1,n2), t2= Gt.tail[2](n1,n2); // t3 = tail[3](n1,n2); COMPLEX a1 = (t1 - I * t2)/2, a2 = (t1 + I * t2)/2; G_in = 0; for (int i = 0; i < N ; i++) { om = Gw.mesh[i]; int p = (i+ N/2)%N; G_in(p) = Gw.data_const(n1,n2,i)/(exp(I*om*(tmin + fact/2))*fact) - ( a1/(om + I) + a2 / (om - I)) ; } fourier_base(G_in, G_out, false); const double corr(1.0/N); for (int i =0; i<N ; i++) { t= real(Gt.mesh[i]); Gt.data(n1,n2,i) = corr*( G_out(i) + ( a1*TH_EXPO(t,1) + a2 * TH_EXPO_NEG(t,1) ) ); } } }
void CollocationIntegratorInternal::setupFG() { // Interpolation order deg_ = getOption("interpolation_order"); // All collocation time points std::vector<long double> tau_root = collocationPointsL(deg_, getOption("collocation_scheme")); // Coefficients of the collocation equation vector<vector<double> > C(deg_+1, vector<double>(deg_+1, 0)); // Coefficients of the continuity equation vector<double> D(deg_+1, 0); // Coefficients of the quadratures vector<double> B(deg_+1, 0); // For all collocation points for (int j=0; j<deg_+1; ++j) { // Construct Lagrange polynomials to get the polynomial basis at the collocation point Polynomial p = 1; for (int r=0; r<deg_+1; ++r) { if (r!=j) { p *= Polynomial(-tau_root[r], 1)/(tau_root[j]-tau_root[r]); } } // Evaluate the polynomial at the final time to get the // coefficients of the continuity equation D[j] = zeroIfSmall(p(1.0L)); // Evaluate the time derivative of the polynomial at all collocation points to // get the coefficients of the continuity equation Polynomial dp = p.derivative(); for (int r=0; r<deg_+1; ++r) { C[j][r] = zeroIfSmall(dp(tau_root[r])); } // Integrate polynomial to get the coefficients of the quadratures Polynomial ip = p.anti_derivative(); B[j] = zeroIfSmall(ip(1.0L)); } // Symbolic inputs MX x0 = MX::sym("x0", f_.input(DAE_X).sparsity()); MX p = MX::sym("p", f_.input(DAE_P).sparsity()); MX t = MX::sym("t", f_.input(DAE_T).sparsity()); // Implicitly defined variables (z and x) MX v = MX::sym("v", deg_*(nx_+nz_)); vector<int> v_offset(1, 0); for (int d=0; d<deg_; ++d) { v_offset.push_back(v_offset.back()+nx_); v_offset.push_back(v_offset.back()+nz_); } vector<MX> vv = vertsplit(v, v_offset); vector<MX>::const_iterator vv_it = vv.begin(); // Collocated states vector<MX> x(deg_+1), z(deg_+1); for (int d=1; d<=deg_; ++d) { x[d] = reshape(*vv_it++, this->x0().shape()); z[d] = reshape(*vv_it++, this->z0().shape()); } casadi_assert(vv_it==vv.end()); // Collocation time points vector<MX> tt(deg_+1); for (int d=0; d<=deg_; ++d) { tt[d] = t + h_*tau_root[d]; } // Equations that implicitly define v vector<MX> eq; // Quadratures MX qf = MX::zeros(f_.output(DAE_QUAD).sparsity()); // End state MX xf = D[0]*x0; // For all collocation points for (int j=1; j<deg_+1; ++j) { //for (int j=deg_; j>=1; --j) { // Evaluate the DAE vector<MX> f_arg(DAE_NUM_IN); f_arg[DAE_T] = tt[j]; f_arg[DAE_P] = p; f_arg[DAE_X] = x[j]; f_arg[DAE_Z] = z[j]; vector<MX> f_res = f_.call(f_arg); // Get an expression for the state derivative at the collocation point MX xp_j = C[0][j] * x0; for (int r=1; r<deg_+1; ++r) { xp_j += C[r][j] * x[r]; } // Add collocation equation eq.push_back(vec(h_*f_res[DAE_ODE] - xp_j)); // Add the algebraic conditions eq.push_back(vec(f_res[DAE_ALG])); // Add contribution to the final state xf += D[j]*x[j]; // Add contribution to quadratures qf += (B[j]*h_)*f_res[DAE_QUAD]; } // Form forward discrete time dynamics vector<MX> F_in(DAE_NUM_IN); F_in[DAE_T] = t; F_in[DAE_X] = x0; F_in[DAE_P] = p; F_in[DAE_Z] = v; vector<MX> F_out(DAE_NUM_OUT); F_out[DAE_ODE] = xf; F_out[DAE_ALG] = vertcat(eq); F_out[DAE_QUAD] = qf; F_ = MXFunction(F_in, F_out); F_.init(); // Backwards dynamics // NOTE: The following is derived so that it will give the exact adjoint // sensitivities whenever g is the reverse mode derivative of f. if (!g_.isNull()) { // Symbolic inputs MX rx0 = MX::sym("x0", g_.input(RDAE_RX).sparsity()); MX rp = MX::sym("p", g_.input(RDAE_RP).sparsity()); // Implicitly defined variables (rz and rx) MX rv = MX::sym("v", deg_*(nrx_+nrz_)); vector<int> rv_offset(1, 0); for (int d=0; d<deg_; ++d) { rv_offset.push_back(rv_offset.back()+nrx_); rv_offset.push_back(rv_offset.back()+nrz_); } vector<MX> rvv = vertsplit(rv, rv_offset); vector<MX>::const_iterator rvv_it = rvv.begin(); // Collocated states vector<MX> rx(deg_+1), rz(deg_+1); for (int d=1; d<=deg_; ++d) { rx[d] = reshape(*rvv_it++, this->rx0().shape()); rz[d] = reshape(*rvv_it++, this->rz0().shape()); } casadi_assert(rvv_it==rvv.end()); // Equations that implicitly define v eq.clear(); // Quadratures MX rqf = MX::zeros(g_.output(RDAE_QUAD).sparsity()); // End state MX rxf = D[0]*rx0; // For all collocation points for (int j=1; j<deg_+1; ++j) { // Evaluate the backward DAE vector<MX> g_arg(RDAE_NUM_IN); g_arg[RDAE_T] = tt[j]; g_arg[RDAE_P] = p; g_arg[RDAE_X] = x[j]; g_arg[RDAE_Z] = z[j]; g_arg[RDAE_RX] = rx[j]; g_arg[RDAE_RZ] = rz[j]; g_arg[RDAE_RP] = rp; vector<MX> g_res = g_.call(g_arg); // Get an expression for the state derivative at the collocation point MX rxp_j = -D[j]*rx0; for (int r=1; r<deg_+1; ++r) { rxp_j += (B[r]*C[j][r]) * rx[r]; } // Add collocation equation eq.push_back(vec(h_*B[j]*g_res[RDAE_ODE] - rxp_j)); // Add the algebraic conditions eq.push_back(vec(g_res[RDAE_ALG])); // Add contribution to the final state rxf += -B[j]*C[0][j]*rx[j]; // Add contribution to quadratures rqf += h_*B[j]*g_res[RDAE_QUAD]; } // Form backward discrete time dynamics vector<MX> G_in(RDAE_NUM_IN); G_in[RDAE_T] = t; G_in[RDAE_X] = x0; G_in[RDAE_P] = p; G_in[RDAE_Z] = v; G_in[RDAE_RX] = rx0; G_in[RDAE_RP] = rp; G_in[RDAE_RZ] = rv; vector<MX> G_out(RDAE_NUM_OUT); G_out[RDAE_ODE] = rxf; G_out[RDAE_ALG] = vertcat(eq); G_out[RDAE_QUAD] = rqf; G_ = MXFunction(G_in, G_out); G_.init(); } }
void LiftedSQPInternal::init(){ // Call the init method of the base class NlpSolverInternal::init(); // Number of lifted variables nv = getOption("num_lifted"); if(verbose_){ cout << "Initializing SQP method with " << nx_ << " variables and " << ng_ << " constraints." << endl; cout << "Lifting " << nv << " variables." << endl; if(gauss_newton_){ cout << "Gauss-Newton objective with " << F_.input().numel() << " terms." << endl; } } // Read options max_iter_ = getOption("max_iter"); max_iter_ls_ = getOption("max_iter_ls"); toldx_ = getOption("toldx"); tolgl_ = getOption("tolgl"); sigma_ = getOption("sigma"); rho_ = getOption("rho"); mu_safety_ = getOption("mu_safety"); eta_ = getOption("eta"); tau_ = getOption("tau"); // Assume SXFunction for now SXFunction ffcn = shared_cast<SXFunction>(F_); casadi_assert(!ffcn.isNull()); SXFunction gfcn = shared_cast<SXFunction>(G_); casadi_assert(!gfcn.isNull()); // Extract the free variables and split into independent and dependent variables SX x = ffcn.inputExpr(0); int nx = x.size(); nu = nx-nv; SX u = x[Slice(0,nu)]; SX v = x[Slice(nu,nu+nv)]; // Extract the constraint equations and split into constraints and definitions of dependent variables SX f1 = ffcn.outputExpr(0); int nf1 = f1.numel(); SX g = gfcn.outputExpr(0); int nf2 = g.numel()-nv; SX v_eq = g(Slice(0,nv)); SX f2 = g(Slice(nv,nv+nf2)); // Definition of v SX v_def = v_eq + v; // Objective function SX f; // Multipliers SX lam_x, lam_g, lam_f2; if(gauss_newton_){ // Least square objective f = inner_prod(f1,f1)/2; } else { // Scalar objective function f = f1; // Lagrange multipliers for the simple bounds on u SX lam_u = ssym("lam_u",nu); // Lagrange multipliers for the simple bounds on v SX lam_v = ssym("lam_v",nv); // Lagrange multipliers for the simple bounds on x lam_x = vertcat(lam_u,lam_v); // Lagrange multipliers corresponding to the definition of the dependent variables SX lam_v_eq = ssym("lam_v_eq",nv); // Lagrange multipliers for the nonlinear constraints that aren't eliminated lam_f2 = ssym("lam_f2",nf2); if(verbose_){ cout << "Allocated intermediate variables." << endl; } // Lagrange multipliers for constraints lam_g = vertcat(lam_v_eq,lam_f2); // Lagrangian function SX lag = f + inner_prod(lam_x,x); if(!f2.empty()) lag += inner_prod(lam_f2,f2); if(!v.empty()) lag += inner_prod(lam_v_eq,v_def); // Gradient of the Lagrangian SX lgrad = casadi::gradient(lag,x); if(!v.empty()) lgrad -= vertcat(SX::zeros(nu),lam_v_eq); // Put here to ensure that lgrad is of the form "h_extended -v_extended" makeDense(lgrad); if(verbose_){ cout << "Generated the gradient of the Lagrangian." << endl; } // Condensed gradient of the Lagrangian f1 = lgrad[Slice(0,nu)]; nf1 = nu; // Gradient of h SX v_eq_grad = lgrad[Slice(nu,nu+nv)]; // Reverse lam_v_eq and v_eq_grad SX v_eq_grad_reversed = v_eq_grad; copy(v_eq_grad.rbegin(),v_eq_grad.rend(),v_eq_grad_reversed.begin()); SX lam_v_eq_reversed = lam_v_eq; copy(lam_v_eq.rbegin(),lam_v_eq.rend(),lam_v_eq_reversed.begin()); // Augment h and lam_v_eq v_eq.append(v_eq_grad_reversed); v.append(lam_v_eq_reversed); } // Residual function G SXVector G_in(G_NUM_IN); G_in[G_X] = x; G_in[G_LAM_X] = lam_x; G_in[G_LAM_G] = lam_g; SXVector G_out(G_NUM_OUT); G_out[G_D] = v_eq; G_out[G_G] = g; G_out[G_F] = f; rfcn_ = SXFunction(G_in,G_out); rfcn_.setOption("number_of_fwd_dir",0); rfcn_.setOption("number_of_adj_dir",0); rfcn_.setOption("live_variables",true); rfcn_.init(); if(verbose_){ cout << "Generated residual function ( " << shared_cast<SXFunction>(rfcn_).getAlgorithmSize() << " nodes)." << endl; } // Difference vector d SX d = ssym("d",nv); if(!gauss_newton_){ vector<SX> dg = ssym("dg",nv).data(); reverse(dg.begin(),dg.end()); d.append(dg); } // Substitute out the v from the h SX d_def = (v_eq + v)-d; SXVector ex(3); ex[0] = f1; ex[1] = f2; ex[2] = f; substituteInPlace(v, d_def, ex, false); SX f1_z = ex[0]; SX f2_z = ex[1]; SX f_z = ex[2]; // Modified function Z enum ZIn{Z_U,Z_D,Z_LAM_X,Z_LAM_F2,Z_NUM_IN}; SXVector zfcn_in(Z_NUM_IN); zfcn_in[Z_U] = u; zfcn_in[Z_D] = d; zfcn_in[Z_LAM_X] = lam_x; zfcn_in[Z_LAM_F2] = lam_f2; enum ZOut{Z_D_DEF,Z_F12,Z_NUM_OUT}; SXVector zfcn_out(Z_NUM_OUT); zfcn_out[Z_D_DEF] = d_def; zfcn_out[Z_F12] = vertcat(f1_z,f2_z); SXFunction zfcn(zfcn_in,zfcn_out); zfcn.init(); if(verbose_){ cout << "Generated reconstruction function ( " << zfcn.getAlgorithmSize() << " nodes)." << endl; } // Matrix A and B in lifted Newton SX B = zfcn.jac(Z_U,Z_F12); SX B1 = B(Slice(0,nf1),Slice(0,B.size2())); SX B2 = B(Slice(nf1,B.size1()),Slice(0,B.size2())); if(verbose_){ cout << "Formed B1 (dimension " << B1.size1() << "-by-" << B1.size2() << ", "<< B1.size() << " nonzeros) " << "and B2 (dimension " << B2.size1() << "-by-" << B2.size2() << ", "<< B2.size() << " nonzeros)." << endl; } // Step in u SX du = ssym("du",nu); SX dlam_f2 = ssym("dlam_f2",lam_f2.sparsity()); SX b1 = f1_z; SX b2 = f2_z; SX e; if(nv > 0){ // Directional derivative of Z vector<vector<SX> > Z_fwdSeed(2,zfcn_in); vector<vector<SX> > Z_fwdSens(2,zfcn_out); vector<vector<SX> > Z_adjSeed; vector<vector<SX> > Z_adjSens; Z_fwdSeed[0][Z_U].setZero(); Z_fwdSeed[0][Z_D] = -d; Z_fwdSeed[0][Z_LAM_X].setZero(); Z_fwdSeed[0][Z_LAM_F2].setZero(); Z_fwdSeed[1][Z_U] = du; Z_fwdSeed[1][Z_D] = -d; Z_fwdSeed[1][Z_LAM_X].setZero(); Z_fwdSeed[1][Z_LAM_F2] = dlam_f2; zfcn.eval(zfcn_in,zfcn_out,Z_fwdSeed,Z_fwdSens,Z_adjSeed,Z_adjSens); b1 += Z_fwdSens[0][Z_F12](Slice(0,nf1)); b2 += Z_fwdSens[0][Z_F12](Slice(nf1,B.size1())); e = Z_fwdSens[1][Z_D_DEF]; } if(verbose_){ cout << "Formed b1 (dimension " << b1.size1() << "-by-" << b1.size2() << ", "<< b1.size() << " nonzeros) " << "and b2 (dimension " << b2.size1() << "-by-" << b2.size2() << ", "<< b2.size() << " nonzeros)." << endl; } // Generate Gauss-Newton Hessian if(gauss_newton_){ b1 = mul(trans(B1),b1); B1 = mul(trans(B1),B1); if(verbose_){ cout << "Gauss Newton Hessian (dimension " << B1.size1() << "-by-" << B1.size2() << ", "<< B1.size() << " nonzeros)." << endl; } } // Make sure b1 and b2 are dense vectors makeDense(b1); makeDense(b2); // Quadratic approximation SXVector lfcn_in(LIN_NUM_IN); lfcn_in[LIN_X] = x; lfcn_in[LIN_D] = d; lfcn_in[LIN_LAM_X] = lam_x; lfcn_in[LIN_LAM_G] = lam_g; SXVector lfcn_out(LIN_NUM_OUT); lfcn_out[LIN_F1] = b1; lfcn_out[LIN_J1] = B1; lfcn_out[LIN_F2] = b2; lfcn_out[LIN_J2] = B2; lfcn_ = SXFunction(lfcn_in,lfcn_out); // lfcn_.setOption("verbose",true); lfcn_.setOption("number_of_fwd_dir",0); lfcn_.setOption("number_of_adj_dir",0); lfcn_.setOption("live_variables",true); lfcn_.init(); if(verbose_){ cout << "Generated linearization function ( " << shared_cast<SXFunction>(lfcn_).getAlgorithmSize() << " nodes)." << endl; } // Step expansion SXVector efcn_in(EXP_NUM_IN); copy(lfcn_in.begin(),lfcn_in.end(),efcn_in.begin()); efcn_in[EXP_DU] = du; efcn_in[EXP_DLAM_F2] = dlam_f2; efcn_ = SXFunction(efcn_in,e); efcn_.setOption("number_of_fwd_dir",0); efcn_.setOption("number_of_adj_dir",0); efcn_.setOption("live_variables",true); efcn_.init(); if(verbose_){ cout << "Generated step expansion function ( " << shared_cast<SXFunction>(efcn_).getAlgorithmSize() << " nodes)." << endl; } // Current guess for the primal solution DMatrix &x_k = output(NLP_SOLVER_X); // Current guess for the dual solution DMatrix &lam_x_k = output(NLP_SOLVER_LAM_X); DMatrix &lam_g_k = output(NLP_SOLVER_LAM_G); // Allocate a QP solver QpSolverCreator qp_solver_creator = getOption("qp_solver"); qp_solver_ = qp_solver_creator(B1.sparsity(),B2.sparsity()); // Set options if provided if(hasSetOption("qp_solver_options")){ Dictionary qp_solver_options = getOption("qp_solver_options"); qp_solver_.setOption(qp_solver_options); } // Initialize the QP solver qp_solver_.init(); if(verbose_){ cout << "Allocated QP solver." << endl; } // Residual d_k_ = DMatrix(d.sparsity(),0); // Primal step dx_k_ = DMatrix(x_k.sparsity()); // Dual step dlam_x_k_ = DMatrix(lam_x_k.sparsity()); dlam_g_k_ = DMatrix(lam_g_k.sparsity()); }