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 f3cdc7b4..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 @@ -58,6 +59,37 @@ 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 = static_cast(std::thread::hardware_concurrency()); + 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) +{ + 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 = static_cast(std::thread::hardware_concurrency()); + opt.initialize(trajToDblVec(prob->GetInitTraj())); + opt.optimize(); + } +} + int main(int argc, char** argv) { gLogLevel = util::LevelError; @@ -79,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); + } } ////////////////////////////////////// @@ -110,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/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/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..ab4d02ff 100644 --- a/trajopt_sco/include/trajopt_sco/optimizers.hpp +++ b/trajopt_sco/include/trajopt_sco/optimizers.hpp @@ -119,9 +119,95 @@ 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(); }; +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& cnts, 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& cnts, 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; +}; + /** * @brief This struct stores iteration results for the BasicTrustRegionSQP * @@ -183,7 +269,8 @@ struct BasicTrustRegionSQPResults BasicTrustRegionSQPResults(std::vector var_names, std::vector cost_names, - std::vector cnt_names); + std::vector cnt_names, + const BasicTrustRegionSQP& parent); /** * @brief Update the structure data for a new iteration @@ -217,38 +304,8 @@ struct BasicTrustRegionSQPResults void writeConstraints(std::FILE* stream, bool header = false) const; /** @brief Prints the raw values to the terminal */ void printRaw() const; -}; - -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) { param_ = param; } - const BasicTrustRegionSQPParameters& getParameters() const { return param_; } - BasicTrustRegionSQPParameters& getParameters() { return param_; } - 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); - Model::Ptr model_; - BasicTrustRegionSQPParameters param_; +private: + const BasicTrustRegionSQP& parent_; }; } // 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/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/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..6550b1ca 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 @@ -30,94 +31,6 @@ std::ostream& operator<<(std::ostream& o, const OptResults& r) return o; } -////////////////////////////////////////////////// -////////// private utility functions for sqp ///////// -////////////////////////////////////////////////// - -static DblVec evaluateCosts(const std::vector& costs, const DblVec& x) -{ - 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 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 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 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 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 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 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 out(cnts.size()); - for (size_t i = 0; i < cnts.size(); ++i) - out[i] = cnts[i]->name(); - return out; -} - -static std::vector getVarNames(const VarVector& vars) -{ - std::vector out; - out.reserve(vars.size()); - for (const auto& var : vars) - out.push_back(var.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,11 +95,14 @@ 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; } +const BasicTrustRegionSQPParameters& BasicTrustRegionSQP::getParameters() const { return param_; } +BasicTrustRegionSQPParameters& BasicTrustRegionSQP::getParameters() { return param_; } void BasicTrustRegionSQP::ctor(const OptProb::Ptr& prob) { Optimizer::setProblem(prob); @@ -209,6 +125,175 @@ void BasicTrustRegionSQP::setTrustBoxConstraints(const DblVec& x) model_->setVarBounds(vars, lbtrust, ubtrust); } +////////////////////////////////////////////////// +////// protected utility functions for sqp ////// +////////////////////////////////////////////////// + +DblVec BasicTrustRegionSQP::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; +} + +DblVec BasicTrustRegionSQP::evaluateConstraintViols(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; +} + +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) + out[i] = costs[i]->convex(x, model); + + return out; +} + +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) + out[i] = cnts[i]->convex(x, model); + + return out; +} + +DblVec BasicTrustRegionSQP::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 BasicTrustRegionSQP::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; +} + +std::vector BasicTrustRegionSQP::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; +} + +std::vector BasicTrustRegionSQP::getCntNames(const std::vector& cnts) const +{ + std::vector out(cnts.size()); + for (size_t i = 0; i < cnts.size(); ++i) + out[i] = cnts[i]->name(); + return out; +} + +std::vector BasicTrustRegionSQP::getVarNames(const VarVector& vars) const +{ + std::vector out; + out.reserve(vars.size()); + for (const auto& var : vars) + out.push_back(var.var_rep->name); + return out; +} + +DblVec BasicTrustRegionSQPMultiThreaded::evaluateCosts(const std::vector& costs, const DblVec& x) const +{ + DblVec out(costs.size()); +#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& cnts, + const DblVec& x) const +{ + 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; +} + +std::vector BasicTrustRegionSQPMultiThreaded::convexifyCosts(const std::vector& costs, + const DblVec& x, + Model* model) const +{ + std::vector out(costs.size()); +#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; +} + +std::vector +BasicTrustRegionSQPMultiThreaded::convexifyConstraints(const std::vector& cnts, + const DblVec& x, + Model* model) const +{ + std::vector out(cnts.size()); +#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; +} + +DblVec BasicTrustRegionSQPMultiThreaded::evaluateModelCosts(const std::vector& costs, + const DblVec& x) const +{ + DblVec out(costs.size()); +#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::evaluateModelCntViols(const std::vector& cnts, + const DblVec& x) const +{ + 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; +} + #if 0 struct MultiCritFilter { /** @@ -233,8 +318,9 @@ 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, + 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(); @@ -250,7 +336,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 +350,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 = 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 @@ -273,7 +359,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 = 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]; @@ -284,8 +370,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 = 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); @@ -572,7 +658,7 @@ OptStatus BasicTrustRegionSQP::optimize() std::vector constraints = prob_->getConstraints(); 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); + BasicTrustRegionSQPResults iteration_results(var_names, cost_names, cnt_names, *this); std::FILE* log_solver_stream = nullptr; std::FILE* log_vars_stream = nullptr; 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)