ipopt.cpp

// BSD 3-Clause License
//
// Copyright (C) 2022-2023, IRI: CSIC-UPC, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#include "crocoddyl/core/solvers/ipopt.hpp"
 
namespace crocoddyl {
 
SolverIpopt::SolverIpopt(boost::shared_ptr<crocoddyl::ShootingProblem> problem)
    : SolverAbstract(problem),
      ipopt_iface_(new IpoptInterface(problem)),
      ipopt_app_(IpoptApplicationFactory()) {
  ipopt_app_->Options()->SetNumericValue("tol", th_stop_);
  ipopt_app_->Options()->SetStringValue("mu_strategy", "adaptive");
 
  ipopt_status_ = ipopt_app_->Initialize();
 
  if (ipopt_status_ != Ipopt::Solve_Succeeded) {
    std::cerr << "Error during IPOPT initialization!" << std::endl;
  }
}
 
bool SolverIpopt::solve(const std::vector<Eigen::VectorXd>& init_xs,
                        const std::vector<Eigen::VectorXd>& init_us,
                        const std::size_t maxiter, const bool is_feasible,
                        const double /*reg_init*/) {
  setCandidate(init_xs, init_us, is_feasible);
  ipopt_iface_->set_xs(xs_);
  ipopt_iface_->set_us(us_);
 
  ipopt_app_->Options()->SetIntegerValue("max_iter",
                                         static_cast<Ipopt::Index>(maxiter));
  ipopt_status_ = ipopt_app_->OptimizeTNLP(ipopt_iface_);
 
  std::copy(ipopt_iface_->get_xs().begin(), ipopt_iface_->get_xs().end(),
            xs_.begin());
  std::copy(ipopt_iface_->get_us().begin(), ipopt_iface_->get_us().end(),
            us_.begin());
  cost_ = ipopt_iface_->get_cost();
  iter_ = ipopt_app_->Statistics()->IterationCount();
 
  return ipopt_status_ == Ipopt::Solve_Succeeded ||
         ipopt_status_ == Ipopt::Solved_To_Acceptable_Level;
}
 
void SolverIpopt::resizeData() {
  SolverAbstract::resizeData();
  ipopt_iface_->resizeData();
}
 
SolverIpopt::~SolverIpopt() {}
 
void SolverIpopt::computeDirection(const bool) {}
 
double SolverIpopt::tryStep(const double) { return 0.; }
 
double SolverIpopt::stoppingCriteria() { return 0.; }
 
const Eigen::Vector2d& SolverIpopt::expectedImprovement() { return d_; }
 
void SolverIpopt::setStringIpoptOption(const std::string& tag,
                                       const std::string& value) {
  ipopt_app_->Options()->SetStringValue(tag, value);
}
 
void SolverIpopt::setNumericIpoptOption(const std::string& tag,
                                        Ipopt::Number value) {
  ipopt_app_->Options()->SetNumericValue(tag, value);
}
 
void SolverIpopt::set_th_stop(const double th_stop) {
  if (th_stop <= 0.) {
    throw_pretty("Invalid argument: "
                 << "th_stop value has to higher than 0.");
  }
  th_stop_ = th_stop;
  ipopt_app_->Options()->SetNumericValue("tol", th_stop_);
}
 
}  // namespace crocoddyl