diff --git a/trajopt_sco/CMakeLists.txt b/trajopt_sco/CMakeLists.txt
index bdc6416b..e51d467f 100644
--- a/trajopt_sco/CMakeLists.txt
+++ b/trajopt_sco/CMakeLists.txt
@@ -23,6 +23,7 @@ elseif(NOT TARGET jsoncpp_lib)
 endif()
 find_package(ros_industrial_cmake_boilerplate REQUIRED)
 find_package(Boost REQUIRED)
+find_package(OpenMP REQUIRED)
 
 # Load variable for clang tidy args, compiler options and cxx version
 trajopt_variables()
@@ -108,7 +109,8 @@ target_link_libraries(
          Boost::boost
          Eigen3::Eigen
          ${CMAKE_DL_LIBS}
-         jsoncpp_lib)
+         jsoncpp_lib
+         OpenMP::OpenMP_CXX)
 target_compile_options(${PROJECT_NAME} PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PRIVATE})
 target_compile_options(${PROJECT_NAME} PUBLIC ${TRAJOPT_COMPILE_OPTIONS_PUBLIC})
 target_compile_definitions(${PROJECT_NAME} PUBLIC ${TRAJOPT_COMPILE_DEFINITIONS})
diff --git a/trajopt_sco/cmake/trajopt_sco-config.cmake.in b/trajopt_sco/cmake/trajopt_sco-config.cmake.in
index a79aafec..9e3fa7f4 100644
--- a/trajopt_sco/cmake/trajopt_sco-config.cmake.in
+++ b/trajopt_sco/cmake/trajopt_sco-config.cmake.in
@@ -19,5 +19,6 @@ if(${CMAKE_VERSION} VERSION_LESS "3.15.0")
 else()
   find_dependency(Boost)
 endif()
+find_dependency(OpenMP)
 
 include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake")
diff --git a/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp b/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp
index bedb48f4..901cc698 100644
--- a/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp
+++ b/trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp
@@ -1,4 +1,8 @@
 #pragma once
+#include <trajopt_utils/macros.h>
+TRAJOPT_IGNORE_WARNINGS_PUSH
+#include <mutex>
+TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_sco/solver_interface.hpp>
 
 namespace sco
@@ -19,13 +23,16 @@ class BPMPDModel : public Model
   int m_pipeOut{ 0 };
   int m_pid{ 0 };
 
+  std::mutex m_mutex; /**< The mutex */
+
   BPMPDModel();
   ~BPMPDModel() override = default;
-  BPMPDModel(const BPMPDModel&) = default;
-  BPMPDModel& operator=(const BPMPDModel&) = default;
-  BPMPDModel(BPMPDModel&&) = default;
-  BPMPDModel& operator=(BPMPDModel&&) = default;
+  BPMPDModel(const BPMPDModel&) = delete;
+  BPMPDModel& operator=(const BPMPDModel&) = delete;
+  BPMPDModel(BPMPDModel&&) = delete;
+  BPMPDModel& operator=(BPMPDModel&&) = delete;
 
+  // Must be threadsafe
   Var addVar(const std::string& name) override;
   Cnt addEqCnt(const AffExpr&, const std::string& name) override;
   Cnt addIneqCnt(const AffExpr&, const std::string& name) override;
@@ -33,12 +40,13 @@ class BPMPDModel : public Model
   void removeVars(const VarVector& vars) override;
   void removeCnts(const CntVector& cnts) override;
 
+  // These do not need to be threadsafe
   void update() override;
-  void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override;
-  DblVec getVarValues(const VarVector& vars) const override;
   CvxOptStatus optimize() override;
   void setObjective(const AffExpr&) override;
   void setObjective(const QuadExpr&) override;
+  void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override;
+  DblVec getVarValues(const VarVector& vars) const override;
   void writeToFile(const std::string& fname) const override;
   VarVector getVars() const override;
 };
diff --git a/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp b/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp
index 97f08c12..2451e808 100644
--- a/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp
+++ b/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp
@@ -1,4 +1,8 @@
 #pragma once
+#include <trajopt_utils/macros.h>
+TRAJOPT_IGNORE_WARNINGS_PUSH
+#include <mutex>
+TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_sco/solver_interface.hpp>
 
 /**
@@ -22,34 +26,31 @@ class GurobiModel : public Model
   GRBmodel* m_model;
   VarVector m_vars;
   CntVector m_cnts;
+  std::mutex m_mutex;
 
   GurobiModel();
+  ~GurobiModel();
 
+  // Must be threadsafe
   Var addVar(const std::string& name) override;
   Var addVar(const std::string& name, double lower, double upper) override;
-
   Cnt addEqCnt(const AffExpr&, const std::string& name) override;
   Cnt addIneqCnt(const AffExpr&, const std::string& name) override;
   Cnt addIneqCnt(const QuadExpr&, const std::string& name) override;
-
   void removeVars(const VarVector&) override;
   void removeCnts(const CntVector&) override;
 
+  // These do not need to be threadsafe
   void update() override;
-  void setVarBounds(const VarVector&, const DblVec& lower, const DblVec& upper) override;
-  DblVec getVarValues(const VarVector&) const override;
-
   CvxOptStatus optimize() override;
-  /** Don't use this function, because it adds constraints that aren't tracked
-   */
-  CvxOptStatus optimizeFeasRelax();
-
   void setObjective(const AffExpr&) override;
   void setObjective(const QuadExpr&) override;
+  void setVarBounds(const VarVector&, const DblVec& lower, const DblVec& upper) override;
+  DblVec getVarValues(const VarVector&) const override;
   void writeToFile(const std::string& fname) const override;
-
   VarVector getVars() const override;
 
