Update trajopt ifopt to support dynamic cartesian waypoints

tesseract-robotics · Mar 7, 2024 · 87fb327 · 87fb327
1 parent 515db21
commit 87fb327
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 46 deletions.
diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
@@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_examples/puzzle_piece_auxillary_axes_example.h>
 #include <tesseract_environment/utils.h>
+#include <tesseract_common/timer.h>
 #include <tesseract_command_language/composite_instruction.h>
 #include <tesseract_command_language/state_waypoint.h>
 #include <tesseract_command_language/cartesian_waypoint.h>
@@ -287,9 +288,14 @@ bool PuzzlePieceAuxillaryAxesExample::run()
   problem->input = program;
 
   // Solve task
+  tesseract_common::Timer stopwatch;
+  stopwatch.start();
   TaskComposerFuture::UPtr future = executor->run(*task, std::move(problem));
   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_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp
@@ -30,8 +30,8 @@ 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_common/timer.h>
 #include <tesseract_command_language/composite_instruction.h>
 #include <tesseract_command_language/state_waypoint.h>
 #include <tesseract_command_language/cartesian_waypoint.h>

diff --git a/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp b/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp
@@ -25,7 +25,7 @@ TEST(TesseractExamples, PuzzlePieceAuxillaryAxesCppTrajOptExampleUnit)  // NOLIN
   EXPECT_TRUE(example.run());
 }
 
-TEST(TesseractExamples, DISABLED_PuzzlePieceAuxillaryAxesCppTrajOptIfoptExampleUnit)  // NOLINT
+TEST(TesseractExamples, PuzzlePieceAuxillaryAxesCppTrajOptIfoptExampleUnit)  // NOLINT
 {
   auto locator = std::make_shared<TesseractSupportResourceLocator>();
   tesseract_common::fs::path urdf_path =

diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp
@@ -38,7 +38,7 @@ void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem,
                                            const CartesianWaypointPoly& cartesian_waypoint,
                                            const InstructionPoly& parent_instruction,
                                            const tesseract_common::ManipulatorInfo& manip_info,
-                                           const std::vector<std::string>& active_links,
+                                           const std::vector<std::string>& /*active_links*/,
                                            int index) const
 {
   assert(parent_instruction.isMoveInstruction());
@@ -61,57 +61,38 @@ void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem,
     throw std::runtime_error("TrajOptIfoptDefaultPlanProfile: cartesian_coeff size must be 6.");
 
   trajopt_ifopt::JointPosition::ConstPtr var = problem.vars[static_cast<std::size_t>(index)];
-
-  /* Check if this cartesian waypoint is dynamic
-   * (i.e. defined relative to a frame that will move with the kinematic chain)
-   */
-  bool is_active_tcp_frame = (std::find(active_links.begin(), active_links.end(), mi.tcp_frame) != active_links.end());
-  bool is_static_working_frame =
-      (std::find(active_links.begin(), active_links.end(), mi.working_frame) == active_links.end());
-
-  if ((is_static_working_frame && is_active_tcp_frame) || (!is_active_tcp_frame && !is_static_working_frame))
+  switch (term_type)
   {
-    switch (term_type)
-    {
-      case TrajOptIfoptTermType::CONSTRAINT:
-        addCartesianPositionConstraint(*problem.nlp,
+    case TrajOptIfoptTermType::CONSTRAINT:
+      addCartesianPositionConstraint(*problem.nlp,
+                                     var,
+                                     problem.manip,
+                                     mi.tcp_frame,
+                                     mi.working_frame,
+                                     tcp_offset,
+                                     cartesian_waypoint.getTransform(),
+                                     cartesian_coeff);
+      break;
+    case TrajOptIfoptTermType::SQUARED_COST:
+      addCartesianPositionSquaredCost(*problem.nlp,
+                                      var,
+                                      problem.manip,
+                                      mi.tcp_frame,
+                                      mi.working_frame,
+                                      tcp_offset,
+                                      cartesian_waypoint.getTransform(),
+                                      cartesian_coeff);
+      break;
+    case TrajOptIfoptTermType::ABSOLUTE_COST:
+      addCartesianPositionAbsoluteCost(*problem.nlp,
                                        var,
                                        problem.manip,
                                        mi.tcp_frame,
                                        mi.working_frame,
                                        tcp_offset,
                                        cartesian_waypoint.getTransform(),
                                        cartesian_coeff);
-        break;
-      case TrajOptIfoptTermType::SQUARED_COST:
-        addCartesianPositionSquaredCost(*problem.nlp,
-                                        var,
-                                        problem.manip,
-                                        mi.tcp_frame,
-                                        mi.working_frame,
-                                        tcp_offset,
-                                        cartesian_waypoint.getTransform(),
-                                        cartesian_coeff);
-        break;
-      case TrajOptIfoptTermType::ABSOLUTE_COST:
-        addCartesianPositionAbsoluteCost(*problem.nlp,
-                                         var,
-                                         problem.manip,
-                                         mi.tcp_frame,
-                                         mi.working_frame,
-                                         tcp_offset,
-                                         cartesian_waypoint.getTransform(),
-                                         cartesian_coeff);
-        break;
-    }
-  }
-  else if (!is_static_working_frame && is_active_tcp_frame)
-  {
-    throw std::runtime_error("TrajOpt IFOPT currently does not support dynamic cartesian waypoints!");
-  }
-  else
-  {
-    throw std::runtime_error("TrajOpt, both tcp_frame and working_frame are both static!");
+      break;
   }
 }