Add FormatPlanningInputTask

tesseract-robotics · Nov 7, 2024 · e5f06eb · e5f06eb
1 parent e506727
commit e5f06eb
Show file tree

Hide file tree

Showing 7 changed files with 562 additions and 3 deletions.
diff --git a/tesseract_motion_planners/core/src/utils.cpp b/tesseract_motion_planners/core/src/utils.cpp
@@ -267,7 +267,8 @@ bool formatProgramHelper(CompositeInstruction& composite_instructions,
         joint_names = it->second;
       }
 
-      if (base_instruction.getWaypoint().isStateWaypoint() || base_instruction.getWaypoint().isJointWaypoint())
+      auto& wp = base_instruction.getWaypoint();
+      if (wp.isStateWaypoint() || wp.isJointWaypoint() || wp.isCartesianWaypoint())
       {
         if (formatJointPosition(joint_names, base_instruction.getWaypoint()))
           format_required = true;