- Added new TrajoptIfopt solver profiles

- Fixed car_seat_example - Added debug option to most examples
tesseract-robotics · Jan 9, 2024 · 445f67b · 445f67b
1 parent 1a3e039
commit 445f67b
Show file tree

Hide file tree

Showing 11 changed files with 106 additions and 40 deletions.
diff --git a/tesseract_examples/include/tesseract_examples/car_seat_example.h b/tesseract_examples/include/tesseract_examples/car_seat_example.h
@@ -44,7 +44,8 @@ class CarSeatExample : public Example
 public:
   CarSeatExample(tesseract_environment::Environment::Ptr env,
                  tesseract_visualization::Visualization::Ptr plotter = nullptr,
-                 bool ifopt = false);
+                 bool ifopt = false,
+                 bool debug = false);
   ~CarSeatExample() override = default;
   CarSeatExample(const CarSeatExample&) = default;
   CarSeatExample& operator=(const CarSeatExample&) = default;
@@ -55,6 +56,7 @@ class CarSeatExample : public Example
 
 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/pick_and_place_example.h b/tesseract_examples/include/tesseract_examples/pick_and_place_example.h
@@ -46,6 +46,7 @@ class PickAndPlaceExample : public Example
   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;
@@ -58,6 +59,7 @@ class PickAndPlaceExample : public Example
 
 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
@@ -48,7 +48,8 @@ class PuzzlePieceAuxillaryAxesExample : public Example
 public:
   PuzzlePieceAuxillaryAxesExample(tesseract_environment::Environment::Ptr env,
                                   tesseract_visualization::Visualization::Ptr plotter = nullptr,
-                                  bool ifopt = false);
+                                  bool ifopt = false,
+                                  bool debug = false);
   ~PuzzlePieceAuxillaryAxesExample() override = default;
   PuzzlePieceAuxillaryAxesExample(const PuzzlePieceAuxillaryAxesExample&) = default;
   PuzzlePieceAuxillaryAxesExample& operator=(const PuzzlePieceAuxillaryAxesExample&) = default;
@@ -59,6 +60,7 @@ class PuzzlePieceAuxillaryAxesExample : public Example
 
 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
@@ -45,7 +45,8 @@ class PuzzlePieceExample : public Example
 public:
   PuzzlePieceExample(tesseract_environment::Environment::Ptr env,
                      tesseract_visualization::Visualization::Ptr plotter = nullptr,
-                     bool ifopt = false);
+                     bool ifopt = false,
+                     bool debug = false);
   ~PuzzlePieceExample() override = default;
   PuzzlePieceExample(const PuzzlePieceExample&) = default;
   PuzzlePieceExample& operator=(const PuzzlePieceExample&) = default;
@@ -56,6 +57,7 @@ class PuzzlePieceExample : public Example
 
 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
@@ -49,6 +49,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #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>
 
@@ -116,8 +117,9 @@ Commands addSeats(const tesseract_common::ResourceLocator::ConstPtr& locator)
 
 CarSeatExample::CarSeatExample(tesseract_environment::Environment::Ptr env,
                                tesseract_visualization::Visualization::Ptr plotter,
-                               bool ifopt)
-  : Example(std::move(env), std::move(plotter)), ifopt_(ifopt)
+                               bool ifopt,
+                               bool debug)
+  : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), debug_(debug)
 {
 }
 
@@ -219,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);
@@ -256,19 +261,19 @@ bool CarSeatExample::run()
     // 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_DISCRETE;
+        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_DISCRETE;
+        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(5);
+        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;
@@ -281,10 +286,17 @@ bool CarSeatExample::run()
     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
   {

diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp
@@ -103,6 +103,8 @@ 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();

diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp
@@ -140,6 +140,8 @@ bool GlassUprightExample::run()
 
   if (debug_)
     console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
+  else
+    console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO);
 
   // Solve Trajectory
   CONSOLE_BRIDGE_logInform("glass upright plan example");

diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp
@@ -24,8 +24,6 @@
  * limitations under the License.
  */
 #include <tesseract_common/macros.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>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <json/json.h>
 #include <console_bridge/console.h>
