- 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

@@ -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_common/macros.h>
+TRAJOPT_IGNORE_WARNINGS_PUSH
+#include <osqp/osqp.h>
+TRAJOPT_IGNORE_WARNINGS_POP
+
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h>
 #include <trajopt_sqp/sqp_callback.h>
 
@@ -38,15 +43,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<trajopt_sqp::SQPCallback::Ptr> callbacks;

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 <osqp.h>
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <vector>
 #include <memory>
+#include <osqp/types.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
@@ -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<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,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;
 }

tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp

@@ -63,6 +63,34 @@ 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;
@@ -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::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);
+  problem->qp_solver = std::make_shared<trajopt_sqp::OSQPEigenSolver>();
+  OSQPEigenSolver_setSettings(dynamic_cast<trajopt_sqp::OSQPEigenSolver&>(*problem->qp_solver).solver_,
+                              problem->convex_solver_settings);
 
+  trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver);
   solver.params = problem->opt_info;
 
   // Add all callbacks