From 42dd48b3c891d22a8886f2b515f08705f1b67e9c Mon Sep 17 00:00:00 2001 From: Rafael Rojas Date: Thu, 22 Aug 2024 11:19:44 +0200 Subject: [PATCH] Return optional in functions that can fail --- include/opstop/ipopt_problem.hpp | 35 ++++++---- src/ipopt_problem.cpp | 77 ++++++++++++++++----- tests/minimum_time_bounded_acceleration.cpp | 6 +- tests/minimum_time_bounded_jerk.cpp | 2 +- tests/minimum_time_bounded_jerk_l2.cpp | 6 +- 5 files changed, 91 insertions(+), 35 deletions(-) diff --git a/include/opstop/ipopt_problem.hpp b/include/opstop/ipopt_problem.hpp index 6646ca7..3b78327 100644 --- a/include/opstop/ipopt_problem.hpp +++ b/include/opstop/ipopt_problem.hpp @@ -61,25 +61,30 @@ class TimeOptimalStopProblem { TimeOptimalStopProblem(); }; -gsplines::functions::FunctionExpression minimum_time_bounded_acceleration( - const gsplines::functions::FunctionBase &_trj, double _ti, double _alpha, - const pinocchio::Model &_model, std::size_t _nglp); +std::optional +minimum_time_bounded_acceleration(const gsplines::functions::FunctionBase &_trj, + double _ti, double _alpha, + const pinocchio::Model &_model, + std::size_t _nglp); -gsplines::functions::FunctionExpression +std::optional minimum_time_bounded_jerk_l2(const gsplines::functions::FunctionBase &_trj, double _ti, double _alpha, const pinocchio::Model &_model, std::size_t _nglp); -gsplines::functions::FunctionExpression minimum_time_bounded_acceleration( - const gsplines::functions::FunctionBase &_trj, double _ti, - const Eigen::VectorXd &_acc_bounds, const pinocchio::Model &_model, - const Eigen::VectorXd &_torque_bounds, std::size_t _nglp); +std::optional +minimum_time_bounded_acceleration(const gsplines::functions::FunctionBase &_trj, + double _ti, + const Eigen::VectorXd &_acc_bounds, + const pinocchio::Model &_model, + const Eigen::VectorXd &_torque_bounds, + std::size_t _nglp); -gsplines::functions::FunctionExpression +std::optional minimum_time_bounded_jerk(const gsplines::functions::FunctionBase &_trj, double _ti, double _acc_bound); -gsplines::functions::FunctionExpression +std::optional minimum_time_bounded_jerk(const gsplines::functions::FunctionBase &_trj, double _ti, std::vector _acc_bounds); @@ -87,10 +92,12 @@ ifopt::Problem base_minimum_time_problem(const gsplines::functions::FunctionBase &_trj, double _ti); -gsplines::functions::FunctionExpression minimum_time_bounded_jerk_l2( - const gsplines::functions::FunctionBase &_trj, double _ti, - double _jerk_l2_bound, pinocchio::Model _model, - const Eigen::VectorXd &_torque_bounds, std::size_t _nglp); +std::optional +minimum_time_bounded_jerk_l2(const gsplines::functions::FunctionBase &_trj, + double _ti, double _jerk_l2_bound, + pinocchio::Model _model, + const Eigen::VectorXd &_torque_bounds, + std::size_t _nglp); } // namespace opstop #endif /* ifndef IPOPT_PROBLEM \ diff --git a/src/ipopt_problem.cpp b/src/ipopt_problem.cpp index 3a2d847..1b2ac0e 100644 --- a/src/ipopt_problem.cpp +++ b/src/ipopt_problem.cpp @@ -5,6 +5,29 @@ namespace opstop { namespace optimization { +enum class IpoptReturnStatus : int { + Solve_Succeeded = 0, + Solved_To_Acceptable_Level = 1, + Infeasible_Problem_Detected = 2, + Search_Direction_Becomes_Too_Small = 3, + Diverging_Iterates = 4, + User_Requested_Stop = 5, + Feasible_Point_Found = 6, + + Maximum_Iterations_Exceeded = -1, + Restoration_Failed = -2, + Error_In_Step_Computation = -3, + Maximum_CpuTime_Exceeded = -4, + Not_Enough_Degrees_Of_Freedom = -10, + Invalid_Problem_Definition = -11, + Invalid_Option = -12, + Invalid_Number_Detected = -13, + + Unrecoverable_Exception = -100, + NonIpopt_Exception_Thrown = -101, + Insufficient_Memory = -102, + Internal_Error = -199 +}; std::optional IpoptSolverOptions::instance_ = std::nullopt; @@ -73,9 +96,11 @@ base_minimum_time_problem(const gsplines::functions::FunctionBase &_trj, return nlp; } -gsplines::functions::FunctionExpression minimum_time_bounded_acceleration( - const gsplines::functions::FunctionBase &_trj, double _ti, double _alpha, - const pinocchio::Model &_model, std::size_t _nglp) { +std::optional +minimum_time_bounded_acceleration(const gsplines::functions::FunctionBase &_trj, + double _ti, double _alpha, + const pinocchio::Model &_model, + std::size_t _nglp) { std::size_t number_of_segments = 100; Eigen::VectorXd bounds = _alpha * _trj.derivate(2) @@ -91,9 +116,11 @@ gsplines::functions::FunctionExpression minimum_time_bounded_acceleration( _model.effortLimit, _nglp); } -gsplines::functions::FunctionExpression minimum_time_bounded_jerk_l2( - const gsplines::functions::FunctionBase &_trj, double _ti, double _alpha, - const pinocchio::Model &_model, std::size_t _nglp) { +std::optional +minimum_time_bounded_jerk_l2(const gsplines::functions::FunctionBase &_trj, + double _ti, double _alpha, + const pinocchio::Model &_model, + std::size_t _nglp) { std::size_t number_of_segments = 100; double jerk_bound = _alpha * @@ -104,10 +131,13 @@ gsplines::functions::FunctionExpression minimum_time_bounded_jerk_l2( _model.effortLimit, _nglp); } -gsplines::functions::FunctionExpression minimum_time_bounded_acceleration( - const gsplines::functions::FunctionBase &_trj, double _ti, - const Eigen::VectorXd &_acc_bounds, const pinocchio::Model &_model, - const Eigen::VectorXd &_torque_bounds, std::size_t _nglp) { +std::optional +minimum_time_bounded_acceleration(const gsplines::functions::FunctionBase &_trj, + double _ti, + const Eigen::VectorXd &_acc_bounds, + const pinocchio::Model &_model, + const Eigen::VectorXd &_torque_bounds, + std::size_t _nglp) { std::vector bound(_acc_bounds.data(), _acc_bounds.data() + _acc_bounds.size()); @@ -143,6 +173,10 @@ gsplines::functions::FunctionExpression minimum_time_bounded_acceleration( // 4. Ask the solver to solve the problem ipopt.Solve(nlp); + if (ipopt.GetReturnStatus() != + static_cast(optimization::IpoptReturnStatus::Solve_Succeeded)) { + return std::nullopt; + } Eigen::VectorXd x = nlp.GetOptVariables()->GetValues(); double tf = _trj.get_domain().second; @@ -152,13 +186,13 @@ gsplines::functions::FunctionExpression minimum_time_bounded_acceleration( return get_diffeo(_ti, x(0), x(1)); } -gsplines::functions::FunctionExpression +std::optional minimum_time_bounded_jerk(const gsplines::functions::FunctionBase &_trj, double _ti, double _acc_bound) { std::vector bounds(_trj.get_codom_dim(), _acc_bound); return minimum_time_bounded_jerk(_trj, _ti, bounds); } -gsplines::functions::FunctionExpression +std::optional minimum_time_bounded_jerk(const gsplines::functions::FunctionBase &_trj, double _ti, std::vector _acc_bounds) { @@ -191,6 +225,10 @@ minimum_time_bounded_jerk(const gsplines::functions::FunctionBase &_trj, // 4. Ask the solver to solve the problem ipopt.Solve(nlp); + if (ipopt.GetReturnStatus() != + static_cast(optimization::IpoptReturnStatus::Solve_Succeeded)) { + return std::nullopt; + } Eigen::VectorXd x = nlp.GetOptVariables()->GetValues(); printf("Ts = %lf sf = %lf ti = %lf\n", x(0), x(1), _ti); @@ -198,10 +236,12 @@ minimum_time_bounded_jerk(const gsplines::functions::FunctionBase &_trj, return get_diffeo(_ti, x(0), x(1)); } -gsplines::functions::FunctionExpression minimum_time_bounded_jerk_l2( - const gsplines::functions::FunctionBase &_trj, double _ti, - double _jerk_l2_bound, pinocchio::Model _model, - const Eigen::VectorXd &_torque_bounds, std::size_t _nglp) { +std::optional +minimum_time_bounded_jerk_l2(const gsplines::functions::FunctionBase &_trj, + double _ti, double _jerk_l2_bound, + pinocchio::Model _model, + const Eigen::VectorXd &_torque_bounds, + std::size_t _nglp) { ifopt::Problem nlp = base_minimum_time_problem(_trj, _ti); @@ -229,6 +269,11 @@ gsplines::functions::FunctionExpression minimum_time_bounded_jerk_l2( // 4. Ask the solver to solve the problem ipopt.Solve(nlp); + + if (ipopt.GetReturnStatus() != + static_cast(optimization::IpoptReturnStatus::Solve_Succeeded)) { + return std::nullopt; + } Eigen::VectorXd x = nlp.GetOptVariables()->GetValues(); double tf = _trj.get_domain().second; diff --git a/tests/minimum_time_bounded_acceleration.cpp b/tests/minimum_time_bounded_acceleration.cpp index f2c7f5b..08bab65 100644 --- a/tests/minimum_time_bounded_acceleration.cpp +++ b/tests/minimum_time_bounded_acceleration.cpp @@ -29,7 +29,8 @@ int main() { gsplines::functions::FunctionExpression diffeo = minimum_time_bounded_acceleration(trj, ti, model.effortLimit, model, - model.effortLimit, 5); + model.effortLimit, 5) + .value(); gsplines::functions::FunctionExpression diffeo_diff_1 = diffeo.derivate(); gsplines::functions::FunctionExpression diffeo_diff_2 = @@ -83,7 +84,8 @@ int main() { auto t1 = std::chrono::high_resolution_clock::now(); gsplines::functions::FunctionExpression _diffeo = minimum_time_bounded_acceleration(trj, ti, 0.5 * model.effortLimit, - model, model.effortLimit, 5); + model, model.effortLimit, 5) + .value(); gsplines::functions::FunctionExpression _diffeo_diff_1 = _diffeo.derivate(); gsplines::functions::FunctionExpression _diffeo_diff_2 = _diffeo_diff_1.derivate(); diff --git a/tests/minimum_time_bounded_jerk.cpp b/tests/minimum_time_bounded_jerk.cpp index 6a9bba8..df9962d 100644 --- a/tests/minimum_time_bounded_jerk.cpp +++ b/tests/minimum_time_bounded_jerk.cpp @@ -20,7 +20,7 @@ int main() { .value(); gsplines::functions::FunctionExpression diffeo = - minimum_time_bounded_jerk(trj, ti, 100); + minimum_time_bounded_jerk(trj, ti, 100).value(); gsplines::functions::FunctionExpression diffeo_diff_1 = diffeo.derivate(); gsplines::functions::FunctionExpression diffeo_diff_2 = diff --git a/tests/minimum_time_bounded_jerk_l2.cpp b/tests/minimum_time_bounded_jerk_l2.cpp index 4f0b859..d481d51 100644 --- a/tests/minimum_time_bounded_jerk_l2.cpp +++ b/tests/minimum_time_bounded_jerk_l2.cpp @@ -30,8 +30,10 @@ TEST(Jerk_l2_constraints, stopping) { double jerk_bound = gsplines::functional_analysis::l2_norm(trj.derivate(3)); - gsplines::functions::FunctionExpression diffeo = minimum_time_bounded_jerk_l2( - trj, ti, jerk_bound, model, model.effortLimit, 5); + gsplines::functions::FunctionExpression diffeo = + minimum_time_bounded_jerk_l2(trj, ti, jerk_bound, model, + model.effortLimit, 5) + .value(); std::cout << "effort limits \n" << model.effortLimit << "\n";