Adding Trajopt_Ifopt option to all examples (#389)

tesseract-robotics · Feb 17, 2024 · c791e74 · c791e74
1 parent 981dd03
commit c791e74
Show file tree

Hide file tree

Showing 24 changed files with 581 additions and 131 deletions.
diff --git a/.clang-format b/.clang-format
@@ -8,7 +8,6 @@ AllowAllParametersOfDeclarationOnNextLine: false
 AllowShortFunctionsOnASingleLine: true
 AllowShortIfStatementsOnASingleLine: false
 AllowShortLoopsOnASingleLine: false
-AllowShortLoopsOnASingleLine: false
 AlwaysBreakBeforeMultilineStrings: false
 AlwaysBreakTemplateDeclarations: true
 BinPackArguments: false

diff --git a/.github/workflows/unstable.yml b/.github/workflows/unstable.yml
@@ -18,7 +18,7 @@ on:
 
 jobs:
   ci:
-    name: ${{ matrix.distro }}
+    name: ${{ matrix.distro }}-unstable
     runs-on: ubuntu-latest
     strategy:
       fail-fast: false

diff --git a/tesseract_examples/include/tesseract_examples/car_seat_example.h b/tesseract_examples/include/tesseract_examples/car_seat_example.h
@@ -30,7 +30,6 @@
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <string>
 #include <vector>
-#include <memory>
 #include <Eigen/Geometry>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
@@ -44,7 +43,9 @@ class CarSeatExample : public Example
 {
 public:
   CarSeatExample(tesseract_environment::Environment::Ptr env,
-                 tesseract_visualization::Visualization::Ptr plotter = nullptr);
+                 tesseract_visualization::Visualization::Ptr plotter = nullptr,
+                 bool ifopt = false,
+                 bool debug = false);
   ~CarSeatExample() override = default;
   CarSeatExample(const CarSeatExample&) = default;
   CarSeatExample& operator=(const CarSeatExample&) = default;
@@ -54,6 +55,8 @@ class CarSeatExample : public Example
   bool run() override final;
 
 private:
+  bool ifopt_;
+  bool debug_;
   std::unordered_map<std::string, std::unordered_map<std::string, double>> saved_positions_;
 
   static std::unordered_map<std::string, std::unordered_map<std::string, double>> getPredefinedPosition();

diff --git a/tesseract_examples/include/tesseract_examples/freespace_hybrid_example.h b/tesseract_examples/include/tesseract_examples/freespace_hybrid_example.h
@@ -27,9 +27,6 @@
 #define TESSERACT_EXAMPLES_FREESPACE_HYBRID_EXAMPLE_H
 
 #include <tesseract_common/macros.h>
-TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
-#include <string>
-TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_examples/example.h>
 
@@ -44,6 +41,8 @@ class FreespaceHybridExample : public Example
 public:
   FreespaceHybridExample(tesseract_environment::Environment::Ptr env,
                          tesseract_visualization::Visualization::Ptr plotter = nullptr,
+                         bool ifopt = false,
+                         bool debug = false,
                          double range = 0.01,
                          double planning_time = 60.0);
   ~FreespaceHybridExample() override = default;
@@ -55,6 +54,8 @@ class FreespaceHybridExample : public Example
   bool run() override final;
 
 private:
+  bool ifopt_;
+  bool debug_;
   double range_;
   double planning_time_;
 

diff --git a/tesseract_examples/include/tesseract_examples/pick_and_place_example.h b/tesseract_examples/include/tesseract_examples/pick_and_place_example.h
@@ -45,6 +45,8 @@ class PickAndPlaceExample : public Example
 public:
   PickAndPlaceExample(tesseract_environment::Environment::Ptr env,
                       tesseract_visualization::Visualization::Ptr plotter = nullptr,
+                      bool ifopt = false,
+                      bool debug = false,
                       double box_size = 0.2,
                       std::array<double, 2> box_position = { 0.15, 0.4 });
   ~PickAndPlaceExample() override = default;
@@ -56,6 +58,8 @@ class PickAndPlaceExample : public Example
   bool run() override final;
 
 private:
+  bool ifopt_;
+  bool debug_;
   double box_size_;
   std::array<double, 2> box_position_;
   static tesseract_environment::Command::Ptr addBox(double box_x, double box_y, double box_side);

diff --git a/tesseract_examples/include/tesseract_examples/puzzle_piece_auxillary_axes_example.h b/tesseract_examples/include/tesseract_examples/puzzle_piece_auxillary_axes_example.h
@@ -47,7 +47,9 @@ class PuzzlePieceAuxillaryAxesExample : public Example
 {
 public:
   PuzzlePieceAuxillaryAxesExample(tesseract_environment::Environment::Ptr env,
-                                  tesseract_visualization::Visualization::Ptr plotter = nullptr);
+                                  tesseract_visualization::Visualization::Ptr plotter = nullptr,
+                                  bool ifopt = false,
+                                  bool debug = false);
   ~PuzzlePieceAuxillaryAxesExample() override = default;
   PuzzlePieceAuxillaryAxesExample(const PuzzlePieceAuxillaryAxesExample&) = default;
   PuzzlePieceAuxillaryAxesExample& operator=(const PuzzlePieceAuxillaryAxesExample&) = default;
@@ -57,6 +59,8 @@ class PuzzlePieceAuxillaryAxesExample : public Example
   bool run() override final;
 
 private:
+  bool ifopt_;
+  bool debug_;
   static tesseract_common::VectorIsometry3d
   makePuzzleToolPoses(const tesseract_common::ResourceLocator::ConstPtr& locator);
 };

