Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add multi-threaded support to TrajOpt #333

Merged
merged 8 commits into from
Jun 20, 2023
Merged

Add multi-threaded support to TrajOpt #333

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
44ec270
Add multithreaded support to TrajOpt
Levi-Armstrong Jun 19, 2023
73676cd
fixup
Levi-Armstrong Jun 19, 2023
15d5786
Leverage inheritance
Levi-Armstrong Jun 19, 2023
d9ab7bb
fixup
Levi-Armstrong Jun 19, 2023
57336e1
fixup
Levi-Armstrong Jun 19, 2023
c043ac0
fixup
Levi-Armstrong Jun 20, 2023
d0b4c47
fixup
Levi-Armstrong Jun 20, 2023
87c622f
fixup
Levi-Armstrong Jun 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .github/workflows/focal_build.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
2 changes: 1 addition & 1 deletion .github/workflows/unstable_build.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
79 changes: 67 additions & 12 deletions trajopt/test/benchmarks/solve_benchmarks.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <benchmark/benchmark.h>
#include <algorithm>
#include <thread>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_environment/environment.h>
#include <tesseract_common/resource_locator.h>
Expand Down Expand Up @@ -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<int>(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<int>(std::thread::hardware_concurrency());
opt.initialize(trajToDblVec(prob->GetInitTraj()));
opt.optimize();
}
}

int main(int argc, char** argv)
{
gLogLevel = util::LevelError;
Expand All @@ -79,12 +111,23 @@ int main(int argc, char** argv)
ipos["spherebot_y_joint"] = 0.75;
env->setState(ipos);

std::function<void(benchmark::State&, Environment::Ptr, Json::Value)> 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<void(benchmark::State&, Environment::Ptr, Json::Value)> 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<void(benchmark::State&, Environment::Ptr, Json::Value)> 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);
}
}

//////////////////////////////////////
Expand All @@ -110,12 +153,24 @@ int main(int argc, char** argv)
ipos["r_wrist_roll_joint"] = 3.074;
env->setState(ipos);

std::function<void(benchmark::State&, Environment::Ptr, Json::Value)> 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<void(benchmark::State&, Environment::Ptr, Json::Value)> 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<void(benchmark::State&, Environment::Ptr, Json::Value)> 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);
Expand Down
80 changes: 60 additions & 20 deletions trajopt/test/cast_cost_attached_unit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,22 +99,22 @@ class CastAttachedTest : public testing::TestWithParam<const char*>
}
};

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<ChangeLinkCollisionEnabledCommand>("box_attached", true));
env->applyCommand(std::make_shared<ChangeLinkCollisionEnabledCommand>("box_attached", true));

Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/box_cast_test.json");

std::unordered_map<std::string, double> 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<ContactResultMap> collisions;
Expand All @@ -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<sco::BasicTrustRegionSQPMultiThreaded>(prob);
opt->getParameters().num_threads = 5;
}
else
{
opt = std::make_shared<sco::BasicTrustRegionSQP>(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<ChangeLinkCollisionEnabledCommand>("box_attached2", true));
env->applyCommand(std::make_shared<ChangeLinkCollisionEnabledCommand>("box_attached2", true));

Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/box_cast_test.json");

std::unordered_map<std::string, double> 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<ContactResultMap> collisions;
Expand All @@ -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<sco::BasicTrustRegionSQPMultiThreaded>(prob);
opt->getParameters().num_threads = 5;
}
else
{
opt = std::make_shared<sco::BasicTrustRegionSQP>(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);
Expand Down
38 changes: 29 additions & 9 deletions trajopt/test/cast_cost_octomap_unit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ class CastOctomapTest : public testing::TestWithParam<const char*>
}
};

TEST_F(CastOctomapTest, boxes) // NOLINT
void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, bool use_multi_threaded)
{
CONSOLE_BRIDGE_logDebug("CastOctomapTest, boxes");

Expand All @@ -103,11 +103,11 @@ TEST_F(CastOctomapTest, boxes) // NOLINT
std::unordered_map<std::string, double> 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<ContactResultMap> collisions;
Expand All @@ -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<sco::BasicTrustRegionSQPMultiThreaded>(prob);
opt->getParameters().num_threads = 5;
}
else
{
opt = std::make_shared<sco::BasicTrustRegionSQP>(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);
Expand Down
Loading
Loading