//--------------------------------------------------------------------------------------
//
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());
}
