int main(){
// Declare variables
SX u = SX::sym("u"); // control
SX r = SX::sym("r"), s = SX::sym("s"); // states
SX x = vertcat(r,s);
// Number of differential states
int nx = x.size1();
// Number of controls
int nu = u.size1();
// Bounds and initial guess for the control
vector<double> u_min = { -0.75 };
vector<double> u_max = { 1.0 };
vector<double> u_init = { 0.0 };
// Bounds and initial guess for the state
vector<double> x0_min = { 0, 1 };
vector<double> x0_max = { 0, 1 };
vector<double> x_min = {-inf, -inf };
vector<double> x_max = { inf, inf };
vector<double> xf_min = { 0, 0 };
vector<double> xf_max = { 0, 0 };
vector<double> x_init = { 0, 0 };
// Final time
double tf = 20.0;
// Number of shooting nodes
int ns = 50;
// ODE right hand side and quadrature
SX ode = vertcat((1 - s*s)*r - s + u, r);
SX quad = r*r + s*s + u*u;
SXDict dae = {{"x", x}, {"p", u}, {"ode", ode}, {"quad", quad}};
// Create an integrator (CVodes)
Function F = integrator("integrator", "cvodes", dae, {{"t0", 0}, {"tf", tf/ns}});
// Total number of NLP variables
int NV = nx*(ns+1) + nu*ns;
// Declare variable vector for the NLP
MX V = MX::sym("V",NV);
// NLP variable bounds and initial guess
vector<double> v_min,v_max,v_init;
// Offset in V
int offset=0;
// State at each shooting node and control for each shooting interval
vector<MX> X, U;
for(int k=0; k<ns; ++k){
// Local state
X.push_back( V.nz(Slice(offset,offset+nx)));
if(k==0){
v_min.insert(v_min.end(), x0_min.begin(), x0_min.end());
v_max.insert(v_max.end(), x0_max.begin(), x0_max.end());
} else {
v_min.insert(v_min.end(), x_min.begin(), x_min.end());
v_max.insert(v_max.end(), x_max.begin(), x_max.end());
}
v_init.insert(v_init.end(), x_init.begin(), x_init.end());
offset += nx;
// Local control
U.push_back( V.nz(Slice(offset,offset+nu)));
v_min.insert(v_min.end(), u_min.begin(), u_min.end());
v_max.insert(v_max.end(), u_max.begin(), u_max.end());
v_init.insert(v_init.end(), u_init.begin(), u_init.end());
offset += nu;
}
// State at end
X.push_back(V.nz(Slice(offset,offset+nx)));
v_min.insert(v_min.end(), xf_min.begin(), xf_min.end());
v_max.insert(v_max.end(), xf_max.begin(), xf_max.end());
v_init.insert(v_init.end(), x_init.begin(), x_init.end());
offset += nx;
// Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
casadi_assert(offset==NV);
// Objective function
MX J = 0;
//Constraint function and bounds
vector<MX> g;
// Loop over shooting nodes
for(int k=0; k<ns; ++k){
// Create an evaluation node
MXDict I_out = F(MXDict{{"x0", X[k]}, {"p", U[k]}});
// Save continuity constraints
g.push_back( I_out.at("xf") - X[k+1] );
// Add objective function contribution
J += I_out.at("qf");
}
// NLP
MXDict nlp = {{"x", V}, {"f", J}, {"g", vertcat(g)}};
// Create an NLP solver and buffers
Function solver = nlpsol("nlpsol", "blocksqp", nlp);
std::map<std::string, DM> arg, res;
// Bounds and initial guess
arg["lbx"] = v_min;
arg["ubx"] = v_max;
arg["lbg"] = 0;
arg["ubg"] = 0;
arg["x0"] = v_init;
// Solve the problem
res = solver(arg);
// Optimal solution of the NLP
vector<double> V_opt(res.at("x"));
// Get the optimal state trajectory
vector<double> r_opt(ns+1), s_opt(ns+1);
for(int i=0; i<=ns; ++i){
r_opt[i] = V_opt.at(i*(nx+1));
s_opt[i] = V_opt.at(1+i*(nx+1));
}
cout << "r_opt = " << endl << r_opt << endl;
cout << "s_opt = " << endl << s_opt << endl;
// Get the optimal control
vector<double> u_opt(ns);
for(int i=0; i<ns; ++i){
u_opt[i] = V_opt.at(nx + i*(nx+1));
}
cout << "u_opt = " << endl << u_opt << endl;
return 0;
}
int main(){
// Declare variables
SX u = SX::sym("u"); // control
SX r = SX::sym("r"), s = SX::sym("s"); // states
SX x = vertcat(r,s);
// Number of differential states
int nx = x.size1();
// Number of controls
int nu = u.size1();
// Bounds and initial guess for the control
vector<double> u_min = { -0.75 };
vector<double> u_max = { 1.0 };
vector<double> u_init = { 0.0 };
// Bounds and initial guess for the state
vector<double> x0_min = { 0, 1 };
vector<double> x0_max = { 0, 1 };
vector<double> x_min = {-inf, -inf };
vector<double> x_max = { inf, inf };
vector<double> xf_min = { 0, 0 };
vector<double> xf_max = { 0, 0 };
vector<double> x_init = { 0, 0 };
// Final time
double tf = 20.0;
// Number of shooting nodes
int ns = 50;
// ODE right hand side and quadrature
SX ode = vertcat((1 - s*s)*r - s + u, r);
SX quad = r*r + s*s + u*u;
SXFunction rhs("rhs", daeIn("x", x, "p", u), daeOut("ode", ode, "quad", quad));
// Create an integrator (CVodes)
Integrator integrator("integrator", "cvodes", rhs, make_dict("t0", 0, "tf", tf/ns));
// Total number of NLP variables
int NV = nx*(ns+1) + nu*ns;
// Declare variable vector for the NLP
MX V = MX::sym("V",NV);
// NLP variable bounds and initial guess
vector<double> v_min,v_max,v_init;
// Offset in V
int offset=0;
// State at each shooting node and control for each shooting interval
vector<MX> X, U;
for(int k=0; k<ns; ++k){
// Local state
X.push_back( V[Slice(offset,offset+nx)] );
if(k==0){
v_min.insert(v_min.end(), x0_min.begin(), x0_min.end());
v_max.insert(v_max.end(), x0_max.begin(), x0_max.end());
} else {
v_min.insert(v_min.end(), x_min.begin(), x_min.end());
v_max.insert(v_max.end(), x_max.begin(), x_max.end());
}
v_init.insert(v_init.end(), x_init.begin(), x_init.end());
offset += nx;
// Local control
U.push_back( V[Slice(offset,offset+nu)] );
v_min.insert(v_min.end(), u_min.begin(), u_min.end());
v_max.insert(v_max.end(), u_max.begin(), u_max.end());
v_init.insert(v_init.end(), u_init.begin(), u_init.end());
offset += nu;
}
// State at end
X.push_back(V[Slice(offset,offset+nx)]);
v_min.insert(v_min.end(), xf_min.begin(), xf_min.end());
v_max.insert(v_max.end(), xf_max.begin(), xf_max.end());
v_init.insert(v_init.end(), x_init.begin(), x_init.end());
offset += nx;
// Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
casadi_assert(offset==NV);
// Objective function
MX J = 0;
//Constraint function and bounds
vector<MX> g;
// Loop over shooting nodes
for(int k=0; k<ns; ++k){
// Create an evaluation node
map<string, MX> I_out = integrator(make_map("x0", X[k], "p", U[k]));
// Save continuity constraints
g.push_back( I_out.at("xf") - X[k+1] );
// Add objective function contribution
J += I_out.at("qf");
}
// NLP
MXFunction nlp("nlp", nlpIn("x", V), nlpOut("f", J, "g", vertcat(g)));
// Set options
Dict opts;
opts["tol"] = 1e-5;
opts["max_iter"] = 100;
opts["linear_solver"] = "ma27";
// Create an NLP solver and buffers
NlpSolver nlp_solver("nlp_solver", "ipopt", nlp, opts);
std::map<std::string, DMatrix> arg, res;
// Bounds and initial guess
arg["lbx"] = v_min;
arg["ubx"] = v_max;
arg["lbg"] = 0;
arg["ubg"] = 0;
arg["x0"] = v_init;
// Solve the problem
res = nlp_solver(arg);
// Optimal solution of the NLP
const Matrix<double>& V_opt = res.at("x");
// Get the optimal state trajectory
vector<double> r_opt(ns+1), s_opt(ns+1);
for(int i=0; i<=ns; ++i){
r_opt[i] = V_opt.at(i*(nx+1));
s_opt[i] = V_opt.at(1+i*(nx+1));
}
cout << "r_opt = " << endl << r_opt << endl;
cout << "s_opt = " << endl << s_opt << endl;
// Get the optimal control
vector<double> u_opt(ns);
for(int i=0; i<ns; ++i){
u_opt[i] = V_opt.at(nx + i*(nx+1));
}
cout << "u_opt = " << endl << u_opt << endl;
return 0;
}
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());
}
