Added ifopt option to puzzle_piece_example

tesseract-robotics · Oct 13, 2023 · d849c9a · d849c9a
1 parent 72a8ac4
commit d849c9a
Show file tree

Hide file tree

Showing 2 changed files with 63 additions and 26 deletions.
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,8 @@ 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);
   ~PuzzlePieceExample() override = default;
   PuzzlePieceExample(const PuzzlePieceExample&) = default;
   PuzzlePieceExample& operator=(const PuzzlePieceExample&) = default;
@@ -55,6 +55,7 @@ class PuzzlePieceExample : public Example
   bool run() override final;
 
 private:
+  bool ifopt_;
   static tesseract_common::VectorIsometry3d
   makePuzzleToolPoses(const tesseract_common::ResourceLocator::ConstPtr& locator);
 };

diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp
@@ -42,6 +42,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
 #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/core/utils.h>
 #include <tesseract_task_composer/planning/planning_task_composer_problem.h>
 #include <tesseract_task_composer/core/task_composer_context.h>
@@ -56,6 +58,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
@@ -123,7 +126,8 @@ PuzzlePieceExample::makePuzzleToolPoses(const tesseract_common::ResourceLocator:
 }
 
 PuzzlePieceExample::PuzzlePieceExample(tesseract_environment::Environment::Ptr env,
-                                       tesseract_visualization::Visualization::Ptr plotter)
+                                       tesseract_visualization::Visualization::Ptr plotter,
+                                       bool ifopt)
   : Example(std::move(env), std::move(plotter))
 {
 }
@@ -191,33 +195,65 @@ bool PuzzlePieceExample::run()
   // Create Executor
   auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");
 
-  // Create TrajOpt Profile
-  auto trajopt_plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
-  trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10);
-  trajopt_plan_profile->cartesian_coeff(5) = 0;
-
-  auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
-  trajopt_composite_profile->collision_constraint_config.enabled = false;
-  trajopt_composite_profile->collision_cost_config.enabled = true;
-  trajopt_composite_profile->collision_cost_config.safety_margin = 0.025;
-  trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP;
-  trajopt_composite_profile->collision_cost_config.coeff = 20;
-
-  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;
-
   // Create profile dictionary
   auto profiles = std::make_shared<ProfileDictionary>();
-  profiles->addProfile<TrajOptPlanProfile>(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
-  profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);
-  profiles->addProfile<TrajOptSolverProfile>(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", 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 = nullptr;
+    trajopt_ifopt_composite_profile->collision_cost_config->type =
+        tesseract_collision::CollisionEvaluatorType::DISCRETE;
+    trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config =
+        tesseract_collision::ContactManagerConfig(0.025);
+    trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.05;
+    trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = trajopt_ifopt::CollisionCoeffData(2);
+    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);
+
+    auto trajopt_ifopt_plan_profile = std::make_shared<TrajOptIfoptDefaultPlanProfile>();
+    trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10);
+    trajopt_ifopt_plan_profile->cartesian_coeff(5) = 0;
+    trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8);
+
+    profiles->addProfile<TrajOptIfoptCompositeProfile>(
+        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile);
+    profiles->addProfile<TrajOptIfoptPlanProfile>(
+        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile);
+  }
+  else
+  {
+    // Create TrajOpt Profile
+    auto trajopt_plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
+    trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10);
+    trajopt_plan_profile->cartesian_coeff(5) = 0;
+
+    auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
+    trajopt_composite_profile->collision_constraint_config.enabled = false;
+    trajopt_composite_profile->collision_cost_config.enabled = true;
+    trajopt_composite_profile->collision_cost_config.safety_margin = 0.025;
+    trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP;
+    trajopt_composite_profile->collision_cost_config.coeff = 20;
+
+    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;
+
+    profiles->addProfile<TrajOptPlanProfile>(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
+    profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);
+    profiles->addProfile<TrajOptSolverProfile>(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile);
+  }
 
   // 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();
 
   if (plotter_ != nullptr)