From 44ec27083d1f3c9622f91c5b6a27286daf27ac0d Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 19 Jun 2023 09:32:35 -0500 Subject: [PATCH 1/8] Add multithreaded support to TrajOpt --- trajopt_sco/CMakeLists.txt | 4 +- trajopt_sco/cmake/trajopt_sco-config.cmake.in | 1 + .../include/trajopt_sco/bpmpd_interface.hpp | 20 +- .../include/trajopt_sco/gurobi_interface.hpp | 23 +- .../include/trajopt_sco/optimizers.hpp | 76 +++++- .../include/trajopt_sco/osqp_interface.hpp | 15 +- .../include/trajopt_sco/qpoases_interface.hpp | 17 +- .../include/trajopt_sco/solver_interface.hpp | 13 + trajopt_sco/src/bpmpd_interface.cpp | 5 + trajopt_sco/src/gurobi_interface.cpp | 7 + trajopt_sco/src/optimizers.cpp | 227 ++++++++++++++---- trajopt_sco/src/osqp_interface.cpp | 5 + trajopt_sco/src/qpoases_interface.cpp | 5 + 13 files changed, 341 insertions(+), 77 deletions(-) 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_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP #include 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_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP #include /** @@ -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& costs, const DblVec& x) const; + + virtual DblVec evaluateConstraintViols(const std::vector& constraints, const DblVec& x) const; + + virtual std::vector convexifyCosts(const std::vector& costs, + const DblVec& x, + Model* model) const; + + virtual std::vector convexifyConstraints(const std::vector& cnts, + const DblVec& x, + Model* model) const; + + virtual DblVec evaluateModelCosts(const std::vector& costs, const DblVec& x) const; + + virtual DblVec evaluateModelCntViols(const std::vector& cnts, const DblVec& x) const; + + virtual std::vector getCostNames(const std::vector& costs) const; + + virtual std::vector getCntNames(const std::vector& cnts) const; + + virtual std::vector getVarNames(const VarVector& vars) const; +}; + +struct BasicTrustRegionSQPUtilFunctionsThreaded : public BasicTrustRegionSQPUtilFunctions +{ + BasicTrustRegionSQPUtilFunctionsThreaded(); + BasicTrustRegionSQPUtilFunctionsThreaded(int num_threads); + ~BasicTrustRegionSQPUtilFunctionsThreaded() override = default; + + DblVec evaluateCosts(const std::vector& costs, const DblVec& x) const override final; + + DblVec evaluateConstraintViols(const std::vector& constraints, const DblVec& x) const override final; + + std::vector convexifyCosts(const std::vector& costs, + const DblVec& x, + Model* model) const override final; + + std::vector convexifyConstraints(const std::vector& cnts, + const DblVec& x, + Model* model) const override final; + + DblVec evaluateModelCosts(const std::vector& costs, const DblVec& x) const override final; + + DblVec evaluateModelCntViols(const std::vector& cnts, const DblVec& x) const override final; + + std::vector getCostNames(const std::vector& costs) const override final; + + std::vector getCntNames(const std::vector& cnts) const override final; + + std::vector 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 var_names, std::vector cost_names, - std::vector cnt_names); + std::vector cnt_names, + std::shared_ptr 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 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 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 #include +#include TRAJOPT_IGNORE_WARNINGS_POP #include @@ -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 #include +#include TRAJOPT_IGNORE_WARNINGS_POP #include @@ -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(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(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(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(name.c_str()))); m_vars.push_back(std::make_shared(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(name.c_str()))); m_vars.push_back(std::make_shared(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(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(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(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 #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP #include @@ -34,74 +35,78 @@ std::ostream& operator<<(std::ostream& o, const OptResults& r) ////////// private utility functions for sqp ///////// ////////////////////////////////////////////////// -static DblVec evaluateCosts(const std::vector& costs, const DblVec& x) +DblVec BasicTrustRegionSQPUtilFunctions::evaluateCosts(const std::vector& 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& constraints, const DblVec& x) + +DblVec BasicTrustRegionSQPUtilFunctions::evaluateConstraintViols(const std::vector& 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 convexifyCosts(const std::vector& costs, - const DblVec& x, - Model* model) + +std::vector BasicTrustRegionSQPUtilFunctions::convexifyCosts(const std::vector& costs, + const DblVec& x, + Model* model) const { std::vector out(costs.size()); for (size_t i = 0; i < costs.size(); ++i) - { out[i] = costs[i]->convex(x, model); - } + return out; } -static std::vector convexifyConstraints(const std::vector& cnts, - const DblVec& x, - Model* model) + +std::vector +BasicTrustRegionSQPUtilFunctions::convexifyConstraints(const std::vector& cnts, + const DblVec& x, + Model* model) const { std::vector 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& costs, const DblVec& x) +DblVec BasicTrustRegionSQPUtilFunctions::evaluateModelCosts(const std::vector& 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& cnts, const DblVec& x) + +DblVec BasicTrustRegionSQPUtilFunctions::evaluateModelCntViols(const std::vector& 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 getCostNames(const std::vector& costs) +std::vector BasicTrustRegionSQPUtilFunctions::getCostNames(const std::vector& costs) const { std::vector out(costs.size()); for (size_t i = 0; i < costs.size(); ++i) out[i] = costs[i]->name(); + return out; } -static std::vector getCntNames(const std::vector& cnts) + +std::vector BasicTrustRegionSQPUtilFunctions::getCntNames(const std::vector& cnts) const { std::vector out(cnts.size()); for (size_t i = 0; i < cnts.size(); ++i) @@ -109,7 +114,7 @@ static std::vector getCntNames(const std::vector& return out; } -static std::vector getVarNames(const VarVector& vars) +std::vector BasicTrustRegionSQPUtilFunctions::getVarNames(const VarVector& vars) const { std::vector out; out.reserve(vars.size()); @@ -118,6 +123,118 @@ static std::vector getVarNames(const VarVector& vars) return out; } +BasicTrustRegionSQPUtilFunctionsThreaded::BasicTrustRegionSQPUtilFunctionsThreaded() + : BasicTrustRegionSQPUtilFunctionsThreaded(static_cast(std::thread::hardware_concurrency())) +{ +} +BasicTrustRegionSQPUtilFunctionsThreaded::BasicTrustRegionSQPUtilFunctionsThreaded(int num_threads) + : num_threads_(num_threads) +{ +} + +DblVec BasicTrustRegionSQPUtilFunctionsThreaded::evaluateCosts(const std::vector& 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& 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 +BasicTrustRegionSQPUtilFunctionsThreaded::convexifyCosts(const std::vector& costs, + const DblVec& x, + Model* model) const +{ + std::vector 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 +BasicTrustRegionSQPUtilFunctionsThreaded::convexifyConstraints(const std::vector& cnts, + const DblVec& x, + Model* model) const +{ + std::vector 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& 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& 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 +BasicTrustRegionSQPUtilFunctionsThreaded::getCostNames(const std::vector& costs) const +{ + std::vector 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 +BasicTrustRegionSQPUtilFunctionsThreaded::getCntNames(const std::vector& cnts) const +{ + std::vector 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 BasicTrustRegionSQPUtilFunctionsThreaded::getVarNames(const VarVector& vars) const +{ + std::vector 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 cntsToCosts(const std::vector& cnts, const std::vector& 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(param_.num_threads); + else + util_funcs_ = std::make_shared(); +} + #if 0 struct MultiCritFilter { /** @@ -233,8 +366,12 @@ struct MultiCritFilter { BasicTrustRegionSQPResults::BasicTrustRegionSQPResults(std::vector var_names, std::vector cost_names, - std::vector cnt_names) - : var_names(std::move(var_names)), cost_names(std::move(cost_names)), cnt_names(std::move(cnt_names)) + std::vector cnt_names, + std::shared_ptr 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 approx_merit_improve = 0; exact_merit_improve = 0; merit_improve_ratio = 0; - merit_error_coeffs = std::vector(cnt_names.size(), 0); + merit_error_coeffs = std::vector(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 var_names = getVarNames(prob_->getVars()); - std::vector cost_names = getCostNames(prob_->getCosts()); + std::vector var_names = util_funcs_->getVarNames(prob_->getVars()); + std::vector cost_names = util_funcs_->getCostNames(prob_->getCosts()); std::vector constraints = prob_->getConstraints(); - std::vector cnt_names = getCntNames(constraints); + std::vector cnt_names = util_funcs_->getCntNames(constraints); std::vector 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 cost_models = convexifyCosts(prob_->getCosts(), results_.x, model_.get()); - std::vector cnt_models = convexifyConstraints(constraints, results_.x, model_.get()); + std::vector cost_models = + util_funcs_->convexifyCosts(prob_->getCosts(), results_.x, model_.get()); + std::vector cnt_models = + util_funcs_->convexifyConstraints(constraints, results_.x, model_.get()); std::vector 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(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(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(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(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(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(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) From 73676cdc6fbd973d81104ca0355e026286893f74 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 19 Jun 2023 10:46:11 -0500 Subject: [PATCH 2/8] fixup --- .../include/trajopt_sco/sco_common.hpp | 13 +++++++ trajopt_sco/src/optimizers.cpp | 38 +++++++++---------- 2 files changed, 32 insertions(+), 19 deletions(-) diff --git a/trajopt_sco/include/trajopt_sco/sco_common.hpp b/trajopt_sco/include/trajopt_sco/sco_common.hpp index 336b46b9..574e63ec 100644 --- a/trajopt_sco/include/trajopt_sco/sco_common.hpp +++ b/trajopt_sco/include/trajopt_sco/sco_common.hpp @@ -21,6 +21,7 @@ using AffExprVector = std::vector; using QuadExprVector = std::vector; using CntVector = std::vector; +// NOLINTNEXTLINE inline double vecSum(const DblVec& v) { double out = 0; @@ -28,6 +29,8 @@ inline double vecSum(const DblVec& v) out += i; return out; } + +// NOLINTNEXTLINE inline double vecAbsSum(const DblVec& v) { double out = 0; @@ -35,8 +38,14 @@ inline double vecAbsSum(const DblVec& v) out += fabs(i); return out; } + +// NOLINTNEXTLINE inline double pospart(double x) { return (x > 0) ? x : 0; } + +// NOLINTNEXTLINE inline double sq(double x) { return x * x; } + +// NOLINTNEXTLINE inline double vecHingeSum(const DblVec& v) { double out = 0; @@ -44,7 +53,11 @@ inline double vecHingeSum(const DblVec& v) out += pospart(i); return out; } + +// NOLINTNEXTLINE inline double vecMax(const DblVec& v) { return *std::max_element(v.begin(), v.end()); } + +// NOLINTNEXTLINE inline double vecDot(const DblVec& a, const DblVec& b) { assert(a.size() == b.size()); diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index 9a31a811..43bfa0f7 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -137,8 +137,8 @@ DblVec BasicTrustRegionSQPUtilFunctionsThreaded::evaluateCosts(const std::vector { 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); + for (int i = 0; i < costs.size(); ++i) // NOLINT + out[i] = costs[i]->value(x); // NOLINT return out; } @@ -149,8 +149,8 @@ BasicTrustRegionSQPUtilFunctionsThreaded::evaluateConstraintViols(const std::vec { 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); + for (int i = 0; i < constraints.size(); ++i) // NOLINT + out[i] = constraints[i]->violation(x); // NOLINT return out; } @@ -162,8 +162,8 @@ BasicTrustRegionSQPUtilFunctionsThreaded::convexifyCosts(const std::vector 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); + for (int i = 0; i < costs.size(); ++i) // NOLINT + out[i] = costs[i]->convex(x, model); // NOLINT return out; } @@ -175,8 +175,8 @@ BasicTrustRegionSQPUtilFunctionsThreaded::convexifyConstraints(const std::vector { std::vector 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); + for (int i = 0; i < cnts.size(); ++i) // NOLINT + out[i] = cnts[i]->convex(x, model); // NOLINT return out; } @@ -186,8 +186,8 @@ DblVec BasicTrustRegionSQPUtilFunctionsThreaded::evaluateModelCosts(const std::v { 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); + for (int i = 0; i < costs.size(); ++i) // NOLINT + out[i] = costs[i]->value(x); // NOLINT return out; } @@ -197,8 +197,8 @@ DblVec BasicTrustRegionSQPUtilFunctionsThreaded::evaluateModelCntViols(const std { 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); + for (int i = 0; i < cnts.size(); ++i) // NOLINT + out[i] = cnts[i]->violation(x); // NOLINT return out; } @@ -208,8 +208,8 @@ BasicTrustRegionSQPUtilFunctionsThreaded::getCostNames(const std::vector 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(); + for (int i = 0; i < costs.size(); ++i) // NOLINT + out[i] = costs[i]->name(); // NOLINT return out; } @@ -219,18 +219,18 @@ BasicTrustRegionSQPUtilFunctionsThreaded::getCntNames(const std::vector 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(); + for (int i = 0; i < cnts.size(); ++i) // NOLINT + out[i] = cnts[i]->name(); // NOLINT + return out; } std::vector BasicTrustRegionSQPUtilFunctionsThreaded::getVarNames(const VarVector& vars) const { std::vector 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; + for (int i = 0; i < vars.size(); ++i) // NOLINT + out[i] = vars[i].var_rep->name; // NOLINT return out; } From 15d57866a2de325d50680d9c54ffcd0dd4d4305e Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 19 Jun 2023 15:38:28 -0500 Subject: [PATCH 3/8] Leverage inheritance --- trajopt/test/benchmarks/solve_benchmarks.cpp | 16 + trajopt/test/cast_cost_attached_unit.cpp | 80 +++- trajopt/test/cast_cost_octomap_unit.cpp | 38 +- trajopt/test/cast_cost_unit.cpp | 40 +- trajopt/test/cast_cost_world_unit.cpp | 38 +- trajopt/test/numerical_ik_unit.cpp | 42 +- trajopt/test/planning_unit.cpp | 36 +- trajopt/test/simple_collision_unit.cpp | 40 +- .../include/trajopt_sco/optimizers.hpp | 128 ++++-- trajopt_sco/src/optimizers.cpp | 370 ++++++++---------- 10 files changed, 512 insertions(+), 316 deletions(-) diff --git a/trajopt/test/benchmarks/solve_benchmarks.cpp b/trajopt/test/benchmarks/solve_benchmarks.cpp index f3cdc7b4..1df3d63d 100644 --- a/trajopt/test/benchmarks/solve_benchmarks.cpp +++ b/trajopt/test/benchmarks/solve_benchmarks.cpp @@ -58,6 +58,22 @@ static void BM_TRAJOPT_PLANNING_SOLVE(benchmark::State& state, Environment::Ptr } } +/** @brief Benchmark trajopt planning solve */ +static void BM_TRAJOPT_MULTI_THREADED_PLANNING_SOLVE(benchmark::State& state, Environment::Ptr env, Json::Value root) +{ + for (auto _ : state) + { + ProblemConstructionInfo pci(env); + pci.fromJson(root); + pci.basic_info.convex_solver = sco::ModelType::OSQP; + TrajOptProb::Ptr prob = ConstructProblem(pci); + sco::BasicTrustRegionSQPMultiThreaded opt(prob); + opt.getParameters().num_threads = 5; + opt.initialize(trajToDblVec(prob->GetInitTraj())); + opt.optimize(); + } +} + int main(int argc, char** argv) { gLogLevel = util::LevelError; diff --git a/trajopt/test/cast_cost_attached_unit.cpp b/trajopt/test/cast_cost_attached_unit.cpp index 6f6c4f7c..0639319b 100644 --- a/trajopt/test/cast_cost_attached_unit.cpp +++ b/trajopt/test/cast_cost_attached_unit.cpp @@ -99,22 +99,22 @@ class CastAttachedTest : public testing::TestWithParam } }; -TEST_F(CastAttachedTest, LinkWithGeom) // NOLINT +void runLinkWithGeomTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, bool use_multi_threaded) { CONSOLE_BRIDGE_logDebug("CastAttachedTest, LinkWithGeom"); - env_->applyCommand(std::make_shared("box_attached", true)); + env->applyCommand(std::make_shared("box_attached", true)); Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/box_cast_test.json"); std::unordered_map ipos; ipos["boxbot_x_joint"] = -1.9; ipos["boxbot_y_joint"] = 0; - env_->setState(ipos); + env->setState(ipos); // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env_); + TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; @@ -132,39 +132,49 @@ TEST_F(CastAttachedTest, LinkWithGeom) // NOLINT EXPECT_TRUE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Initial trajectory is in collision") : ("Initial trajectory is collision free")); - sco::BasicTrustRegionSQP opt(prob); + sco::BasicTrustRegionSQP::Ptr opt; + if (use_multi_threaded) + { + opt = std::make_shared(prob); + opt->getParameters().num_threads = 5; + } + else + { + opt = std::make_shared(prob); + } + if (plotting) - opt.addCallback(PlotCallback(plotter_)); - opt.initialize(trajToDblVec(prob->GetInitTraj())); - opt.optimize(); + opt->addCallback(PlotCallback(plotter)); + opt->initialize(trajToDblVec(prob->GetInitTraj())); + opt->optimize(); if (plotting) - plotter_->clear(); + plotter->clear(); collisions.clear(); found = checkTrajectory( - collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt.x(), prob->GetVars()), config); + collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt->x(), prob->GetVars()), config); EXPECT_FALSE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Final trajectory is in collision") : ("Final trajectory is collision free")); } -TEST_F(CastAttachedTest, LinkWithoutGeom) // NOLINT +void runLinkWithoutGeomTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, bool use_multi_threaded) { CONSOLE_BRIDGE_logDebug("CastAttachedTest, LinkWithGeom"); - env_->applyCommand(std::make_shared("box_attached2", true)); + env->applyCommand(std::make_shared("box_attached2", true)); Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/box_cast_test.json"); std::unordered_map ipos; ipos["boxbot_x_joint"] = -1.9; ipos["boxbot_y_joint"] = 0; - env_->setState(ipos); + env->setState(ipos); // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env_); + TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; @@ -182,23 +192,53 @@ TEST_F(CastAttachedTest, LinkWithoutGeom) // NOLINT EXPECT_TRUE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Initial trajectory is in collision") : ("Initial trajectory is collision free")); - sco::BasicTrustRegionSQP opt(prob); + sco::BasicTrustRegionSQP::Ptr opt; + if (use_multi_threaded) + { + opt = std::make_shared(prob); + opt->getParameters().num_threads = 5; + } + else + { + opt = std::make_shared(prob); + } + if (plotting) - opt.addCallback(PlotCallback(plotter_)); - opt.initialize(trajToDblVec(prob->GetInitTraj())); - opt.optimize(); + opt->addCallback(PlotCallback(plotter)); + opt->initialize(trajToDblVec(prob->GetInitTraj())); + opt->optimize(); if (plotting) - plotter_->clear(); + plotter->clear(); collisions.clear(); found = checkTrajectory( - collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt.x(), prob->GetVars()), config); + collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt->x(), prob->GetVars()), config); EXPECT_FALSE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Final trajectory is in collision") : ("Final trajectory is collision free")); } +TEST_F(CastAttachedTest, LinkWithGeom) // NOLINT +{ + runLinkWithGeomTest(env_, plotter_, false); +} + +TEST_F(CastAttachedTest, LinkWithGeomMultiThreaded) // NOLINT +{ + runLinkWithGeomTest(env_, plotter_, true); +} + +TEST_F(CastAttachedTest, LinkWithoutGeom) // NOLINT +{ + runLinkWithoutGeomTest(env_, plotter_, false); +} + +TEST_F(CastAttachedTest, LinkWithoutGeomMultiThreaded) // NOLINT +{ + runLinkWithoutGeomTest(env_, plotter_, false); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/trajopt/test/cast_cost_octomap_unit.cpp b/trajopt/test/cast_cost_octomap_unit.cpp index c5d78c4f..036e2c0a 100644 --- a/trajopt/test/cast_cost_octomap_unit.cpp +++ b/trajopt/test/cast_cost_octomap_unit.cpp @@ -94,7 +94,7 @@ class CastOctomapTest : public testing::TestWithParam } }; -TEST_F(CastOctomapTest, boxes) // NOLINT +void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, bool use_multi_threaded) { CONSOLE_BRIDGE_logDebug("CastOctomapTest, boxes"); @@ -103,11 +103,11 @@ TEST_F(CastOctomapTest, boxes) // NOLINT std::unordered_map ipos; ipos["boxbot_x_joint"] = -1.9; ipos["boxbot_y_joint"] = 0; - env_->setState(ipos); + env->setState(ipos); // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env_); + TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; @@ -125,23 +125,43 @@ TEST_F(CastOctomapTest, boxes) // NOLINT EXPECT_TRUE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Initial trajectory is in collision") : ("Initial trajectory is collision free")); - sco::BasicTrustRegionSQP opt(prob); + sco::BasicTrustRegionSQP::Ptr opt; + if (use_multi_threaded) + { + opt = std::make_shared(prob); + opt->getParameters().num_threads = 5; + } + else + { + opt = std::make_shared(prob); + } + if (plotting) - opt.addCallback(PlotCallback(plotter_)); - opt.initialize(trajToDblVec(prob->GetInitTraj())); - opt.optimize(); + opt->addCallback(PlotCallback(plotter)); + opt->initialize(trajToDblVec(prob->GetInitTraj())); + opt->optimize(); if (plotting) - plotter_->clear(); + plotter->clear(); collisions.clear(); found = checkTrajectory( - collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt.x(), prob->GetVars()), config); + collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt->x(), prob->GetVars()), config); EXPECT_FALSE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Final trajectory is in collision") : ("Final trajectory is collision free")); } +TEST_F(CastOctomapTest, boxes) // NOLINT +{ + runTest(env_, plotter_, false); +} + +TEST_F(CastOctomapTest, boxes_milti_threaded) // NOLINT +{ + runTest(env_, plotter_, false); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/trajopt/test/cast_cost_unit.cpp b/trajopt/test/cast_cost_unit.cpp index 51f85ce6..4c60ba0a 100644 --- a/trajopt/test/cast_cost_unit.cpp +++ b/trajopt/test/cast_cost_unit.cpp @@ -52,7 +52,7 @@ class CastTest : public testing::TestWithParam } }; -TEST_F(CastTest, boxes) // NOLINT +void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, bool use_multi_threaded) { CONSOLE_BRIDGE_logDebug("CastTest, boxes"); @@ -61,11 +61,11 @@ TEST_F(CastTest, boxes) // NOLINT std::unordered_map ipos; ipos["boxbot_x_joint"] = -1.9; ipos["boxbot_y_joint"] = 0; - env_->setState(ipos); + env->setState(ipos); // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env_); + TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; @@ -84,25 +84,45 @@ TEST_F(CastTest, boxes) // NOLINT EXPECT_TRUE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Initial trajectory is in collision") : ("Initial trajectory is collision free")); - sco::BasicTrustRegionSQP opt(prob); + sco::BasicTrustRegionSQP::Ptr opt; + if (use_multi_threaded) + { + opt = std::make_shared(prob); + opt->getParameters().num_threads = 5; + } + else + { + opt = std::make_shared(prob); + } + if (plotting) - opt.addCallback(PlotCallback(plotter_)); - opt.initialize(trajToDblVec(prob->GetInitTraj())); - opt.optimize(); + opt->addCallback(PlotCallback(plotter)); + opt->initialize(trajToDblVec(prob->GetInitTraj())); + opt->optimize(); if (plotting) - plotter_->clear(); + plotter->clear(); collisions.clear(); - std::cout << getTraj(opt.x(), prob->GetVars()) << std::endl; + std::cout << getTraj(opt->x(), prob->GetVars()) << std::endl; found = checkTrajectory( - collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt.x(), prob->GetVars()), config); + collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt->x(), prob->GetVars()), config); EXPECT_FALSE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Final trajectory is in collision") : ("Final trajectory is collision free")); } +TEST_F(CastTest, boxes) // NOLINT +{ + runTest(env_, plotter_, false); +} + +TEST_F(CastTest, boxes_multi_threaded) // NOLINT +{ + runTest(env_, plotter_, true); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/trajopt/test/cast_cost_world_unit.cpp b/trajopt/test/cast_cost_world_unit.cpp index 08d64ba9..e3afcb36 100644 --- a/trajopt/test/cast_cost_world_unit.cpp +++ b/trajopt/test/cast_cost_world_unit.cpp @@ -77,7 +77,7 @@ class CastWorldTest : public testing::TestWithParam } }; -TEST_F(CastWorldTest, boxes) // NOLINT +void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, bool use_multi_threaded) { CONSOLE_BRIDGE_logDebug("CastWorldTest, boxes"); @@ -86,11 +86,11 @@ TEST_F(CastWorldTest, boxes) // NOLINT std::unordered_map ipos; ipos["boxbot_x_joint"] = -1.9; ipos["boxbot_y_joint"] = 0; - env_->setState(ipos); + env->setState(ipos); // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env_); + TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; @@ -108,23 +108,43 @@ TEST_F(CastWorldTest, boxes) // NOLINT EXPECT_TRUE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Initial trajectory is in collision") : ("Initial trajectory is collision free")); - sco::BasicTrustRegionSQP opt(prob); + sco::BasicTrustRegionSQP::Ptr opt; + if (use_multi_threaded) + { + opt = std::make_shared(prob); + opt->getParameters().num_threads = 5; + } + else + { + opt = std::make_shared(prob); + } + if (plotting) - opt.addCallback(PlotCallback(plotter_)); - opt.initialize(trajToDblVec(prob->GetInitTraj())); - opt.optimize(); + opt->addCallback(PlotCallback(plotter)); + opt->initialize(trajToDblVec(prob->GetInitTraj())); + opt->optimize(); if (plotting) - plotter_->clear(); + plotter->clear(); collisions.clear(); found = checkTrajectory( - collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt.x(), prob->GetVars()), config); + collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt->x(), prob->GetVars()), config); EXPECT_FALSE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Final trajectory is in collision") : ("Final trajectory is collision free")); } +TEST_F(CastWorldTest, boxes) // NOLINT +{ + runTest(env_, plotter_, false); +} + +TEST_F(CastWorldTest, boxes_multi_threaded) // NOLINT +{ + runTest(env_, plotter_, true); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/trajopt/test/numerical_ik_unit.cpp b/trajopt/test/numerical_ik_unit.cpp index 6b0d3490..bd20a1c2 100644 --- a/trajopt/test/numerical_ik_unit.cpp +++ b/trajopt/test/numerical_ik_unit.cpp @@ -53,7 +53,7 @@ class NumericalIKTest : public testing::TestWithParam } }; -TEST_F(NumericalIKTest, numerical_ik1) // NOLINT +void runTest(const Environment::Ptr& env, const Visualization::Ptr& /*plotter*/, bool use_multi_threaded) { CONSOLE_BRIDGE_logDebug("NumericalIKTest, numerical_ik1"); @@ -61,35 +61,45 @@ TEST_F(NumericalIKTest, numerical_ik1) // NOLINT // plotter_->plotScene(); - ProblemConstructionInfo pci(env_); + ProblemConstructionInfo pci(env); pci.fromJson(root); pci.basic_info.convex_solver = sco::ModelType::OSQP; TrajOptProb::Ptr prob = ConstructProblem(pci); ASSERT_TRUE(!!prob); - sco::BasicTrustRegionSQP opt(prob); + sco::BasicTrustRegionSQP::Ptr opt; + if (use_multi_threaded) + { + opt = std::make_shared(prob); + opt->getParameters().num_threads = 5; + } + else + { + opt = std::make_shared(prob); + } + // if (plotting) // { - // opt.addCallback(PlotCallback(*prob, plotter_)); + // opt.addCallback(PlotCallback(*prob, plotter)); // } CONSOLE_BRIDGE_logDebug("DOF: %d", prob->GetNumDOF()); - opt.initialize(DblVec(static_cast(prob->GetNumDOF()), 0)); + opt->initialize(DblVec(static_cast(prob->GetNumDOF()), 0)); double tStart = GetClock(); - CONSOLE_BRIDGE_logDebug("Size: %d", opt.x().size()); + CONSOLE_BRIDGE_logDebug("Size: %d", opt->x().size()); std::stringstream ss; - ss << toVectorXd(opt.x()).transpose(); + ss << toVectorXd(opt->x()).transpose(); CONSOLE_BRIDGE_logDebug("Initial Vars: %s", ss.str().c_str()); Eigen::Isometry3d change_base = prob->GetEnv()->getLinkTransform(prob->GetKin()->getBaseLinkName()); - Eigen::Isometry3d initial_pose = prob->GetKin()->calcFwdKin(toVectorXd(opt.x())).at("l_gripper_tool_frame"); + Eigen::Isometry3d initial_pose = prob->GetKin()->calcFwdKin(toVectorXd(opt->x())).at("l_gripper_tool_frame"); initial_pose = change_base * initial_pose; ss = std::stringstream(); ss << initial_pose.translation().transpose(); CONSOLE_BRIDGE_logDebug("Initial Position: %s", ss.str().c_str()); - sco::OptStatus status = opt.optimize(); + sco::OptStatus status = opt->optimize(); CONSOLE_BRIDGE_logDebug("Status: %s", sco::statusToString(status).c_str()); - Eigen::Isometry3d final_pose = prob->GetKin()->calcFwdKin(toVectorXd(opt.x())).at("l_gripper_tool_frame"); + Eigen::Isometry3d final_pose = prob->GetKin()->calcFwdKin(toVectorXd(opt->x())).at("l_gripper_tool_frame"); final_pose = change_base * final_pose; Eigen::Isometry3d goal; @@ -110,12 +120,22 @@ TEST_F(NumericalIKTest, numerical_ik1) // NOLINT CONSOLE_BRIDGE_logDebug("Final Position: %s", ss.str().c_str()); ss = std::stringstream(); - ss << toVectorXd(opt.x()).transpose(); + ss << toVectorXd(opt->x()).transpose(); CONSOLE_BRIDGE_logDebug("Final Vars: ", ss.str().c_str()); CONSOLE_BRIDGE_logDebug("planning time: %.3f", GetClock() - tStart); } +TEST_F(NumericalIKTest, numerical_ik1) // NOLINT +{ + runTest(env_, plotter_, false); +} + +TEST_F(NumericalIKTest, numerical_ik1_multi_threaded) // NOLINT +{ + runTest(env_, plotter_, true); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/trajopt/test/planning_unit.cpp b/trajopt/test/planning_unit.cpp index 1e72a1fb..1217851d 100644 --- a/trajopt/test/planning_unit.cpp +++ b/trajopt/test/planning_unit.cpp @@ -55,7 +55,7 @@ class PlanningTest : public testing::TestWithParam } }; -TEST_F(PlanningTest, arm_around_table) // NOLINT +void runTest(const Environment::Ptr& env, bool use_multi_threaded) { CONSOLE_BRIDGE_logDebug("PlanningTest, arm_around_table"); @@ -70,11 +70,11 @@ TEST_F(PlanningTest, arm_around_table) // NOLINT ipos["r_forearm_roll_joint"] = -1.1; ipos["r_wrist_flex_joint"] = -1.926; ipos["r_wrist_roll_joint"] = 3.074; - env_->setState(ipos); + env->setState(ipos); // plotter_->plotScene(); - ProblemConstructionInfo pci(env_); + ProblemConstructionInfo pci(env); pci.fromJson(root); pci.basic_info.convex_solver = sco::ModelType::OSQP; TrajOptProb::Ptr prob = ConstructProblem(pci); @@ -96,21 +96,31 @@ TEST_F(PlanningTest, arm_around_table) // NOLINT EXPECT_TRUE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Initial trajectory is in collision") : ("Initial trajectory is collision free")); - sco::BasicTrustRegionSQP opt(prob); + sco::BasicTrustRegionSQP::Ptr opt; + if (use_multi_threaded) + { + opt = std::make_shared(prob); + opt->getParameters().num_threads = 5; + } + else + { + opt = std::make_shared(prob); + } + CONSOLE_BRIDGE_logDebug("DOF: %d", prob->GetNumDOF()); // if (plotting) // { // opt.addCallback(PlotCallback(*prob, plotter_)); // } - opt.initialize(trajToDblVec(prob->GetInitTraj())); + opt->initialize(trajToDblVec(prob->GetInitTraj())); double tStart = GetClock(); - sco::OptStatus status = opt.optimize(); + sco::OptStatus status = opt->optimize(); EXPECT_TRUE(status == sco::OptStatus::OPT_CONVERGED); CONSOLE_BRIDGE_logDebug("planning time: %.3f", GetClock() - tStart); double d = 0; - TrajArray traj = getTraj(opt.x(), prob->GetVars()); + TrajArray traj = getTraj(opt->x(), prob->GetVars()); for (unsigned i = 1; i < traj.rows(); ++i) { for (unsigned j = 0; j < traj.cols(); ++j) @@ -127,12 +137,22 @@ TEST_F(PlanningTest, arm_around_table) // NOLINT collisions.clear(); found = checkTrajectory( - collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt.x(), prob->GetVars()), config); + collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt->x(), prob->GetVars()), config); EXPECT_FALSE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Final trajectory is in collision") : ("Final trajectory is collision free")); } +TEST_F(PlanningTest, arm_around_table) // NOLINT +{ + runTest(env_, false); +} + +TEST_F(PlanningTest, arm_around_table_multi_threaded) // NOLINT +{ + runTest(env_, true); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/trajopt/test/simple_collision_unit.cpp b/trajopt/test/simple_collision_unit.cpp index ccfd2c60..d80bc876 100644 --- a/trajopt/test/simple_collision_unit.cpp +++ b/trajopt/test/simple_collision_unit.cpp @@ -53,7 +53,7 @@ class SimpleCollisionTest : public testing::TestWithParam } }; -TEST_F(SimpleCollisionTest, spheres) // NOLINT +void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, bool use_multi_threaded) { CONSOLE_BRIDGE_logDebug("SimpleCollisionTest, spheres"); @@ -62,11 +62,11 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT std::unordered_map ipos; ipos["spherebot_x_joint"] = -0.75; ipos["spherebot_y_joint"] = 0.75; - env_->setState(ipos); + env->setState(ipos); // plotter_->plotScene(); - TrajOptProb::Ptr prob = ConstructProblem(root, env_); + TrajOptProb::Ptr prob = ConstructProblem(root, env); ASSERT_TRUE(!!prob); std::vector collisions; @@ -85,25 +85,45 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT EXPECT_TRUE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Initial trajectory is in collision") : ("Initial trajectory is collision free")); - sco::BasicTrustRegionSQP opt(prob); + sco::BasicTrustRegionSQP::Ptr opt; + if (use_multi_threaded) + { + opt = std::make_shared(prob); + opt->getParameters().num_threads = 5; + } + else + { + opt = std::make_shared(prob); + } + if (plotting) - opt.addCallback(PlotCallback(plotter_)); - opt.initialize(trajToDblVec(prob->GetInitTraj())); - opt.optimize(); + opt->addCallback(PlotCallback(plotter)); + opt->initialize(trajToDblVec(prob->GetInitTraj())); + opt->optimize(); if (plotting) - plotter_->clear(); + plotter->clear(); collisions.clear(); - std::cout << getTraj(opt.x(), prob->GetVars()) << std::endl; + std::cout << getTraj(opt->x(), prob->GetVars()) << std::endl; found = checkTrajectory( - collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt.x(), prob->GetVars()), config); + collisions, *manager, *state_solver, prob->GetKin()->getJointNames(), getTraj(opt->x(), prob->GetVars()), config); EXPECT_FALSE(found); CONSOLE_BRIDGE_logDebug((found) ? ("Final trajectory is in collision") : ("Final trajectory is collision free")); } +TEST_F(SimpleCollisionTest, spheres) // NOLINT +{ + runTest(env_, plotter_, false); +} + +TEST_F(SimpleCollisionTest, spheres_multi_threaded) // NOLINT +{ + runTest(env_, plotter_, true); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/trajopt_sco/include/trajopt_sco/optimizers.hpp b/trajopt_sco/include/trajopt_sco/optimizers.hpp index c33cb45e..3e0b6522 100644 --- a/trajopt_sco/include/trajopt_sco/optimizers.hpp +++ b/trajopt_sco/include/trajopt_sco/optimizers.hpp @@ -184,6 +184,95 @@ struct BasicTrustRegionSQPUtilFunctionsThreaded : public BasicTrustRegionSQPUtil int num_threads_; }; +class BasicTrustRegionSQP : public Optimizer +{ + /* + * Alternates between convexifying objectives and constraints and then solving + * convex subproblem + * Uses a merit function to decide whether or not to accept the step + * merit function = objective + merit_err_coeff * | constraint_error | + * Note: sometimes the convexified objectives and constraints lead to an + * infeasible subproblem + * In that case, you should turn them into penalties and solve that problem + * (todo: implement penalty-based sqp that gracefully handles infeasible + * constraints) + */ +public: + using Ptr = std::shared_ptr; + + BasicTrustRegionSQP() = default; + BasicTrustRegionSQP(const OptProb::Ptr& prob); + void setProblem(OptProb::Ptr prob) override; + OptStatus optimize() override; + + // Parameters + void setParameters(const BasicTrustRegionSQPParameters& param); + const BasicTrustRegionSQPParameters& getParameters() const; + BasicTrustRegionSQPParameters& getParameters(); + + // Utility functions, exposed to allow overriding for multi threaded implementations + virtual DblVec evaluateCosts(const std::vector& costs, const DblVec& x) const; + + virtual DblVec evaluateConstraintViols(const std::vector& constraints, const DblVec& x) const; + + virtual std::vector convexifyCosts(const std::vector& costs, + const DblVec& x, + Model* model) const; + + virtual std::vector convexifyConstraints(const std::vector& cnts, + const DblVec& x, + Model* model) const; + + virtual DblVec evaluateModelCosts(const std::vector& costs, const DblVec& x) const; + + virtual DblVec evaluateModelCntViols(const std::vector& cnts, const DblVec& x) const; + + virtual std::vector getCostNames(const std::vector& costs) const; + + virtual std::vector getCntNames(const std::vector& cnts) const; + + virtual std::vector getVarNames(const VarVector& vars) const; + +protected: + void ctor(const OptProb::Ptr& prob); + void adjustTrustRegion(double ratio); + void setTrustRegionSize(double trust_box_size); + void setTrustBoxConstraints(const DblVec& x); + + Model::Ptr model_; + BasicTrustRegionSQPParameters param_; +}; + +class BasicTrustRegionSQPMultiThreaded : public BasicTrustRegionSQP +{ +public: + using Ptr = std::shared_ptr; + using BasicTrustRegionSQP::BasicTrustRegionSQP; + + // Utility functions, exposed to allow overriding for multi threaded implementations + DblVec evaluateCosts(const std::vector& costs, const DblVec& x) const override final; + + DblVec evaluateConstraintViols(const std::vector& constraints, const DblVec& x) const override final; + + std::vector convexifyCosts(const std::vector& costs, + const DblVec& x, + Model* model) const override final; + + std::vector convexifyConstraints(const std::vector& cnts, + const DblVec& x, + Model* model) const override final; + + DblVec evaluateModelCosts(const std::vector& costs, const DblVec& x) const override final; + + DblVec evaluateModelCntViols(const std::vector& cnts, const DblVec& x) const override final; + + std::vector getCostNames(const std::vector& costs) const override final; + + std::vector getCntNames(const std::vector& cnts) const override final; + + std::vector getVarNames(const VarVector& vars) const override final; +}; + /** * @brief This struct stores iteration results for the BasicTrustRegionSQP * @@ -246,7 +335,7 @@ struct BasicTrustRegionSQPResults BasicTrustRegionSQPResults(std::vector var_names, std::vector cost_names, std::vector cnt_names, - std::shared_ptr util_funcs); + const BasicTrustRegionSQP& parent); /** * @brief Update the structure data for a new iteration @@ -282,41 +371,6 @@ struct BasicTrustRegionSQPResults void printRaw() const; private: - std::shared_ptr util_funcs_; -}; - -class BasicTrustRegionSQP : public Optimizer -{ - /* - * Alternates between convexifying objectives and constraints and then solving - * convex subproblem - * Uses a merit function to decide whether or not to accept the step - * merit function = objective + merit_err_coeff * | constraint_error | - * Note: sometimes the convexified objectives and constraints lead to an - * infeasible subproblem - * In that case, you should turn them into penalties and solve that problem - * (todo: implement penalty-based sqp that gracefully handles infeasible - * constraints) - */ -public: - using Ptr = std::shared_ptr; - - BasicTrustRegionSQP() = default; - BasicTrustRegionSQP(const OptProb::Ptr& prob); - void setProblem(OptProb::Ptr prob) override; - void setParameters(const BasicTrustRegionSQPParameters& param); - const BasicTrustRegionSQPParameters& getParameters() const; - BasicTrustRegionSQPParameters& getParameters(); - OptStatus optimize() override; - -protected: - void ctor(const OptProb::Ptr& prob); - 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 util_funcs_; + const BasicTrustRegionSQP& parent_; }; } // namespace sco diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index 43bfa0f7..ca792222 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -31,11 +31,105 @@ std::ostream& operator<<(std::ostream& o, const OptResults& r) return o; } +// todo: use different coeffs for each constraint +std::vector cntsToCosts(const std::vector& cnts, + const std::vector& err_coeffs, + Model* model) +{ + assert(cnts.size() == err_coeffs.size()); + std::vector out; + for (std::size_t c = 0; c < cnts.size(); ++c) + { + auto obj = std::make_shared(model); + for (std::size_t idx = 0; idx < cnts[c]->eqs_.size(); ++idx) + { + const AffExpr& aff = cnts[c]->eqs_[idx]; + obj->addAbs(aff, err_coeffs[c]); + } + for (std::size_t idx = 0; idx < cnts[c]->ineqs_.size(); ++idx) + { + const AffExpr& aff = cnts[c]->ineqs_[idx]; + obj->addHinge(aff, err_coeffs[c]); + } + out.push_back(obj); + } + return out; +} + +void Optimizer::addCallback(const Callback& cb) { callbacks_.push_back(cb); } +void Optimizer::callCallbacks() +{ + for (auto& callback : callbacks_) + { + callback(prob_.get(), results_); + } +} + +void Optimizer::initialize(const DblVec& x) +{ + if (!prob_) + PRINT_AND_THROW("need to set the problem before initializing"); + if (prob_->getVars().size() != x.size()) + PRINT_AND_THROW(boost::format("initialization vector has wrong length. expected %i got %i") % + prob_->getVars().size() % x.size()); + results_.clear(); + results_.x = x; +} + +BasicTrustRegionSQPParameters::BasicTrustRegionSQPParameters() +{ + improve_ratio_threshold = 0.25; + min_trust_box_size = 1e-4; + min_approx_improve = 1e-4; + min_approx_improve_frac = static_cast(-INFINITY); + max_iter = 50; + trust_shrink_ratio = 0.1; + trust_expand_ratio = 1.5; + cnt_tolerance = 1e-4; + max_merit_coeff_increases = 5; + max_qp_solver_failures = 3; + merit_coeff_increase_ratio = 10; + max_time = static_cast(INFINITY); + initial_merit_error_coeff = 10; + inflate_constraints_individually = true; + 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; } +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(); +} + +void BasicTrustRegionSQP::adjustTrustRegion(double ratio) { setTrustRegionSize(param_.trust_box_size * ratio); } +void BasicTrustRegionSQP::setTrustRegionSize(double trust_box_size) { param_.trust_box_size = trust_box_size; } +void BasicTrustRegionSQP::setTrustBoxConstraints(const DblVec& x) +{ + const VarVector& vars = prob_->getVars(); + assert(vars.size() == x.size()); + const DblVec &lb = prob_->getLowerBounds(), ub = prob_->getUpperBounds(); + DblVec lbtrust(x.size()), ubtrust(x.size()); + for (size_t i = 0; i < x.size(); ++i) + { + lbtrust[i] = fmax(x[i] - param_.trust_box_size, lb[i]); + ubtrust[i] = fmin(x[i] + param_.trust_box_size, ub[i]); + } + model_->setVarBounds(vars, lbtrust, ubtrust); +} + ////////////////////////////////////////////////// -////////// private utility functions for sqp ///////// +////// protected utility functions for sqp ////// ////////////////////////////////////////////////// -DblVec BasicTrustRegionSQPUtilFunctions::evaluateCosts(const std::vector& costs, const DblVec& x) const +DblVec BasicTrustRegionSQP::evaluateCosts(const std::vector& costs, const DblVec& x) const { DblVec out(costs.size()); for (size_t i = 0; i < costs.size(); ++i) @@ -44,8 +138,8 @@ DblVec BasicTrustRegionSQPUtilFunctions::evaluateCosts(const std::vector& constraints, - const DblVec& x) const +DblVec BasicTrustRegionSQP::evaluateConstraintViols(const std::vector& constraints, + const DblVec& x) const { DblVec out(constraints.size()); for (size_t i = 0; i < constraints.size(); ++i) @@ -54,9 +148,9 @@ DblVec BasicTrustRegionSQPUtilFunctions::evaluateConstraintViols(const std::vect return out; } -std::vector BasicTrustRegionSQPUtilFunctions::convexifyCosts(const std::vector& costs, - const DblVec& x, - Model* model) const +std::vector BasicTrustRegionSQP::convexifyCosts(const std::vector& costs, + const DblVec& x, + Model* model) const { std::vector out(costs.size()); for (size_t i = 0; i < costs.size(); ++i) @@ -65,10 +159,9 @@ std::vector BasicTrustRegionSQPUtilFunctions::convexifyCos return out; } -std::vector -BasicTrustRegionSQPUtilFunctions::convexifyConstraints(const std::vector& cnts, - const DblVec& x, - Model* model) const +std::vector BasicTrustRegionSQP::convexifyConstraints(const std::vector& cnts, + const DblVec& x, + Model* model) const { std::vector out(cnts.size()); for (size_t i = 0; i < cnts.size(); ++i) @@ -77,8 +170,7 @@ BasicTrustRegionSQPUtilFunctions::convexifyConstraints(const std::vector& costs, - const DblVec& x) const +DblVec BasicTrustRegionSQP::evaluateModelCosts(const std::vector& costs, const DblVec& x) const { DblVec out(costs.size()); for (size_t i = 0; i < costs.size(); ++i) @@ -87,8 +179,8 @@ DblVec BasicTrustRegionSQPUtilFunctions::evaluateModelCosts(const std::vector& cnts, - const DblVec& x) const +DblVec BasicTrustRegionSQP::evaluateModelCntViols(const std::vector& cnts, + const DblVec& x) const { DblVec out(cnts.size()); for (size_t i = 0; i < cnts.size(); ++i) @@ -97,7 +189,7 @@ DblVec BasicTrustRegionSQPUtilFunctions::evaluateModelCntViols(const std::vector return out; } -std::vector BasicTrustRegionSQPUtilFunctions::getCostNames(const std::vector& costs) const +std::vector BasicTrustRegionSQP::getCostNames(const std::vector& costs) const { std::vector out(costs.size()); for (size_t i = 0; i < costs.size(); ++i) @@ -106,7 +198,7 @@ std::vector BasicTrustRegionSQPUtilFunctions::getCostNames(const st return out; } -std::vector BasicTrustRegionSQPUtilFunctions::getCntNames(const std::vector& cnts) const +std::vector BasicTrustRegionSQP::getCntNames(const std::vector& cnts) const { std::vector out(cnts.size()); for (size_t i = 0; i < cnts.size(); ++i) @@ -114,7 +206,7 @@ std::vector BasicTrustRegionSQPUtilFunctions::getCntNames(const std return out; } -std::vector BasicTrustRegionSQPUtilFunctions::getVarNames(const VarVector& vars) const +std::vector BasicTrustRegionSQP::getVarNames(const VarVector& vars) const { std::vector out; out.reserve(vars.size()); @@ -123,225 +215,104 @@ std::vector BasicTrustRegionSQPUtilFunctions::getVarNames(const Var return out; } -BasicTrustRegionSQPUtilFunctionsThreaded::BasicTrustRegionSQPUtilFunctionsThreaded() - : BasicTrustRegionSQPUtilFunctionsThreaded(static_cast(std::thread::hardware_concurrency())) -{ -} -BasicTrustRegionSQPUtilFunctionsThreaded::BasicTrustRegionSQPUtilFunctionsThreaded(int num_threads) - : num_threads_(num_threads) -{ -} - -DblVec BasicTrustRegionSQPUtilFunctionsThreaded::evaluateCosts(const std::vector& costs, - const DblVec& x) const +DblVec BasicTrustRegionSQPMultiThreaded::evaluateCosts(const std::vector& costs, const DblVec& x) const { DblVec out(costs.size()); -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < costs.size(); ++i) // NOLINT - out[i] = costs[i]->value(x); // NOLINT +#pragma omp parallel for num_threads(param_.num_threads) + for (int i = 0; i < static_cast(costs.size()); ++i) + out[static_cast(i)] = costs[static_cast(i)]->value(x); return out; } -DblVec -BasicTrustRegionSQPUtilFunctionsThreaded::evaluateConstraintViols(const std::vector& constraints, - const DblVec& x) const +DblVec BasicTrustRegionSQPMultiThreaded::evaluateConstraintViols(const std::vector& constraints, + const DblVec& x) const { DblVec out(constraints.size()); -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < constraints.size(); ++i) // NOLINT - out[i] = constraints[i]->violation(x); // NOLINT +#pragma omp parallel for num_threads(param_.num_threads) + for (int i = 0; i < static_cast(constraints.size()); ++i) + out[static_cast(i)] = constraints[static_cast(i)]->violation(x); return out; } -std::vector -BasicTrustRegionSQPUtilFunctionsThreaded::convexifyCosts(const std::vector& costs, - const DblVec& x, - Model* model) const +std::vector BasicTrustRegionSQPMultiThreaded::convexifyCosts(const std::vector& costs, + const DblVec& x, + Model* model) const { std::vector out(costs.size()); -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < costs.size(); ++i) // NOLINT - out[i] = costs[i]->convex(x, model); // NOLINT +#pragma omp parallel for num_threads(param_.num_threads) + for (int i = 0; i < static_cast(costs.size()); ++i) + out[static_cast(i)] = costs[static_cast(i)]->convex(x, model); return out; } std::vector -BasicTrustRegionSQPUtilFunctionsThreaded::convexifyConstraints(const std::vector& cnts, - const DblVec& x, - Model* model) const +BasicTrustRegionSQPMultiThreaded::convexifyConstraints(const std::vector& cnts, + const DblVec& x, + Model* model) const { std::vector out(cnts.size()); -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < cnts.size(); ++i) // NOLINT - out[i] = cnts[i]->convex(x, model); // NOLINT +#pragma omp parallel for num_threads(param_.num_threads) + for (int i = 0; i < static_cast(cnts.size()); ++i) + out[static_cast(i)] = cnts[static_cast(i)]->convex(x, model); return out; } -DblVec BasicTrustRegionSQPUtilFunctionsThreaded::evaluateModelCosts(const std::vector& costs, - const DblVec& x) const +DblVec BasicTrustRegionSQPMultiThreaded::evaluateModelCosts(const std::vector& costs, + const DblVec& x) const { DblVec out(costs.size()); -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < costs.size(); ++i) // NOLINT - out[i] = costs[i]->value(x); // NOLINT +#pragma omp parallel for num_threads(param_.num_threads) + for (int i = 0; i < static_cast(costs.size()); ++i) + out[static_cast(i)] = costs[static_cast(i)]->value(x); return out; } -DblVec BasicTrustRegionSQPUtilFunctionsThreaded::evaluateModelCntViols(const std::vector& cnts, - const DblVec& x) const +DblVec BasicTrustRegionSQPMultiThreaded::evaluateModelCntViols(const std::vector& cnts, + const DblVec& x) const { DblVec out(cnts.size()); -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < cnts.size(); ++i) // NOLINT - out[i] = cnts[i]->violation(x); // NOLINT +#pragma omp parallel for num_threads(param_.num_threads) + for (int i = 0; i < static_cast(cnts.size()); ++i) + out[static_cast(i)] = cnts[static_cast(i)]->violation(x); return out; } -std::vector -BasicTrustRegionSQPUtilFunctionsThreaded::getCostNames(const std::vector& costs) const +std::vector BasicTrustRegionSQPMultiThreaded::getCostNames(const std::vector& costs) const { std::vector out(costs.size()); -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < costs.size(); ++i) // NOLINT - out[i] = costs[i]->name(); // NOLINT +#pragma omp parallel for num_threads(param_.num_threads) + for (int i = 0; i < static_cast(costs.size()); ++i) + out[static_cast(i)] = costs[static_cast(i)]->name(); return out; } -std::vector -BasicTrustRegionSQPUtilFunctionsThreaded::getCntNames(const std::vector& cnts) const +std::vector BasicTrustRegionSQPMultiThreaded::getCntNames(const std::vector& cnts) const { std::vector out(cnts.size()); -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < cnts.size(); ++i) // NOLINT - out[i] = cnts[i]->name(); // NOLINT +#pragma omp parallel for num_threads(param_.num_threads) + for (int i = 0; i < static_cast(cnts.size()); ++i) + out[static_cast(i)] = cnts[static_cast(i)]->name(); return out; } -std::vector BasicTrustRegionSQPUtilFunctionsThreaded::getVarNames(const VarVector& vars) const +std::vector BasicTrustRegionSQPMultiThreaded::getVarNames(const VarVector& vars) const { std::vector out(vars.size()); -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < vars.size(); ++i) // NOLINT - out[i] = vars[i].var_rep->name; // NOLINT +#pragma omp parallel for num_threads(param_.num_threads) + for (int i = 0; i < static_cast(vars.size()); ++i) + out[static_cast(i)] = vars[static_cast(i)].var_rep->name; return out; } -// todo: use different coeffs for each constraint -std::vector cntsToCosts(const std::vector& cnts, - const std::vector& err_coeffs, - Model* model) -{ - assert(cnts.size() == err_coeffs.size()); - std::vector out; - for (std::size_t c = 0; c < cnts.size(); ++c) - { - auto obj = std::make_shared(model); - for (std::size_t idx = 0; idx < cnts[c]->eqs_.size(); ++idx) - { - const AffExpr& aff = cnts[c]->eqs_[idx]; - obj->addAbs(aff, err_coeffs[c]); - } - for (std::size_t idx = 0; idx < cnts[c]->ineqs_.size(); ++idx) - { - const AffExpr& aff = cnts[c]->ineqs_[idx]; - obj->addHinge(aff, err_coeffs[c]); - } - out.push_back(obj); - } - return out; -} - -void Optimizer::addCallback(const Callback& cb) { callbacks_.push_back(cb); } -void Optimizer::callCallbacks() -{ - for (auto& callback : callbacks_) - { - callback(prob_.get(), results_); - } -} - -void Optimizer::initialize(const DblVec& x) -{ - if (!prob_) - PRINT_AND_THROW("need to set the problem before initializing"); - if (prob_->getVars().size() != x.size()) - PRINT_AND_THROW(boost::format("initialization vector has wrong length. expected %i got %i") % - prob_->getVars().size() % x.size()); - results_.clear(); - results_.x = x; -} - -BasicTrustRegionSQPParameters::BasicTrustRegionSQPParameters() -{ - improve_ratio_threshold = 0.25; - min_trust_box_size = 1e-4; - min_approx_improve = 1e-4; - min_approx_improve_frac = static_cast(-INFINITY); - max_iter = 50; - trust_shrink_ratio = 0.1; - trust_expand_ratio = 1.5; - cnt_tolerance = 1e-4; - max_merit_coeff_increases = 5; - max_qp_solver_failures = 3; - merit_coeff_increase_ratio = 10; - max_time = static_cast(INFINITY); - initial_merit_error_coeff = 10; - inflate_constraints_individually = true; - 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); } -void BasicTrustRegionSQP::setTrustRegionSize(double trust_box_size) { param_.trust_box_size = trust_box_size; } -void BasicTrustRegionSQP::setTrustBoxConstraints(const DblVec& x) -{ - const VarVector& vars = prob_->getVars(); - assert(vars.size() == x.size()); - const DblVec &lb = prob_->getLowerBounds(), ub = prob_->getUpperBounds(); - DblVec lbtrust(x.size()), ubtrust(x.size()); - for (size_t i = 0; i < x.size(); ++i) - { - lbtrust[i] = fmax(x[i] - param_.trust_box_size, lb[i]); - ubtrust[i] = fmin(x[i] + param_.trust_box_size, ub[i]); - } - model_->setVarBounds(vars, lbtrust, ubtrust); -} - -void BasicTrustRegionSQP::updateUtilsFunctions() -{ - if (param_.num_threads > 1) - util_funcs_ = std::make_shared(param_.num_threads); - else - util_funcs_ = std::make_shared(); -} - #if 0 struct MultiCritFilter { /** @@ -367,11 +338,8 @@ struct MultiCritFilter { BasicTrustRegionSQPResults::BasicTrustRegionSQPResults(std::vector var_names, std::vector cost_names, std::vector cnt_names, - std::shared_ptr 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)) + const BasicTrustRegionSQP& parent) + : var_names(std::move(var_names)), cost_names(std::move(cost_names)), cnt_names(std::move(cnt_names)), parent_(parent) { model_var_vals.clear(); model_cost_vals.clear(); @@ -401,8 +369,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 = util_funcs_->evaluateModelCosts(cost_models, model_var_vals); - model_cnt_viols = util_funcs_->evaluateModelCntViols(cnt_models, model_var_vals); + model_cost_vals = parent_.evaluateModelCosts(cost_models, model_var_vals); + model_cnt_viols = parent_.evaluateModelCntViols(cnt_models, model_var_vals); // the n variables of the OptProb happen to be the first n variables in // the Model @@ -410,7 +378,7 @@ void BasicTrustRegionSQPResults::update(const OptResults& prev_opt_results, if (util::GetLogLevel() >= util::LevelDebug) { - DblVec cnt_costs1 = util_funcs_->evaluateModelCosts(cnt_cost_models, model_var_vals); + DblVec cnt_costs1 = parent_.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]; @@ -421,8 +389,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 = util_funcs_->evaluateCosts(costs, new_x); - new_cnt_viols = util_funcs_->evaluateConstraintViols(constraints, new_x); + new_cost_vals = parent_.evaluateCosts(costs, new_x); + new_cnt_viols = parent_.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); @@ -704,12 +672,12 @@ void BasicTrustRegionSQPResults::printRaw() const OptStatus BasicTrustRegionSQP::optimize() { - std::vector var_names = util_funcs_->getVarNames(prob_->getVars()); - std::vector cost_names = util_funcs_->getCostNames(prob_->getCosts()); + std::vector var_names = getVarNames(prob_->getVars()); + std::vector cost_names = getCostNames(prob_->getCosts()); std::vector constraints = prob_->getConstraints(); - std::vector cnt_names = util_funcs_->getCntNames(constraints); + std::vector cnt_names = getCntNames(constraints); std::vector merit_error_coeffs(constraints.size(), param_.initial_merit_error_coeff); - BasicTrustRegionSQPResults iteration_results(var_names, cost_names, cnt_names, util_funcs_); + BasicTrustRegionSQPResults iteration_results(var_names, cost_names, cnt_names, *this); std::FILE* log_solver_stream = nullptr; std::FILE* log_vars_stream = nullptr; @@ -766,8 +734,8 @@ OptStatus BasicTrustRegionSQP::optimize() // that if (results_.cost_vals.empty() && results_.cnt_viols.empty()) { // only happens on the first iteration - results_.cnt_viols = util_funcs_->evaluateConstraintViols(constraints, results_.x); - results_.cost_vals = util_funcs_->evaluateCosts(prob_->getCosts(), results_.x); + results_.cnt_viols = evaluateConstraintViols(constraints, results_.x); + results_.cost_vals = evaluateCosts(prob_->getCosts(), results_.x); assert(results_.n_func_evals == 0); ++results_.n_func_evals; } @@ -784,10 +752,8 @@ OptStatus BasicTrustRegionSQP::optimize() // results_.cost_vals[i] << endl; // } - std::vector cost_models = - util_funcs_->convexifyCosts(prob_->getCosts(), results_.x, model_.get()); - std::vector cnt_models = - util_funcs_->convexifyConstraints(constraints, results_.x, model_.get()); + std::vector cost_models = convexifyCosts(prob_->getCosts(), results_.x, model_.get()); + std::vector cnt_models = convexifyConstraints(constraints, results_.x, model_.get()); std::vector cnt_cost_models = cntsToCosts(cnt_models, merit_error_coeffs, model_.get()); model_->update(); for (ConvexObjective::Ptr& cost : cost_models) From d9ab7bbddfa94c01d2746104ad6f79007042a7d5 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 19 Jun 2023 15:58:22 -0500 Subject: [PATCH 4/8] fixup --- .github/workflows/focal_build.yml | 2 +- .github/workflows/unstable_build.yml | 2 +- trajopt/test/benchmarks/solve_benchmarks.cpp | 15 +++++++++++++++ 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/.github/workflows/focal_build.yml b/.github/workflows/focal_build.yml index f3b5aec7..a6e6de52 100644 --- a/.github/workflows/focal_build.yml +++ b/.github/workflows/focal_build.yml @@ -26,7 +26,7 @@ jobs: UPSTREAM_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Release" TARGET_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug -DTRAJOPT_ENABLE_TESTING=ON -DTRAJOPT_ENABLE_CLANG_TIDY=ON" AFTER_SCRIPT: 'catkin build -w $target_ws --no-deps --verbose trajopt trajopt_ifopt trajopt_sco trajopt_sqp --make-args test' - ADDITIONAL_DEBS: clang-tidy-12 + ADDITIONAL_DEBS: clang-tidy steps: - uses: actions/checkout@v1 diff --git a/.github/workflows/unstable_build.yml b/.github/workflows/unstable_build.yml index a3213da4..a7983231 100644 --- a/.github/workflows/unstable_build.yml +++ b/.github/workflows/unstable_build.yml @@ -26,7 +26,7 @@ jobs: UPSTREAM_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Release" TARGET_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug -DTRAJOPT_ENABLE_TESTING=ON -DTRAJOPT_ENABLE_CLANG_TIDY=ON" AFTER_SCRIPT: 'catkin build -w $target_ws --no-deps --verbose trajopt trajopt_ifopt trajopt_sco trajopt_sqp --make-args test' - ADDITIONAL_DEBS: clang-tidy-12 + ADDITIONAL_DEBS: clang-tidy steps: - uses: actions/checkout@v1 diff --git a/trajopt/test/benchmarks/solve_benchmarks.cpp b/trajopt/test/benchmarks/solve_benchmarks.cpp index 1df3d63d..10e5ac70 100644 --- a/trajopt/test/benchmarks/solve_benchmarks.cpp +++ b/trajopt/test/benchmarks/solve_benchmarks.cpp @@ -58,6 +58,21 @@ static void BM_TRAJOPT_PLANNING_SOLVE(benchmark::State& state, Environment::Ptr } } +/** @brief Benchmark trajopt simple collision solve */ +static void BM_TRAJOPT_MULTI_THREADED_SIMPLE_COLLISION_SOLVE(benchmark::State& state, + Environment::Ptr env, + Json::Value root) +{ + for (auto _ : state) + { + TrajOptProb::Ptr prob = ConstructProblem(root, env); + sco::BasicTrustRegionSQPMultiThreaded opt(prob); + opt.getParameters().num_threads = 5; + opt.initialize(trajToDblVec(prob->GetInitTraj())); + opt.optimize(); + } +} + /** @brief Benchmark trajopt planning solve */ static void BM_TRAJOPT_MULTI_THREADED_PLANNING_SOLVE(benchmark::State& state, Environment::Ptr env, Json::Value root) { From 57336e124a0b8d1fc878144ed045fa857c235805 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 19 Jun 2023 17:25:48 -0500 Subject: [PATCH 5/8] fixup --- trajopt/test/benchmarks/solve_benchmarks.cpp | 52 ++++++++++++++----- .../include/trajopt_sco/optimizers.hpp | 6 --- trajopt_sco/src/optimizers.cpp | 42 +++------------ 3 files changed, 44 insertions(+), 56 deletions(-) diff --git a/trajopt/test/benchmarks/solve_benchmarks.cpp b/trajopt/test/benchmarks/solve_benchmarks.cpp index 10e5ac70..0fea093a 100644 --- a/trajopt/test/benchmarks/solve_benchmarks.cpp +++ b/trajopt/test/benchmarks/solve_benchmarks.cpp @@ -2,6 +2,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include @@ -67,7 +68,7 @@ static void BM_TRAJOPT_MULTI_THREADED_SIMPLE_COLLISION_SOLVE(benchmark::State& s { TrajOptProb::Ptr prob = ConstructProblem(root, env); sco::BasicTrustRegionSQPMultiThreaded opt(prob); - opt.getParameters().num_threads = 5; + opt.getParameters().num_threads = static_cast(std::thread::hardware_concurrency()); opt.initialize(trajToDblVec(prob->GetInitTraj())); opt.optimize(); } @@ -83,7 +84,7 @@ static void BM_TRAJOPT_MULTI_THREADED_PLANNING_SOLVE(benchmark::State& state, En pci.basic_info.convex_solver = sco::ModelType::OSQP; TrajOptProb::Ptr prob = ConstructProblem(pci); sco::BasicTrustRegionSQPMultiThreaded opt(prob); - opt.getParameters().num_threads = 5; + opt.getParameters().num_threads = static_cast(std::thread::hardware_concurrency()); opt.initialize(trajToDblVec(prob->GetInitTraj())); opt.optimize(); } @@ -110,12 +111,23 @@ int main(int argc, char** argv) ipos["spherebot_y_joint"] = 0.75; env->setState(ipos); - std::function BM_SOLVE_FUNC = - BM_TRAJOPT_SIMPLE_COLLISION_SOLVE; - std::string name = "BM_TRAJOPT_SIMPLE_COLLISION_SOLVE"; - benchmark::RegisterBenchmark(name.c_str(), BM_SOLVE_FUNC, env, root) - ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMicrosecond); + { + std::function BM_SOLVE_FUNC = + BM_TRAJOPT_SIMPLE_COLLISION_SOLVE; + std::string name = "BM_TRAJOPT_SIMPLE_COLLISION_SOLVE"; + benchmark::RegisterBenchmark(name.c_str(), BM_SOLVE_FUNC, env, root) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMicrosecond); + } + + { + std::function BM_SOLVE_FUNC = + BM_TRAJOPT_MULTI_THREADED_SIMPLE_COLLISION_SOLVE; + std::string name = "BM_TRAJOPT_MULTI_THREADED_SIMPLE_COLLISION_SOLVE"; + benchmark::RegisterBenchmark(name.c_str(), BM_SOLVE_FUNC, env, root) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMicrosecond); + } } ////////////////////////////////////// @@ -141,12 +153,24 @@ int main(int argc, char** argv) ipos["r_wrist_roll_joint"] = 3.074; env->setState(ipos); - std::function BM_SOLVE_FUNC = BM_TRAJOPT_PLANNING_SOLVE; - std::string name = "BM_TRAJOPT_PLANNING_SOLVE"; - benchmark::RegisterBenchmark(name.c_str(), BM_SOLVE_FUNC, env, root) - ->UseRealTime() - ->Unit(benchmark::TimeUnit::kMillisecond) - ->MinTime(100); + { + std::function BM_SOLVE_FUNC = BM_TRAJOPT_PLANNING_SOLVE; + std::string name = "BM_TRAJOPT_PLANNING_SOLVE"; + benchmark::RegisterBenchmark(name.c_str(), BM_SOLVE_FUNC, env, root) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMillisecond) + ->MinTime(100); + } + + { + std::function BM_SOLVE_FUNC = + BM_TRAJOPT_MULTI_THREADED_PLANNING_SOLVE; + std::string name = "BM_TRAJOPT_MULTI_THREADED_PLANNING_SOLVE"; + benchmark::RegisterBenchmark(name.c_str(), BM_SOLVE_FUNC, env, root) + ->UseRealTime() + ->Unit(benchmark::TimeUnit::kMillisecond) + ->MinTime(100); + } } benchmark::Initialize(&argc, argv); diff --git a/trajopt_sco/include/trajopt_sco/optimizers.hpp b/trajopt_sco/include/trajopt_sco/optimizers.hpp index 3e0b6522..0e5bf2c8 100644 --- a/trajopt_sco/include/trajopt_sco/optimizers.hpp +++ b/trajopt_sco/include/trajopt_sco/optimizers.hpp @@ -265,12 +265,6 @@ class BasicTrustRegionSQPMultiThreaded : public BasicTrustRegionSQP DblVec evaluateModelCosts(const std::vector& costs, const DblVec& x) const override final; DblVec evaluateModelCntViols(const std::vector& cnts, const DblVec& x) const override final; - - std::vector getCostNames(const std::vector& costs) const override final; - - std::vector getCntNames(const std::vector& cnts) const override final; - - std::vector getVarNames(const VarVector& vars) const override final; }; /** diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index ca792222..e31af45a 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -218,7 +218,7 @@ std::vector BasicTrustRegionSQP::getVarNames(const VarVector& vars) DblVec BasicTrustRegionSQPMultiThreaded::evaluateCosts(const std::vector& costs, const DblVec& x) const { DblVec out(costs.size()); -#pragma omp parallel for num_threads(param_.num_threads) +#pragma omp parallel for schedule(dynamic, 1) num_threads(param_.num_threads) for (int i = 0; i < static_cast(costs.size()); ++i) out[static_cast(i)] = costs[static_cast(i)]->value(x); @@ -229,7 +229,7 @@ DblVec BasicTrustRegionSQPMultiThreaded::evaluateConstraintViols(const std::vect const DblVec& x) const { DblVec out(constraints.size()); -#pragma omp parallel for num_threads(param_.num_threads) +#pragma omp parallel for schedule(dynamic, 1) num_threads(param_.num_threads) for (int i = 0; i < static_cast(constraints.size()); ++i) out[static_cast(i)] = constraints[static_cast(i)]->violation(x); @@ -241,7 +241,7 @@ std::vector BasicTrustRegionSQPMultiThreaded::convexifyCos Model* model) const { std::vector out(costs.size()); -#pragma omp parallel for num_threads(param_.num_threads) +#pragma omp parallel for schedule(dynamic, 1) num_threads(param_.num_threads) for (int i = 0; i < static_cast(costs.size()); ++i) out[static_cast(i)] = costs[static_cast(i)]->convex(x, model); @@ -254,7 +254,7 @@ BasicTrustRegionSQPMultiThreaded::convexifyConstraints(const std::vector out(cnts.size()); -#pragma omp parallel for num_threads(param_.num_threads) +#pragma omp parallel for schedule(dynamic, 1) num_threads(param_.num_threads) for (int i = 0; i < static_cast(cnts.size()); ++i) out[static_cast(i)] = cnts[static_cast(i)]->convex(x, model); @@ -265,7 +265,7 @@ DblVec BasicTrustRegionSQPMultiThreaded::evaluateModelCosts(const std::vector(costs.size()); ++i) out[static_cast(i)] = costs[static_cast(i)]->value(x); @@ -276,43 +276,13 @@ DblVec BasicTrustRegionSQPMultiThreaded::evaluateModelCntViols(const std::vector const DblVec& x) const { DblVec out(cnts.size()); -#pragma omp parallel for num_threads(param_.num_threads) +#pragma omp parallel for schedule(dynamic, 1) num_threads(param_.num_threads) for (int i = 0; i < static_cast(cnts.size()); ++i) out[static_cast(i)] = cnts[static_cast(i)]->violation(x); return out; } -std::vector BasicTrustRegionSQPMultiThreaded::getCostNames(const std::vector& costs) const -{ - std::vector out(costs.size()); -#pragma omp parallel for num_threads(param_.num_threads) - for (int i = 0; i < static_cast(costs.size()); ++i) - out[static_cast(i)] = costs[static_cast(i)]->name(); - - return out; -} - -std::vector BasicTrustRegionSQPMultiThreaded::getCntNames(const std::vector& cnts) const -{ - std::vector out(cnts.size()); -#pragma omp parallel for num_threads(param_.num_threads) - for (int i = 0; i < static_cast(cnts.size()); ++i) - out[static_cast(i)] = cnts[static_cast(i)]->name(); - - return out; -} - -std::vector BasicTrustRegionSQPMultiThreaded::getVarNames(const VarVector& vars) const -{ - std::vector out(vars.size()); -#pragma omp parallel for num_threads(param_.num_threads) - for (int i = 0; i < static_cast(vars.size()); ++i) - out[static_cast(i)] = vars[static_cast(i)].var_rep->name; - - return out; -} - #if 0 struct MultiCritFilter { /** From c043ac0998ee6496850715a6f98e25951e18b859 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 19 Jun 2023 20:38:08 -0500 Subject: [PATCH 6/8] fixup --- trajopt_sco/src/optimizers.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index e31af45a..f4736979 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -710,8 +710,8 @@ OptStatus BasicTrustRegionSQP::optimize() ++results_.n_func_evals; } - // DblVec new_cnt_viols = util_funcs_->evaluateConstraintViols(constraints, results_.x); - // DblVec new_cost_vals = util_funcs_->evaluateCosts(prob_->getCosts(), results_.x); + // DblVec new_cnt_viols = evaluateConstraintViols(constraints, results_.x); + // DblVec new_cost_vals = 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] - From d0b4c47feb387f929bfe9701f00ea1e44fe8731b Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 19 Jun 2023 20:40:53 -0500 Subject: [PATCH 7/8] fixup --- .../include/trajopt_sco/optimizers.hpp | 59 ------------------- 1 file changed, 59 deletions(-) diff --git a/trajopt_sco/include/trajopt_sco/optimizers.hpp b/trajopt_sco/include/trajopt_sco/optimizers.hpp index 0e5bf2c8..726a99bd 100644 --- a/trajopt_sco/include/trajopt_sco/optimizers.hpp +++ b/trajopt_sco/include/trajopt_sco/optimizers.hpp @@ -125,65 +125,6 @@ struct BasicTrustRegionSQPParameters BasicTrustRegionSQPParameters(); }; -struct BasicTrustRegionSQPUtilFunctions -{ - virtual ~BasicTrustRegionSQPUtilFunctions() = default; - - virtual DblVec evaluateCosts(const std::vector& costs, const DblVec& x) const; - - virtual DblVec evaluateConstraintViols(const std::vector& constraints, const DblVec& x) const; - - virtual std::vector convexifyCosts(const std::vector& costs, - const DblVec& x, - Model* model) const; - - virtual std::vector convexifyConstraints(const std::vector& cnts, - const DblVec& x, - Model* model) const; - - virtual DblVec evaluateModelCosts(const std::vector& costs, const DblVec& x) const; - - virtual DblVec evaluateModelCntViols(const std::vector& cnts, const DblVec& x) const; - - virtual std::vector getCostNames(const std::vector& costs) const; - - virtual std::vector getCntNames(const std::vector& cnts) const; - - virtual std::vector getVarNames(const VarVector& vars) const; -}; - -struct BasicTrustRegionSQPUtilFunctionsThreaded : public BasicTrustRegionSQPUtilFunctions -{ - BasicTrustRegionSQPUtilFunctionsThreaded(); - BasicTrustRegionSQPUtilFunctionsThreaded(int num_threads); - ~BasicTrustRegionSQPUtilFunctionsThreaded() override = default; - - DblVec evaluateCosts(const std::vector& costs, const DblVec& x) const override final; - - DblVec evaluateConstraintViols(const std::vector& constraints, const DblVec& x) const override final; - - std::vector convexifyCosts(const std::vector& costs, - const DblVec& x, - Model* model) const override final; - - std::vector convexifyConstraints(const std::vector& cnts, - const DblVec& x, - Model* model) const override final; - - DblVec evaluateModelCosts(const std::vector& costs, const DblVec& x) const override final; - - DblVec evaluateModelCntViols(const std::vector& cnts, const DblVec& x) const override final; - - std::vector getCostNames(const std::vector& costs) const override final; - - std::vector getCntNames(const std::vector& cnts) const override final; - - std::vector getVarNames(const VarVector& vars) const override final; - -private: - int num_threads_; -}; - class BasicTrustRegionSQP : public Optimizer { /* From 87c622f46c1ab84e8c873ad97270fa6f47eb9ee7 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 19 Jun 2023 22:09:08 -0500 Subject: [PATCH 8/8] fixup --- .../include/trajopt_sco/optimizers.hpp | 4 +- trajopt_sco/src/optimizers.cpp | 41 ++++++++++++------- 2 files changed, 28 insertions(+), 17 deletions(-) diff --git a/trajopt_sco/include/trajopt_sco/optimizers.hpp b/trajopt_sco/include/trajopt_sco/optimizers.hpp index 726a99bd..ab4d02ff 100644 --- a/trajopt_sco/include/trajopt_sco/optimizers.hpp +++ b/trajopt_sco/include/trajopt_sco/optimizers.hpp @@ -154,7 +154,7 @@ class BasicTrustRegionSQP : public Optimizer // Utility functions, exposed to allow overriding for multi threaded implementations virtual DblVec evaluateCosts(const std::vector& costs, const DblVec& x) const; - virtual DblVec evaluateConstraintViols(const std::vector& constraints, const DblVec& x) const; + virtual DblVec evaluateConstraintViols(const std::vector& cnts, const DblVec& x) const; virtual std::vector convexifyCosts(const std::vector& costs, const DblVec& x, @@ -193,7 +193,7 @@ class BasicTrustRegionSQPMultiThreaded : public BasicTrustRegionSQP // Utility functions, exposed to allow overriding for multi threaded implementations DblVec evaluateCosts(const std::vector& costs, const DblVec& x) const override final; - DblVec evaluateConstraintViols(const std::vector& constraints, const DblVec& x) const override final; + DblVec evaluateConstraintViols(const std::vector& cnts, const DblVec& x) const override final; std::vector convexifyCosts(const std::vector& costs, const DblVec& x, diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index f4736979..6550b1ca 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -138,12 +138,11 @@ DblVec BasicTrustRegionSQP::evaluateCosts(const std::vector& costs, c return out; } -DblVec BasicTrustRegionSQP::evaluateConstraintViols(const std::vector& constraints, - const DblVec& x) const +DblVec BasicTrustRegionSQP::evaluateConstraintViols(const std::vector& cnts, const DblVec& x) const { - DblVec out(constraints.size()); - for (size_t i = 0; i < constraints.size(); ++i) - out[i] = constraints[i]->violation(x); + DblVec out(cnts.size()); + for (size_t i = 0; i < cnts.size(); ++i) + out[i] = cnts[i]->violation(x); return out; } @@ -218,20 +217,24 @@ std::vector BasicTrustRegionSQP::getVarNames(const VarVector& vars) DblVec BasicTrustRegionSQPMultiThreaded::evaluateCosts(const std::vector& costs, const DblVec& x) const { DblVec out(costs.size()); -#pragma omp parallel for schedule(dynamic, 1) num_threads(param_.num_threads) +#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, costs, x) for (int i = 0; i < static_cast(costs.size()); ++i) + { out[static_cast(i)] = costs[static_cast(i)]->value(x); + } return out; } -DblVec BasicTrustRegionSQPMultiThreaded::evaluateConstraintViols(const std::vector& constraints, +DblVec BasicTrustRegionSQPMultiThreaded::evaluateConstraintViols(const std::vector& cnts, const DblVec& x) const { - DblVec out(constraints.size()); -#pragma omp parallel for schedule(dynamic, 1) num_threads(param_.num_threads) - for (int i = 0; i < static_cast(constraints.size()); ++i) - out[static_cast(i)] = constraints[static_cast(i)]->violation(x); + DblVec out(cnts.size()); +#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, cnts, x) + for (int i = 0; i < static_cast(cnts.size()); ++i) + { + out[static_cast(i)] = cnts[static_cast(i)]->violation(x); + } return out; } @@ -241,9 +244,11 @@ std::vector BasicTrustRegionSQPMultiThreaded::convexifyCos Model* model) const { std::vector out(costs.size()); -#pragma omp parallel for schedule(dynamic, 1) num_threads(param_.num_threads) +#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, costs, x, model) for (int i = 0; i < static_cast(costs.size()); ++i) + { out[static_cast(i)] = costs[static_cast(i)]->convex(x, model); + } return out; } @@ -254,9 +259,11 @@ BasicTrustRegionSQPMultiThreaded::convexifyConstraints(const std::vector out(cnts.size()); -#pragma omp parallel for schedule(dynamic, 1) num_threads(param_.num_threads) +#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, cnts, x, model) for (int i = 0; i < static_cast(cnts.size()); ++i) + { out[static_cast(i)] = cnts[static_cast(i)]->convex(x, model); + } return out; } @@ -265,9 +272,11 @@ DblVec BasicTrustRegionSQPMultiThreaded::evaluateModelCosts(const std::vector(costs.size()); ++i) + { out[static_cast(i)] = costs[static_cast(i)]->value(x); + } return out; } @@ -276,9 +285,11 @@ DblVec BasicTrustRegionSQPMultiThreaded::evaluateModelCntViols(const std::vector const DblVec& x) const { DblVec out(cnts.size()); -#pragma omp parallel for schedule(dynamic, 1) num_threads(param_.num_threads) +#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, cnts, x) for (int i = 0; i < static_cast(cnts.size()); ++i) + { out[static_cast(i)] = cnts[static_cast(i)]->violation(x); + } return out; }