diff --git a/tesseract_examples/include/tesseract_examples/puzzle_piece_example.h b/tesseract_examples/include/tesseract_examples/puzzle_piece_example.h
@@ -29,7 +29,6 @@
 
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
-#include <string>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/types.h>
@@ -45,7 +44,9 @@ class PuzzlePieceExample : public Example
 {
 public:
   PuzzlePieceExample(tesseract_environment::Environment::Ptr env,
-                     tesseract_visualization::Visualization::Ptr plotter = nullptr);
+                     tesseract_visualization::Visualization::Ptr plotter = nullptr,
+                     bool ifopt = false,
+                     bool debug = false);
   ~PuzzlePieceExample() override = default;
   PuzzlePieceExample(const PuzzlePieceExample&) = default;
   PuzzlePieceExample& operator=(const PuzzlePieceExample&) = default;
@@ -55,6 +56,8 @@ class PuzzlePieceExample : public Example
   bool run() override final;
 
 private:
+  bool ifopt_;
+  bool debug_;
   static tesseract_common::VectorIsometry3d
   makePuzzleToolPoses(const tesseract_common::ResourceLocator::ConstPtr& locator);
 };

diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp
@@ -152,6 +152,8 @@ bool BasicCartesianExample::run()
 
   if (debug_)
     console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
+  else
+    console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO);
 
   // Create Task Composer Plugin Factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);

diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp
@@ -47,6 +47,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
+#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
+#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
+#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
 #include <tesseract_motion_planners/core/utils.h>
 #include <tesseract_visualization/markers/toolpath_marker.h>
 
@@ -58,6 +61,7 @@ using namespace tesseract_visualization;
 using namespace tesseract_planning;
 using tesseract_common::ManipulatorInfo;
 
+static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TrajOptIfoptMotionPlannerTask";
 static const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask";
 namespace tesseract_examples
 {
@@ -112,8 +116,10 @@ Commands addSeats(const tesseract_common::ResourceLocator::ConstPtr& locator)
 }
 
 CarSeatExample::CarSeatExample(tesseract_environment::Environment::Ptr env,
-                               tesseract_visualization::Visualization::Ptr plotter)
-  : Example(std::move(env), std::move(plotter))
+                               tesseract_visualization::Visualization::Ptr plotter,
+                               bool ifopt,
+                               bool debug)
+  : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), debug_(debug)
 {
 }
 