-  ~GurobiModel();
+  /** Don't use this function, because it adds constraints that aren't tracked*/
+  CvxOptStatus optimizeFeasRelax();
 };
 }  // namespace sco
diff --git a/trajopt_sco/include/trajopt_sco/optimizers.hpp b/trajopt_sco/include/trajopt_sco/optimizers.hpp
index 22f4d289..c33cb45e 100644
--- a/trajopt_sco/include/trajopt_sco/optimizers.hpp
+++ b/trajopt_sco/include/trajopt_sco/optimizers.hpp
@@ -119,9 +119,71 @@ struct BasicTrustRegionSQPParameters
   bool log_results;     // Log results to file
   std::string log_dir;  // Directory to store log results (Default: /tmp)
 
+  /** @brief If greater than one, multi threaded functions are called */
+  int num_threads;
+
   BasicTrustRegionSQPParameters();
 };
 
+struct BasicTrustRegionSQPUtilFunctions
+{
+  virtual ~BasicTrustRegionSQPUtilFunctions() = default;
+
+  virtual DblVec evaluateCosts(const std::vector<Cost::Ptr>& costs, const DblVec& x) const;
+
+  virtual DblVec evaluateConstraintViols(const std::vector<Constraint::Ptr>& constraints, const DblVec& x) const;
+
+  virtual std::vector<ConvexObjective::Ptr> convexifyCosts(const std::vector<Cost::Ptr>& costs,
+                                                           const DblVec& x,
+                                                           Model* model) const;
+
+  virtual std::vector<ConvexConstraints::Ptr> convexifyConstraints(const std::vector<Constraint::Ptr>& cnts,
+                                                                   const DblVec& x,
+                                                                   Model* model) const;
+
+  virtual DblVec evaluateModelCosts(const std::vector<ConvexObjective::Ptr>& costs, const DblVec& x) const;
+
+  virtual DblVec evaluateModelCntViols(const std::vector<ConvexConstraints::Ptr>& cnts, const DblVec& x) const;
+
+  virtual std::vector<std::string> getCostNames(const std::vector<Cost::Ptr>& costs) const;
+
+  virtual std::vector<std::string> getCntNames(const std::vector<Constraint::Ptr>& cnts) const;
+
+  virtual std::vector<std::string> getVarNames(const VarVector& vars) const;
+};
+
+struct BasicTrustRegionSQPUtilFunctionsThreaded : public BasicTrustRegionSQPUtilFunctions
+{
+  BasicTrustRegionSQPUtilFunctionsThreaded();
+  BasicTrustRegionSQPUtilFunctionsThreaded(int num_threads);
+  ~BasicTrustRegionSQPUtilFunctionsThreaded() override = default;
+
+  DblVec evaluateCosts(const std::vector<Cost::Ptr>& costs, const DblVec& x) const override final;
+
+  DblVec evaluateConstraintViols(const std::vector<Constraint::Ptr>& constraints, const DblVec& x) const override final;
+
+  std::vector<ConvexObjective::Ptr> convexifyCosts(const std::vector<Cost::Ptr>& costs,
+                                                   const DblVec& x,
+                                                   Model* model) const override final;
+
+  std::vector<ConvexConstraints::Ptr> convexifyConstraints(const std::vector<Constraint::Ptr>& cnts,
+                                                           const DblVec& x,
+                                                           Model* model) const override final;
+
+  DblVec evaluateModelCosts(const std::vector<ConvexObjective::Ptr>& costs, const DblVec& x) const override final;
+
+  DblVec evaluateModelCntViols(const std::vector<ConvexConstraints::Ptr>& cnts, const DblVec& x) const override final;
+
+  std::vector<std::string> getCostNames(const std::vector<Cost::Ptr>& costs) const override final;
+
+  std::vector<std::string> getCntNames(const std::vector<Constraint::Ptr>& cnts) const override final;
+
+  std::vector<std::string> getVarNames(const VarVector& vars) const override final;
+
+private:
+  int num_threads_;
+};
+
 /**
  * @brief This struct stores iteration results for the BasicTrustRegionSQP
  *
@@ -183,7 +245,8 @@ struct BasicTrustRegionSQPResults
 
   BasicTrustRegionSQPResults(std::vector<std::string> var_names,
                              std::vector<std::string> cost_names,
-                             std::vector<std::string> cnt_names);
+                             std::vector<std::string> cnt_names,
+                             std::shared_ptr<BasicTrustRegionSQPUtilFunctions> util_funcs);
 
   /**
    * @brief Update the structure data for a new iteration
@@ -217,6 +280,9 @@ struct BasicTrustRegionSQPResults
   void writeConstraints(std::FILE* stream, bool header = false) const;
   /** @brief Prints the raw values to the terminal */
   void printRaw() const;
+
+private:
+  std::shared_ptr<BasicTrustRegionSQPUtilFunctions> util_funcs_;
 };
 
 class BasicTrustRegionSQP : public Optimizer
@@ -238,9 +304,9 @@ class BasicTrustRegionSQP : public Optimizer
   BasicTrustRegionSQP() = default;
   BasicTrustRegionSQP(const OptProb::Ptr& prob);
   void setProblem(OptProb::Ptr prob) override;
-  void setParameters(const BasicTrustRegionSQPParameters& param) { param_ = param; }
-  const BasicTrustRegionSQPParameters& getParameters() const { return param_; }
-  BasicTrustRegionSQPParameters& getParameters() { return param_; }
+  void setParameters(const BasicTrustRegionSQPParameters& param);
+  const BasicTrustRegionSQPParameters& getParameters() const;
+  BasicTrustRegionSQPParameters& getParameters();
   OptStatus optimize() override;
 
 protected:
@@ -248,7 +314,9 @@ class BasicTrustRegionSQP : public Optimizer
   void adjustTrustRegion(double ratio);
   void setTrustRegionSize(double trust_box_size);
   void setTrustBoxConstraints(const DblVec& x);
+  void updateUtilsFunctions();
   Model::Ptr model_;
   BasicTrustRegionSQPParameters param_;
+  std::shared_ptr<BasicTrustRegionSQPUtilFunctions> util_funcs_;
 };
 }  // namespace sco
diff --git a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp
index d16057b0..fcbbc6f1 100644
--- a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp
+++ b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp
@@ -3,6 +3,7 @@
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Core>
 #include <osqp.h>
+#include <mutex>
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_sco/solver_interface.hpp>
@@ -75,14 +76,17 @@ class OSQPModel : public Model
 
   OSQPModelConfig config_; /**< The configuration settings */
 
+  std::mutex mutex_; /**< The mutex */
+
 public:
   OSQPModel(const ModelConfig::ConstPtr& config = nullptr);
   ~OSQPModel() override;
   OSQPModel(const OSQPModel& model) = delete;
   OSQPModel& operator=(const OSQPModel& model) = delete;
-  OSQPModel(OSQPModel&&) = default;
-  OSQPModel& operator=(OSQPModel&&) = default;
+  OSQPModel(OSQPModel&&) = delete;
+  OSQPModel& operator=(OSQPModel&&) = delete;
 
+  // Must be threadsafe
   Var addVar(const std::string& name) override;
   Cnt addEqCnt(const AffExpr&, const std::string& name) override;
   Cnt addIneqCnt(const AffExpr&, const std::string& name) override;
@@ -90,13 +94,14 @@ class OSQPModel : public Model
   void removeVars(const VarVector& vars) override;
   void removeCnts(const CntVector& cnts) override;
 
+  // These do not need to be threadsafe
   void update() override;
-  void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override;
-  DblVec getVarValues(const VarVector& vars) const override;
   CvxOptStatus optimize() override;
   void setObjective(const AffExpr&) override;
   void setObjective(const QuadExpr&) override;
-  VarVector getVars() const override;
+  void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override;
+  DblVec getVarValues(const VarVector& vars) const override;
   void writeToFile(const std::string& fname) const override;
+  VarVector getVars() const override;
 };
 }  // namespace sco
diff --git a/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp b/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp
index 55138baa..6ed45ca1 100644
--- a/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp
+++ b/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp
@@ -3,6 +3,7 @@
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Core>
 #include <qpOASES.hpp>
+#include <mutex>
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_sco/solver_interface.hpp>
@@ -71,14 +72,17 @@ class qpOASESModel : public Model
 
   QuadExpr objective_; /**< objective QuadExpr expression */
 
+  std::mutex mutex_; /**< The mutex */
+
 public:
   qpOASESModel();
   ~qpOASESModel() override;
-  qpOASESModel(const qpOASESModel&) = default;
-  qpOASESModel& operator=(const qpOASESModel&) = default;
-  qpOASESModel(qpOASESModel&&) = default;
-  qpOASESModel& operator=(qpOASESModel&&) = default;
+  qpOASESModel(const qpOASESModel&) = delete;
+  qpOASESModel& operator=(const qpOASESModel&) = delete;
+  qpOASESModel(qpOASESModel&&) = delete;
+  qpOASESModel& operator=(qpOASESModel&&) = delete;
 
+  // Must be thread safe
   Var addVar(const std::string& name) override;
   Cnt addEqCnt(const AffExpr&, const std::string& name) override;
   Cnt addIneqCnt(const AffExpr&, const std::string& name) override;
@@ -86,12 +90,13 @@ class qpOASESModel : public Model
   void removeVars(const VarVector& vars) override;
   void removeCnts(const CntVector& cnts) override;
 
+  // These do not need to be threadsafe
   void update() override;
-  void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override;
-  DblVec getVarValues(const VarVector& vars) const override;
   CvxOptStatus optimize() override;
   void setObjective(const AffExpr&) override;
   void setObjective(const QuadExpr&) override;
+  void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override;
+  DblVec getVarValues(const VarVector& vars) const override;
   void writeToFile(const std::string& fname) const override;
   VarVector getVars() const override;
 };
diff --git a/trajopt_sco/include/trajopt_sco/solver_interface.hpp b/trajopt_sco/include/trajopt_sco/solver_interface.hpp
index 546b7f04..e4f4bd24 100644
--- a/trajopt_sco/include/trajopt_sco/solver_interface.hpp
+++ b/trajopt_sco/include/trajopt_sco/solver_interface.hpp
@@ -64,18 +64,31 @@ class Model
   Model(Model&&) = default;
   Model& operator=(Model&&) = default;
 
+  /**
+   * @brief Add a var to the model
+   * @details These must be threadsafe
+   */
   virtual Var addVar(const std::string& name) = 0;
   virtual Var addVar(const std::string& name, double lb, double ub);
 
+  /**
+   * @brief Add a equation to the model
+   * @details These must be threadsafe
+   */
   virtual Cnt addEqCnt(const AffExpr&, const std::string& name) = 0;     // expr == 0
   virtual Cnt addIneqCnt(const AffExpr&, const std::string& name) = 0;   // expr <= 0
   virtual Cnt addIneqCnt(const QuadExpr&, const std::string& name) = 0;  // expr <= 0
 
+  /**
+   * @brief Remove items from model
+   * @details These must be threadsafe
+   */
   virtual void removeVar(const Var& var);
   virtual void removeCnt(const Cnt& cnt);
   virtual void removeVars(const VarVector& vars) = 0;
   virtual void removeCnts(const CntVector& cnts) = 0;
 