@@ -36,6 +34,9 @@ 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/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
 #include <tesseract_motion_planners/core/utils.h>
 #include <tesseract_command_language/composite_instruction.h>
 #include <tesseract_command_language/state_waypoint.h>
@@ -72,9 +73,14 @@ namespace tesseract_examples
 PickAndPlaceExample::PickAndPlaceExample(tesseract_environment::Environment::Ptr env,
                                          tesseract_visualization::Visualization::Ptr plotter,
                                          bool ifopt,
+                                         bool debug,
                                          double box_size,
                                          std::array<double, 2> box_position)
-  : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), box_size_(box_size), box_position_(box_position)
+  : Example(std::move(env), std::move(plotter))
+  , ifopt_(ifopt)
+  , debug_(debug)
+  , box_size_(box_size)
+  , box_position_(box_position)
 {
 }
 
@@ -108,7 +114,10 @@ bool PickAndPlaceExample::run()
   /// SETUP ///
   /////////////
 
-  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);
 
   // Set default contact distance
   Command::Ptr cmd_default_dist = std::make_shared<tesseract_environment::ChangeCollisionMarginsCommand>(0.005);
@@ -200,16 +209,20 @@ bool PickAndPlaceExample::run()
   if (ifopt_)
   {
     // Create TrajOpt_Ifopt Profile
+    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(7);
+
     auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
     trajopt_ifopt_composite_profile->collision_constraint_config->type =
-        tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
+        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_DISCRETE;
+        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;
@@ -223,14 +236,15 @@ bool PickAndPlaceExample::run()
     trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1);
     trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.05;
 
-    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(7);
+    auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptDefaultSolverProfile>();
+    trajopt_ifopt_solver_profile->opt_info.max_iterations = 100;
 
-    profiles->addProfile<TrajOptIfoptCompositeProfile>(
-        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile);
     profiles->addProfile<TrajOptIfoptPlanProfile>(
         TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile);
+    profiles->addProfile<TrajOptIfoptCompositeProfile>(
+        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile);
+    profiles->addProfile<TrajOptIfoptSolverProfile>(
+        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile);
   }
   else
   {

diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
@@ -43,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #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_task_composer/planning/planning_task_composer_problem.h>
 #include <tesseract_task_composer/core/task_composer_context.h>
@@ -126,14 +127,18 @@ PuzzlePieceAuxillaryAxesExample::makePuzzleToolPoses(const tesseract_common::Res
 
 PuzzlePieceAuxillaryAxesExample::PuzzlePieceAuxillaryAxesExample(tesseract_environment::Environment::Ptr env,
                                                                  tesseract_visualization::Visualization::Ptr plotter,
-                                                                 bool ifopt)
-  : Example(std::move(env), std::move(plotter)), ifopt_(ifopt)
+                                                                 bool ifopt,
+                                                                 bool debug)
+  : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), debug_(debug)
 {
 }
 
 bool PuzzlePieceAuxillaryAxesExample::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);
 
   if (plotter_ != nullptr)
     plotter_->waitForConnection();
@@ -203,6 +208,13 @@ bool PuzzlePieceAuxillaryAxesExample::run()
   if (ifopt_)
   {
     // Create TrajOpt_Ifopt Profile
+    auto trajopt_ifopt_plan_profile = std::make_shared<TrajOptIfoptDefaultPlanProfile>();
+    trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5);
+    trajopt_ifopt_plan_profile->cartesian_coeff(3) = 2;
+    trajopt_ifopt_plan_profile->cartesian_coeff(4) = 2;
+    trajopt_ifopt_plan_profile->cartesian_coeff(5) = 0;
+    trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8);
+
     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 =
@@ -219,15 +231,18 @@ bool PuzzlePieceAuxillaryAxesExample::run()
     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);
+    auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptDefaultSolverProfile>();
+    trajopt_ifopt_solver_profile->convex_solver_settings.adaptive_rho = 0;
+    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, "DEFAULT", trajopt_ifopt_composite_profile);
     profiles->addProfile<TrajOptIfoptPlanProfile>(
         TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile);
+    profiles->addProfile<TrajOptIfoptCompositeProfile>(
+        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile);
+    profiles->addProfile<TrajOptIfoptSolverProfile>(
+        TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile);
   }
   else
   {