void Sqpmethod::solve_QP(const Matrix<double>& H, const std::vector<double>& g,
const std::vector<double>& lbx, const std::vector<double>& ubx,
const Matrix<double>& A, const std::vector<double>& lbA,
const std::vector<double>& ubA,
std::vector<double>& x_opt, std::vector<double>& lambda_x_opt,
std::vector<double>& lambda_A_opt) {
// Pass data to QP solver
qp_solver_.setInput(H, QP_SOLVER_H);
qp_solver_.setInputNZ(g, QP_SOLVER_G);
// Hot-starting if possible
qp_solver_.setInputNZ(x_opt, QP_SOLVER_X0);
//TODO(Joel): Fix hot-starting of dual variables
//qp_solver_.setInput(lambda_A_opt, QP_SOLVER_LAMBDA_INIT);
// Pass simple bounds
qp_solver_.setInputNZ(lbx, QP_SOLVER_LBX);
qp_solver_.setInputNZ(ubx, QP_SOLVER_UBX);
// Pass linear bounds
if (ng_>0) {
qp_solver_.setInput(A, QP_SOLVER_A);
qp_solver_.setInputNZ(lbA, QP_SOLVER_LBA);
qp_solver_.setInputNZ(ubA, QP_SOLVER_UBA);
}
if (monitored("qp")) {
userOut() << "H = " << endl;
H.printDense();
userOut() << "A = " << endl;
A.printDense();
userOut() << "g = " << g << endl;
userOut() << "lbx = " << lbx << endl;
userOut() << "ubx = " << ubx << endl;
userOut() << "lbA = " << lbA << endl;
userOut() << "ubA = " << ubA << endl;
}
// Solve the QP
qp_solver_.evaluate();
// Get the optimal solution
qp_solver_.getOutputNZ(x_opt, QP_SOLVER_X);
qp_solver_.getOutputNZ(lambda_x_opt, QP_SOLVER_LAM_X);
qp_solver_.getOutputNZ(lambda_A_opt, QP_SOLVER_LAM_A);
if (monitored("dx")) {
userOut() << "dx = " << x_opt << endl;
}
}
void SQPInternal::solve_QP(const Matrix<double>& H, const std::vector<double>& g,
const std::vector<double>& lbx, const std::vector<double>& ubx,
const Matrix<double>& A, const std::vector<double>& lbA, const std::vector<double>& ubA,
std::vector<double>& x_opt, std::vector<double>& lambda_x_opt, std::vector<double>& lambda_A_opt){
// Pass data to QP solver
qp_solver_.setInput(H, QP_H);
qp_solver_.setInput(g,QP_G);
// Hot-starting if possible
qp_solver_.setInput(x_opt, QP_X_INIT);
//TODO: Fix hot-starting of dual variables
//qp_solver_.setInput(lambda_A_opt, QP_LAMBDA_INIT);
// Pass simple bounds
qp_solver_.setInput(lbx, QP_LBX);
qp_solver_.setInput(ubx, QP_UBX);
// Pass linear bounds
if(m_>0){
qp_solver_.setInput(A, QP_A);
qp_solver_.setInput(lbA, QP_LBA);
qp_solver_.setInput(ubA, QP_UBA);
}
if (monitored("qp")) {
cout << "H = " << endl;
H.printDense();
cout << "A = " << endl;
A.printDense();
cout << "g = " << g << endl;
cout << "lbx = " << lbx << endl;
cout << "ubx = " << ubx << endl;
cout << "lbA = " << lbA << endl;
cout << "ubA = " << ubA << endl;
}
// Solve the QP
qp_solver_.evaluate();
// Get the optimal solution
qp_solver_.getOutput(x_opt,QP_PRIMAL);
qp_solver_.getOutput(lambda_x_opt,QP_LAMBDA_X);
qp_solver_.getOutput(lambda_A_opt,QP_LAMBDA_A);
if (monitored("dx")){
cout << "dx = " << x_opt << endl;
}
}
void Sqpmethod::eval_g(const std::vector<double>& x, std::vector<double>& g) {
try {
double time1 = clock();
// Quick return if no constraints
if (ng_==0) return;
// Pass the argument to the function
nlp_.setInputNZ(x, NL_X);
nlp_.setInput(input(NLP_SOLVER_P), NL_P);
// Evaluate the function and tape
nlp_.evaluate();
// Ge the result
nlp_.output(NL_G).get(g);
// Printing
if (monitored("eval_g")) {
userOut() << "x = " << nlp_.input(NL_X) << endl;
userOut() << "g = " << nlp_.output(NL_G) << endl;
}
double time2 = clock();
t_eval_g_ += (time2-time1)/CLOCKS_PER_SEC;
n_eval_g_ += 1;
} catch(exception& ex) {
userOut<true, PL_WARN>() << "eval_g failed: " << ex.what() << endl;
throw;
}
}
int
chown3d(const char* path, uid_t uid, gid_t gid)
{
register char* sp;
register int r;
#if FS
register Mount_t* mp;
if (!fscall(NiL, MSG_chown, 0, path, uid, gid))
return(state.ret);
mp = monitored();
#endif
if (!(sp = pathreal(path, P_SAFE|P_TOP, NiL)))
return(-1);
r = CHOWN(sp, uid, gid);
#if FS
if (!r)
{
if (mp)
fscall(mp, MSG_chown, 0, path, uid, gid);
for (mp = state.global; mp; mp = mp->global)
if (fssys(mp, MSG_chown))
fscall(mp, MSG_chown, 0, path, uid, gid);
}
#endif
return(r);
}
void Sqpmethod::eval_f(const std::vector<double>& x, double& f) {
try {
// Log time
double time1 = clock();
// Pass the argument to the function
nlp_.setInputNZ(x, NL_X);
nlp_.setInput(input(NLP_SOLVER_P), NL_P);
// Evaluate the function
nlp_.evaluate();
// Get the result
nlp_.getOutput(f, NL_F);
// Printing
if (monitored("eval_f")) {
userOut() << "x = " << nlp_.input(NL_X) << endl;
userOut() << "f = " << f << endl;
}
double time2 = clock();
t_eval_f_ += (time2-time1)/CLOCKS_PER_SEC;
n_eval_f_ += 1;
} catch(exception& ex) {
userOut<true, PL_WARN>() << "eval_f failed: " << ex.what() << endl;
throw;
}
}
bool IpoptInternal::eval_grad_f(int n, const double* x, bool new_x, double* grad_f)
{
try {
log("eval_grad_f started");
double time1 = clock();
casadi_assert(n == nx_);
// Pass the argument to the function
gradF_.setInput(x,NL_X);
gradF_.setInput(input(NLP_SOLVER_P),NL_P);
// Evaluate, adjoint mode
gradF_.evaluate();
// Get the result
gradF_.output().getArray(grad_f,n,DENSE);
// Printing
if(monitored("eval_grad_f")){
cout << "x = " << gradF_.input(NL_X) << endl;
cout << "grad_f = " << gradF_.output() << endl;
}
if (regularity_check_ && !isRegular(gradF_.output().data())) casadi_error("IpoptInternal::grad_f: NaN or Inf detected.");
double time2 = clock();
t_eval_grad_f_ += double(time2-time1)/CLOCKS_PER_SEC;
n_eval_grad_f_ += 1;
log("eval_grad_f ok");
return true;
} catch (exception& ex){
cerr << "eval_grad_f failed: " << ex.what() << endl;
return false;
}
}
void SQPInternal::eval_grad_f(const std::vector<double>& x, double& f, std::vector<double>& grad_f){
F_.setInput(x);
F_.setAdjSeed(1.0);
F_.evaluate(0,1);
F_.output().get(f);
F_.adjSens().get(grad_f,DENSE);
if (monitored("eval_f")){
cout << "x = " << x << endl;
cout << "f = " << f << endl;
}
if (monitored("eval_grad_f")) {
cout << "x = " << x << endl;
cout << "grad_f = " << grad_f << endl;
}
}
void SQPInternal::eval_f(const std::vector<double>& x, double& f){
F_.setInput(x);
F_.evaluate();
F_.output().get(f);
if (monitored("eval_f")){
cout << "x = " << x << endl;
cout << "f = " << f << endl;
}
}
bool IpoptInternal::eval_jac_g(int n, const double* x, bool new_x,int m, int nele_jac, int* iRow, int *jCol,double* values){
try{
log("eval_jac_g started");
// Quich finish if no constraints
if(m==0){
log("eval_jac_g quick return (m==0)");
return true;
}
// Get function
Function& jacG = this->jacG();
double time1 = clock();
if (values == NULL) {
int nz=0;
const vector<int>& colind = jacG.output().colind();
const vector<int>& row = jacG.output().row();
for(int cc=0; cc<colind.size()-1; ++cc)
for(int el=colind[cc]; el<colind[cc+1]; ++el){
int rr = row[el];
iRow[nz] = rr;
jCol[nz] = cc;
nz++;
}
} else {
// Pass the argument to the function
jacG.setInput(x,NL_X);
jacG.setInput(input(NLP_SOLVER_P),NL_P);
// Evaluate the function
jacG.evaluate();
// Get the output
jacG.getOutput(values);
if(monitored("eval_jac_g")){
cout << "x = " << jacG.input(NL_X).data() << endl;
cout << "J = " << endl;
jacG.output().printSparse();
}
if (regularity_check_ && !isRegular(jacG.output().data())) casadi_error("IpoptInternal::jac_g: NaN or Inf detected.");
}
double time2 = clock();
t_eval_jac_g_ += double(time2-time1)/CLOCKS_PER_SEC;
n_eval_jac_g_ += 1;
log("eval_jac_g ok");
return true;
} catch (exception& ex){
cerr << "eval_jac_g failed: " << ex.what() << endl;
return false;
}
}
void SQPInternal::reset_h(){
// Initial Hessian approximation of BFGS
if ( hess_mode_ == HESS_BFGS){
Bk_.set(B_init_);
}
if (monitored("eval_h")) {
cout << "x = " << x_ << endl;
cout << "H = " << endl;
Bk_.printSparse();
}
}
void Sqpmethod::reset_h() {
// Initial Hessian approximation of BFGS
if (!exact_hessian_) {
Bk_.set(B_init_);
}
if (monitored("eval_h")) {
userOut() << "x = " << x_ << endl;
userOut() << "H = " << endl;
Bk_.printSparse();
}
}
void SQPInternal::eval_g(const std::vector<double>& x, std::vector<double>& g){
// Quick return if no constraints
if(m_==0) return;
G_.setInput(x);
G_.evaluate();
G_.output().get(g,DENSE);
if (monitored("eval_g")) {
cout << "x = " << x << endl;
cout << "g = " << g << endl;
}
}
void Sqpmethod::eval_grad_f(const std::vector<double>& x, double& f,
std::vector<double>& grad_f) {
try {
double time1 = clock();
// Get function
Function& gradF = this->gradF();
// Pass the argument to the function
gradF.setInputNZ(x, NL_X);
gradF.setInput(input(NLP_SOLVER_P), NL_P);
// Evaluate, adjoint mode
gradF.evaluate();
// Get the result
gradF.output().get(grad_f);
gradF.output(1+NL_X).get(f);
// Printing
if (monitored("eval_f")) {
userOut() << "x = " << x << endl;
userOut() << "f = " << f << endl;
}
if (monitored("eval_grad_f")) {
userOut() << "x = " << x << endl;
userOut() << "grad_f = " << grad_f << endl;
}
double time2 = clock();
t_eval_grad_f_ += (time2-time1)/CLOCKS_PER_SEC;
n_eval_grad_f_ += 1;
} catch(exception& ex) {
userOut<true, PL_WARN>() << "eval_grad_f failed: " << ex.what() << endl;
throw;
}
}
int
fstatfs3d(int fd, struct statfs* fs)
{
#if FS
Mount_t* mp;
#if !_vfs_statfs
struct statvfs vfs;
#endif
if (!fscall(NiL, MSG_fstatfs, 0, fd, VFS))
{
#if !_vfs_statfs
if (!state.ret)
{
memset(fs, 0, sizeof(*fs));
fs->f_bsize = vfs.f_bsize;
fs->f_blocks = vfs.f_blocks;
fs->f_bfree = vfs.f_bfree;
fs->f_files = vfs.f_files;
fs->f_ffree = vfs.f_ffree;
}
#endif
return state.ret;
}
mp = monitored();
#endif
if (FSTATFS(fd, fs))
return -1;
#if FS
#if !_vfs_statfs
if (mp || state.global)
{
memset(&vfs, 0, sizeof(vfs));
vfs.f_bsize = fs->f_bsize;
vfs.f_blocks = fs->f_blocks;
vfs.f_bfree = fs->f_bfree;
vfs.f_files = fs->f_files;
vfs.f_ffree = fs->f_ffree;
}
#endif
if (mp)
fscall(mp, MSG_fstatfs, 0, fd, VFS);
for (mp = state.global; mp; mp = mp->global)
if (fssys(mp, MSG_fstatfs))
fscall(mp, MSG_fstatfs, 0, fd, VFS);
#endif
return 0;
}
void SQPInternal::eval_jac_g(const std::vector<double>& x, std::vector<double>& g, Matrix<double>& J){
// Quick return if no constraints
if(m_==0) return;
J_.setInput(x);
J_.evaluate();
J_.output(1).get(g,DENSE);
J_.output(0).get(J);
if (monitored("eval_jac_g")) {
cout << "x = " << x << endl;
cout << "g = " << g << endl;
cout << "J = " << endl;
J.printSparse();
}
}
bool WorhpInternal::eval_grad_f(const double* x, double scale , double* grad_f )
{
try {
log("eval_grad_f started");
double time1 = clock();
// Pass the argument to the function
gradF_.setInput(x,NL_X);
gradF_.setInput(input(NLP_SOLVER_P),NL_P);
// Evaluate, adjoint mode
gradF_.evaluate();
// Get the result
gradF_.output().get(grad_f,DENSE);
// Scale
for(int i=0; i<nx_; ++i){
grad_f[i] *= scale;
}
// Printing
if(monitored("eval_grad_f")){
cout << "grad_f = " << gradF_.output() << endl;
}
if (regularity_check_ && !isRegular(gradF_.output().data())) casadi_error("WorhpInternal::eval_grad_f: NaN or Inf detected.");
double time2 = clock();
t_eval_grad_f_ += double(time2-time1)/CLOCKS_PER_SEC;
n_eval_grad_f_ += 1;
// Check the result for regularity
for(int i=0; i<nx_; ++i){
if(isnan(grad_f[i]) || isinf(grad_f[i])){
log("eval_grad_f: result not regular");
return false;
}
}
log("eval_grad_f ok");
return true;
} catch (exception& ex){
cerr << "eval_jac_f failed: " << ex.what() << endl;
return false;
}
}
bool IpoptInternal::eval_h(const double* x, bool new_x, double obj_factor, const double* lambda,bool new_lambda, int nele_hess, int* iRow,int* jCol, double* values){
try{
log("eval_h started");
double time1 = clock();
if (values == NULL) {
int nz=0;
const vector<int>& colind = hessLag_.output().colind();
const vector<int>& row = hessLag_.output().row();
for(int cc=0; cc<colind.size()-1; ++cc)
for(int el=colind[cc]; el<colind[cc+1] && row[el]<=cc; ++el){
iRow[nz] = row[el];
jCol[nz] = cc;
nz++;
}
} else {
// Pass the argument to the function
hessLag_.setInput(x,NL_X);
hessLag_.setInput(input(NLP_SOLVER_P),NL_P);
hessLag_.setInput(obj_factor,NL_NUM_IN+NL_F);
hessLag_.setInput(lambda,NL_NUM_IN+NL_G);
// Evaluate
hessLag_.evaluate();
// Get results
hessLag_.output().get(values,SPARSESYM);
if(monitored("eval_h")){
cout << "x = " << hessLag_.input(NL_X).data() << endl;
cout << "H = " << endl;
hessLag_.output().printSparse();
}
if (regularity_check_ && !isRegular(hessLag_.output().data())) casadi_error("IpoptInternal::h: NaN or Inf detected.");
}
double time2 = clock();
t_eval_h_ += double(time2-time1)/CLOCKS_PER_SEC;
n_eval_h_ += 1;
log("eval_h ok");
return true;
} catch (exception& ex){
cerr << "eval_h failed: " << ex.what() << endl;
return false;
}
}
bool WorhpInternal::eval_jac_g(const double* x,double* values){
try{
log("eval_jac_g started");
// Quich finish if no constraints
if(worhp_o_.m==0){
log("eval_jac_g quick return (m==0)");
return true;
}
// Make sure generated
casadi_assert(!jacG_.isNull());
// Get Jacobian
Function& jacG = this->jacG();
double time1 = clock();
// Pass the argument to the function
jacG.setInput(x,JACG_X);
jacG.setInput(input(NLP_SOLVER_P),JACG_P);
// Evaluate the function
jacG.evaluate();
const DMatrix& J = jacG.output(JACG_JAC);
std::copy(J.data().begin(),J.data().end(),values);
if(monitored("eval_jac_g")){
cout << "x = " << jacG_.input().data() << endl;
cout << "J = " << endl;
jacG_.output().printSparse();
}
double time2 = clock();
t_eval_jac_g_ += double(time2-time1)/CLOCKS_PER_SEC;
n_eval_jac_g_ += 1;
log("eval_jac_g ok");
return true;
} catch (exception& ex){
cerr << "eval_jac_g failed: " << ex.what() << endl;
return false;
}
}
int
unlink3d(register const char* path)
{
register char* sp;
register int r;
#if FS
Mount_t* mp;
if (!fscall(NiL, MSG_remove, 0, path))
return state.ret;
mp = monitored();
#endif
if (!(sp = pathreal(path, P_PATHONLY|P_SAFE, NiL)))
return -1;
if (state.path.level)
return 0;
if (!(r = LSTAT(sp, &state.path.st)))
{
if (S_ISLNK(state.path.st.st_mode) && !checklink(sp, &state.path.st, P_PATHONLY|P_LSTAT) && state.path.linksize > 0)
{
/*
* remove instance if not default
*/
r = strlen(sp) - (sizeof(state.vdefault) - 1);
if (r > 3 && streq(sp + r, state.vdefault))
return 0;
}
r = UNLINK(sp);
}
if (!r)
{
#if FS
if (mp)
fscall(mp, MSG_remove, 0, path);
for (mp = state.global; mp; mp = mp->global)
if (fssys(mp, MSG_remove))
fscall(mp, MSG_remove, 0, path);
#endif
}
else if (errno == ENOENT && pathreal(path, 0, NiL))
r = 0;
return r;
}
void GslInternal::reset(int fsens_order, int asens_order){
if(monitored("reset")){
cout << "initial state: " << endl;
cout << "p = " << input(INTEGRATOR_P) << endl;
cout << "x0 = " << input(INTEGRATOR_X0) << endl;
}
// Reset timers
t_res = t_fres = t_jac = t_lsolve = t_lsetup_jac = t_lsetup_fac = 0;
// Get the time horizon
t_ = t0_;
int flag = gsl_odeiv_evolve_reset(evolve_ptr);
if(flag!=GSL_SUCCESS) gsl_error("Reset",flag);
output(INTEGRATOR_XF).set(input(INTEGRATOR_X0));
}
int
utimensat3d(int dir, const char* path, const struct timespec* tms, int flags)
{
register char* sp;
register int r;
time_t atime;
time_t mtime;
#if FS
Mount_t* mp;
#endif
if (state.in_2d)
return(UTIMETS(path, tms));
if (tms)
{
atime = atimeof(tms);
mtime = mtimeof(tms);
}
else atime = mtime = time((time_t*)0);
#if FS
if (!fscall(NiL, MSG_utime, 0, path, atime, mtime))
return(state.ret);
mp = monitored();
#endif
if (dir != AT_FDCWD && *path != '/')
sp = (char*)path;
else if (!(sp = pathreal(path, P_TOP, NiL)))
return(-1);
r = UTIMENSAT(dir, sp, tms, flags);
#if FS
if (!r)
{
if (mp)
fscall(mp, MSG_utime, 0, path, atime, mtime);
for (mp = state.global; mp; mp = mp->global)
if (fssys(mp, MSG_utime))
fscall(mp, MSG_utime, 0, path, atime, mtime);
}
#endif
return(r);
}
int
fchmod3d(int fd, mode_t mode)
{
#if FS
Mount_t* mp;
if (!fscall(NiL, MSG_fchmod, 0, fd, mode))
return(state.ret);
mp = monitored();
#endif
if (FCHMOD(fd, mode))
return(-1);
#if FS
if (mp)
fscall(mp, MSG_fchmod, 0, fd, mode);
for (mp = state.global; mp; mp = mp->global)
if (fssys(mp, MSG_fchmod))
fscall(mp, MSG_fchmod, 0, fd, mode);
#endif
return(0);
}
int
fchown3d(int fd, uid_t uid, gid_t gid)
{
#if FS
Mount_t* mp;
if (!fscall(NiL, MSG_fchown, 0, fd, uid, gid))
return state.ret;
mp = monitored();
#endif
if (FCHOWN(fd, uid, gid))
return -1;
#if FS
if (mp)
fscall(mp, MSG_fchown, 0, fd, uid, gid);
for (mp = state.global; mp; mp = mp->global)
if (fssys(mp, MSG_fchown))
fscall(mp, MSG_fchown, 0, fd, uid, gid);
#endif
return 0;
}
bool IpoptInternal::eval_f(int n, const double* x, bool new_x, double& obj_value)
{
try {
log("eval_f started");
// Log time
double time1 = clock();
casadi_assert(n == nx_);
// Pass the argument to the function
nlp_.setInput(x,NL_X);
nlp_.setInput(input(NLP_SOLVER_P),NL_P);
// Evaluate the function
nlp_.evaluate();
// Get the result
nlp_.getOutput(obj_value,NL_F);
// Printing
if(monitored("eval_f")){
cout << "x = " << nlp_.input(NL_X) << endl;
cout << "obj_value = " << obj_value << endl;
}
if (regularity_check_ && !isRegular(nlp_.output(NL_F).data())) casadi_error("IpoptInternal::f: NaN or Inf detected.");
double time2 = clock();
t_eval_f_ += double(time2-time1)/CLOCKS_PER_SEC;
n_eval_f_ += 1;
log("eval_f ok");
return true;
} catch (exception& ex){
cerr << "eval_f failed: " << ex.what() << endl;
return false;
}
}
void Sqpmethod::eval_jac_g(const std::vector<double>& x, std::vector<double>& g,
Matrix<double>& J) {
try {
double time1 = clock();
// Quich finish if no constraints
if (ng_==0) return;
// Get function
Function& jacG = this->jacG();
// Pass the argument to the function
jacG.setInputNZ(x, NL_X);
jacG.setInput(input(NLP_SOLVER_P), NL_P);
// Evaluate the function
jacG.evaluate();
// Get the output
jacG.output(1+NL_G).get(g);
jacG.output().get(J);
if (monitored("eval_jac_g")) {
userOut() << "x = " << x << endl;
userOut() << "g = " << g << endl;
userOut() << "J = " << endl;
J.printSparse();
}
double time2 = clock();
t_eval_jac_g_ += (time2-time1)/CLOCKS_PER_SEC;
n_eval_jac_g_ += 1;
} catch(exception& ex) {
userOut<true, PL_WARN>() << "eval_jac_g failed: " << ex.what() << endl;
throw;
}
}
void Sqpmethod::eval_h(const std::vector<double>& x, const std::vector<double>& lambda,
double sigma, Matrix<double>& H) {
try {
// Get function
Function& hessLag = this->hessLag();
// Pass the argument to the function
hessLag.setInputNZ(x, HESSLAG_X);
hessLag.setInput(input(NLP_SOLVER_P), HESSLAG_P);
hessLag.setInput(sigma, HESSLAG_LAM_F);
hessLag.setInputNZ(lambda, HESSLAG_LAM_G);
// Evaluate
hessLag.evaluate();
// Get results
hessLag.getOutput(H);
if (monitored("eval_h")) {
userOut() << "x = " << x << endl;
userOut() << "H = " << endl;
H.printSparse();
}
// Determing regularization parameter with Gershgorin theorem
if (regularize_) {
reg_ = getRegularization(H);
if (reg_ > 0) {
regularize(H, reg_);
}
}
} catch(exception& ex) {
userOut<true, PL_WARN>() << "eval_h failed: " << ex.what() << endl;
throw;
}
}
bool WorhpInternal::eval_g(const double* x, double* g)
{
try {
log("eval_g started");
double time1 = clock();
if(worhp_o_.m>0){
// Pass the argument to the function
nlp_.setInput(x,NL_X);
nlp_.setInput(input(NLP_SOLVER_P),NL_P);
// Evaluate the function and tape
nlp_.evaluate();
// Ge the result
nlp_.getOutput(g,NL_G);
// Printing
if(monitored("eval_g")){
cout << "x = " << nlp_.input(NL_X) << endl;
cout << "g = " << nlp_.output(NL_G) << endl;
}
}
if (regularity_check_ && !isRegular(nlp_.output(NL_G).data())) casadi_error("WorhpInternal::eval_g: NaN or Inf detected.");
double time2 = clock();
t_eval_g_ += double(time2-time1)/CLOCKS_PER_SEC;
n_eval_g_ += 1;
log("eval_g ok");
return true;
} catch (exception& ex){
cerr << "eval_g failed: " << ex.what() << endl;
return false;
}
}
void SQPInternal::eval_h(const std::vector<double>& x, const std::vector<double>& lambda, double sigma, Matrix<double>& H){
int n_hess_in = H_.getNumInputs() - (parametric_ ? 1 : 0);
H_.setInput(x);
if(n_hess_in>1){
H_.setInput(lambda, n_hess_in == 4 ? 2 : 1);
H_.setInput(sigma, n_hess_in == 4 ? 3 : 2);
}
H_.evaluate();
H_.getOutput(H);
// Determing regularization parameter with Gershgorin theorem
if(regularize_){
reg_ = getRegularization(H);
if(reg_ > 0){
regularize(H,reg_);
}
}
if (monitored("eval_h")) {
cout << "x = " << x << endl;
cout << "H = " << endl;
H.printSparse();
}
}
void Sqpmethod::evaluate() {
if (inputs_check_) checkInputs();
checkInitialBounds();
if (gather_stats_) {
Dict iterations;
iterations["inf_pr"] = std::vector<double>();
iterations["inf_du"] = std::vector<double>();
iterations["ls_trials"] = std::vector<double>();
iterations["d_norm"] = std::vector<double>();
iterations["obj"] = std::vector<double>();
stats_["iterations"] = iterations;
}
// Get problem data
const vector<double>& x_init = input(NLP_SOLVER_X0).data();
const vector<double>& lbx = input(NLP_SOLVER_LBX).data();
const vector<double>& ubx = input(NLP_SOLVER_UBX).data();
const vector<double>& lbg = input(NLP_SOLVER_LBG).data();
const vector<double>& ubg = input(NLP_SOLVER_UBG).data();
// Set linearization point to initial guess
copy(x_init.begin(), x_init.end(), x_.begin());
// Initialize Lagrange multipliers of the NLP
copy(input(NLP_SOLVER_LAM_G0).begin(), input(NLP_SOLVER_LAM_G0).end(), mu_.begin());
copy(input(NLP_SOLVER_LAM_X0).begin(), input(NLP_SOLVER_LAM_X0).end(), mu_x_.begin());
t_eval_f_ = t_eval_grad_f_ = t_eval_g_ = t_eval_jac_g_ = t_eval_h_ =
t_callback_fun_ = t_callback_prepare_ = t_mainloop_ = 0;
n_eval_f_ = n_eval_grad_f_ = n_eval_g_ = n_eval_jac_g_ = n_eval_h_ = 0;
double time1 = clock();
// Initial constraint Jacobian
eval_jac_g(x_, gk_, Jk_);
// Initial objective gradient
eval_grad_f(x_, fk_, gf_);
// Initialize or reset the Hessian or Hessian approximation
reg_ = 0;
if (exact_hessian_) {
eval_h(x_, mu_, 1.0, Bk_);
} else {
reset_h();
}
// Evaluate the initial gradient of the Lagrangian
copy(gf_.begin(), gf_.end(), gLag_.begin());
if (ng_>0) casadi_mv_t(Jk_.ptr(), Jk_.sparsity(), getPtr(mu_), getPtr(gLag_));
// gLag += mu_x_;
transform(gLag_.begin(), gLag_.end(), mu_x_.begin(), gLag_.begin(), plus<double>());
// Number of SQP iterations
int iter = 0;
// Number of line-search iterations
int ls_iter = 0;
// Last linesearch successfull
bool ls_success = true;
// Reset
merit_mem_.clear();
sigma_ = 0.; // NOTE: Move this into the main optimization loop
// Default stepsize
double t = 0;
// MAIN OPTIMIZATION LOOP
while (true) {
// Primal infeasability
double pr_inf = primalInfeasibility(x_, lbx, ubx, gk_, lbg, ubg);
// inf-norm of lagrange gradient
double gLag_norminf = norm_inf(gLag_);
// inf-norm of step
double dx_norminf = norm_inf(dx_);
// Print header occasionally
if (iter % 10 == 0) printIteration(userOut());
// Printing information about the actual iterate
printIteration(userOut(), iter, fk_, pr_inf, gLag_norminf, dx_norminf,
reg_, ls_iter, ls_success);
if (gather_stats_) {
Dict iterations = stats_["iterations"];
std::vector<double> tmp=iterations["inf_pr"];
tmp.push_back(pr_inf);
iterations["inf_pr"] = tmp;
tmp=iterations["inf_du"];
tmp.push_back(gLag_norminf);
iterations["inf_du"] = tmp;
tmp=iterations["d_norm"];
tmp.push_back(dx_norminf);
iterations["d_norm"] = tmp;
std::vector<int> tmp2=iterations["ls_trials"];
tmp2.push_back(ls_iter);
iterations["ls_trials"] = tmp2;
tmp=iterations["obj"];
tmp.push_back(fk_);
iterations["obj"] = tmp;
stats_["iterations"] = iterations;
}
// Call callback function if present
if (!callback_.isNull()) {
double time1 = clock();
if (!output(NLP_SOLVER_F).isempty()) output(NLP_SOLVER_F).set(fk_);
if (!output(NLP_SOLVER_X).isempty()) output(NLP_SOLVER_X).setNZ(x_);
if (!output(NLP_SOLVER_LAM_G).isempty()) output(NLP_SOLVER_LAM_G).setNZ(mu_);
if (!output(NLP_SOLVER_LAM_X).isempty()) output(NLP_SOLVER_LAM_X).setNZ(mu_x_);
if (!output(NLP_SOLVER_G).isempty()) output(NLP_SOLVER_G).setNZ(gk_);
Dict iteration;
iteration["iter"] = iter;
iteration["inf_pr"] = pr_inf;
iteration["inf_du"] = gLag_norminf;
iteration["d_norm"] = dx_norminf;
iteration["ls_trials"] = ls_iter;
iteration["obj"] = fk_;
stats_["iteration"] = iteration;
double time2 = clock();
t_callback_prepare_ += (time2-time1)/CLOCKS_PER_SEC;
time1 = clock();
int ret = callback_(ref_, user_data_);
time2 = clock();
t_callback_fun_ += (time2-time1)/CLOCKS_PER_SEC;
if (ret) {
userOut() << endl;
userOut() << "casadi::SQPMethod: aborted by callback..." << endl;
stats_["return_status"] = "User_Requested_Stop";
break;
}
}
// Checking convergence criteria
if (pr_inf < tol_pr_ && gLag_norminf < tol_du_) {
userOut() << endl;
userOut() << "casadi::SQPMethod: Convergence achieved after "
<< iter << " iterations." << endl;
stats_["return_status"] = "Solve_Succeeded";
break;
}
if (iter >= max_iter_) {
userOut() << endl;
userOut() << "casadi::SQPMethod: Maximum number of iterations reached." << endl;
stats_["return_status"] = "Maximum_Iterations_Exceeded";
break;
}
if (iter > 0 && dx_norminf <= min_step_size_) {
userOut() << endl;
userOut() << "casadi::SQPMethod: Search direction becomes too small without "
"convergence criteria being met." << endl;
stats_["return_status"] = "Search_Direction_Becomes_Too_Small";
break;
}
// Start a new iteration
iter++;
log("Formulating QP");
// Formulate the QP
transform(lbx.begin(), lbx.end(), x_.begin(), qp_LBX_.begin(), minus<double>());
transform(ubx.begin(), ubx.end(), x_.begin(), qp_UBX_.begin(), minus<double>());
transform(lbg.begin(), lbg.end(), gk_.begin(), qp_LBA_.begin(), minus<double>());
transform(ubg.begin(), ubg.end(), gk_.begin(), qp_UBA_.begin(), minus<double>());
// Solve the QP
solve_QP(Bk_, gf_, qp_LBX_, qp_UBX_, Jk_, qp_LBA_, qp_UBA_, dx_, qp_DUAL_X_, qp_DUAL_A_);
log("QP solved");
// Detecting indefiniteness
double gain = casadi_quad_form(Bk_.ptr(), Bk_.sparsity(), getPtr(dx_));
if (gain < 0) {
casadi_warning("Indefinite Hessian detected...");
}
// Calculate penalty parameter of merit function
sigma_ = std::max(sigma_, 1.01*norm_inf(qp_DUAL_X_));
sigma_ = std::max(sigma_, 1.01*norm_inf(qp_DUAL_A_));
// Calculate L1-merit function in the actual iterate
double l1_infeas = primalInfeasibility(x_, lbx, ubx, gk_, lbg, ubg);
// Right-hand side of Armijo condition
double F_sens = inner_prod(dx_, gf_);
double L1dir = F_sens - sigma_ * l1_infeas;
double L1merit = fk_ + sigma_ * l1_infeas;
// Storing the actual merit function value in a list
merit_mem_.push_back(L1merit);
if (merit_mem_.size() > merit_memsize_) {
merit_mem_.pop_front();
}
// Stepsize
t = 1.0;
double fk_cand;
// Merit function value in candidate
double L1merit_cand = 0;
// Reset line-search counter, success marker
ls_iter = 0;
ls_success = true;
// Line-search
log("Starting line-search");
if (max_iter_ls_>0) { // max_iter_ls_== 0 disables line-search
// Line-search loop
while (true) {
for (int i=0; i<nx_; ++i) x_cand_[i] = x_[i] + t * dx_[i];
try {
// Evaluating objective and constraints
eval_f(x_cand_, fk_cand);
eval_g(x_cand_, gk_cand_);
} catch(const CasadiException& ex) {
// Silent ignore; line-search failed
ls_iter++;
// Backtracking
t = beta_ * t;
continue;
}
ls_iter++;
// Calculating merit-function in candidate
l1_infeas = primalInfeasibility(x_cand_, lbx, ubx, gk_cand_, lbg, ubg);
L1merit_cand = fk_cand + sigma_ * l1_infeas;
// Calculating maximal merit function value so far
double meritmax = *max_element(merit_mem_.begin(), merit_mem_.end());
if (L1merit_cand <= meritmax + t * c1_ * L1dir) {
// Accepting candidate
log("Line-search completed, candidate accepted");
break;
}
// Line-search not successful, but we accept it.
if (ls_iter == max_iter_ls_) {
ls_success = false;
log("Line-search completed, maximum number of iterations");
break;
}
// Backtracking
t = beta_ * t;
}
// Candidate accepted, update dual variables
for (int i=0; i<ng_; ++i) mu_[i] = t * qp_DUAL_A_[i] + (1 - t) * mu_[i];
for (int i=0; i<nx_; ++i) mu_x_[i] = t * qp_DUAL_X_[i] + (1 - t) * mu_x_[i];
// Candidate accepted, update the primal variable
copy(x_.begin(), x_.end(), x_old_.begin());
copy(x_cand_.begin(), x_cand_.end(), x_.begin());
} else {
// Full step
copy(qp_DUAL_A_.begin(), qp_DUAL_A_.end(), mu_.begin());
copy(qp_DUAL_X_.begin(), qp_DUAL_X_.end(), mu_x_.begin());
copy(x_.begin(), x_.end(), x_old_.begin());
// x+=dx
transform(x_.begin(), x_.end(), dx_.begin(), x_.begin(), plus<double>());
}
if (!exact_hessian_) {
// Evaluate the gradient of the Lagrangian with the old x but new mu (for BFGS)
copy(gf_.begin(), gf_.end(), gLag_old_.begin());
if (ng_>0) casadi_mv_t(Jk_.ptr(), Jk_.sparsity(), getPtr(mu_), getPtr(gLag_old_));
// gLag_old += mu_x_;
transform(gLag_old_.begin(), gLag_old_.end(), mu_x_.begin(), gLag_old_.begin(),
plus<double>());
}
// Evaluate the constraint Jacobian
log("Evaluating jac_g");
eval_jac_g(x_, gk_, Jk_);
// Evaluate the gradient of the objective function
log("Evaluating grad_f");
eval_grad_f(x_, fk_, gf_);
// Evaluate the gradient of the Lagrangian with the new x and new mu
copy(gf_.begin(), gf_.end(), gLag_.begin());
if (ng_>0) casadi_mv_t(Jk_.ptr(), Jk_.sparsity(), getPtr(mu_), getPtr(gLag_));
// gLag += mu_x_;
transform(gLag_.begin(), gLag_.end(), mu_x_.begin(), gLag_.begin(), plus<double>());
// Updating Lagrange Hessian
if (!exact_hessian_) {
log("Updating Hessian (BFGS)");
// BFGS with careful updates and restarts
if (iter % lbfgs_memory_ == 0) {
// Reset Hessian approximation by dropping all off-diagonal entries
const int* colind = Bk_.colind(); // Access sparsity (column offset)
int ncol = Bk_.size2();
const int* row = Bk_.row(); // Access sparsity (row)
vector<double>& data = Bk_.data(); // Access nonzero elements
for (int cc=0; cc<ncol; ++cc) { // Loop over the columns of the Hessian
for (int el=colind[cc]; el<colind[cc+1]; ++el) {
// Loop over the nonzero elements of the column
if (cc!=row[el]) data[el] = 0; // Remove if off-diagonal entries
}
}
}
// Pass to BFGS update function
bfgs_.setInput(Bk_, BFGS_BK);
bfgs_.setInputNZ(x_, BFGS_X);
bfgs_.setInputNZ(x_old_, BFGS_X_OLD);
bfgs_.setInputNZ(gLag_, BFGS_GLAG);
bfgs_.setInputNZ(gLag_old_, BFGS_GLAG_OLD);
// Update the Hessian approximation
bfgs_.evaluate();
// Get the updated Hessian
bfgs_.getOutput(Bk_);
if (monitored("bfgs")) {
userOut() << "x = " << x_ << endl;
userOut() << "BFGS = " << endl;
Bk_.printSparse();
}
} else {
// Exact Hessian
log("Evaluating hessian");
eval_h(x_, mu_, 1.0, Bk_);
}
}
double time2 = clock();
t_mainloop_ = (time2-time1)/CLOCKS_PER_SEC;
// Save results to outputs
output(NLP_SOLVER_F).set(fk_);
output(NLP_SOLVER_X).setNZ(x_);
output(NLP_SOLVER_LAM_G).setNZ(mu_);
output(NLP_SOLVER_LAM_X).setNZ(mu_x_);
output(NLP_SOLVER_G).setNZ(gk_);
if (hasOption("print_time") && static_cast<bool>(getOption("print_time"))) {
// Write timings
userOut() << "time spent in eval_f: " << t_eval_f_ << " s.";
if (n_eval_f_>0)
userOut() << " (" << n_eval_f_ << " calls, " << (t_eval_f_/n_eval_f_)*1000
<< " ms. average)";
userOut() << endl;
userOut() << "time spent in eval_grad_f: " << t_eval_grad_f_ << " s.";
if (n_eval_grad_f_>0)
userOut() << " (" << n_eval_grad_f_ << " calls, "
<< (t_eval_grad_f_/n_eval_grad_f_)*1000 << " ms. average)";
userOut() << endl;
userOut() << "time spent in eval_g: " << t_eval_g_ << " s.";
if (n_eval_g_>0)
userOut() << " (" << n_eval_g_ << " calls, " << (t_eval_g_/n_eval_g_)*1000
<< " ms. average)";
userOut() << endl;
userOut() << "time spent in eval_jac_g: " << t_eval_jac_g_ << " s.";
if (n_eval_jac_g_>0)
userOut() << " (" << n_eval_jac_g_ << " calls, "
<< (t_eval_jac_g_/n_eval_jac_g_)*1000 << " ms. average)";
userOut() << endl;
userOut() << "time spent in eval_h: " << t_eval_h_ << " s.";
if (n_eval_h_>1)
userOut() << " (" << n_eval_h_ << " calls, " << (t_eval_h_/n_eval_h_)*1000
<< " ms. average)";
userOut() << endl;
userOut() << "time spent in main loop: " << t_mainloop_ << " s." << endl;
userOut() << "time spent in callback function: " << t_callback_fun_ << " s." << endl;
userOut() << "time spent in callback preparation: " << t_callback_prepare_ << " s." << endl;
}
// Save statistics
stats_["iter_count"] = iter;
stats_["t_eval_f"] = t_eval_f_;
stats_["t_eval_grad_f"] = t_eval_grad_f_;
stats_["t_eval_g"] = t_eval_g_;
stats_["t_eval_jac_g"] = t_eval_jac_g_;
stats_["t_eval_h"] = t_eval_h_;
stats_["t_mainloop"] = t_mainloop_;
stats_["t_callback_fun"] = t_callback_fun_;
stats_["t_callback_prepare"] = t_callback_prepare_;
stats_["n_eval_f"] = n_eval_f_;
stats_["n_eval_grad_f"] = n_eval_grad_f_;
stats_["n_eval_g"] = n_eval_g_;
stats_["n_eval_jac_g"] = n_eval_jac_g_;
stats_["n_eval_h"] = n_eval_h_;
}
void Newton::solveNonLinear() {
casadi_log("Newton::solveNonLinear:begin");
// Set up timers for profiling
double time_zero=0;
double time_start=0;
double time_stop=0;
if (CasadiOptions::profiling && !CasadiOptions::profilingBinary) {
time_zero = getRealTime();
CasadiOptions::profilingLog << "start " << this << ":" <<getOption("name") << std::endl;
}
// Pass the inputs to J
for (int i=0; i<getNumInputs(); ++i) {
if (i!=iin_) jac_.setInput(input(i), i);
}
// Aliases
DMatrix &u = output(iout_);
DMatrix &J = jac_.output(0);
DMatrix &F = jac_.output(1+iout_);
// Perform the Newton iterations
int iter=0;
bool success = true;
while (true) {
// Break if maximum number of iterations already reached
if (iter >= max_iter_) {
log("evaluate", "Max. iterations reached.");
stats_["return_status"] = "max_iteration_reached";
success = false;
break;
}
// Start a new iteration
iter++;
// Print progress
if (monitored("step") || monitored("stepsize")) {
std::cout << "Step " << iter << "." << std::endl;
}
if (monitored("step")) {
std::cout << " u = " << u << std::endl;
}
// Use u to evaluate J
jac_.setInput(u, iin_);
for (int i=0; i<getNumInputs(); ++i)
if (i!=iin_) jac_.setInput(input(i), i);
if (CasadiOptions::profiling) {
time_start = getRealTime(); // Start timer
}
jac_.evaluate();
// Write out profiling information
if (CasadiOptions::profiling && !CasadiOptions::profilingBinary) {
time_stop = getRealTime(); // Stop timer
CasadiOptions::profilingLog
<< (time_stop-time_start)*1e6 << " ns | "
<< (time_stop-time_zero)*1e3 << " ms | "
<< this << ":" << getOption("name") << ":0|" << jac_.get() << ":"
<< jac_.getOption("name") << "|evaluate jacobian" << std::endl;
}
if (monitored("F")) std::cout << " F = " << F << std::endl;
if (monitored("normF"))
std::cout << " F (min, max, 1-norm, 2-norm) = "
<< (*std::min_element(F.data().begin(), F.data().end()))
<< ", " << (*std::max_element(F.data().begin(), F.data().end()))
<< ", " << sumAll(fabs(F)) << ", " << sqrt(sumAll(F*F)) << std::endl;
if (monitored("J")) std::cout << " J = " << J << std::endl;
double abstol = 0;
if (numeric_limits<double>::infinity() != abstol_) {
abstol = std::max((*std::max_element(F.data().begin(),
F.data().end())),
-(*std::min_element(F.data().begin(),
F.data().end())));
if (abstol <= abstol_) {
casadi_log("Converged to acceptable tolerance - abstol: " << abstol_);
break;
}
}
// Prepare the linear solver with J
linsol_.setInput(J, LINSOL_A);
if (CasadiOptions::profiling) {
time_start = getRealTime(); // Start timer
}
linsol_.prepare();
// Write out profiling information
if (CasadiOptions::profiling && !CasadiOptions::profilingBinary) {
time_stop = getRealTime(); // Stop timer
CasadiOptions::profilingLog
<< (time_stop-time_start)*1e6 << " ns | "
<< (time_stop-time_zero)*1e3 << " ms | "
<< this << ":" << getOption("name")
<< ":1||prepare linear system" << std::endl;
}
if (CasadiOptions::profiling) {
time_start = getRealTime(); // Start timer
}
// Solve against F
linsol_.solve(&F.front(), 1, false);
if (CasadiOptions::profiling && !CasadiOptions::profilingBinary) {
time_stop = getRealTime(); // Stop timer
CasadiOptions::profilingLog
<< (time_stop-time_start)*1e6 << " ns | "
<< (time_stop-time_zero)*1e3 << " ms | "
<< this << ":" << getOption("name") << ":2||solve linear system" << std::endl;
}
if (monitored("step")) {
std::cout << " step = " << F << std::endl;
}
double abstolStep=0;
if (numeric_limits<double>::infinity() != abstolStep_) {
abstolStep = std::max((*std::max_element(F.data().begin(),
F.data().end())),
-(*std::min_element(F.data().begin(),
F.data().end())));
if (monitored("stepsize")) {
std::cout << " stepsize = " << abstolStep << std::endl;
}
if (abstolStep <= abstolStep_) {
casadi_log("Converged to acceptable tolerance - abstolStep: " << abstolStep_);
break;
}
}
if (print_iteration_) {
// Only print iteration header once in a while
if (iter % 10==0) {
printIteration(std::cout);
}
// Print iteration information
printIteration(std::cout, iter, abstol, abstolStep);
}
// Update Xk+1 = Xk - J^(-1) F
std::transform(u.begin(), u.end(), F.begin(), u.begin(), std::minus<double>());
// Get auxiliary outputs
for (int i=0; i<getNumOutputs(); ++i) {
if (i!=iout_) jac_.getOutput(output(i), 1+i);
}
}
// Store the iteration count
if (gather_stats_) stats_["iter"] = iter;
if (success) stats_["return_status"] = "success";
// Factorization up-to-date
fact_up_to_date_ = true;
casadi_log("Newton::solveNonLinear():end after " << iter << " steps");
}
