bool GenericType::operator!=(const GenericType& op2) const{
if(isString() && op2.isString()){
return toString().compare(op2.toString()) != 0;
}
if(isInt() && op2.isInt()){
return toInt() != op2.toInt();
}
if(isDouble() && op2.isDouble()){
return toDouble() != op2.toDouble();
}
if(isDoubleVector() && op2.isDoubleVector()){
const vector<double> &v1 = toDoubleVector();
const vector<double> &v2 = op2.toDoubleVector();
if(v1.size() != v2.size()) return true;
for(int i=0; i<v1.size(); ++i)
if(v1[i] != v2[i]) return true;
return false;
}
if(isIntVector() && op2.isIntVector()){
const vector<int> &v1 = toIntVector();
const vector<int> &v2 = op2.toIntVector();
if(v1.size() != v2.size()) return true;
for(int i=0; i<v1.size(); ++i)
if(v1[i] != v2[i]) return true;
return false;
}
// Different types
return true;
}
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 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
}
