From 87fb327a505250a4d84dfeae0875986a9bf4bb95 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 7 Mar 2024 12:06:53 -0600 Subject: [PATCH 1/2] Update trajopt ifopt to support dynamic cartesian waypoints --- .../puzzle_piece_auxillary_axes_example.cpp | 6 ++ .../src/puzzle_piece_example.cpp | 2 +- ...zzle_piece_auxillary_axes_example_unit.cpp | 2 +- .../trajopt_ifopt_default_plan_profile.cpp | 69 +++++++------------ 4 files changed, 33 insertions(+), 46 deletions(-) diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index ccb49fd73e8..c97f84ad8af 100644 --- 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 #include +#include #include #include #include @@ -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 index b1020c0ef83..8ce1f9ce76a 100644 --- 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 -#include #include +#include #include #include #include diff --git a/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp b/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp index e99a450a5e3..5ae2048da01 100644 --- 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(); 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 index 62963f892fd..f0450734465 100644 --- 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& active_links, + const std::vector& /*active_links*/, int index) const { assert(parent_instruction.isMoveInstruction()); @@ -61,20 +61,30 @@ 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(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, @@ -82,36 +92,7 @@ void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem, 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; } } From d83a991319a17b262800447b1218e67a12e05c76 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 7 Mar 2024 12:12:38 -0600 Subject: [PATCH 2/2] Use trajopt docker --- .github/workflows/code_quality.yml | 4 ++-- .github/workflows/nightly.yml | 4 ++-- .github/workflows/ubuntu.yml | 4 ++-- .github/workflows/unstable.yml | 4 ++-- dependencies.repos | 4 ---- dependencies_unstable.repos | 4 ---- docker/Dockerfile | 3 ++- 7 files changed, 10 insertions(+), 17 deletions(-) diff --git a/.github/workflows/code_quality.yml b/.github/workflows/code_quality.yml index 88c0c977c06..acfa0c4031c 100644 --- a/.github/workflows/code_quality.yml +++ b/.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 diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml index 6801cd2d023..a3eeb6623da 100644 --- a/.github/workflows/nightly.yml +++ b/.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 diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 5f32555db19..cad8a4b2757 100644 --- a/.github/workflows/ubuntu.yml +++ b/.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 diff --git a/.github/workflows/unstable.yml b/.github/workflows/unstable.yml index 291fb1178a0..21cabe6f18a 100644 --- a/.github/workflows/unstable.yml +++ b/.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 diff --git a/dependencies.repos b/dependencies.repos index e437cdefdc5..5c840941f17 100644 --- a/dependencies.repos +++ b/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 diff --git a/dependencies_unstable.repos b/dependencies_unstable.repos index e4b84836f2c..6af679a1868 100644 --- a/dependencies_unstable.repos +++ b/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 diff --git a/docker/Dockerfile b/docker/Dockerfile index 66b92c6c034..e40853ae515 100644 --- a/docker/Dockerfile +++ b/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