@@ -215,7 +221,10 @@ Eigen::VectorXd CarSeatExample::getPositionVectorXd(const JointGroup& joint_grou
 
 bool CarSeatExample::run()
 {
-  console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
+  if (debug_)
+    console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
+  else
+    console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO);
 
   // Create Task Composer Plugin Factory
   const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
@@ -245,25 +254,70 @@ bool CarSeatExample::run()
   // Create Executor
   auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");
 
-  // Create TrajOpt Profile
-  auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
-  trajopt_composite_profile->collision_constraint_config.enabled = true;
-  trajopt_composite_profile->collision_constraint_config.safety_margin = 0.00;
-  trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005;
-  trajopt_composite_profile->collision_constraint_config.coeff = 10;
-  trajopt_composite_profile->collision_cost_config.safety_margin = 0.005;
-  trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
-  trajopt_composite_profile->collision_cost_config.coeff = 50;
-
-  auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
-  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;
-
   // Create profile dictionary
   auto profiles = std::make_shared<ProfileDictionary>();
-  profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile);
-  profiles->addProfile<TrajOptSolverProfile>(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile);
+  if (ifopt_)
+  {
+    // Create TrajOpt_Ifopt Profile
+    auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
+    trajopt_ifopt_composite_profile->collision_constraint_config->type =
+        tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
+    trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config =
+        tesseract_collision::ContactManagerConfig(0.00);
+    trajopt_ifopt_composite_profile->collision_constraint_config->collision_margin_buffer = 0.005;
+    trajopt_ifopt_composite_profile->collision_constraint_config->collision_coeff_data =
+        trajopt_common::CollisionCoeffData(1);
+    trajopt_ifopt_composite_profile->collision_cost_config->type =
+        tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
+    trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config =
+        tesseract_collision::ContactManagerConfig(0.005);
+    trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.01;
+    trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data =
+        trajopt_common::CollisionCoeffData(50);
+    trajopt_ifopt_composite_profile->smooth_velocities = false;
+    trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
+    trajopt_ifopt_composite_profile->smooth_accelerations = true;
+    trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1);
+    trajopt_ifopt_composite_profile->smooth_jerks = true;
+    trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1);
+    trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.1;
+
+    auto trajopt_ifopt_plan_profile = std::make_shared<TrajOptIfoptDefaultPlanProfile>();
+    trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6);
+    trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8);
+
+    auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptDefaultSolverProfile>();
+    trajopt_ifopt_solver_profile->opt_info.max_iterations = 200;
+    trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3;
+    trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
+
+    profiles->addProfile<TrajOptIfoptCompositeProfile>(
+        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile);
+    profiles->addProfile<TrajOptIfoptPlanProfile>(
+        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile);
+    profiles->addProfile<TrajOptIfoptSolverProfile>(
+        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_solver_profile);
+  }
+  else
+  {
+    // Create TrajOpt Profile
+    auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
+    trajopt_composite_profile->collision_constraint_config.enabled = true;
+    trajopt_composite_profile->collision_constraint_config.safety_margin = 0.00;
+    trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005;
+    trajopt_composite_profile->collision_constraint_config.coeff = 10;
+    trajopt_composite_profile->collision_cost_config.safety_margin = 0.005;
+    trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
+    trajopt_composite_profile->collision_cost_config.coeff = 50;
+
+    auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
+    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;
+
+    profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile);
+    profiles->addProfile<TrajOptSolverProfile>(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile);
+  }
 
   // Solve Trajectory
   CONSOLE_BRIDGE_logInform("Car Seat Demo Started");
@@ -297,7 +351,8 @@ bool CarSeatExample::run()
     CONSOLE_BRIDGE_logInform("Freespace plan to pick seat 1 example");
 
     // Create task
