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 feb882dc353..323472fe5dc 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 @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -44,15 +45,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 005e1e04cde..15e61bd5d71 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 @@ -30,7 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include +#include #include #include #include @@ -56,6 +56,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 @@ -67,6 +69,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; + std::shared_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 1071a687d99..79fbd615acc 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 @@ -27,7 +27,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -35,8 +34,22 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP 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 8fa4e3dfa8a..e64759a05f4 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 @@ -71,6 +71,34 @@ std::unique_ptr TrajOptIfoptMotionPlanner::clone() const return std::make_unique(name_); } +void OSQPEigenSolver_setSettings(const std::unique_ptr& 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; @@ -107,16 +135,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); + OSQPEigenSolver_setSettings(qp_solver->solver_, problem->convex_solver_settings); + qp_solver->solver_->settings()->setVerbosity((problem->convex_solver_settings.verbose != 0) || request.verbose); + problem->qp_solver = qp_solver; + trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver); solver.params = problem->opt_info; // Add all callbacks