Expose convex solver settings and set ospq adaptive_rho to default value (#285)

* Expose convex solver settings and set ospq adaptive_rho to default value

* Fix windows CI build

* Fix unit tests

Co-authored-by: Tyler Marr <[email protected]>

Author: Levi-Armstrong

.github/workflows/windows_noetic_build.yml

@@ -12,7 +12,7 @@ on:
 jobs:
   windows_ci:
     name: Noetic
-    runs-on: windows-latest
+    runs-on: windows-2019
     env:
       ROS_DISTRO: noetic
     steps:

trajopt/include/trajopt/problem_description.hpp

@@ -114,6 +114,9 @@ struct BasicInfo
   /** @brief The convex solver to use */
   sco::ModelType convex_solver;
 
+  /** @brief The convex solver configuration settings */
+  sco::ModelConfig::Ptr convex_solver_config;
+
   /** @brief If true, the last column in the optimization matrix will be 1/dt */
   bool use_time = false;
 

trajopt/src/problem_description.cpp

@@ -539,8 +539,10 @@ TrajOptProb::Ptr ConstructProblem(const Json::Value& root, const tesseract_envir
   return ConstructProblem(pci);
 }
 
+TrajOptProb::TrajOptProb() = default;
+
 TrajOptProb::TrajOptProb(int n_steps, const ProblemConstructionInfo& pci)
-  : OptProb(pci.basic_info.convex_solver), m_kin(pci.kin), m_env(pci.env)
+  : OptProb(pci.basic_info.convex_solver, pci.basic_info.convex_solver_config), m_kin(pci.kin), m_env(pci.env)
 {
   const Eigen::MatrixX2d& limits = m_kin->getLimits().joint_limits;
   auto n_dof = static_cast<int>(m_kin->numJoints());
@@ -570,8 +572,6 @@ TrajOptProb::TrajOptProb(int n_steps, const ProblemConstructionInfo& pci)
   m_traj_vars = VarArray(n_steps, n_dof + (pci.basic_info.use_time ? 1 : 0), trajvarvec.data());
 }
 
-TrajOptProb::TrajOptProb() = default;
-
 void UserDefinedTermInfo::fromJson(ProblemConstructionInfo& /*pci*/, const Json::Value& /*v*/)
 {
   PRINT_AND_THROW("UserDefinedTermInfo does not support fromJson!");

trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp

@@ -37,7 +37,7 @@ OSQPEigenSolver::OSQPEigenSolver()
     solver_.settings()->setVerbosity(false);
   solver_.settings()->setWarmStart(true);
   solver_.settings()->setPolish(true);
-  solver_.settings()->setAdaptiveRho(false);
+  solver_.settings()->setAdaptiveRho(true);
   solver_.settings()->setMaxIteration(8192);
   solver_.settings()->setAbsoluteTolerance(1e-4);
   solver_.settings()->setRelativeTolerance(1e-6);

trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_trajopt_sco_unit.cpp

@@ -155,8 +155,11 @@ TEST(CartPositionOptimizationTrajoptSCO, cart_position_optimization_trajopt_sco)
 
   tesseract_common::TrajArray traj = getTraj(opt.x(), prob->GetVars());
 
-  for (Eigen::Index i = 0; i < traj.cols(); i++)
-    EXPECT_NEAR(traj(0, i), joint_target[i], 1.1e-3);
+  auto optimized_pose = manip->calcFwdKin(traj.row(0)).at("r_gripper_tool_frame");
+  EXPECT_TRUE(target_pose.translation().isApprox(optimized_pose.translation(), 1e-4));
+  Eigen::Quaterniond target_q(target_pose.rotation());
+  Eigen::Quaterniond optimized_q(optimized_pose.rotation());
+  EXPECT_TRUE(target_q.isApprox(optimized_q, 1e-5));
 
   if (DEBUG)
   {

trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp

@@ -152,8 +152,11 @@ void runCartPositionOptimization(const trajopt_sqp::QPProblem::Ptr& qp_problem,
   solver.solve(qp_problem);
   Eigen::VectorXd x = qp_problem->getVariableValues();
 
-  for (Eigen::Index i = 0; i < joint_target.size(); i++)
-    EXPECT_NEAR(x[i], joint_target[i], 1.1e-3);
+  auto optimized_pose = manip->calcFwdKin(x).at("r_gripper_tool_frame");
+  EXPECT_TRUE(target_pose.translation().isApprox(optimized_pose.translation(), 1e-4));
+  Eigen::Quaterniond target_q(target_pose.rotation());
+  Eigen::Quaterniond optimized_q(optimized_pose.rotation());
+  EXPECT_TRUE(target_q.isApprox(optimized_q, 1e-5));
 
   if (DEBUG)
   {

trajopt_sco/include/trajopt_sco/modeling.hpp

@@ -197,7 +197,8 @@ class OptProb
 public:
   using Ptr = std::shared_ptr<OptProb>;
 
-  OptProb(ModelType convex_solver = ModelType::AUTO_SOLVER);
+  OptProb(ModelType convex_solver = ModelType::AUTO_SOLVER,
+          const ModelConfig::ConstPtr& convex_solver_config = nullptr);
   virtual ~OptProb() = default;
   OptProb(const OptProb&) = delete;
   OptProb& operator=(const OptProb&) = delete;

trajopt_sco/include/trajopt_sco/osqp_interface.hpp

@@ -9,6 +9,17 @@ TRAJOPT_IGNORE_WARNINGS_POP
 
 namespace sco
 {
+/** @brief The OSQP configuration settings */
+struct OSQPModelConfig : public ModelConfig
+{
+  using Ptr = std::shared_ptr<OSQPModelConfig>;
+  using ConstPtr = std::shared_ptr<const OSQPModelConfig>;
+
+  OSQPModelConfig();
+
+  OSQPSettings settings;
+};
+
 /**
  * OSQPModel uses the BSD solver OSQP to solve a linearly constrained QP.
  * OSQP solves a problem in the form:
@@ -22,9 +33,8 @@ namespace sco
  */
 class OSQPModel : public Model
 {
-  OSQPSettings osqp_settings_; /**< OSQP Settings */
-                               /** OSQPData. Some fields here (`osp_data_.A` and `osp_data_.P`) are
-                                *  automatically allocated by OSQP, but deallocated by us. */
+  /** OSQPData. Some fields here (`osp_data_.A` and `osp_data_.P`) are *  automatically allocated by OSQP, but
+   * deallocated by us. */
   OSQPData osqp_data_;
 
   /** OSQP Workspace. Memory here is managed by OSQP */
@@ -63,8 +73,10 @@ class OSQPModel : public Model
 
   QuadExpr objective_; /**< objective QuadExpr expression */
 
+  OSQPModelConfig config_; /**< The configuration settings */
+
 public:
-  OSQPModel();
+  OSQPModel(const ModelConfig::ConstPtr& config = nullptr);
   ~OSQPModel() override;
   OSQPModel(const OSQPModel& model) = delete;
   OSQPModel& operator=(const OSQPModel& model) = delete;

trajopt_sco/include/trajopt_sco/solver_interface.hpp

@@ -86,6 +86,13 @@ class Model
   virtual VarVector getVars() const = 0;
 };
 
+struct ModelConfig
+{
+  using Ptr = std::shared_ptr<ModelConfig>;
+  using ConstPtr = std::shared_ptr<const ModelConfig>;
+  virtual ~ModelConfig() = default;
+};
+
 struct VarRep
 {
   using Ptr = std::shared_ptr<VarRep>;
@@ -232,7 +239,8 @@ std::vector<ModelType> availableSolvers();
 
 std::ostream& operator<<(std::ostream& os, const ModelType& cs);
 
-Model::Ptr createModel(ModelType model_type = ModelType::AUTO_SOLVER);
+Model::Ptr createModel(ModelType model_type = ModelType::AUTO_SOLVER,
+                       const ModelConfig::ConstPtr& model_config = nullptr);
 
 void vars2inds(const VarVector& vars, SizeTVec& inds);
 

trajopt_sco/src/modeling.cpp

@@ -160,7 +160,10 @@ DblVec Constraint::violations(const DblVec& x)
 }
 
 double Constraint::violation(const DblVec& x) { return vecSum(violations(x)); }
-OptProb::OptProb(ModelType convex_solver) : model_(createModel(convex_solver)) {}
+OptProb::OptProb(ModelType convex_solver, const ModelConfig::ConstPtr& convex_solver_config)
+  : model_(createModel(convex_solver, convex_solver_config))
+{
+}
 VarVector OptProb::createVariables(const std::vector<std::string>& names)
 {
   return createVariables(names, DblVec(names.size(), -INFINITY), DblVec(names.size(), INFINITY));