Add TrajOpt multi threaded support

tesseract-robotics · Jun 20, 2023 · f069770 · f069770
1 parent e3013c6
commit f069770
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 8 deletions.
diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp
@@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_examples/puzzle_piece_example.h>
+#include <tesseract_common/timer.h>
 #include <tesseract_environment/utils.h>
 #include <tesseract_command_language/composite_instruction.h>
 #include <tesseract_command_language/state_waypoint.h>
@@ -129,7 +130,7 @@ PuzzlePieceExample::PuzzlePieceExample(tesseract_environment::Environment::Ptr e
 
 bool PuzzlePieceExample::run()
 {
-  console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
+  console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO);
 
   if (plotter_ != nullptr)
     plotter_->waitForConnection();
@@ -204,6 +205,7 @@ bool PuzzlePieceExample::run()
 
   auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
   trajopt_solver_profile->convex_solver = sco::ModelType::OSQP;
+  trajopt_solver_profile->opt_info.num_threads = 0;
   trajopt_solver_profile->opt_info.max_iter = 200;
   trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
   trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
@@ -230,10 +232,15 @@ bool PuzzlePieceExample::run()
   auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, input_data, profiles);
 
   // Solve task
+  tesseract_common::Timer stopwatch;
+  stopwatch.start();
   TaskComposerInput input(std::move(problem));
   TaskComposerFuture::UPtr future = executor->run(*task, input);
   future->wait();
 
+  stopwatch.stop();
+  CONSOLE_BRIDGE_logInform("Planning took %f seconds.", stopwatch.elapsedSeconds());
+
   // Plot Process Trajectory
   if (plotter_ != nullptr && plotter_->isConnected())
   {

diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp
@@ -106,19 +106,24 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
     util::gLogLevel = util::LevelWarn;
 
   // Create optimizer
-  sco::BasicTrustRegionSQP opt(problem);
-  opt.setParameters(pci->opt_info);
+  sco::BasicTrustRegionSQP::Ptr opt;
+  if (pci->opt_info.num_threads > 1)
+    opt = std::make_shared<sco::BasicTrustRegionSQPMultiThreaded>(problem);
+  else
+    opt = std::make_shared<sco::BasicTrustRegionSQP>(problem);
+
+  opt->setParameters(pci->opt_info);
 
   // Add all callbacks
   for (const sco::Optimizer::Callback& callback : pci->callbacks)
-    opt.addCallback(callback);
+    opt->addCallback(callback);
 
   // Initialize
-  opt.initialize(trajToDblVec(problem->GetInitTraj()));
+  opt->initialize(trajToDblVec(problem->GetInitTraj()));
 
   // Optimize
-  opt.optimize();
-  if (opt.results().status != sco::OptStatus::OPT_CONVERGED)
+  opt->optimize();
+  if (opt->results().status != sco::OptStatus::OPT_CONVERGED)
   {
     response.successful = false;
     response.message = ERROR_FAILED_TO_FIND_VALID_SOLUTION;
@@ -129,7 +134,7 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
   const Eigen::MatrixX2d joint_limits = problem->GetKin()->getLimits().joint_limits;
 
   // Get the results
-  tesseract_common::TrajArray traj = getTraj(opt.x(), problem->GetVars());
+  tesseract_common::TrajArray traj = getTraj(opt->x(), problem->GetVars());
 
   // Enforce limits
   for (Eigen::Index i = 0; i < traj.rows(); i++)