Revert "- Added convex_solver_settings with proper defaults to TrajOp…

…tIfoptDefaultSolverProfile"

This reverts commit 0242aa8.

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_common/macros.h>
-TRAJOPT_IGNORE_WARNINGS_PUSH
-#include <osqp.h>
-TRAJOPT_IGNORE_WARNINGS_POP
-
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h>
 #include <trajopt_sqp/sqp_callback.h>
 
@@ -43,19 +38,15 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile
   using Ptr = std::shared_ptr<TrajOptIfoptDefaultSolverProfile>;
   using ConstPtr = std::shared_ptr<const TrajOptIfoptDefaultSolverProfile>;
 
-  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<trajopt_sqp::SQPCallback::Ptr> callbacks;

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 <vector>
 #include <memory>
-#include <osqp.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_environment/environment.h>
 #include <trajopt_ifopt/variable_sets/joint_position_variable.h>
-#include <trajopt_sqp/qp_solver.h>
 #include <trajopt_sqp/sqp_callback.h>
 
 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<trajopt_sqp::SQPCallback::Ptr> 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<trajopt_sqp::QPSolver> qp_solver;
-
   trajopt_sqp::QPProblem::Ptr nlp;
   std::vector<trajopt_ifopt::JointPosition::ConstPtr> vars;
 };

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

tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp

@@ -63,34 +63,6 @@ MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const
   return std::make_shared<TrajOptIfoptMotionPlanner>(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<trajopt_sqp::OSQPEigenSolver>();
-  OSQPEigenSolver_setSettings(dynamic_cast<trajopt_sqp::OSQPEigenSolver&>(*problem->qp_solver).solver_,
-                              problem->convex_solver_settings);
-  dynamic_cast<trajopt_sqp::OSQPEigenSolver&>(*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::OSQPEigenSolver>();
+  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