Add convex_solver_settings to TrajOptIfoptDefaultSolverProfile (#425)

Co-authored-by: Levi Armstrong <[email protected]>

tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt

@@ -5,6 +5,7 @@ find_package(trajopt_sqp REQUIRED)
 add_library(
   ${PROJECT_NAME}_trajopt_ifopt SHARED
   src/trajopt_ifopt_motion_planner.cpp
+  src/trajopt_ifopt_problem.cpp
   src/trajopt_ifopt_utils.cpp
   src/profile/trajopt_ifopt_default_plan_profile.cpp
   src/profile/trajopt_ifopt_default_composite_profile.cpp

tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h

@@ -35,6 +35,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h>
 
+namespace OsqpEigen
+{
+class Settings;
+}
+
 namespace tesseract_planning
 {
 /** @brief The contains the default solver parameters available for setting up TrajOpt */
@@ -44,15 +49,19 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile
   using Ptr = std::shared_ptr<TrajOptIfoptDefaultSolverProfile>;
   using ConstPtr = std::shared_ptr<const TrajOptIfoptDefaultSolverProfile>;
 
-  TrajOptIfoptDefaultSolverProfile() = default;
-  ~TrajOptIfoptDefaultSolverProfile() override = default;
-  TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = default;
-  TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = default;
+  TrajOptIfoptDefaultSolverProfile();
+  ~TrajOptIfoptDefaultSolverProfile() override;
+  TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = delete;
+  TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = delete;
   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 */
+  std::unique_ptr<OsqpEigen::Settings> convex_solver_settings{ nullptr };
+
+  /** @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,6 @@
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <vector>
 #include <memory>
-#include <Eigen/Core>
 #include <trajopt_ifopt/fwd.h>
 #include <trajopt_sqp/fwd.h>
 #include <trajopt_sqp/types.h>
@@ -41,6 +40,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_scene_graph/scene_state.h>
 
+namespace OsqpEigen
+{
+class Settings;
+}
+
 namespace tesseract_planning
 {
 enum class TrajOptIfoptTermType
@@ -56,6 +60,8 @@ struct TrajOptIfoptProblem
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   // LCOV_EXCL_STOP
 
+  TrajOptIfoptProblem();
+
   trajopt_sqp::SQPParameters opt_info;
 
   // These are required for Tesseract to configure Descartes
@@ -67,6 +73,11 @@ struct TrajOptIfoptProblem
 
   std::vector<std::shared_ptr<trajopt_sqp::SQPCallback>> callbacks;
 
+  /** @brief The OSQP convex solver settings to use */
+  std::unique_ptr<OsqpEigen::Settings> convex_solver_settings{ nullptr };
+
+  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/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h

@@ -42,8 +42,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_environment/fwd.h>
 #include <tesseract_command_language/fwd.h>
 
+namespace OsqpEigen
+{
+class Settings;
+}
+
 namespace tesseract_planning
 {
+void copyOSQPEigenSettings(OsqpEigen::Settings& lhs, const OsqpEigen::Settings& rhs);
+
 bool addCartesianPositionConstraint(trajopt_sqp::QPProblem& nlp,
                                     const std::shared_ptr<const trajopt_ifopt::JointPosition>& var,
                                     const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
@@ -73,17 +80,17 @@ bool addCartesianPositionAbsoluteCost(trajopt_sqp::QPProblem& nlp,
 
 bool addJointPositionConstraint(trajopt_sqp::QPProblem& nlp,
                                 const JointWaypointPoly& joint_waypoint,
-                                const trajopt_ifopt::JointPosition::ConstPtr& var,
+                                const std::shared_ptr<const trajopt_ifopt::JointPosition>& var,
                                 const Eigen::VectorXd& coeffs);
 
 bool addJointPositionSquaredCost(trajopt_sqp::QPProblem& nlp,
                                  const JointWaypointPoly& joint_waypoint,
-                                 const trajopt_ifopt::JointPosition::ConstPtr& var,
+                                 const std::shared_ptr<const trajopt_ifopt::JointPosition>& var,
                                  const Eigen::VectorXd& coeffs);
 
 bool addJointPositionAbsoluteCost(trajopt_sqp::QPProblem& nlp,
                                   const JointWaypointPoly& joint_waypoint,
-                                  const trajopt_ifopt::JointPosition::ConstPtr& var,
+                                  const std::shared_ptr<const trajopt_ifopt::JointPosition>& var,
                                   const Eigen::VectorXd& coeffs);
 
 bool addCollisionConstraint(trajopt_sqp::QPProblem& nlp,

tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp

@@ -27,16 +27,32 @@
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <tinyxml2.h>
-#include <trajopt_sqp/sqp_callback.h>
+#include <OsqpEigen/Settings.hpp>
 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>
+#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h>
 
 namespace tesseract_planning
 {
+TrajOptIfoptDefaultSolverProfile::TrajOptIfoptDefaultSolverProfile()
+{
+  convex_solver_settings = std::make_unique<OsqpEigen::Settings>();
+  convex_solver_settings->setVerbosity(false);
+  convex_solver_settings->setWarmStart(true);
+  convex_solver_settings->setPolish(true);
+  convex_solver_settings->setAdaptiveRho(true);
+  convex_solver_settings->setMaxIteration(8192);
+  convex_solver_settings->setAbsoluteTolerance(1e-4);
+  convex_solver_settings->setRelativeTolerance(1e-6);
+}
+
+TrajOptIfoptDefaultSolverProfile::~TrajOptIfoptDefaultSolverProfile() = default;
+
 void TrajOptIfoptDefaultSolverProfile::apply(TrajOptIfoptProblem& problem) const
 {
+  copyOSQPEigenSettings(*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

@@ -36,6 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h>
 #include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h>
+#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
@@ -107,16 +108,16 @@ 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);
 
+  // There seems to be no way to set objects solver_->settings() (OsqpEigen::Settings)
+  // or solver_->settings()->getSettings() (OSQPSettings) at once
+  copyOSQPEigenSettings(*qp_solver->solver_->settings(), *problem->convex_solver_settings);
+  qp_solver->solver_->settings()->setVerbosity((problem->convex_solver_settings->getSettings()->verbose != 0) ||
+                                               request.verbose);
+
+  problem->qp_solver = qp_solver;
+
+  trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver);
   solver.params = problem->opt_info;
 
   // Add all callbacks

tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_problem.cpp

@@ -0,0 +1,48 @@
+/**
+ * @file trajopt_ifopt_problem.cpp
+ * @brief
+ *
+ * @author Levi Armstrong
+ * @date June 18, 2020
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2020, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <OsqpEigen/Settings.hpp>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h>
+
+namespace tesseract_planning
+{
+TrajOptIfoptProblem::TrajOptIfoptProblem()
+{
+  convex_solver_settings = std::make_unique<OsqpEigen::Settings>();
+  convex_solver_settings->setVerbosity(false);
+  convex_solver_settings->setWarmStart(true);
+  convex_solver_settings->setPolish(true);
+  convex_solver_settings->setAdaptiveRho(true);
+  convex_solver_settings->setMaxIteration(8192);
+  convex_solver_settings->setAbsoluteTolerance(1e-4);
+  convex_solver_settings->setRelativeTolerance(1e-6);
+}
+
+}  // namespace tesseract_planning

tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_utils.cpp

@@ -42,6 +42,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <trajopt_common/collision_types.h>
 #include <trajopt_sqp/qp_problem.h>
 #include <ifopt/constraint_set.h>
+#include <OsqpEigen/Settings.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h>
@@ -58,6 +59,33 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
+void copyOSQPEigenSettings(OsqpEigen::Settings& lhs, const OsqpEigen::Settings& rhs)
+{
+  const auto& settings = *rhs.getSettings();
+  lhs.setRho(settings.rho);
+  lhs.setSigma(settings.sigma);
+  lhs.setScaling(static_cast<int>(settings.scaling));
+  lhs.setAdaptiveRho(static_cast<bool>(settings.adaptive_rho));
+  lhs.setAdaptiveRhoInterval(static_cast<int>(settings.adaptive_rho_interval));
+  lhs.setAdaptiveRhoTolerance(settings.adaptive_rho_tolerance);
+  lhs.setAdaptiveRhoFraction(settings.adaptive_rho_fraction);
+  lhs.setMaxIteration(static_cast<int>(settings.max_iter));
+  lhs.setAbsoluteTolerance(settings.eps_abs);
+  lhs.setRelativeTolerance(settings.eps_rel);
+  lhs.setPrimalInfeasibilityTollerance(settings.eps_prim_inf);
+  lhs.setDualInfeasibilityTollerance(settings.eps_dual_inf);
+  lhs.setAlpha(settings.alpha);
+  lhs.setLinearSystemSolver(settings.linsys_solver);
+  lhs.setDelta(settings.delta);
+  lhs.setPolish(static_cast<bool>(settings.polish));
+  lhs.setPolishRefineIter(static_cast<int>(settings.polish_refine_iter));
+  lhs.setVerbosity(static_cast<bool>(settings.verbose));
+  lhs.setScaledTerimination(static_cast<bool>(settings.scaled_termination));
+  lhs.setCheckTermination(static_cast<int>(settings.check_termination));
+  lhs.setWarmStart(static_cast<bool>(settings.warm_start));
+  lhs.setTimeLimit(settings.time_limit);
+}
+
 ifopt::ConstraintSet::Ptr
 createCartesianPositionConstraint(const std::shared_ptr<const trajopt_ifopt::JointPosition>& var,
                                   const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
@@ -380,7 +408,7 @@ bool addCartesianPositionAbsoluteCost(trajopt_sqp::QPProblem& nlp,
 
 bool addJointPositionConstraint(trajopt_sqp::QPProblem& nlp,
                                 const JointWaypointPoly& joint_waypoint,
-                                const trajopt_ifopt::JointPosition::ConstPtr& var,
+                                const std::shared_ptr<const trajopt_ifopt::JointPosition>& var,
                                 const Eigen::VectorXd& coeffs)
 {
   auto constraint = createJointPositionConstraint(joint_waypoint, var, coeffs);
@@ -390,7 +418,7 @@ bool addJointPositionConstraint(trajopt_sqp::QPProblem& nlp,
 
 bool addJointPositionSquaredCost(trajopt_sqp::QPProblem& nlp,
                                  const JointWaypointPoly& joint_waypoint,
-                                 const trajopt_ifopt::JointPosition::ConstPtr& var,
+                                 const std::shared_ptr<const trajopt_ifopt::JointPosition>& var,
                                  const Eigen::VectorXd& coeffs)
 {
   auto constraint = createJointPositionConstraint(joint_waypoint, var, coeffs);
@@ -399,7 +427,7 @@ bool addJointPositionSquaredCost(trajopt_sqp::QPProblem& nlp,
 }
 bool addJointPositionAbsoluteCost(trajopt_sqp::QPProblem& nlp,
                                   const JointWaypointPoly& joint_waypoint,
-                                  const trajopt_ifopt::JointPosition::ConstPtr& var,
+                                  const std::shared_ptr<const trajopt_ifopt::JointPosition>& var,
                                   const Eigen::VectorXd& coeffs)
 {
   auto constraint = createJointPositionConstraint(joint_waypoint, var, coeffs);