-    TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptPipeline");
+    const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline";
+    TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name);
     const std::string output_key = task->getOutputKeys().front();
 
     // Create Task Composer Problem
@@ -381,7 +436,8 @@ bool CarSeatExample::run()
     CONSOLE_BRIDGE_logInform("Freespace plan to pick seat 1 example");
 
     // Create task
-    TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptPipeline");
+    const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline";
+    TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name);
     const std::string output_key = task->getOutputKeys().front();
 
     // Create Task Composer Problem

diff --git a/tesseract_examples/src/car_seat_example_node.cpp b/tesseract_examples/src/car_seat_example_node.cpp
@@ -42,7 +42,7 @@ int main(int /*argc*/, char** /*argv*/)
   if (!env->init(urdf_path, srdf_path, locator))
     exit(1);
 
-  CarSeatExample example(env, nullptr);
+  CarSeatExample example(env, nullptr, false);
   if (!example.run())
     exit(1);
 }
diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp
@@ -40,6 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/utils.h>
 
 #include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
+#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
 #include <tesseract_motion_planners/core/utils.h>
 #include <tesseract_task_composer/planning/planning_task_composer_problem.h>
 #include <tesseract_task_composer/core/task_composer_context.h>
@@ -56,14 +57,21 @@ using namespace tesseract_planning;
 using tesseract_common::ManipulatorInfo;
 
 static const std::string OMPL_DEFAULT_NAMESPACE = "OMPLMotionPlannerTask";
+static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TrajOptIfoptMotionPlannerTask";
 
 namespace tesseract_examples
 {
 FreespaceHybridExample::FreespaceHybridExample(tesseract_environment::Environment::Ptr env,
                                                tesseract_visualization::Visualization::Ptr plotter,
+                                               bool ifopt,
+                                               bool debug,
                                                double range,
                                                double planning_time)
-  : Example(std::move(env), std::move(plotter)), range_(range), planning_time_(planning_time)
+  : Example(std::move(env), std::move(plotter))
+  , ifopt_(ifopt)
+  , debug_(debug)
+  , range_(range)
+  , planning_time_(planning_time)
 {
 }
 
@@ -93,6 +101,11 @@ Command::Ptr FreespaceHybridExample::addSphere()
 
 bool FreespaceHybridExample::run()
 {
+  if (debug_)
+    console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
+  else
+    console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO);
+
   // Add sphere to environment
   Command::Ptr cmd = addSphere();
   if (!env_->applyCommand(cmd))
@@ -163,19 +176,37 @@ bool FreespaceHybridExample::run()
   // Create Executor
   auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");
 
+  // Create profile dictionary
+  auto profiles = std::make_shared<ProfileDictionary>();
+
   // Create OMPL Profile
   auto ompl_profile = std::make_shared<OMPLDefaultPlanProfile>();
   auto ompl_planner_config = std::make_shared<RRTConnectConfigurator>();
   ompl_planner_config->range = range_;
   ompl_profile->planning_time = planning_time_;
   ompl_profile->planners = { ompl_planner_config, ompl_planner_config };
 
-  // Create profile dictionary
-  auto profiles = std::make_shared<ProfileDictionary>();
   profiles->addProfile<OMPLPlanProfile>(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile);
 
+  if (ifopt_)
+  {
+    // Create TrajOpt_Ifopt Profile
+    auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
+    trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config =
+        tesseract_collision::ContactManagerConfig(0.025);
+    trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config =
+        tesseract_collision::ContactManagerConfig(0.025);
+    trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
+    trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1);
+    trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1);
+
+    profiles->addProfile<TrajOptIfoptCompositeProfile>(
+        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile);
+  }
+
   // Create task
-  TaskComposerNode::UPtr task = factory.createTaskComposerNode("FreespacePipeline");
+  const std::string task_name = (ifopt_) ? "FreespaceIfoptPipeline" : "FreespacePipeline";
+  TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name);
   const std::string output_key = task->getOutputKeys().front();
 
   // Create Task Composer Problem