+  /**  @details It is not neccessary to make the following methods threadsafe */
   virtual void update() = 0;  // call after adding/deleting stuff
   virtual void setVarBounds(const Var& var, double lower, double upper);
   virtual void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) = 0;
diff --git a/trajopt_sco/src/bpmpd_interface.cpp b/trajopt_sco/src/bpmpd_interface.cpp
index c5c35d00..01358669 100644
--- a/trajopt_sco/src/bpmpd_interface.cpp
+++ b/trajopt_sco/src/bpmpd_interface.cpp
@@ -235,6 +235,7 @@ BPMPDModel::BPMPDModel()
 
 Var BPMPDModel::addVar(const std::string& name)
 {
+  std::scoped_lock lock(m_mutex);
   m_vars.push_back(std::make_shared<VarRep>(m_vars.size(), name, this));
   m_lbs.push_back(-BPMPD_BIG);
   m_ubs.push_back(BPMPD_BIG);
@@ -242,6 +243,7 @@ Var BPMPDModel::addVar(const std::string& name)
 }
 Cnt BPMPDModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/)
 {
+  std::scoped_lock lock(m_mutex);
   m_cnts.push_back(std::make_shared<CntRep>(m_cnts.size(), this));
   m_cntExprs.push_back(expr);
   m_cntTypes.push_back(EQ);
@@ -249,6 +251,7 @@ Cnt BPMPDModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/)
 }
 Cnt BPMPDModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/)
 {
+  std::scoped_lock lock(m_mutex);
   m_cnts.push_back(std::make_shared<CntRep>(m_cnts.size(), this));
   m_cntExprs.push_back(expr);
   m_cntTypes.push_back(INEQ);
@@ -261,6 +264,7 @@ Cnt BPMPDModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/)
 
 void BPMPDModel::removeVars(const VarVector& vars)
 {
+  std::scoped_lock lock(m_mutex);
   SizeTVec inds;
   vars2inds(vars, inds);
   for (const auto& var : vars)
@@ -269,6 +273,7 @@ void BPMPDModel::removeVars(const VarVector& vars)
 
 void BPMPDModel::removeCnts(const CntVector& cnts)
 {
+  std::scoped_lock lock(m_mutex);
   SizeTVec inds;
   cnts2inds(cnts, inds);
   for (auto& cnt : cnts)  // NOLINT
diff --git a/trajopt_sco/src/gurobi_interface.cpp b/trajopt_sco/src/gurobi_interface.cpp
index c02f4401..f3099c00 100644
--- a/trajopt_sco/src/gurobi_interface.cpp
+++ b/trajopt_sco/src/gurobi_interface.cpp
@@ -83,6 +83,7 @@ GurobiModel::GurobiModel()
 
 Var GurobiModel::addVar(const std::string& name)
 {
+  std::scoped_lock lock(m_mutex);
   ENSURE_SUCCESS(GRBaddvar(
       m_model, 0, nullptr, nullptr, 0, -GRB_INFINITY, GRB_INFINITY, GRB_CONTINUOUS, const_cast<char*>(name.c_str())));
   m_vars.push_back(std::make_shared<VarRep>(m_vars.size(), name, this));
@@ -91,6 +92,7 @@ Var GurobiModel::addVar(const std::string& name)
 
 Var GurobiModel::addVar(const std::string& name, double lb, double ub)
 {
+  std::scoped_lock lock(m_mutex);
   ENSURE_SUCCESS(GRBaddvar(m_model, 0, nullptr, nullptr, 0, lb, ub, GRB_CONTINUOUS, const_cast<char*>(name.c_str())));
   m_vars.push_back(std::make_shared<VarRep>(m_vars.size(), name, this));
   return m_vars.back();
@@ -98,6 +100,7 @@ Var GurobiModel::addVar(const std::string& name, double lb, double ub)
 
 Cnt GurobiModel::addEqCnt(const AffExpr& expr, const std::string& name)
 {
+  std::scoped_lock lock(m_mutex);
   LOG_TRACE("adding eq constraint: %s = 0", CSTR(expr));
   IntVec inds;
   vars2inds(expr.vars, inds);
@@ -115,6 +118,7 @@ Cnt GurobiModel::addEqCnt(const AffExpr& expr, const std::string& name)
 }
 Cnt GurobiModel::addIneqCnt(const AffExpr& expr, const std::string& name)
 {
+  std::scoped_lock lock(m_mutex);
   LOG_TRACE("adding ineq: %s <= 0", CSTR(expr));
   IntVec inds;
   vars2inds(expr.vars, inds);
@@ -132,6 +136,7 @@ Cnt GurobiModel::addIneqCnt(const AffExpr& expr, const std::string& name)
 }
 Cnt GurobiModel::addIneqCnt(const QuadExpr& qexpr, const std::string& name)
 {
+  std::scoped_lock lock(m_mutex);
   int numlnz = static_cast<int>(qexpr.affexpr.size());
   IntVec linds;
   vars2inds(qexpr.affexpr.vars, linds);
@@ -167,6 +172,7 @@ void resetIndices(CntVector& cnts)
 
 void GurobiModel::removeVars(const VarVector& vars)
 {
+  std::scoped_lock lock(m_mutex);
   IntVec inds;
   vars2inds(vars, inds);
   ENSURE_SUCCESS(GRBdelvars(m_model, static_cast<int>(inds.size()), inds.data()));
@@ -176,6 +182,7 @@ void GurobiModel::removeVars(const VarVector& vars)
 
 void GurobiModel::removeCnts(const CntVector& cnts)
 {
+  std::scoped_lock lock(m_mutex);
   IntVec inds;
   cnts2inds(cnts, inds);
   ENSURE_SUCCESS(GRBdelconstrs(m_model, static_cast<int>(inds.size()), inds.data()));
diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp
index f5b0d1f3..9a31a811 100644
--- a/trajopt_sco/src/optimizers.cpp
+++ b/trajopt_sco/src/optimizers.cpp
@@ -4,6 +4,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <cmath>
 #include <chrono>
 #include <cstdio>
+#include <thread>
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_sco/expr_ops.hpp>
@@ -34,74 +35,78 @@ std::ostream& operator<<(std::ostream& o, const OptResults& r)
 ////////// private utility functions for  sqp /////////
 //////////////////////////////////////////////////
 
-static DblVec evaluateCosts(const std::vector<Cost::Ptr>& costs, const DblVec& x)
+DblVec BasicTrustRegionSQPUtilFunctions::evaluateCosts(const std::vector<Cost::Ptr>& costs, const DblVec& x) const
 {
   DblVec out(costs.size());
   for (size_t i = 0; i < costs.size(); ++i)
-  {
     out[i] = costs[i]->value(x);
-  }
+
   return out;
 }
-static DblVec evaluateConstraintViols(const std::vector<Constraint::Ptr>& constraints, const DblVec& x)
+
+DblVec BasicTrustRegionSQPUtilFunctions::evaluateConstraintViols(const std::vector<Constraint::Ptr>& constraints,
+                                                                 const DblVec& x) const
 {
   DblVec out(constraints.size());
   for (size_t i = 0; i < constraints.size(); ++i)
-  {
     out[i] = constraints[i]->violation(x);
-  }
+
   return out;
 }
-static std::vector<ConvexObjective::Ptr> convexifyCosts(const std::vector<Cost::Ptr>& costs,
-                                                        const DblVec& x,
-                                                        Model* model)
+
+std::vector<ConvexObjective::Ptr> BasicTrustRegionSQPUtilFunctions::convexifyCosts(const std::vector<Cost::Ptr>& costs,
+                                                                                   const DblVec& x,
+                                                                                   Model* model) const
 {
   std::vector<ConvexObjective::Ptr> out(costs.size());
   for (size_t i = 0; i < costs.size(); ++i)
-  {
     out[i] = costs[i]->convex(x, model);
-  }
+
   return out;
 }
-static std::vector<ConvexConstraints::Ptr> convexifyConstraints(const std::vector<Constraint::Ptr>& cnts,
-                                                                const DblVec& x,
-                                                                Model* model)
+
+std::vector<ConvexConstraints::Ptr>
+BasicTrustRegionSQPUtilFunctions::convexifyConstraints(const std::vector<Constraint::Ptr>& cnts,
+                                                       const DblVec& x,
+                                                       Model* model) const
 {
   std::vector<ConvexConstraints::Ptr> out(cnts.size());
   for (size_t i = 0; i < cnts.size(); ++i)
-  {
     out[i] = cnts[i]->convex(x, model);
-  }
+
   return out;
 }
 
-DblVec evaluateModelCosts(const std::vector<ConvexObjective::Ptr>& costs, const DblVec& x)
+DblVec BasicTrustRegionSQPUtilFunctions::evaluateModelCosts(const std::vector<ConvexObjective::Ptr>& costs,
+                                                            const DblVec& x) const
 {
   DblVec out(costs.size());
   for (size_t i = 0; i < costs.size(); ++i)
-  {
     out[i] = costs[i]->value(x);
-  }
+
   return out;
 }
-DblVec evaluateModelCntViols(const std::vector<ConvexConstraints::Ptr>& cnts, const DblVec& x)
+
+DblVec BasicTrustRegionSQPUtilFunctions::evaluateModelCntViols(const std::vector<ConvexConstraints::Ptr>& cnts,
+                                                               const DblVec& x) const
 {
   DblVec out(cnts.size());
   for (size_t i = 0; i < cnts.size(); ++i)
-  {
     out[i] = cnts[i]->violation(x);
-  }
+
   return out;
 }
 
-static std::vector<std::string> getCostNames(const std::vector<Cost::Ptr>& costs)
+std::vector<std::string> BasicTrustRegionSQPUtilFunctions::getCostNames(const std::vector<Cost::Ptr>& costs) const
 {
   std::vector<std::string> out(costs.size());
   for (size_t i = 0; i < costs.size(); ++i)
     out[i] = costs[i]->name();
+
   return out;
 }
-static std::vector<std::string> getCntNames(const std::vector<Constraint::Ptr>& cnts)
+
+std::vector<std::string> BasicTrustRegionSQPUtilFunctions::getCntNames(const std::vector<Constraint::Ptr>& cnts) const
 {
   std::vector<std::string> out(cnts.size());
   for (size_t i = 0; i < cnts.size(); ++i)
@@ -109,7 +114,7 @@ static std::vector<std::string> getCntNames(const std::vector<Constraint::Ptr>&
   return out;
 }
 
-static std::vector<std::string> getVarNames(const VarVector& vars)
+std::vector<std::string> BasicTrustRegionSQPUtilFunctions::getVarNames(const VarVector& vars) const
 {
   std::vector<std::string> out;
   out.reserve(vars.size());
@@ -118,6 +123,118 @@ static std::vector<std::string> getVarNames(const VarVector& vars)
   return out;
 }
 
+BasicTrustRegionSQPUtilFunctionsThreaded::BasicTrustRegionSQPUtilFunctionsThreaded()
+  : BasicTrustRegionSQPUtilFunctionsThreaded(static_cast<int>(std::thread::hardware_concurrency()))
+{
+}
+BasicTrustRegionSQPUtilFunctionsThreaded::BasicTrustRegionSQPUtilFunctionsThreaded(int num_threads)
+  : num_threads_(num_threads)
+{
+}
+
+DblVec BasicTrustRegionSQPUtilFunctionsThreaded::evaluateCosts(const std::vector<Cost::Ptr>& costs,
+                                                               const DblVec& x) const
+{
+  DblVec out(costs.size());
+#pragma omp parallel for num_threads(num_threads_)
+  for (size_t i = 0; i < costs.size(); ++i)
+    out[i] = costs[i]->value(x);
+
+  return out;
+}
+
+DblVec
+BasicTrustRegionSQPUtilFunctionsThreaded::evaluateConstraintViols(const std::vector<Constraint::Ptr>& constraints,
+                                                                  const DblVec& x) const
+{
+  DblVec out(constraints.size());
+#pragma omp parallel for num_threads(num_threads_)
+  for (size_t i = 0; i < constraints.size(); ++i)
+    out[i] = constraints[i]->violation(x);
+
+  return out;
+}
+
+std::vector<ConvexObjective::Ptr>
+BasicTrustRegionSQPUtilFunctionsThreaded::convexifyCosts(const std::vector<Cost::Ptr>& costs,
+                                                         const DblVec& x,
+                                                         Model* model) const
+{
+  std::vector<ConvexObjective::Ptr> out(costs.size());
+#pragma omp parallel for num_threads(num_threads_)
+  for (size_t i = 0; i < costs.size(); ++i)
+    out[i] = costs[i]->convex(x, model);
+
+  return out;
+}
+
+std::vector<ConvexConstraints::Ptr>
+BasicTrustRegionSQPUtilFunctionsThreaded::convexifyConstraints(const std::vector<Constraint::Ptr>& cnts,
+                                                               const DblVec& x,
+                                                               Model* model) const
+{
+  std::vector<ConvexConstraints::Ptr> out(cnts.size());
+#pragma omp parallel for num_threads(num_threads_)
+  for (size_t i = 0; i < cnts.size(); ++i)
+    out[i] = cnts[i]->convex(x, model);
+
+  return out;
+}
+
+DblVec BasicTrustRegionSQPUtilFunctionsThreaded::evaluateModelCosts(const std::vector<ConvexObjective::Ptr>& costs,
+                                                                    const DblVec& x) const
+{
+  DblVec out(costs.size());
+#pragma omp parallel for num_threads(num_threads_)
+  for (size_t i = 0; i < costs.size(); ++i)
+    out[i] = costs[i]->value(x);
+
+  return out;
+}
+
+DblVec BasicTrustRegionSQPUtilFunctionsThreaded::evaluateModelCntViols(const std::vector<ConvexConstraints::Ptr>& cnts,
+                                                                       const DblVec& x) const
+{
+  DblVec out(cnts.size());
+#pragma omp parallel for num_threads(num_threads_)
+  for (size_t i = 0; i < cnts.size(); ++i)
+    out[i] = cnts[i]->violation(x);
+
+  return out;
+}
+
+std::vector<std::string>
+BasicTrustRegionSQPUtilFunctionsThreaded::getCostNames(const std::vector<Cost::Ptr>& costs) const
+{
+  std::vector<std::string> out(costs.size());
+#pragma omp parallel for num_threads(num_threads_)
+  for (size_t i = 0; i < costs.size(); ++i)
+    out[i] = costs[i]->name();
+
+  return out;
+}
+
+std::vector<std::string>
+BasicTrustRegionSQPUtilFunctionsThreaded::getCntNames(const std::vector<Constraint::Ptr>& cnts) const
+{
+  std::vector<std::string> out(cnts.size());
+#pragma omp parallel for num_threads(num_threads_)
+  for (size_t i = 0; i < cnts.size(); ++i)
+    out[i] = cnts[i]->name();
+  return out;
+}
+
+std::vector<std::string> BasicTrustRegionSQPUtilFunctionsThreaded::getVarNames(const VarVector& vars) const
+{
+  std::vector<std::string> out(vars.size());
+  out.reserve(vars.size());
+#pragma omp parallel for num_threads(num_threads_)
+  for (size_t i = 0; i < vars.size(); ++i)
+    out[i] = vars[i].var_rep->name;
+
+  return out;
+}
+
 // todo: use different coeffs for each constraint
 std::vector<ConvexObjective::Ptr> cntsToCosts(const std::vector<ConvexConstraints::Ptr>& cnts,
                                               const std::vector<double>& err_coeffs,
@@ -182,15 +299,23 @@ BasicTrustRegionSQPParameters::BasicTrustRegionSQPParameters()
   trust_box_size = 1e-1;
   log_results = false;
   log_dir = "/tmp";
+  num_threads = 0;
 }
 
 BasicTrustRegionSQP::BasicTrustRegionSQP(const OptProb::Ptr& prob) { ctor(prob); }
 void BasicTrustRegionSQP::setProblem(OptProb::Ptr prob) { ctor(prob); }
-
+void BasicTrustRegionSQP::setParameters(const BasicTrustRegionSQPParameters& param)
+{
+  param_ = param;
+  updateUtilsFunctions();
+}
+const BasicTrustRegionSQPParameters& BasicTrustRegionSQP::getParameters() const { return param_; }
+BasicTrustRegionSQPParameters& BasicTrustRegionSQP::getParameters() { return param_; }
 void BasicTrustRegionSQP::ctor(const OptProb::Ptr& prob)
 {
   Optimizer::setProblem(prob);
   model_ = prob->getModel();
+  updateUtilsFunctions();
 }
 
 void BasicTrustRegionSQP::adjustTrustRegion(double ratio) { setTrustRegionSize(param_.trust_box_size * ratio); }
@@ -209,6 +334,14 @@ void BasicTrustRegionSQP::setTrustBoxConstraints(const DblVec& x)
   model_->setVarBounds(vars, lbtrust, ubtrust);
 }
 
+void BasicTrustRegionSQP::updateUtilsFunctions()
+{
+  if (param_.num_threads > 1)
+    util_funcs_ = std::make_shared<BasicTrustRegionSQPUtilFunctionsThreaded>(param_.num_threads);
+  else
+    util_funcs_ = std::make_shared<BasicTrustRegionSQPUtilFunctions>();
+}
+
 #if 0
 struct MultiCritFilter {
   /**
@@ -233,8 +366,12 @@ struct MultiCritFilter {
 
 BasicTrustRegionSQPResults::BasicTrustRegionSQPResults(std::vector<std::string> var_names,
                                                        std::vector<std::string> cost_names,
-                                                       std::vector<std::string> cnt_names)
-  : var_names(std::move(var_names)), cost_names(std::move(cost_names)), cnt_names(std::move(cnt_names))
+                                                       std::vector<std::string> cnt_names,
+                                                       std::shared_ptr<BasicTrustRegionSQPUtilFunctions> util_funcs)
+  : var_names(std::move(var_names))
+  , cost_names(std::move(cost_names))
+  , cnt_names(std::move(cnt_names))
+  , util_funcs_(std::move(util_funcs))
 {
   model_var_vals.clear();
   model_cost_vals.clear();
@@ -250,7 +387,7 @@ BasicTrustRegionSQPResults::BasicTrustRegionSQPResults(std::vector<std::string>
   approx_merit_improve = 0;
   exact_merit_improve = 0;
   merit_improve_ratio = 0;
-  merit_error_coeffs = std::vector<double>(cnt_names.size(), 0);
+  merit_error_coeffs = std::vector<double>(this->cnt_names.size(), 0);
 }
 
 void BasicTrustRegionSQPResults::update(const OptResults& prev_opt_results,
@@ -264,8 +401,8 @@ void BasicTrustRegionSQPResults::update(const OptResults& prev_opt_results,
 {
   this->merit_error_coeffs = merit_error_coeffs;
   model_var_vals = model.getVarValues(model.getVars());
-  model_cost_vals = evaluateModelCosts(cost_models, model_var_vals);
-  model_cnt_viols = evaluateModelCntViols(cnt_models, model_var_vals);
+  model_cost_vals = util_funcs_->evaluateModelCosts(cost_models, model_var_vals);
+  model_cnt_viols = util_funcs_->evaluateModelCntViols(cnt_models, model_var_vals);
 
   // the n variables of the OptProb happen to be the first n variables in
   // the Model
@@ -273,7 +410,7 @@ void BasicTrustRegionSQPResults::update(const OptResults& prev_opt_results,
 
   if (util::GetLogLevel() >= util::LevelDebug)
   {
-    DblVec cnt_costs1 = evaluateModelCosts(cnt_cost_models, model_var_vals);
+    DblVec cnt_costs1 = util_funcs_->evaluateModelCosts(cnt_cost_models, model_var_vals);
     DblVec cnt_costs2 = model_cnt_viols;
     for (unsigned i = 0; i < cnt_costs2.size(); ++i)
       cnt_costs2[i] *= merit_error_coeffs[i];
@@ -284,8 +421,8 @@ void BasicTrustRegionSQPResults::update(const OptResults& prev_opt_results,
 
   old_cost_vals = prev_opt_results.cost_vals;
   old_cnt_viols = prev_opt_results.cnt_viols;
-  new_cost_vals = evaluateCosts(costs, new_x);
-  new_cnt_viols = evaluateConstraintViols(constraints, new_x);
+  new_cost_vals = util_funcs_->evaluateCosts(costs, new_x);
+  new_cnt_viols = util_funcs_->evaluateConstraintViols(constraints, new_x);
 
   old_merit = vecSum(old_cost_vals) + vecDot(old_cnt_viols, merit_error_coeffs);
   model_merit = vecSum(model_cost_vals) + vecDot(model_cnt_viols, merit_error_coeffs);
@@ -567,12 +704,12 @@ void BasicTrustRegionSQPResults::printRaw() const
 
 OptStatus BasicTrustRegionSQP::optimize()
 {
-  std::vector<std::string> var_names = getVarNames(prob_->getVars());
-  std::vector<std::string> cost_names = getCostNames(prob_->getCosts());
+  std::vector<std::string> var_names = util_funcs_->getVarNames(prob_->getVars());
+  std::vector<std::string> cost_names = util_funcs_->getCostNames(prob_->getCosts());
   std::vector<Constraint::Ptr> constraints = prob_->getConstraints();
-  std::vector<std::string> cnt_names = getCntNames(constraints);
+  std::vector<std::string> cnt_names = util_funcs_->getCntNames(constraints);
   std::vector<double> merit_error_coeffs(constraints.size(), param_.initial_merit_error_coeff);
-  BasicTrustRegionSQPResults iteration_results(var_names, cost_names, cnt_names);
+  BasicTrustRegionSQPResults iteration_results(var_names, cost_names, cnt_names, util_funcs_);
 
   std::FILE* log_solver_stream = nullptr;
   std::FILE* log_vars_stream = nullptr;
@@ -629,14 +766,14 @@ OptStatus BasicTrustRegionSQP::optimize()
       // that
       if (results_.cost_vals.empty() && results_.cnt_viols.empty())
       {  // only happens on the first iteration
-        results_.cnt_viols = evaluateConstraintViols(constraints, results_.x);
-        results_.cost_vals = evaluateCosts(prob_->getCosts(), results_.x);
+        results_.cnt_viols = util_funcs_->evaluateConstraintViols(constraints, results_.x);
+        results_.cost_vals = util_funcs_->evaluateCosts(prob_->getCosts(), results_.x);
         assert(results_.n_func_evals == 0);
         ++results_.n_func_evals;
       }
 
-      // DblVec new_cnt_viols = evaluateConstraintViols(constraints, results_.x);
-      // DblVec new_cost_vals = evaluateCosts(prob_->getCosts(), results_.x);
+      // DblVec new_cnt_viols = util_funcs_->evaluateConstraintViols(constraints, results_.x);
+      // DblVec new_cost_vals = util_funcs_->evaluateCosts(prob_->getCosts(), results_.x);
       // cout << "costs" << endl;
       // for (int i=0; i < new_cnt_viols.size(); ++i) {
       //   cout << cnt_names[i] << " " << new_cnt_viols[i] -
@@ -647,8 +784,10 @@ OptStatus BasicTrustRegionSQP::optimize()
       //   results_.cost_vals[i] << endl;
       // }
 
-      std::vector<ConvexObjective::Ptr> cost_models = convexifyCosts(prob_->getCosts(), results_.x, model_.get());
-      std::vector<ConvexConstraints::Ptr> cnt_models = convexifyConstraints(constraints, results_.x, model_.get());
+      std::vector<ConvexObjective::Ptr> cost_models =
+          util_funcs_->convexifyCosts(prob_->getCosts(), results_.x, model_.get());
+      std::vector<ConvexConstraints::Ptr> cnt_models =
+          util_funcs_->convexifyConstraints(constraints, results_.x, model_.get());
       std::vector<ConvexObjective::Ptr> cnt_cost_models = cntsToCosts(cnt_models, merit_error_coeffs, model_.get());
       model_->update();
       for (ConvexObjective::Ptr& cost : cost_models)
diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp
index f2e161ce..f882cf35 100644
--- a/trajopt_sco/src/osqp_interface.cpp
+++ b/trajopt_sco/src/osqp_interface.cpp
@@ -62,6 +62,7 @@ OSQPModel::~OSQPModel()
 
 Var OSQPModel::addVar(const std::string& name)
 {
+  std::scoped_lock lock(mutex_);
   vars_.push_back(std::make_shared<VarRep>(vars_.size(), name, this));
   lbs_.push_back(-OSQP_INFINITY);
   ubs_.push_back(OSQP_INFINITY);
@@ -70,6 +71,7 @@ Var OSQPModel::addVar(const std::string& name)
 
 Cnt OSQPModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/)
 {
+  std::scoped_lock lock(mutex_);
   cnts_.push_back(std::make_shared<CntRep>(cnts_.size(), this));
   cnt_exprs_.push_back(expr);
   cnt_types_.push_back(EQ);
@@ -78,6 +80,7 @@ Cnt OSQPModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/)
 
 Cnt OSQPModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/)
 {
+  std::scoped_lock lock(mutex_);
   cnts_.push_back(std::make_shared<CntRep>(cnts_.size(), this));
   cnt_exprs_.push_back(expr);
   cnt_types_.push_back(INEQ);
@@ -88,6 +91,7 @@ Cnt OSQPModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/) { throw
 
 void OSQPModel::removeVars(const VarVector& vars)
 {
+  std::scoped_lock lock(mutex_);
   SizeTVec inds;
   vars2inds(vars, inds);
   for (const auto& var : vars)
@@ -96,6 +100,7 @@ void OSQPModel::removeVars(const VarVector& vars)
 
 void OSQPModel::removeCnts(const CntVector& cnts)
 {
+  std::scoped_lock lock(mutex_);
   SizeTVec inds;
   cnts2inds(cnts, inds);
   for (const auto& cnt : cnts)
diff --git a/trajopt_sco/src/qpoases_interface.cpp b/trajopt_sco/src/qpoases_interface.cpp
index c202e0bd..876f75c4 100644
--- a/trajopt_sco/src/qpoases_interface.cpp
+++ b/trajopt_sco/src/qpoases_interface.cpp
@@ -38,6 +38,7 @@ qpOASESModel::qpOASESModel()
 qpOASESModel::~qpOASESModel() = default;
 Var qpOASESModel::addVar(const std::string& name)
 {
+  std::scoped_lock lock(mutex_);
   vars_.push_back(std::make_shared<VarRep>(vars_.size(), name, this));
   lb_.push_back(-QPOASES_INFTY);
   ub_.push_back(QPOASES_INFTY);
@@ -46,6 +47,7 @@ Var qpOASESModel::addVar(const std::string& name)
 
 Cnt qpOASESModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/)
 {
+  std::scoped_lock lock(mutex_);
   cnts_.push_back(std::make_shared<CntRep>(cnts_.size(), this));
   cnt_exprs_.push_back(expr);
   cnt_types_.push_back(EQ);
@@ -54,6 +56,7 @@ Cnt qpOASESModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/)
 
 Cnt qpOASESModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/)
 {
+  std::scoped_lock lock(mutex_);
   cnts_.push_back(std::make_shared<CntRep>(cnts_.size(), this));
   cnt_exprs_.push_back(expr);
   cnt_types_.push_back(INEQ);
@@ -68,6 +71,7 @@ Cnt qpOASESModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/)
 
 void qpOASESModel::removeVars(const VarVector& vars)
 {
+  std::scoped_lock lock(mutex_);
   IntVec inds;
   vars2inds(vars, inds);
   for (const auto& var : vars)
@@ -76,6 +80,7 @@ void qpOASESModel::removeVars(const VarVector& vars)
 
 void qpOASESModel::removeCnts(const CntVector& cnts)
 {
+  std::scoped_lock lock(mutex_);
   IntVec inds;
   cnts2inds(cnts, inds);
   for (const auto& cnt : cnts)