void QpToImplicit::init() {
// Call the base class initializer
ImplicitFunctionInternal::init();
// Free variable in the NLP
MX u = MX::sym("u", input(iin_).sparsity());
// So that we can pass it on to createParent
std::vector<Sparsity> sps;
for (int i=0; i<getNumInputs(); ++i)
if (i!=iin_) sps.push_back(input(i).sparsity());
// u groups all parameters in an MX
std::vector< MX > inputs;
MX p = createParent(sps, inputs);
// Dummy NLP objective
MX nlp_f = 0;
// NLP constraints
std::vector< MX > args_call(getNumInputs());
args_call[iin_] = u;
for (int i=0, i2=0; i<getNumInputs(); ++i)
if (i!=iin_) args_call[i] = inputs[i2++];
MX nlp_g = f_.call(args_call).at(iout_);
// We're going to use two-argument objective and constraints to allow the use of parameters
MXFunction nlp(nlpIn("x", u, "p", p), nlpOut("f", nlp_f, "g", nlp_g));
// Create an NlpSolver instance
solver_ = NlpSolver(getOption(solvername()), nlp);
if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname()));
solver_.init();
}
NlpSolver::NlpSolver(const std::string& name,
const std::string& solver,
const MXDict& nlp,
const Dict& opts) {
// Create an NLP function
MX x, p, f, g;
for (MXDict::const_iterator i=nlp.begin(); i!=nlp.end(); ++i) {
if (i->first=="x") {
x = i->second;
} else if (i->first=="p") {
p = i->second;
} else if (i->first=="f") {
f = i->second;
} else if (i->first=="g") {
g = i->second;
} else {
casadi_error("No such field: \"" + i->first + "\"");
}
}
MXFunction nlpf("nlp", nlpIn("x", x, "p", p), nlpOut("f", f, "g", g));
// Create the solver instance
assignNode(NlpSolverInternal::instantiatePlugin(solver, nlpf));
setOption("name", name);
setOption(opts);
init();
}
RealtimeAPCSCP::RealtimeAPCSCP(Model* model) :
RealtimeAPCSCP::Solver(model) {
this->nlp = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut("f",
model->getf(0), "g", G));
this->dG_f = this->nlp.jacobian("x", "g");
this->dG_fast = this->nlp.derReverse(1);
this->choiceH = 0;
this->exactA = true;
this->n_updateA = 1;
this->n_updateH = 1;
this->assumedData = std::vector<Matrix<double> >(this->model->getN_F() + 1);
this->firstIteration = true;
this->name = "APCSCP";
this->hess_gi = std::vector<Function>(G.size());
for (int i = 0; i < G.size(); i++) {
hess_gi[i] = MXFunction("constraint_i", nlpIn("x", model->getV()),
nlpOut("g", (MX) G[i])).hessian("x", "g");
}
hess_fi = nlp.hessian("x", "f");
}
void QpToImplicit::init() {
// Call the base class initializer
ImplicitFunctionInternal::init();
// Free variable in the NLP
MX u = MX::sym("u", input(iin_).sparsity());
// So that we can pass it on to createParent
std::vector<MX> inputs;
for (int i=0; i<nIn(); ++i) {
if (i!=iin_) {
stringstream ss;
ss << "p" << i;
inputs.push_back(MX::sym(ss.str(), input(i).sparsity()));
}
}
MX p = veccat(inputs);
// Dummy NLP objective
MX nlp_f = 0;
// NLP constraints
std::vector< MX > args_call(nIn());
args_call[iin_] = u;
for (int i=0, i2=0; i<nIn(); ++i)
if (i!=iin_) args_call[i] = inputs[i2++];
MX nlp_g = f_(args_call).at(iout_);
// We're going to use two-argument objective and constraints to allow the use of parameters
MXFunction nlp("nlp", nlpIn("x", u, "p", p), nlpOut("f", nlp_f, "g", nlp_g));
Dict options;
if (hasSetOption(optionsname())) options = getOption(optionsname());
// Create an NlpSolver instance
solver_ = NlpSolver("nlpsolver", getOption(solvername()), nlp, options);
}
void DirectMultipleShootingInternal::init(){
// Initialize the base classes
OCPSolverInternal::init();
// Create an integrator instance
integratorCreator integrator_creator = getOption("integrator");
integrator_ = integrator_creator(ffcn_,Function());
if(hasSetOption("integrator_options")){
integrator_.setOption(getOption("integrator_options"));
}
// Set t0 and tf
integrator_.setOption("t0",0);
integrator_.setOption("tf",tf_/nk_);
integrator_.init();
// Path constraints present?
bool path_constraints = nh_>0;
// Count the total number of NLP variables
int NV = np_ + // global parameters
nx_*(nk_+1) + // local state
nu_*nk_; // local control
// Declare variable vector for the NLP
// The structure is as follows:
// np x 1 (parameters)
// ------
// nx x 1 (states at time i=0)
// nu x 1 (controls in interval i=0)
// ------
// nx x 1 (states at time i=1)
// nu x 1 (controls in interval i=1)
// ------
// .....
// ------
// nx x 1 (states at time i=nk)
MX V = MX::sym("V",NV);
// Global parameters
MX P = V(Slice(0,np_));
// offset in the variable vector
int v_offset=np_;
// Disretized variables for each shooting node
vector<MX> X(nk_+1), U(nk_);
for(int k=0; k<=nk_; ++k){ // interior nodes
// Local state
X[k] = V[Slice(v_offset,v_offset+nx_)];
v_offset += nx_;
// Variables below do not appear at the end point
if(k==nk_) break;
// Local control
U[k] = V[Slice(v_offset,v_offset+nu_)];
v_offset += nu_;
}
// Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
casadi_assert(v_offset==NV);
// Input to the parallel integrator evaluation
vector<vector<MX> > int_in(nk_);
for(int k=0; k<nk_; ++k){
int_in[k].resize(INTEGRATOR_NUM_IN);
int_in[k][INTEGRATOR_P] = vertcat(P,U[k]);
int_in[k][INTEGRATOR_X0] = X[k];
}
// Input to the parallel function evaluation
vector<vector<MX> > fcn_in(nk_);
for(int k=0; k<nk_; ++k){
fcn_in[k].resize(DAE_NUM_IN);
fcn_in[k][DAE_T] = (k*tf_)/nk_;
fcn_in[k][DAE_P] = vertcat(P,U.at(k));
fcn_in[k][DAE_X] = X[k];
}
// Options for the parallelizer
Dictionary paropt;
// Transmit parallelization mode
if(hasSetOption("parallelization"))
paropt["parallelization"] = getOption("parallelization");
// Evaluate function in parallel
vector<vector<MX> > pI_out = integrator_.callParallel(int_in,paropt);
// Evaluate path constraints in parallel
vector<vector<MX> > pC_out;
if(path_constraints)
pC_out = cfcn_.callParallel(fcn_in,paropt);
//Constraint function
vector<MX> gg(2*nk_);
// Collect the outputs
for(int k=0; k<nk_; ++k){
//append continuity constraints
gg[2*k] = pI_out[k][INTEGRATOR_XF] - X[k+1];
// append the path constraints
if(path_constraints)
gg[2*k+1] = pC_out[k][0];
}
// Terminal constraints
MX g = vertcat(gg);
// Objective function
MX f;
if (mfcn_.getNumInputs()==1) {
f = mfcn_(X.back()).front();
} else {
vector<MX> mfcn_argin(MAYER_NUM_IN);
mfcn_argin[MAYER_X] = X.back();
mfcn_argin[MAYER_P] = P;
f = mfcn_.call(mfcn_argin).front();
}
// NLP
nlp_ = MXFunction(nlpIn("x",V),nlpOut("f",f,"g",g));
nlp_.setOption("ad_mode","forward");
nlp_.init();
// Get the NLP creator function
NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
// Allocate an NLP solver
nlp_solver_ = nlp_solver_creator(nlp_);
// Pass user options
if(hasSetOption("nlp_solver_options")){
const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
nlp_solver_.setOption(nlp_solver_options);
}
// Initialize the solver
nlp_solver_.init();
}
void DirectCollocationInternal::init(){
// Initialize the base classes
OCPSolverInternal::init();
// Free parameters currently not supported
casadi_assert_message(np_==0, "Not implemented");
// Interpolation order
deg_ = getOption("interpolation_order");
// All collocation time points
std::vector<double> tau_root = collocationPoints(deg_,getOption("collocation_scheme"));
// Size of the finite elements
double h = tf_/nk_;
// Coefficients of the collocation equation
vector<vector<MX> > C(deg_+1,vector<MX>(deg_+1));
// Coefficients of the collocation equation as DMatrix
DMatrix C_num = DMatrix::zeros(deg_+1,deg_+1);
// Coefficients of the continuity equation
vector<MX> D(deg_+1);
// Coefficients of the collocation equation as DMatrix
DMatrix D_num = DMatrix::zeros(deg_+1);
// Collocation point
SX tau = SX::sym("tau");
// For all collocation points
for(int j=0; j<deg_+1; ++j){
// Construct Lagrange polynomials to get the polynomial basis at the collocation point
SX L = 1;
for(int j2=0; j2<deg_+1; ++j2){
if(j2 != j){
L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]);
}
}
SXFunction lfcn(tau,L);
lfcn.init();
// Evaluate the polynomial at the final time to get the coefficients of the continuity equation
lfcn.setInput(1.0);
lfcn.evaluate();
D[j] = lfcn.output();
D_num(j) = lfcn.output();
// Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
Function tfcn = lfcn.tangent();
tfcn.init();
for(int j2=0; j2<deg_+1; ++j2){
tfcn.setInput(tau_root[j2]);
tfcn.evaluate();
C[j][j2] = tfcn.output();
C_num(j,j2) = tfcn.output();
}
}
C_num(std::vector<int>(1,0),ALL) = 0;
C_num(0,0) = 1;
// All collocation time points
vector<vector<double> > T(nk_);
for(int k=0; k<nk_; ++k){
T[k].resize(deg_+1);
for(int j=0; j<=deg_; ++j){
T[k][j] = h*(k + tau_root[j]);
}
}
// Total number of variables
int nlp_nx = 0;
nlp_nx += nk_*(deg_+1)*nx_; // Collocated states
nlp_nx += nk_*nu_; // Parametrized controls
nlp_nx += nx_; // Final state
// NLP variable vector
MX nlp_x = MX::sym("x",nlp_nx);
int offset = 0;
// Get collocated states and parametrized control
vector<vector<MX> > X(nk_+1);
vector<MX> U(nk_);
for(int k=0; k<nk_; ++k){
// Collocated states
X[k].resize(deg_+1);
for(int j=0; j<=deg_; ++j){
// Get the expression for the state vector
X[k][j] = nlp_x[Slice(offset,offset+nx_)];
offset += nx_;
}
// Parametrized controls
U[k] = nlp_x[Slice(offset,offset+nu_)];
offset += nu_;
}
// State at end time
X[nk_].resize(1);
X[nk_][0] = nlp_x[Slice(offset,offset+nx_)];
offset += nx_;
casadi_assert(offset==nlp_nx);
// Constraint function for the NLP
vector<MX> nlp_g;
// Objective function
MX nlp_j = 0;
// For all finite elements
for(int k=0; k<nk_; ++k){
// For all collocation points
for(int j=1; j<=deg_; ++j){
// Get an expression for the state derivative at the collocation point
MX xp_jk = 0;
for(int r=0; r<=deg_; ++r){
xp_jk += C[r][j]*X[k][r];
}
// Add collocation equations to the NLP
MX fk = ffcn_.call(daeIn("x",X[k][j],"p",U[k]))[DAE_ODE];
nlp_g.push_back(h*fk - xp_jk);
}
// Get an expression for the state at the end of the finite element
MX xf_k = 0;
for(int r=0; r<=deg_; ++r){
xf_k += D[r]*X[k][r];
}
// Add continuity equation to NLP
nlp_g.push_back(X[k+1][0] - xf_k);
// Add path constraints
if(nh_>0){
MX pk = cfcn_.call(daeIn("x",X[k+1][0],"p",U[k])).at(0);
nlp_g.push_back(pk);
}
// Add integral objective function term
// [Jk] = lfcn.call([X[k+1,0], U[k]])
// nlp_j += Jk
}
// Add end cost
MX Jk = mfcn_.call(mayerIn("x",X[nk_][0])).at(0);
nlp_j += Jk;
// Objective function of the NLP
nlp_ = MXFunction(nlpIn("x",nlp_x), nlpOut("f",nlp_j,"g",vertcat(nlp_g)));
// Get the NLP creator function
NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
// Allocate an NLP solver
nlp_solver_ = nlp_solver_creator(nlp_);
// Pass options
if(hasSetOption("nlp_solver_options")){
const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
nlp_solver_.setOption(nlp_solver_options);
}
// Initialize the solver
nlp_solver_.init();
}
void RealtimeAPCSCP::solve() {
//Initialisation (calculate the exact solution for inital point)
int t_act = 0;
std::cout << "Time: " + to_string(t_act) << std::endl;
startIt = std::chrono::system_clock::now();
startSolver = std::chrono::system_clock::now();
//Solve the first problem exactly
std::map<std::string, Matrix<double> > arg = make_map("lbx",
model->getVMIN(), "ubx", model->getVMAX(), "lbg", this->G_bound,
"ubg", this->G_bound, "x0", model->getVINIT());
NlpSolver nlpSolver = NlpSolver("solver", "ipopt", nlp, opts);
nlpSolver.setOption("warn_initial_bounds", true);
nlpSolver.setOption("eval_errors_fatal", true);
std::map<string, Matrix<double>> result_tact = nlpSolver(arg);
endSolver = std::chrono::system_clock::now();
// Store data
assumedData[t_act] = result_tact["x"];
//Setup new iteration
model->storeAndShiftValues(result_tact, t_act);
storeStatsIpopt(&nlpSolver);
//Define values
this->x_act = result_tact["x"];
this->y_act = result_tact["lam_g"];
evaluateOriginalF(t_act,x_act);
updateA_act(t_act);
updateH_act(t_act);
updateM_act();
m_act = mul(transpose(Dg_act - A_act), y_act);
firstIteration = false;
opts["warm_start_init_point"] = "yes";
endIt = std::chrono::system_clock::now();
printTimingData(t_act);
//Iteration
for (t_act = 1; t_act < model->getN_F(); t_act++) {
std::cout << "Time: " + to_string(t_act) << std::endl;
startIt = std::chrono::system_clock::now();
startMS = std::chrono::system_clock::now();
G_sub = mul(A_act, model->getV() - x_act) + model->doMultipleShooting(x_act);
endMS = std::chrono::system_clock::now();
f_sub = model->getf(t_act) + mul(transpose(m_act), model->getV()
- x_act) + 0.5 * quad_form(model->getV() - x_act, H_act);
this->nlp_sub = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut(
"f", f_sub, "g", G_sub));
this->nlp = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut("f",
model->getf(t_act), "g", G));
// Step2 solve convex subproblem
startSolver = std::chrono::system_clock::now();
result_tact = solveConvexSubproblem();
endSolver = std::chrono::system_clock::now();
// Step3 update matrices and retrieve new measurement (step 1)
x_act = result_tact["x"];
y_act = result_tact["lam_g"];
model->storeAndShiftValues(result_tact, t_act);
// update
this->x_act = result_tact["x"];
this->y_act = result_tact["lam_g"];
evaluateOriginalF(t_act, x_act);
updateA_act(t_act);
updateH_act(t_act);
updateM_act();
endIt = std::chrono::system_clock::now();
printTimingData(t_act);
}
}
