Update to leverage osqp eigen settings object

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

@@ -29,13 +29,17 @@
 #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
 
 #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 */
@@ -46,15 +50,15 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile
   using ConstPtr = std::shared_ptr<const TrajOptIfoptDefaultSolverProfile>;
 
   TrajOptIfoptDefaultSolverProfile();
-  ~TrajOptIfoptDefaultSolverProfile() override = default;
+  ~TrajOptIfoptDefaultSolverProfile() override;
   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{};
+  std::unique_ptr<OsqpEigen::Settings> convex_solver_settings{ nullptr };
 
   /** @brief Optimization parameters */
   trajopt_sqp::SQPParameters opt_info{};

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 <osqp.h>
 #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,7 +60,7 @@ struct TrajOptIfoptProblem
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   // LCOV_EXCL_STOP
 
-  TrajOptIfoptProblem() { osqp_set_default_settings(&convex_solver_settings); };
+  TrajOptIfoptProblem();
 
   trajopt_sqp::SQPParameters opt_info;
 
@@ -69,9 +73,8 @@ 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{};
+  /** @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;
 

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,29 +27,32 @@
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <tinyxml2.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()
 {
-  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;
+  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
 {
-  problem.convex_solver_settings = convex_solver_settings;
+  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>
@@ -71,34 +72,6 @@ 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;
@@ -135,10 +108,15 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request)
   // Create optimizer
   /** @todo Enable solver selection (e.g. IPOPT) */
   auto qp_solver = std::make_shared<trajopt_sqp::OSQPEigenSolver>();
-  OSQPEigenSolver_setSettings(qp_solver->solver_, problem->convex_solver_settings);
-  qp_solver->solver_->settings()->setVerbosity((problem->convex_solver_settings.verbose != 0) || request.verbose);
+
+  // 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;
 

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(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(settings.polish);
+  lhs.setPolishRefineIter(static_cast<int>(settings.polish_refine_iter));
+  lhs.setVerbosity(settings.verbose);
+  lhs.setScaledTerimination(settings.scaled_termination);
+  lhs.setCheckTermination(static_cast<int>(settings.check_termination));
+  lhs.setWarmStart(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);