void LapackLuDense::prepare() {
double time_start=0;
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
time_start = getRealTime(); // Start timer
profileWriteEntry(CasadiOptions::profilingLog, this);
}
prepared_ = false;
// Get the elements of the matrix, dense format
input(0).get(mat_);
if (equilibriate_) {
// Calculate the col and row scaling factors
double colcnd, rowcnd; // ratio of the smallest to the largest col/row scaling factor
double amax; // absolute value of the largest matrix element
int info = -100;
dgeequ_(&ncol_, &nrow_, getPtr(mat_), &ncol_, getPtr(r_),
getPtr(c_), &colcnd, &rowcnd, &amax, &info);
if (info < 0)
throw CasadiException("LapackQrDense::prepare: "
"dgeequ_ failed to calculate the scaling factors");
if (info>0) {
stringstream ss;
ss << "LapackLuDense::prepare: ";
if (info<=ncol_) ss << (info-1) << "-th row (zero-based) is exactly zero";
else ss << (info-1-ncol_) << "-th col (zero-based) is exactly zero";
userOut() << "Warning: " << ss.str() << endl;
if (allow_equilibration_failure_) userOut() << "Warning: " << ss.str() << endl;
else casadi_error(ss.str());
}
// Equilibrate the matrix if scaling was successful
if (info!=0)
dlaqge_(&ncol_, &nrow_, getPtr(mat_), &ncol_, getPtr(r_), getPtr(c_),
&colcnd, &rowcnd, &amax, &equed_);
else
equed_ = 'N';
}
// Factorize the matrix
int info = -100;
dgetrf_(&ncol_, &ncol_, getPtr(mat_), &ncol_, getPtr(ipiv_), &info);
if (info != 0) throw CasadiException("LapackLuDense::prepare: "
"dgetrf_ failed to factorize the Jacobian");
// Success if reached this point
prepared_ = true;
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
double time_stop = getRealTime(); // Stop timer
profileWriteTime(CasadiOptions::profilingLog, this, 0, time_stop-time_start,
time_stop-time_start);
profileWriteExit(CasadiOptions::profilingLog, this, time_stop-time_start);
}
}
XmlNode TinyXmlInterface::addNode(TiXmlNode* n) {
if (!n) throw CasadiException("Error in TinyXmlInterface::addNode: Node is 0");
XmlNode ret;
// Save name
ret.setName(n->Value());
// Save attributes
int type = n->Type();
if (type == TiXmlNode::TINYXML_ELEMENT) {
if (n->ToElement()!=0) {
for (TiXmlAttribute* pAttrib=n->ToElement()->FirstAttribute();
pAttrib;
pAttrib=pAttrib->Next()) {
ret.setAttribute(pAttrib->Name(), pAttrib->Value());
}
}
} else if (type == TiXmlNode::TINYXML_DOCUMENT) {
// do nothing
} else {
throw CasadiException("TinyXmlInterface::addNode");
}
// Count the number of children
int num_children = 0;
for (TiXmlNode* child = n->FirstChild(); child != 0; child= child->NextSibling()) {
num_children++;
}
ret.children_.reserve(num_children);
// add children
int ch = 0;
for (TiXmlNode* child = n->FirstChild(); child != 0; child= child->NextSibling(), ++ch) {
int childtype = child->Type();
if (childtype == TiXmlNode::TINYXML_ELEMENT) {
XmlNode newnode = addNode(child);
ret.children_.push_back(newnode);
ret.child_indices_[newnode.getName()] = ch;
} else if (childtype == TiXmlNode::TINYXML_COMMENT) {
ret.comment_ = child->Value();
} else if (childtype == TiXmlNode::TINYXML_TEXT) {
ret.text_ = child->ToText()->Value();
} else if (childtype == TiXmlNode::TINYXML_DECLARATION) {
cout << "Warning: Skipped TiXmlNode::TINYXML_DECLARATION" << endl;
} else {
throw CasadiException("Error in TinyXmlInterface::addNode: Unknown node type");
}
}
// Note: Return value optimization
return ret;
}
void CSparseCholeskyInternal::prepare() {
prepared_ = false;
// Get a reference to the nonzeros of the linear system
const vector<double>& linsys_nz = input().data();
// Make sure that all entries of the linear system are valid
for (int k=0; k<linsys_nz.size(); ++k) {
casadi_assert_message(!isnan(linsys_nz[k]), "Nonzero " << k << " is not-a-number");
casadi_assert_message(!isinf(linsys_nz[k]), "Nonzero " << k << " is infinite");
}
if (verbose()) {
userOut() << "CSparseCholeskyInternal::prepare: numeric factorization" << endl;
userOut() << "linear system to be factorized = " << endl;
input(0).printSparse();
}
if (L_) cs_nfree(L_);
L_ = cs_chol(&AT_, S_) ; // numeric Cholesky factorization
if (L_==0) {
DMatrix temp = input();
temp.makeSparse();
if (temp.sparsity().issingular()) {
stringstream ss;
ss << "CSparseCholeskyInternal::prepare: factorization failed due "
"to matrix being singular. Matrix contains numerical zeros which are"
" structurally non-zero. Promoting these zeros to be structural "
"zeros, the matrix was found to be structurally rank deficient. "
"sprank: " << sprank(temp.sparsity()) << " <-> " << temp.size2() << endl;
if (verbose()) {
ss << "Sparsity of the linear system: " << endl;
input(LINSOL_A).sparsity().print(ss); // print detailed
}
throw CasadiException(ss.str());
} else {
stringstream ss;
ss << "CSparseCholeskyInternal::prepare: factorization failed, "
"check if Jacobian is singular" << endl;
if (verbose()) {
ss << "Sparsity of the linear system: " << endl;
input(LINSOL_A).sparsity().print(ss); // print detailed
}
throw CasadiException(ss.str());
}
}
casadi_assert(L_!=0);
prepared_ = true;
}
void LapackLuDense::solve(double* x, int nrhs, bool transpose) {
double time_start=0;
if (CasadiOptions::profiling&& CasadiOptions::profilingBinary) {
time_start = getRealTime(); // Start timer
profileWriteEntry(CasadiOptions::profilingLog, this);
}
// Scale the right hand side
if (transpose) {
rowScaling(x, nrhs);
} else {
colScaling(x, nrhs);
}
// Solve the system of equations
int info = 100;
char trans = transpose ? 'T' : 'N';
dgetrs_(&trans, &ncol_, &nrhs, getPtr(mat_), &ncol_, getPtr(ipiv_), x, &ncol_, &info);
if (info != 0) throw CasadiException("LapackLuDense::solve: "
"failed to solve the linear system");
// Scale the solution
if (transpose) {
colScaling(x, nrhs);
} else {
rowScaling(x, nrhs);
}
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
double time_stop = getRealTime(); // Stop timer
profileWriteTime(CasadiOptions::profilingLog, this, 1,
time_stop-time_start, time_stop-time_start);
profileWriteExit(CasadiOptions::profilingLog, this, time_stop-time_start);
}
}
void LapackLuDense::init() {
// Call the base class initializer
LinearSolverInternal::init();
// Get dimensions
ncol_ = ncol();
nrow_ = nrow();
// Currently only square matrices tested
if (ncol_!=nrow_) throw CasadiException(
"LapackLuDense::LapackLuDense: currently only square matrices implemented.");
// Allocate matrix
mat_.resize(ncol_*ncol_);
ipiv_.resize(ncol_);
// Equilibrate?
equilibriate_ = getOption("equilibration").toInt();
if (equilibriate_) {
r_.resize(ncol_);
c_.resize(nrow_);
}
equed_ = 'N'; // No equilibration
// Allow equilibration failures
allow_equilibration_failure_ = getOption("allow_equilibration_failure").toInt();
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
profileWriteName(CasadiOptions::profilingLog, this, "LapackLUDense",
ProfilingData_FunctionType_Other, 2);
profileWriteSourceLine(CasadiOptions::profilingLog, this, 0, "prepare", -1);
profileWriteSourceLine(CasadiOptions::profilingLog, this, 1, "solve", -1);
}
}
void LapackQRDenseInternal::solve(double* x, int nrhs, bool transpose) {
// Properties of R
char uploR = 'U';
char diagR = 'N';
char sideR = 'L';
double alphaR = 1.;
char transR = transpose ? 'T' : 'N';
// Properties of Q
char transQ = transpose ? 'N' : 'T';
char sideQ = 'L';
int k = tau_.size(); // minimum of ncol_ and nrow_
int lwork = work_.size();
if (transpose) {
// Solve for transpose(R)
dtrsm_(&sideR, &uploR, &transR, &diagR, &ncol_, &nrhs, &alphaR,
getPtr(mat_), &ncol_, x, &ncol_);
// Multiply by Q
int info = 100;
dormqr_(&sideQ, &transQ, &ncol_, &nrhs, &k, getPtr(mat_), &ncol_, getPtr(tau_), x,
&ncol_, getPtr(work_), &lwork, &info);
if (info != 0) throw CasadiException("LapackQRDenseInternal::solve: dormqr_ failed "
"to solve the linear system");
} else {
// Multiply by transpose(Q)
int info = 100;
dormqr_(&sideQ, &transQ, &ncol_, &nrhs, &k, getPtr(mat_), &ncol_, getPtr(tau_), x,
&ncol_, getPtr(work_), &lwork, &info);
if (info != 0) throw CasadiException("LapackQRDenseInternal::solve: dormqr_ failed to "
"solve the linear system");
// Solve for R
dtrsm_(&sideR, &uploR, &transR, &diagR, &ncol_, &nrhs, &alphaR,
getPtr(mat_), &ncol_, x, &ncol_);
}
}
void LapackQRDenseInternal::prepare() {
prepared_ = false;
// Get the elements of the matrix, dense format
input(0).get(mat_, DENSE);
// Factorize the matrix
int info = -100;
int lwork = work_.size();
dgeqrf_(&ncol_, &ncol_, getPtr(mat_), &ncol_, getPtr(tau_), getPtr(work_), &lwork, &info);
if (info != 0) throw CasadiException("LapackQRDenseInternal::prepare: dgeqrf_ "
"failed to factorize the Jacobian");
// Success if reached this point
prepared_ = true;
}
void LapackQRDenseInternal::init() {
// Call the base class initializer
LinearSolverInternal::init();
// Get dimensions
ncol_ = ncol();
nrow_ = nrow();
// Currently only square matrices tested
if (ncol_!=nrow_) throw CasadiException("LapackQRDenseInternal::init: currently only "
"square matrices implemented.");
// Allocate matrix
mat_.resize(ncol_*ncol_);
tau_.resize(ncol_);
work_.resize(10*ncol_);
}
void AcadoFunction::init(){
if(f_.isNull())
throw CasadiException("AcadoFunction::init: function is null");
// Initialize the casadi function
f_.init();
// Get dimensions
dim_.resize(f_.getNumInputs());
for(int i=0; i<dim_.size(); ++i)
dim_[i] = f_.input(i).numel();
// Get number of equations
neq_ = f_.output().numel();
// Create an acado function
fcn_ = new ACADO::CFunction( neq_, fcn_wrapper, fcn_fwd_wrapper, fcn_adj_wrapper);
fcn_->setUserData(this);
}
CRSSparsity sp_tril(int n) {
if (n<0)
throw CasadiException("sp_tril expects a positive integer as argument");
int c=0;
int t=0;
std::vector< int > col((n*(n+1))/2,0);
for (int i=0;i<(n*(n+1))/2;i++) {
col[i]=t++;
if (t>c) {
t=0;
c++;
}
}
std::vector< int > rowind(n+1,0);
c=0;
for (int i=1;i<n+1;i++)
rowind[i]=rowind[i-1]+1+(c++);
return CRSSparsity(n,n,col,rowind);
}
int SXNode::getIntValue() const{
throw CasadiException(string("getIntValue() not defined for class ") + typeid(*this).name());
}
void CsparseInterface::prepare() {
double time_start=0;
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
time_start = getRealTime(); // Start timer
profileWriteEntry(CasadiOptions::profilingLog, this);
}
if (!called_once_) {
if (verbose()) {
cout << "CsparseInterface::prepare: symbolic factorization" << endl;
}
// ordering and symbolic analysis
int order = 0; // ordering?
if (S_) cs_sfree(S_);
S_ = cs_sqr(order, &A_, 0) ;
}
prepared_ = false;
called_once_ = true;
// Get a referebce to the nonzeros of the linear system
const vector<double>& linsys_nz = input().data();
// Make sure that all entries of the linear system are valid
for (int k=0; k<linsys_nz.size(); ++k) {
casadi_assert_message(!isnan(linsys_nz[k]), "Nonzero " << k << " is not-a-number");
casadi_assert_message(!isinf(linsys_nz[k]), "Nonzero " << k << " is infinite");
}
if (verbose()) {
cout << "CsparseInterface::prepare: numeric factorization" << endl;
cout << "linear system to be factorized = " << endl;
input(0).printSparse();
}
double tol = 1e-8;
if (N_) cs_nfree(N_);
N_ = cs_lu(&A_, S_, tol) ; // numeric LU factorization
if (N_==0) {
DMatrix temp = input();
temp.makeSparse();
if (temp.sparsity().isSingular()) {
stringstream ss;
ss << "CsparseInterface::prepare: factorization failed due to matrix"
" being singular. Matrix contains numerical zeros which are "
"structurally non-zero. Promoting these zeros to be structural "
"zeros, the matrix was found to be structurally rank deficient."
" sprank: " << sprank(temp.sparsity()) << " <-> " << temp.size2() << endl;
if (verbose()) {
ss << "Sparsity of the linear system: " << endl;
input(LINSOL_A).sparsity().print(ss); // print detailed
}
throw CasadiException(ss.str());
} else {
stringstream ss;
ss << "CsparseInterface::prepare: factorization failed, check if Jacobian is singular"
<< endl;
if (verbose()) {
ss << "Sparsity of the linear system: " << endl;
input(LINSOL_A).sparsity().print(ss); // print detailed
}
throw CasadiException(ss.str());
}
}
casadi_assert(N_!=0);
prepared_ = true;
if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
double time_stop = getRealTime(); // Stop timer
profileWriteTime(CasadiOptions::profilingLog, this, 0,
time_stop-time_start,
time_stop-time_start);
profileWriteExit(CasadiOptions::profilingLog, this, time_stop-time_start);
}
}
void StabilizedQpSolverInternal::solve() {
throw CasadiException("StabilizedQpSolverInternal::solve: Not implemented");
}
void CSparseInternal::prepare(){
if(!called_once_){
// ordering and symbolic analysis
int order = 3; // ordering?
int qr = 1; // LU
if(S_) cs_sfree(S_);
S_ = cs_sqr (order, &AT_, qr) ;
std::vector<int> pinv;
std::vector<int> q;
std::vector<int> parent;
std::vector<int> cp;
std::vector<int> leftmost;
int m2;
double lnz;
double unz;
input().sparsity()->prefactorize(order, qr, pinv, q, parent, cp, leftmost, m2, lnz, unz);
cout << "pinv" << endl;
cout << pinv << endl;
if(S_->pinv!=0)
cout << vector<int>(S_->pinv, S_->pinv + pinv.size()) << endl;
cout << endl;
cout << "q" << endl;
cout << q << endl;
if(S_->q!=0)
cout << vector<int>(S_->q, S_->q + q.size()) << endl;
cout << endl;
cout << "parent" << endl;
cout << parent << endl;
if(S_->parent!=0)
cout << vector<int>(S_->parent, S_->parent + parent.size()) << endl;
cout << endl;
cout << "cp" << endl;
cout << cp << endl;
if(S_->cp!=0)
cout << vector<int>(S_->cp, S_->cp + cp.size()) << endl;
cout << endl;
cout << "leftmost" << endl;
cout << leftmost << endl;
if(S_->leftmost!=0)
cout << vector<int>(S_->leftmost, S_->leftmost + leftmost.size()) << endl;
cout << endl;
}
prepared_ = false;
called_once_ = true;
double tol = 1e-8;
if(N_) cs_nfree(N_);
N_ = cs_lu(&AT_, S_, tol) ; // numeric LU factorization
if(N_==0){
throw CasadiException("factorization failed, Jacobian singular?");
}
casadi_assert(N_!=0);
prepared_ = true;
}
void LpSolverInternal::solve() {
throw CasadiException("LpSolverInternal::solve: Not implemented");
}
const std::string& SXNode::getName() const{
throw CasadiException("SXNode::getName failed, the node must be symbolic");
}
CRSSparsity sp_banded(int n, int p) {
throw CasadiException("sp_banded: Not implemented yet");
}
void QpSolverInternal::evaluate() {
throw CasadiException("QpSolverInternal::evaluate: Not implemented");
}
void AcadoOCPInternal::evaluate(int nfdir, int nadir){
// Initial constraint function
if(!rfcn_.f_.isNull()){
const Matrix<double>& lbr = input(ACADO_LBR);
ACADO::Vector lb(lbr.size(),&lbr.front());
const Matrix<double>& ubr = input(ACADO_UBR);
ACADO::Vector ub(ubr.size(),&ubr.front());
ocp_->subjectTo( ACADO::AT_START, lb <= (*rfcn_.fcn_)(*arg_) <= ub);
}
// Path constraint function
if(!cfcn_.f_.isNull()){
const Matrix<double>& lbc = input(ACADO_LBC);
ACADO::Vector lb(lbc.size(),&lbc.front());
const Matrix<double>& ubc = input(ACADO_UBC);
ACADO::Vector ub(ubc.size(),&ubc.front());
ocp_->subjectTo( lb <= (*cfcn_.fcn_)(*arg_) <= ub );
}
// State bounds
Matrix<double> &lbx = input(ACADO_LBX);
Matrix<double> &ubx = input(ACADO_UBX);
for(int i=0; i<nxd_; ++i)
ocp_->subjectTo( lbx.at(i) <= xd_[i] <= ubx.at(i) );
for(int i=nxd_; i<nx_; ++i)
ocp_->subjectTo( lbx.at(i) <= xa_[i-nxd_] <= ubx.at(i) );
// Pass bounds on state at initial time
Matrix<double> &lbx0 = input(ACADO_LBX0);
Matrix<double> &ubx0 = input(ACADO_UBX0);
for(int i=0; i<nxd_; ++i)
ocp_->subjectTo( ACADO::AT_START, lbx0.at(i) <= xd_[i] <= ubx0.at(i) );
for(int i=nxd_; i<nx_; ++i)
ocp_->subjectTo( ACADO::AT_START, lbx0.at(i) <= xa_[i-nxd_] <= ubx0.at(i) );
// ocp_->subjectTo( AT_END , xd_[1] == 0.0 );
// ocp_->subjectTo( AT_END , xd_[2] == 0.0 );
// Control bounds
Matrix<double> &lbu = input(ACADO_LBU);
Matrix<double> &ubu = input(ACADO_UBU);
for(int i=0; i<nu_; ++i)
ocp_->subjectTo( lbu.at(i) <= u_[i] <= ubu.at(i) );
// Parameter bounds
Matrix<double> &lbp = input(ACADO_LBP);
Matrix<double> &ubp = input(ACADO_UBP);
for(int i=0; i<np_; ++i)
ocp_->subjectTo( lbp.at(i) <= p_[i] <= ubp.at(i) );
// Periodic boundary condition
if(hasSetOption("periodic_bounds")){
const vector<int>& periodic = getOption("periodic_bounds");
if(periodic.size()!=nx_) throw CasadiException("wrong dimension for periodic_bounds");
for(int i=0; i<nxd_; ++i)
if(periodic[i])
ocp_->subjectTo( 0.0, xd_[i], -xd_[i], 0.0);
for(int i=nxd_; i<nx_; ++i)
if(periodic[i])
ocp_->subjectTo( 0.0, xa_[i-nxd_], -xa_[i-nxd_], 0.0);
}
algorithm_ = new ACADO::OptimizationAlgorithm(*ocp_);
// set print level
ACADO::PrintLevel printlevel;
if(getOption("print_level")=="none") printlevel = ACADO::NONE;
else if(getOption("print_level")=="low") printlevel = ACADO::LOW;
else if(getOption("print_level")=="medium") printlevel = ACADO::MEDIUM;
else if(getOption("print_level")=="high") printlevel = ACADO::HIGH;
else if(getOption("print_level")=="debug") printlevel = ACADO::DEBUG;
else throw CasadiException("Illegal print level. Allowed are \"none\", \"low\", \"medium\", \"high\", \"debug\"");
algorithm_->set(ACADO::INTEGRATOR_PRINTLEVEL, printlevel );
// Set integrator
if(hasSetOption("integrator")){
GenericType integ = getOption("integrator");
ACADO::IntegratorType itype;
if(integ=="rk4") itype=ACADO::INT_RK4;
else if(integ=="rk12") itype=ACADO::INT_RK12;
else if(integ=="rk23") itype=ACADO::INT_RK23;
else if(integ=="rk45") itype=ACADO::INT_RK45;
else if(integ=="rk78") itype=ACADO::INT_RK78;
else if(integ=="bdf") itype=ACADO::INT_BDF;
else if(integ=="discrete") itype=ACADO::INT_DISCRETE;
else if(integ=="unknown") itype=ACADO::INT_UNKNOWN;
#ifdef ACADO_HAS_USERDEF_INTEGRATOR
else if(integ=="casadi"){
if(ACADO::Integrator::integrator_creator_ || ACADO::Integrator::integrator_user_data_)
throw CasadiException("AcadoOCPInternal::AcadoOCPInternal: An instance already exists");
if(integrators_.size() <= n_nodes_)
throw CasadiException("AcadoOCPInternal::AcadoOCPInternal: Number of integrators does not match number of shooting nodes");
ACADO::Integrator::integrator_creator_ = &AcadoIntegratorBackend::create;
ACADO::Integrator::integrator_user_data_ = this;
itype=ACADO::INT_UNKNOWN;
}
#endif
else throw CasadiException("AcadoOCPInternal::evaluate: no such integrator: " + integ.toString());
algorithm_->set(ACADO::INTEGRATOR_TYPE, itype);
};
// Set integrator tolerance
if(hasSetOption("integrator_tolerance")) algorithm_->set( ACADO::INTEGRATOR_TOLERANCE, getOption("integrator_tolerance").toDouble());
if(hasSetOption("absolute_tolerance")) algorithm_->set( ACADO::ABSOLUTE_TOLERANCE, getOption("absolute_tolerance").toDouble());
if(hasSetOption("kkt_tolerance")) algorithm_->set( ACADO::KKT_TOLERANCE, getOption("kkt_tolerance").toDouble());
if(hasSetOption("max_num_iterations")) algorithm_->set( ACADO::MAX_NUM_ITERATIONS, getOption("max_num_iterations").toInt() );
if(hasSetOption("max_num_integrator_steps")) algorithm_->set( ACADO::MAX_NUM_INTEGRATOR_STEPS, getOption("max_num_integrator_steps").toInt() );
if(hasSetOption("relaxation_parameter")) algorithm_->set( ACADO::RELAXATION_PARAMETER, getOption("relaxation_parameter").toDouble());
if(hasSetOption("dynamic_sensitivity")){
if(getOption("dynamic_sensitivity") == "forward_sensitivities")
algorithm_->set( ACADO::DYNAMIC_SENSITIVITY, ACADO::FORWARD_SENSITIVITY );
else if(getOption("dynamic_sensitivity") == "backward_sensitivities")
algorithm_->set( ACADO::DYNAMIC_SENSITIVITY, ACADO::BACKWARD_SENSITIVITY );
else
throw CasadiException("illegal dynamic_sensitivity");
}
if(hasSetOption("hessian_approximation")){
int hess;
GenericType op = getOption("hessian_approximation");
if(op=="exact_hessian") hess = ACADO::EXACT_HESSIAN;
else if(op == "constant_hessian") hess = ACADO::CONSTANT_HESSIAN;
else if(op == "full_bfgs_update") hess = ACADO::FULL_BFGS_UPDATE;
else if(op == "block_bfgs_update") hess = ACADO::BLOCK_BFGS_UPDATE;
else if(op == "gauss_newton") hess = ACADO::GAUSS_NEWTON;
else if(op == "gauss_newton_with_block_bfgs") hess = ACADO::GAUSS_NEWTON_WITH_BLOCK_BFGS;
else throw CasadiException("illegal hessian approximation");
algorithm_->set( ACADO::HESSIAN_APPROXIMATION, hess);
}
// should the states be initialized by a forward integration?
bool auto_init = getOption("auto_init").toInt();
// Initialize differential states
if(nxd_>0){
// Initial guess
Matrix<double> &x0 = input(ACADO_X_GUESS);
// Assemble the variables grid
ACADO::VariablesGrid xd(nxd_, n_nodes_+1);
for(int i=0; i<n_nodes_+1; ++i){
ACADO::Vector v(nxd_,&x0.at(i*nx_));
xd.setVector(i,v);
}
// Pass to acado
algorithm_->initializeDifferentialStates(xd,auto_init ? ACADO::BT_TRUE : ACADO::BT_FALSE);
}
// Initialize algebraic states
if(nxa_>0){
// Initial guess
Matrix<double> &x0 = input(ACADO_X_GUESS);
// Assemble the variables grid
ACADO::VariablesGrid xa(nxa_, n_nodes_+1);
for(int i=0; i<n_nodes_+1; ++i){
ACADO::Vector v(nxa_,&x0.at(i*nx_+nxd_));
xa.setVector(i,v);
}
// Pass to acado
algorithm_->initializeAlgebraicStates(xa,auto_init ? ACADO::BT_TRUE : ACADO::BT_FALSE);
}
// Initialize controls
if(nu_>0){
// Initial guess
Matrix<double> &u0 = input(ACADO_U_GUESS);
// Assemble the variables grid
ACADO::VariablesGrid u(nu_, n_nodes_+1);
for(int i=0; i<n_nodes_+1; ++i){
ACADO::Vector v(nu_,&u0.at(i*nu_));
u.setVector(i,v);
}
// Pass to acado
algorithm_->initializeControls(u);
}
// Initialize parameters
if(np_>0){
// Initial guess
Matrix<double> &p0 = input(ACADO_P_GUESS);
// Assemble the variables grid
ACADO::VariablesGrid p(np_, n_nodes_+1);
for(int i=0; i<n_nodes_+1; ++i){
ACADO::Vector v(np_,&p0.front()); // NB!
p.setVector(i,v);
}
// Pass to acado
algorithm_->initializeParameters(p);
}
// Solve
algorithm_->solve();
// Get the optimal state trajectory
if(nxd_>0){
Matrix<double> &xopt = output(ACADO_X_OPT);
ACADO::VariablesGrid xd;
algorithm_->getDifferentialStates(xd);
assert(xd.getNumPoints()==n_nodes_+1);
for(int i=0; i<n_nodes_+1; ++i){
// Copy to result
ACADO::Vector v = xd.getVector(i);
&xopt.at(i*nx_) << v;
}
}
if(nxa_>0){
Matrix<double> &xopt = output(ACADO_X_OPT);
ACADO::VariablesGrid xa;
algorithm_->getAlgebraicStates(xa);
assert(xa.getNumPoints()==n_nodes_+1);
for(int i=0; i<n_nodes_+1; ++i){
// Copy to result
ACADO::Vector v = xa.getVector(i);
&xopt.at(i*nx_ + nxd_) << v;
}
}
// Get the optimal control trajectory
if(nu_>0){
Matrix<double> &uopt = output(ACADO_U_OPT);
ACADO::VariablesGrid u;
algorithm_->getControls(u);
assert(u.getNumPoints()==n_nodes_+1);
for(int i=0; i<n_nodes_+1; ++i){
// Copy to result
ACADO::Vector v = u.getVector(i);
&uopt.at(i*nu_) << v;
}
}
// Get the optimal parameters
if(np_>0){
Matrix<double> &popt = output(ACADO_P_OPT);
ACADO::Vector p;
algorithm_->getParameters(p);
&popt.front() << p;
}
// Get the optimal cost
double cost = algorithm_->getObjectiveValue();
output(ACADO_COST).set(cost);
}
void QpoasesInterface::evaluate() {
if (inputs_check_) checkInputs();
if (verbose()) {
// cout << "X_INIT = " << input(QP_SOLVER_X_INIT) << endl;
// cout << "LAMBDA_INIT = " << input(QP_SOLVER_LAMBDA_INIT) << endl;
cout << "LBX = " << input(QP_SOLVER_LBX) << endl;
cout << "UBX = " << input(QP_SOLVER_UBX) << endl;
cout << "LBA = " << input(QP_SOLVER_LBA) << endl;
cout << "UBA = " << input(QP_SOLVER_UBA) << endl;
}
// Get pointer to H
const double* h=0;
if (h_data_.empty()) {
// No copying needed
h = getPtr(input(QP_SOLVER_H));
} else {
// First copy to dense array
input(QP_SOLVER_H).get(h_data_, DENSE);
h = getPtr(h_data_);
}
// Copy A to a row-major dense vector
const double* a=0;
if (nc_>0) {
input(QP_SOLVER_A).get(a_data_, DENSETRANS);
a = getPtr(a_data_);
}
// Maxiumum number of working set changes
int nWSR = max_nWSR_;
double cputime = max_cputime_;
double *cputime_ptr = cputime<=0 ? 0 : &cputime;
// Get the arguments to call qpOASES with
const double* g = getPtr(input(QP_SOLVER_G));
const double* lb = getPtr(input(QP_SOLVER_LBX));
const double* ub = getPtr(input(QP_SOLVER_UBX));
const double* lbA = getPtr(input(QP_SOLVER_LBA));
const double* ubA = getPtr(input(QP_SOLVER_UBA));
int flag;
if (!called_once_) {
if (ALLOW_QPROBLEMB && nc_==0) {
flag = static_cast<qpOASES::QProblemB*>(qp_)->init(h, g, lb, ub, nWSR, cputime_ptr);
} else {
flag = static_cast<qpOASES::SQProblem*>(qp_)->init(h, g, a, lb, ub, lbA, ubA,
nWSR, cputime_ptr);
}
called_once_ = true;
} else {
if (ALLOW_QPROBLEMB && nc_==0) {
static_cast<qpOASES::QProblemB*>(qp_)->reset();
flag = static_cast<qpOASES::QProblemB*>(qp_)->init(h, g, lb, ub, nWSR, cputime_ptr);
//flag = static_cast<qpOASES::QProblemB*>(qp_)->hotstart(g, lb, ub, nWSR, cputime_ptr);
} else {
flag = static_cast<qpOASES::SQProblem*>(qp_)->hotstart(h, g, a, lb, ub, lbA, ubA,
nWSR, cputime_ptr);
}
}
if (flag!=qpOASES::SUCCESSFUL_RETURN && flag!=qpOASES::RET_MAX_NWSR_REACHED) {
throw CasadiException("qpOASES failed: " + getErrorMessage(flag));
}
// Get optimal cost
output(QP_SOLVER_COST).set(qp_->getObjVal());
// Get the primal solution
qp_->getPrimalSolution(&output(QP_SOLVER_X).front());
// Get the dual solution
qp_->getDualSolution(&dual_.front());
// Split up the dual solution in multipliers for the simple bounds and the linear bounds
transform(dual_.begin(), dual_.begin()+n_, output(QP_SOLVER_LAM_X).begin(), negate<double>());
transform(dual_.begin()+n_, dual_.end(), output(QP_SOLVER_LAM_A).begin(), negate<double>());
}
void IpoptInternal::init(){
// Free existing IPOPT instance
freeIpopt();
// Call the init method of the base class
NLPSolverInternal::init();
// Read user options
exact_hessian_ = !hasSetOption("hessian_approximation") || getOption("hessian_approximation")=="exact";
#ifdef WITH_SIPOPT
if(hasSetOption("run_sens")){
run_sens_ = getOption("run_sens")=="yes";
} else {
run_sens_ = false;
}
if(hasSetOption("compute_red_hessian")){
compute_red_hessian_ = getOption("compute_red_hessian")=="yes";
} else {
compute_red_hessian_ = false;
}
#endif // WITH_SIPOPT
// Get/generate required functions
gradF();
jacG();
if(exact_hessian_){
hessLag();
}
// Start an IPOPT application
Ipopt::SmartPtr<Ipopt::IpoptApplication> *app = new Ipopt::SmartPtr<Ipopt::IpoptApplication>();
app_ = static_cast<void*>(app);
*app = new Ipopt::IpoptApplication();
#ifdef WITH_SIPOPT
if(run_sens_ || compute_red_hessian_){
// Start an sIPOPT application
Ipopt::SmartPtr<Ipopt::SensApplication> *app_sens = new Ipopt::SmartPtr<Ipopt::SensApplication>();
app_sens_ = static_cast<void*>(app_sens);
*app_sens = new Ipopt::SensApplication((*app)->Jnlst(),(*app)->Options(),(*app)->RegOptions());
// Register sIPOPT options
Ipopt::RegisterOptions_sIPOPT((*app)->RegOptions());
(*app)->Options()->SetRegisteredOptions((*app)->RegOptions());
}
#endif // WITH_SIPOPT
// Create an Ipopt user class -- need to use Ipopts spart pointer class
Ipopt::SmartPtr<Ipopt::TNLP> *userclass = new Ipopt::SmartPtr<Ipopt::TNLP>();
userclass_ = static_cast<void*>(userclass);
*userclass = new IpoptUserClass(this);
if(verbose_){
cout << "There are " << nx_ << " variables and " << ng_ << " constraints." << endl;
if(exact_hessian_) cout << "Using exact Hessian" << endl;
else cout << "Using limited memory Hessian approximation" << endl;
}
bool ret = true;
// Pass all the options to ipopt
for(map<string,opt_type>::const_iterator it=ops_.begin(); it!=ops_.end(); ++it)
if(hasSetOption(it->first)){
GenericType op = getOption(it->first);
switch(it->second){
case OT_REAL:
ret &= (*app)->Options()->SetNumericValue(it->first,op.toDouble(),false);
break;
case OT_INTEGER:
ret &= (*app)->Options()->SetIntegerValue(it->first,op.toInt(),false);
break;
case OT_STRING:
ret &= (*app)->Options()->SetStringValue(it->first,op.toString(),false);
break;
default:
throw CasadiException("Illegal type");
}
}
if (!ret) casadi_error("IpoptInternal::Init: Invalid options were detected by Ipopt.");
// Extra initialization required by sIPOPT
// #ifdef WITH_SIPOPT
// if(run_sens_ || compute_red_hessian_){
// Ipopt::ApplicationReturnStatus status = (*app)->Initialize("");
// casadi_assert_message(status == Solve_Succeeded, "Error during IPOPT initialization");
// }
// #endif // WITH_SIPOPT
// Intialize the IpoptApplication and process the options
Ipopt::ApplicationReturnStatus status = (*app)->Initialize();
casadi_assert_message(status == Solve_Succeeded, "Error during IPOPT initialization");
#ifdef WITH_SIPOPT
if(run_sens_ || compute_red_hessian_){
Ipopt::SmartPtr<Ipopt::SensApplication> *app_sens = static_cast<Ipopt::SmartPtr<Ipopt::SensApplication> *>(app_sens_);
(*app_sens)->Initialize();
}
#endif // WITH_SIPOPT
}
