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 bd81351e0e4..46759986031 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,11 +26,6 @@ #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 @@ -43,19 +38,15 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptDefaultSolverProfile(); + TrajOptIfoptDefaultSolverProfile() = default; ~TrajOptIfoptDefaultSolverProfile() override = default; TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = default; TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = default; TrajOptIfoptDefaultSolverProfile(TrajOptIfoptDefaultSolverProfile&&) = default; TrajOptIfoptDefaultSolverProfile& operator=(TrajOptIfoptDefaultSolverProfile&&) = default; - /** @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 paramters */ + 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 da32e20b27c..84113c103c7 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,12 +30,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include #include namespace tesseract_planning @@ -53,8 +51,6 @@ 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 @@ -66,12 +62,6 @@ 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 de6646961b9..3aa9ef80b8e 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,22 +28,8 @@ 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 3e533872dfa..302c0daa76a 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,34 +63,6 @@ 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; @@ -125,14 +97,17 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) // Create optimizer /** @todo Enable solver selection (e.g. IPOPT) */ - problem->qp_solver = std::make_shared(); - OSQPEigenSolver_setSettings(dynamic_cast(*problem->qp_solver).solver_, - problem->convex_solver_settings); - dynamic_cast(*problem->qp_solver) - .solver_.settings() - ->setVerbosity((problem->convex_solver_settings.verbose != 0) || request.verbose); - - trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver); + 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); + solver.params = problem->opt_info; // Add all callbacks