Skip to content

Commit

Permalink
Return optional in functions that can fail
Browse files Browse the repository at this point in the history
  • Loading branch information
@rafaelrojasmiliani
rafaelrojasmiliani committed Aug 22, 2024
1 parent c551795 commit 42dd48b
Show file tree
Hide file tree
Showing 5 changed files with 91 additions and 35 deletions.
35 changes: 21 additions & 14 deletions include/opstop/ipopt_problem.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,36 +61,43 @@ 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<gsplines::functions::FunctionExpression>
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<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);

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<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);

gsplines::functions::FunctionExpression
std::optional<gsplines::functions::FunctionExpression>
minimum_time_bounded_jerk(const gsplines::functions::FunctionBase &_trj,
double _ti, double _acc_bound);

gsplines::functions::FunctionExpression
std::optional<gsplines::functions::FunctionExpression>
minimum_time_bounded_jerk(const gsplines::functions::FunctionBase &_trj,
double _ti, std::vector<double> _acc_bounds);

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<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);
} // namespace opstop

#endif /* ifndef IPOPT_PROBLEM \
Expand Down
77 changes: 61 additions & 16 deletions src/ipopt_problem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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> IpoptSolverOptions::instance_ = std::nullopt;

Expand Down Expand Up @@ -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<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::size_t number_of_segments = 100;
Eigen::VectorXd bounds =
_alpha * _trj.derivate(2)
Expand All @@ -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<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::size_t number_of_segments = 100;
double jerk_bound =
_alpha *
Expand All @@ -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<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::vector<double> bound(_acc_bounds.data(),
_acc_bounds.data() + _acc_bounds.size());
Expand Down Expand Up @@ -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<int>(optimization::IpoptReturnStatus::Solve_Succeeded)) {
return std::nullopt;
}
Eigen::VectorXd x = nlp.GetOptVariables()->GetValues();

double tf = _trj.get_domain().second;
Expand All @@ -152,13 +186,13 @@ gsplines::functions::FunctionExpression minimum_time_bounded_acceleration(
return get_diffeo(_ti, x(0), x(1));
}

gsplines::functions::FunctionExpression
std::optional<gsplines::functions::FunctionExpression>
minimum_time_bounded_jerk(const gsplines::functions::FunctionBase &_trj,
double _ti, double _acc_bound) {
std::vector<double> bounds(_trj.get_codom_dim(), _acc_bound);
return minimum_time_bounded_jerk(_trj, _ti, bounds);
}
gsplines::functions::FunctionExpression
std::optional<gsplines::functions::FunctionExpression>
minimum_time_bounded_jerk(const gsplines::functions::FunctionBase &_trj,
double _ti, std::vector<double> _acc_bounds) {

Expand Down Expand Up @@ -191,17 +225,23 @@ 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<int>(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);

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<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) {

ifopt::Problem nlp = base_minimum_time_problem(_trj, _ti);

Expand Down Expand Up @@ -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<int>(optimization::IpoptReturnStatus::Solve_Succeeded)) {
return std::nullopt;
}
Eigen::VectorXd x = nlp.GetOptVariables()->GetValues();

double tf = _trj.get_domain().second;
Expand Down
6 changes: 4 additions & 2 deletions tests/minimum_time_bounded_acceleration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand Down Expand Up @@ -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();
Expand Down
2 changes: 1 addition & 1 deletion tests/minimum_time_bounded_jerk.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand Down
6 changes: 4 additions & 2 deletions tests/minimum_time_bounded_jerk_l2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";

Expand Down

0 comments on commit 42dd48b

Please sign in to comment.