Unverified Commit 2519645e authored by Carlos Mastalli's avatar Carlos Mastalli Committed by GitHub
Browse files

Merge pull request #1020 from PepMS/ipopt

Interfacing IpOpt from Crocoddyl
parents d6566188 f41eca40
Pipeline #19182 passed with stage
in 35 minutes and 26 seconds
......@@ -72,6 +72,7 @@ IF(BUILD_EXAMPLES OR BUILD_TESTING OR BUILD_BENCHMARK)
ELSE()
ADD_OPTIONAL_DEPENDENCY(example-robot-data)
ENDIF()
ADD_OPTIONAL_DEPENDENCY(ipopt)
ADD_OPTIONAL_DEPENDENCY("scipy")
OPTION(BUILD_WITH_CODEGEN_SUPPORT "Build the library with the Code Generation support (required CppADCodeGen)" OFF)
......@@ -127,6 +128,11 @@ IF(OPENMP_FOUND AND BUILD_WITH_MULTITHREADS)
SET(OMP_NUM_THREADS ${BUILD_WITH_NTHREADS})
ENDIF()
# Add Ipopt
IF(IPOPT_FOUND)
ADD_DEFINITIONS(-DCROCODDYL_WITH_IPOPT)
ENDIF()
SET(BOOST_REQUIERED_COMPONENTS filesystem serialization system)
SET(BOOST_BUILD_COMPONENTS unit_test_framework)
SET_BOOST_DEFAULT_OPTIONS()
......@@ -168,10 +174,19 @@ IF(UNIX)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY}
${Boost_SERIALIZATION_LIBRARY})
if(OPENMP_FOUND)
IF(OPENMP_FOUND)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenMP_CXX_LIBRARIES})
ENDIF()
IF(IPOPT_FOUND)
IF(APPLE)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${IPOPT_LIBRARY_DIRS}/libipopt.dylib)
ELSE()
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ipopt)
ENDIF()
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC ${IPOPT_INCLUDE_DIRS})
ENDIF()
INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
ENDIF(UNIX)
......
......@@ -59,7 +59,7 @@ pip install --user crocoddyl
**Crocoddyl** is versatible:
* various optimal control solvers (DDP, FDDP, BoxDDP, etc) - single and multi-shooting methods
* various optimal control solvers (DDP, FDDP, BoxFDDP, Ipopt, etc) - single and multi-shooting methods
* analytical and sparse derivatives via **[Pinocchio](https://github.com/stack-of-tasks/pinocchio)**
* Euclidian and non-Euclidian geometry friendly via **[Pinocchio](https://github.com/stack-of-tasks/pinocchio)**
* handle autonomous and nonautomous dynamical systems
......@@ -132,8 +132,10 @@ export PYTHONPATH=/opt/openrobots/lib/python2.7/site-packages:$PYTHONPATH
* [eigenpy](https://github.com/stack-of-tasks/eigenpy)
* [Boost](https://www.boost.org/)
* [example-robot-data](https://github.com/gepetto/example-robot-data) (optional for examples, install Python loaders)
* [OpenMP](https://www.openmp.org/) (optional for multi-threading installation)
* [OpenMP](https://www.openmp.org/) (optional for multi-threading support)
* [Ipopt](https://github.com/coin-or/Ipopt) (optional for Ipopt support)
* [gepetto-viewer-corba](https://github.com/Gepetto/gepetto-viewer-corba) (optional for display)
* [meshcat-python](https://github.com/rdeits/meshcat-python) (optional for display)
* [jupyter](https://jupyter.org/) (optional for notebooks)
* [matplotlib](https://matplotlib.org/) (optional for examples)
......@@ -249,4 +251,4 @@ The following people have been involved in the development of **Crocoddyl**:
## Acknowledgments
The development of **Crocoddyl** is supported by the [EU MEMMO project](http://www.memmo-project.eu/), and the [EU RoboCom++ project](http://robocomplusplus.eu/).
It is maintained by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), the [Statistical Machine Learning and Motor Control Group](http://wcms.inf.ed.ac.uk/ipab/slmc) [@University of Edinburgh](https://www.edinburgh-robotics.org/), and the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](https://www.inria.fr/fr/centre-inria-de-paris).
It is maintained by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), the [Robot Motor Intelligence (RoMI) Lab](https://www.romilab.org) [@Heriot-Watt University](https://www.edinburgh-robotics.org/), and the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](https://www.inria.fr/fr/centre-inria-de-paris).
......@@ -61,6 +61,9 @@ void exposeCore() {
exposeSolverBoxQP();
exposeSolverBoxDDP();
exposeSolverBoxFDDP();
#ifdef CROCODDYL_WITH_IPOPT
exposeSolverIpopt();
#endif
exposeCallbacks();
exposeStopWatch();
}
......
......@@ -63,6 +63,9 @@ void exposeSolverFDDP();
void exposeSolverBoxQP();
void exposeSolverBoxDDP();
void exposeSolverBoxFDDP();
#ifdef CROCODDYL_WITH_IPOPT
void exposeSolverIpopt();
#endif
void exposeCallbacks();
void exposeStopWatch();
......
#ifdef CROCODDYL_WITH_IPOPT
#include "python/crocoddyl/core/core.hpp"
#include "crocoddyl/core/solvers/ipopt.hpp"
namespace crocoddyl {
namespace python {
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverIpopt_solves, SolverIpopt::solve, 0, 5)
void exposeSolverIpopt() {
bp::register_ptr_to_python<boost::shared_ptr<SolverIpopt>>();
bp::class_<SolverIpopt, bp::bases<SolverAbstract>>(
"SolverIpopt",
bp::init<const boost::shared_ptr<crocoddyl::ShootingProblem>&>(bp::args("self", "problem"), "Initialize solver"))
.def("solve", &SolverIpopt::solve,
SolverIpopt_solves(
bp::args("self", "init_xs", "init_us", "maxiter", "is_feasible", "init_reg"),
"Compute the optimal trajectory xopt, uopt as lists of T+1 and T terms.\n\n"
"From an initial guess init_xs,init_us (feasible or not), iterate\n"
"over computeDirection and tryStep until stoppingCriteria is below\n"
"threshold. It also describes the globalization strategy used\n"
"during the numerical optimization.\n"
":param init_xs: initial guess for state trajectory with T+1 elements (default []).\n"
":param init_us: initial guess for control trajectory with T elements (default []) (default []).\n"
":param maxiter: maximum allowed number of iterations (default 100).\n"
":param is_feasible: true if the init_xs are obtained from integrating the init_us (rollout) (default "
"False).\n"
":param init_reg: initial guess for the regularization value. Very low values are typical\n"
" used with very good guess points (init_xs, init_us) (default None).\n"
":returns the optimal trajectory xopt, uopt and a boolean that describes if convergence was reached."))
.def("setStringIpoptOption", &SolverIpopt::setStringIpoptOption, bp::args("self", "param_name", "param_value"),
"Sets a string option for IPOPT\n\n")
.def("setNumericIpoptOption", &SolverIpopt::setNumericIpoptOption, bp::args("self", "param_name", "param_value"),
"Sets a numeric option for IPOPT\n\n")
.add_property("th_stop", bp::make_function(&SolverIpopt::get_th_stop),
bp::make_function(&SolverIpopt::set_th_stop), "threshold for stopping criteria");
}
} // namespace python
} // namespace crocoddyl
#endif
\ No newline at end of file
......@@ -489,7 +489,7 @@ void ShootingProblemTpl<Scalar>::set_nthreads(const int nthreads) {
} else {
nthreads_ = static_cast<std::size_t>(nthreads);
}
if (enableMultithreading()) {
if (!enableMultithreading()) {
std::cerr << "Warning: the number of threads won't affect the computational performance as multithreading "
"support is not enabled."
<< std::endl;
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2022, IRI: CSIC-UPC, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#ifdef CROCODDYL_WITH_IPOPT
#ifndef CROCODDYL_CORE_SOLVERS_IPOPT_HPP_
#define CROCODDYL_CORE_SOLVERS_IPOPT_HPP_
#define HAVE_CSTDDEF
#include <IpIpoptApplication.hpp>
#include <IpSolveStatistics.hpp>
#undef HAVE_CSTDDEF
#include "crocoddyl/core/solver-base.hpp"
#include "crocoddyl/core/solvers/ipopt/ipopt-iface.hpp"
namespace crocoddyl {
/**
* @brief Ipopt solver
*
* This solver solves the optimal control problem by transcribing with the multiple shooting approach.
*
* \sa `solve()`
*/
class SolverIpopt : public SolverAbstract {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* @brief Initialize the Ipopt solver
*
* @param[in] problem solver to be diagnostic
*/
SolverIpopt(boost::shared_ptr<crocoddyl::ShootingProblem> problem);
~SolverIpopt();
bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t maxiter = 100,
const bool is_feasible = false, const double reg_init = 1e-9);
virtual void resizeData();
/**
* @brief Set a string ipopt option
*
* @param[in] tag name of the parameter
* @param[in] value string value for the parameter
*/
void setStringIpoptOption(const std::string& tag, const std::string& value);
/**
* @brief Set a string ipopt option
*
* @param[in] tag name of the parameter
* @param[in] value numeric value for the parameter
*/
void setNumericIpoptOption(const std::string& tag, Ipopt::Number value);
void set_th_stop(const double th_stop);
private:
Ipopt::SmartPtr<IpoptInterface> ipopt_iface_;
Ipopt::SmartPtr<Ipopt::IpoptApplication> ipopt_app_;
Ipopt::ApplicationReturnStatus ipopt_status_;
virtual void computeDirection(const bool recalc);
virtual double tryStep(const double steplength = 1);
virtual double stoppingCriteria();
virtual const Eigen::Vector2d& expectedImprovement();
};
} // namespace crocoddyl
#endif
#endif
This diff is collapsed.
......@@ -14,13 +14,13 @@
<url type="website">https://github.com/loco-3d/crocoddyl</url>
<build_depend>git</build_depend>
<!-- The following tags are recommended by REP-136 -->
<!-- The following tag is recommended by REP-136 -->
<exec_depend condition="$ROS_VERSION == 1">catkin</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">ament_cmake</exec_depend>
<depend condition="$ROS_PYTHON_VERSION == 2">python</depend>
<depend condition="$ROS_PYTHON_VERSION == 2">python-numpy</depend>
<depend condition="$ROS_PYTHON_VERSION == 3">python3</depend>
<depend condition="$ROS_PYTHON_VERSION == 3">python3-numpy</depend>
<depend>coinor-libipopt-dev</depend>
<test_depend condition="$ROS_PYTHON_VERSION == 2">python-scipy</test_depend>
<test_depend condition="$ROS_PYTHON_VERSION == 3">python3-scipy</test_depend>
<depend>boost</depend>
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2022, IRI: CSIC-UPC, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#ifdef CROCODDYL_WITH_IPOPT
#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
#endif
\ No newline at end of file
This diff is collapsed.
......@@ -12,6 +12,9 @@
#include "crocoddyl/core/solvers/fddp.hpp"
#include "crocoddyl/core/solvers/box-ddp.hpp"
#include "crocoddyl/core/solvers/box-fddp.hpp"
#ifdef CROCODDYL_WITH_IPOPT
#include "crocoddyl/core/solvers/ipopt.hpp"
#endif
#include "crocoddyl/core/utils/exception.hpp"
namespace crocoddyl {
......@@ -36,6 +39,11 @@ std::ostream& operator<<(std::ostream& os, SolverTypes::Type type) {
case SolverTypes::SolverBoxFDDP:
os << "SolverBoxFDDP";
break;
#ifdef CROCODDYL_WITH_IPOPT
case SolverTypes::SolverIpopt:
os << "SolverIpopt";
break;
#endif
case SolverTypes::NbSolverTypes:
os << "NbSolverTypes";
break;
......@@ -83,6 +91,11 @@ boost::shared_ptr<crocoddyl::SolverAbstract> SolverFactory::create(SolverTypes::
case SolverTypes::SolverBoxFDDP:
solver = boost::make_shared<crocoddyl::SolverBoxFDDP>(problem);
break;
#ifdef CROCODDYL_WITH_IPOPT
case SolverTypes::SolverIpopt:
solver = boost::make_shared<crocoddyl::SolverIpopt>(problem);
break;
#endif
default:
throw_pretty(__FILE__ ": Wrong SolverTypes::Type given");
break;
......
......@@ -18,11 +18,16 @@ namespace crocoddyl {
namespace unittest {
struct SolverTypes {
enum Type { SolverKKT, SolverDDP, SolverFDDP, SolverBoxDDP, SolverBoxFDDP, NbSolverTypes };
enum Type { SolverKKT, SolverDDP, SolverFDDP, SolverBoxDDP, SolverBoxFDDP, SolverIpopt, NbSolverTypes };
static std::vector<Type> init_all() {
std::vector<Type> v;
v.clear();
for (int i = 0; i < NbSolverTypes; ++i) {
#ifndef CROCODDYL_WITH_IPOPT
if ((Type)i == SolverIpopt) {
continue;
}
#endif
v.push_back((Type)i);
}
return v;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment