- Added convex_solver_settings with proper defaults to TrajOptIfoptDe…

…faultSolverProfile

- Added handling of convex_solver_settings to trajopt_ifopt_motion_planner

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/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <memory>
+#include <osqp.h>
 #include <trajopt_sqp/fwd.h>
 #include <trajopt_sqp/types.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
@@ -44,15 +45,19 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile
   using Ptr = std::shared_ptr<TrajOptIfoptDefaultSolverProfile>;
   using ConstPtr = std::shared_ptr<const TrajOptIfoptDefaultSolverProfile>;
 
-  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<std::shared_ptr<trajopt_sqp::SQPCallback>> callbacks;

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 <vector>
 #include <memory>
-#include <Eigen/Core>
+#include <osqp.h>
 #include <trajopt_ifopt/fwd.h>
 #include <trajopt_sqp/fwd.h>
 #include <trajopt_sqp/types.h>
@@ -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<std::shared_ptr<trajopt_sqp::SQPCallback>> 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;
+
   std::shared_ptr<trajopt_sqp::QPProblem> nlp;
   std::vector<std::shared_ptr<const trajopt_ifopt::JointPosition>> vars;
 };

tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp

@@ -27,16 +27,29 @@
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <tinyxml2.h>
-#include <trajopt_sqp/sqp_callback.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h>
 
 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

@@ -71,6 +71,34 @@ std::unique_ptr<MotionPlanner> TrajOptIfoptMotionPlanner::clone() const
   return std::make_unique<TrajOptIfoptMotionPlanner>(name_);
 }
 
+void OSQPEigenSolver_setSettings(const std::unique_ptr<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;
@@ -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::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);
+  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