From 43c4e60eb7a53a67aa14a06218e3251fd9fb6280 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 28 Nov 2023 16:35:29 +0100 Subject: [PATCH] - Added convex_solver_settings with proper defaults to TrajOptIfoptDefaultSolverProfile - Added handling of convex_solver_settings to trajopt_ifopt_motion_planner --- .../trajopt_ifopt_default_solver_profile.h | 15 +++++-- .../trajopt_ifopt/trajopt_ifopt_problem.h | 11 +++++ .../trajopt_ifopt_default_solver_profile.cpp | 14 +++++++ .../src/trajopt_ifopt_motion_planner.cpp | 42 ++++++++++++++----- 4 files changed, 69 insertions(+), 13 deletions(-) diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h index 46759986031..24db86a6cd7 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h @@ -26,6 +26,11 @@ #ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H #define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + #include #include @@ -38,15 +43,19 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptDefaultSolverProfile() = default; + TrajOptIfoptDefaultSolverProfile(); ~TrajOptIfoptDefaultSolverProfile() override = default; TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = default; TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = default; TrajOptIfoptDefaultSolverProfile(TrajOptIfoptDefaultSolverProfile&&) = default; TrajOptIfoptDefaultSolverProfile& operator=(TrajOptIfoptDefaultSolverProfile&&) = default; - /** @brief Optimization paramters */ - trajopt_sqp::SQPParameters opt_info; + /** @brief The OSQP convex solver settings to use + * @todo Replace by convex_solver_config (cf. sco::ModelConfig) once solver selection is possible */ + OSQPSettings convex_solver_settings{}; + + /** @brief Optimization parameters */ + trajopt_sqp::SQPParameters opt_info{}; /** @brief Optimization callbacks */ std::vector callbacks; diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h index 84113c103c7..c7f494be3c4 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h @@ -26,14 +26,17 @@ #ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROBLEM_H #define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROBLEM_H +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include namespace tesseract_planning @@ -51,6 +54,8 @@ struct TrajOptIfoptProblem EIGEN_MAKE_ALIGNED_OPERATOR_NEW // LCOV_EXCL_STOP + TrajOptIfoptProblem() { osqp_set_default_settings(&convex_solver_settings); }; + trajopt_sqp::SQPParameters opt_info; // These are required for Tesseract to configure Descartes @@ -62,6 +67,12 @@ struct TrajOptIfoptProblem std::vector callbacks; + /** @brief The OSQP convex solver settings to use + * @todo Replace by convex_solver_config (cf. sco::ModelConfig) once solver selection is possible */ + OSQPSettings convex_solver_settings{}; + + std::shared_ptr qp_solver; + trajopt_sqp::QPProblem::Ptr nlp; std::vector vars; }; diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp index 3aa9ef80b8e..de6646961b9 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp @@ -28,8 +28,22 @@ namespace tesseract_planning { + +TrajOptIfoptDefaultSolverProfile::TrajOptIfoptDefaultSolverProfile() +{ + osqp_set_default_settings(&convex_solver_settings); + convex_solver_settings.verbose = 0; + convex_solver_settings.warm_start = 1; + convex_solver_settings.polish = 1; + convex_solver_settings.adaptive_rho = 1; + convex_solver_settings.max_iter = 8192; + convex_solver_settings.eps_abs = 1e-4; + convex_solver_settings.eps_rel = 1e-6; +} + void TrajOptIfoptDefaultSolverProfile::apply(TrajOptIfoptProblem& problem) const { + problem.convex_solver_settings = convex_solver_settings; problem.opt_info = opt_info; problem.callbacks = callbacks; } diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index 302c0daa76a..bc12e6af02d 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp @@ -63,6 +63,34 @@ MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const return std::make_shared(name_); } +void OSQPEigenSolver_setSettings(const OsqpEigen::Solver& solver_, OSQPSettings settings) +{ + // There seems to be no way to set objects solver_.settings() (OsqpEigen::Settings) + // or solver_.settings()->getSettings() (OSQPSettings) at once + solver_.settings()->setRho(settings.rho); + solver_.settings()->setSigma(settings.sigma); + solver_.settings()->setScaling((int)settings.scaling); + solver_.settings()->setAdaptiveRho(settings.adaptive_rho); + solver_.settings()->setAdaptiveRhoInterval((int)settings.adaptive_rho_interval); + solver_.settings()->setAdaptiveRhoTolerance(settings.adaptive_rho_tolerance); + solver_.settings()->setAdaptiveRhoFraction(settings.adaptive_rho_fraction); + solver_.settings()->setMaxIteration((int)settings.max_iter); + solver_.settings()->setAbsoluteTolerance(settings.eps_abs); + solver_.settings()->setRelativeTolerance(settings.eps_rel); + solver_.settings()->setPrimalInfeasibilityTollerance(settings.eps_prim_inf); + solver_.settings()->setDualInfeasibilityTollerance(settings.eps_dual_inf); + solver_.settings()->setAlpha(settings.alpha); + solver_.settings()->setLinearSystemSolver(settings.linsys_solver); + solver_.settings()->setDelta(settings.delta); + solver_.settings()->setPolish(settings.polish); + solver_.settings()->setPolishRefineIter((int)settings.polish_refine_iter); + solver_.settings()->setVerbosity(settings.verbose); + solver_.settings()->setScaledTerimination(settings.scaled_termination); + solver_.settings()->setCheckTermination((int)settings.check_termination); + solver_.settings()->setWarmStart(settings.warm_start); + solver_.settings()->setTimeLimit(settings.time_limit); +} + PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) const { PlannerResponse response; @@ -97,17 +125,11 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) // Create optimizer /** @todo Enable solver selection (e.g. IPOPT) */ - auto qp_solver = std::make_shared(); - trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - /** @todo Set these as the defaults in trajopt and allow setting */ - qp_solver->solver_.settings()->setVerbosity(request.verbose); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + problem->qp_solver = std::make_shared(); + OSQPEigenSolver_setSettings(dynamic_cast(*problem->qp_solver).solver_, + problem->convex_solver_settings); + trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver); solver.params = problem->opt_info; // Add all callbacks