Add trajopt ifopt dynamic cartesian support and fix CI

.github/workflows/code_quality.yml

@@ -29,7 +29,7 @@ jobs:
             env:
               TARGET_CMAKE_ARGS: "-DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_CODE_COVERAGE=ON -DTESSERACT_WARNINGS_AS_ERRORS=OFF"
     container:
-      image: ghcr.io/tesseract-robotics/tesseract:jammy-master
+      image: ghcr.io/tesseract-robotics/trajopt:jammy-master
       env:
         CCACHE_DIR: "$GITHUB_WORKSPACE/${{ matrix.job_type }}/.ccache"
         DEBIAN_FRONTEND: noninteractive
@@ -52,7 +52,7 @@ jobs:
       - name: Build and Tests
         uses: tesseract-robotics/colcon-action@v2
         with:
-          before-script: source /opt/tesseract/install/setup.bash
+          before-script: source /opt/tesseract/install/setup.bash && source /opt/trajopt/install/setup.bash
           ccache-prefix: ${{ matrix.job_type }}
           vcs-file: dependencies_unstable.repos
           run-tests: false

.github/workflows/nightly.yml

@@ -19,7 +19,7 @@ jobs:
       matrix:
         distro: [focal, jammy]
     container:
-      image: ghcr.io/tesseract-robotics/tesseract:${{ matrix.distro }}
+      image: ghcr.io/tesseract-robotics/trajopt:${{ matrix.distro }}
       env:
         CCACHE_DIR: "$GITHUB_WORKSPACE/${{ matrix.distro }}/.ccache"
         DEBIAN_FRONTEND: noninteractive
@@ -42,7 +42,7 @@ jobs:
       - name: Build and Tests
         uses: tesseract-robotics/colcon-action@v2
         with:
-          before-script: source /opt/tesseract/install/setup.bash
+          before-script: source /opt/tesseract/install/setup.bash && source /opt/trajopt/install/setup.bash
           ccache-prefix: ${{ matrix.distro }}
           vcs-file: dependencies.repos
           upstream-args: --cmake-args -DCMAKE_BUILD_TYPE=Release

.github/workflows/ubuntu.yml

@@ -25,7 +25,7 @@ jobs:
       matrix:
         distro: [focal, jammy]
     container:
-      image: ghcr.io/tesseract-robotics/tesseract:${{ matrix.distro }}-0.21
+      image: ghcr.io/tesseract-robotics/trajopt:${{ matrix.distro }}-0.21
       env:
         CCACHE_DIR: "$GITHUB_WORKSPACE/${{ matrix.distro }}/.ccache"
         DEBIAN_FRONTEND: noninteractive
@@ -48,7 +48,7 @@ jobs:
       - name: Build and Tests
         uses: tesseract-robotics/colcon-action@v2
         with:
-          before-script: source /opt/tesseract/install/setup.bash
+          before-script: source /opt/tesseract/install/setup.bash && source /opt/trajopt/install/setup.bash
           ccache-prefix: ${{ matrix.distro }}
           vcs-file: dependencies.repos
           upstream-args: --cmake-args -DCMAKE_BUILD_TYPE=Release

.github/workflows/unstable.yml

@@ -25,7 +25,7 @@ jobs:
       matrix:
         distro: [focal, jammy]
     container:
-      image: ghcr.io/tesseract-robotics/tesseract:${{ matrix.distro }}-master
+      image: ghcr.io/tesseract-robotics/trajopt:${{ matrix.distro }}-master
       env:
         CCACHE_DIR: "$GITHUB_WORKSPACE/${{ matrix.distro }}/.ccache"
         DEBIAN_FRONTEND: noninteractive
@@ -48,7 +48,7 @@ jobs:
       - name: Build and Tests
         uses: tesseract-robotics/colcon-action@v2
         with:
-          before-script: source /opt/tesseract/install/setup.bash
+          before-script: source /opt/tesseract/install/setup.bash && source /opt/trajopt/install/setup.bash
           ccache-prefix: ${{ matrix.distro }}
           vcs-file: dependencies_unstable.repos
           upstream-args: --cmake-args -DCMAKE_BUILD_TYPE=Release

dependencies.repos

@@ -2,10 +2,6 @@
     local-name: ros_industrial_cmake_boilerplate
     uri: https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git
     version: 0.5.4
-- git:
-    local-name: trajopt
-    uri: https://github.com/tesseract-robotics/trajopt.git
-    version: 0.7.0
 - git:
     local-name: descartes_light
     uri: https://github.com/swri-robotics/descartes_light.git

dependencies_unstable.repos

@@ -2,10 +2,6 @@
     local-name: ros_industrial_cmake_boilerplate
     uri: https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git
     version: master
-- git:
-    local-name: trajopt
-    uri: https://github.com/tesseract-robotics/trajopt.git
-    version: master
 - git:
     local-name: descartes_light
     uri: https://github.com/swri-robotics/descartes_light.git

docker/Dockerfile

@@ -1,5 +1,5 @@
 ARG TAG
-FROM ghcr.io/tesseract-robotics/tesseract:${TAG}
+FROM ghcr.io/tesseract-robotics/trajopt:${TAG}
 
 SHELL ["/bin/bash", "-c"]
 
@@ -26,6 +26,7 @@ RUN --mount=type=bind,target=${WORKSPACE_DIR}/src/tesseract_planning \
 # Bind mount the source directory so as not to unnecessarily copy source code into the docker image
 RUN --mount=type=bind,target=${WORKSPACE_DIR}/src/tesseract_planning \
   source /opt/tesseract/install/setup.bash \
+  && source /opt/trajopt/install/setup.bash \
   && cd ${WORKSPACE_DIR} \ 
   && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release \
   && rm -rf build log

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())
   {

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>

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 =

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;
   }
 }