diff --git a/.clang-tidy b/.clang-tidy index 9589fc7d5..181fbf69c 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,5 +1,7 @@ --- Checks: 'performance-*, + -performance-noexcept-move-constructor, + -clang-analyzer-core.CallAndMessage, llvm-namespace-comment, modernize-redundant-void-arg, modernize-use-nullptr, diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index d80c4d33b..b5552d85e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -8,42 +8,40 @@ on: pull_request: push: +permissions: + contents: read + pages: write + id-token: write + jobs: default: strategy: fail-fast: false matrix: env: - # TODO: We have to use -Wno-redundant-decls since rosidl_generator_c is generating broken headers - - IMAGE: galactic-source + - IMAGE: rolling-source NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: rolling-source - CXXFLAGS: >- - -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls - -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter - - IMAGE: rolling-source - CXX: clang++ - CLANG_TIDY: true - CXXFLAGS: >- - -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls - -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy - # Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense - # see https://github.com/google/sanitizers/wiki/AddressSanitizer + CLANG_TIDY: pedantic - IMAGE: rolling-source NAME: asan + # Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense + # see https://github.com/google/sanitizers/wiki/AddressSanitizer + # Disable alloc/dealloc mismatch warnings: https://github.com/ros2/rclcpp/pull/1324 DOCKER_RUN_OPTS: >- -e PRELOAD=libasan.so.5 -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0" - TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1" + -e ASAN_OPTIONS="new_delete_type_mismatch=0,alloc_dealloc_mismatch=0" + TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g" env: - CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file - DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }} + CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml + DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }} UNDERLAY: /root/ws_moveit/install # TODO: Port to ROS2 # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" - UPSTREAM_WORKSPACE: .repos + TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}" @@ -55,28 +53,52 @@ jobs: name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}" runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 + with: + submodules: recursive - name: Cache ccache - uses: pat-s/always-upload-cache@v2.1.5 + uses: rhaschke/cache@main with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} restore-keys: | ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }} + env: + GHA_CACHE_SAVE: always - id: ici name: Run industrial_ci - uses: ros-industrial/industrial_ci@master + uses: rhaschke/industrial_ci@clang-tidy env: ${{ matrix.env }} + - name: Upload ici's target_ws/install folder + uses: rhaschke/upload-ici-workspace@main + if: success() && matrix.env.CLANG_TIDY + with: + subdir: target_ws/install + - name: Upload test artifacts (on failure) - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results) with: - name: test-results-${{ matrix.env.IMAGE }} + name: test-results-${{ matrix.env.IMAGE }}${{ matrix.env.NAME && '-' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && '-clang-tidy' || '' }} path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml + + - name: Upload clang-tidy fixes (on failure) + uses: actions/upload-artifact@v3 + if: failure() && steps.ici.outputs.clang_tidy_checks + with: + name: clang-tidy-fixes.yaml + path: ${{ env.BASEDIR }}/clang-tidy-fixes.yaml + + - name: Show clang-tidy warnings + if: always() && matrix.env.CLANG_TIDY + uses: asarium/clang-tidy-action@v1 + with: + fixesFile: ${{ env.BASEDIR }}/clang-tidy-fixes.yaml + - name: Generate codecov report uses: rhaschke/lcov-action@main if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0' @@ -85,7 +107,7 @@ jobs: workdir: ${{ env.BASEDIR }}/target_ws ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"' - name: Upload codecov report - uses: codecov/codecov-action@v2 + uses: codecov/codecov-action@v3 if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0' with: files: ${{ env.BASEDIR }}/target_ws/coverage.info diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 39ea78667..4051b571f 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -13,11 +13,12 @@ jobs: name: pre-commit runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 - - uses: actions/setup-python@v2 - - name: Install clang-format-10 - run: sudo apt-get install clang-format-10 - - uses: pre-commit/action@v2.0.0 + - uses: actions/checkout@v3 + with: + submodules: recursive + - name: Install clang-format-12 + run: sudo apt-get install clang-format-12 + - uses: pre-commit/action@v3.0.0 id: precommit - name: Upload pre-commit changes if: failure() && steps.precommit.outcome == 'failure' diff --git a/.github/workflows/lsan.suppressions b/.github/workflows/lsan.suppressions index 79bf7e06c..3164a5a4e 100644 --- a/.github/workflows/lsan.suppressions +++ b/.github/workflows/lsan.suppressions @@ -1 +1,3 @@ leak:class_loader +leak:rviz_default_plugins +leak:static_transform_broadcaster_node diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml new file mode 100644 index 000000000..347a23daa --- /dev/null +++ b/.github/workflows/prerelease.yaml @@ -0,0 +1,41 @@ +# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). +# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) + +name: pre-release + +on: + workflow_dispatch: + +permissions: + contents: read # to fetch code (actions/checkout) + +jobs: + default: + strategy: + matrix: + distro: [noetic] + + env: + # https://github.com/ros-industrial/industrial_ci/issues/666 + BUILDER: catkin_make_isolated + ROS_DISTRO: ${{ matrix.distro }} + PRERELEASE: true + BASEDIR: ${{ github.workspace }}/.work + + name: "${{ matrix.distro }}" + runs-on: ubuntu-latest + steps: + - name: "Free up disk space" + run: | + sudo apt-get -qq purge build-essential "ghc*" + sudo apt-get clean + # cleanup docker images not used by us + docker system prune -af + # free up a lot of stuff from /usr/local + sudo rm -rf /usr/local + df -h + - uses: actions/checkout@v3 + with: + submodules: recursive + - name: industrial_ci + uses: ros-industrial/industrial_ci@master diff --git a/.gitignore b/.gitignore index 1377554eb..5d9904d50 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ *.swp +*.pyc +__pycache__/ diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..9339bbb20 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,5 @@ +[submodule "core/python/pybind11"] + path = core/python/pybind11 + url = https://github.com/pybind/pybind11 + branch = smart_holder + shallow = true diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e4e33057d..24733c4dc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v3.4.0 + rev: v4.0.1 hooks: - id: check-added-large-files - id: check-case-conflict @@ -32,13 +32,14 @@ repos: rev: 22.3.0 hooks: - id: black + args: ["--line-length", "100"] - repo: local hooks: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-10 + entry: clang-format-12 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ['-fallback-style=none', '-i'] + args: ["-fallback-style=none", "-i"] diff --git a/.repos b/.repos deleted file mode 100644 index 54a045dec..000000000 --- a/.repos +++ /dev/null @@ -1,5 +0,0 @@ -repositories: - rosparam_shortcuts: - type: git - url: https://github.com/PickNikRobotics/rosparam_shortcuts - version: ros2 diff --git a/README.md b/README.md index 24979ef0e..16847501b 100644 --- a/README.md +++ b/README.md @@ -5,9 +5,14 @@ It draws on the planning capabilities of [MoveIt](https://moveit.ros.org/) to so A common interface, based on MoveIt's PlanningScene is used to pass solution hypotheses between stages. The framework enables the hierarchical organization of basic stages using *containers*, allowing for sequential as well as parallel compositions. -## Video +## Videos -[![Video associated with ICRA 2019 paper](https://img.youtube.com/vi/fCORKVYsdDI/0.jpg )](https://www.youtube.com/watch?v=fCORKVYsdDI) +- Demo video associated with [ICRA 2019 paper](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf) + + [![](https://img.youtube.com/vi/fCORKVYsdDI/0.jpg)](https://www.youtube.com/watch?v=fCORKVYsdDI) + +- [Presentation @ ROSCon 2018 (Madrid)](https://vimeo.com/293432325) +- [Presentation @ MoveIt workshop 2019 (Macau)](https://www.youtube.com/watch?v=a8r7O2bs1Mc) ## Tutorial @@ -32,10 +37,9 @@ Ideas and requests for other interesting/useful features are welcome. If you use this framework in your project, please cite the associated paper: - Michael Görner*, Robert Haschke*, Helge Ritter, and Jianwei Zhang, -MoveIt! Task Constructor for Task-Level Motion Planning, -International Conference on Robotics and Automation, ICRA 2019, Montreal, Canada. +"MoveIt! Task Constructor for Task-Level Motion Planning", +_International Conference on Robotics and Automation (ICRA)_, 2019, Montreal, Canada. [[DOI]](https://doi.org/10.1109/ICRA.2019.8793898) [[PDF]](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf). diff --git a/capabilities/CHANGELOG.rst b/capabilities/CHANGELOG.rst new file mode 100644 index 000000000..1d1f57f56 --- /dev/null +++ b/capabilities/CHANGELOG.rst @@ -0,0 +1,17 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_task_constructor_capabilities +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.3 (2023-03-06) +------------------ + +0.1.2 (2023-02-24) +------------------ + +0.1.1 (2023-02-15) +------------------ + +0.1.0 (2023-02-02) +------------------ +* Initial release +* Contributors: Michael Görner, Robert Haschke diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt index ed8282109..f07b78e46 100644 --- a/capabilities/CMakeLists.txt +++ b/capabilities/CMakeLists.txt @@ -1,16 +1,16 @@ cmake_minimum_required(VERSION 3.5) project(moveit_task_constructor_capabilities) -add_compile_options(-std=c++17) - find_package(ament_cmake REQUIRED) find_package(Boost REQUIRED) -find_package(rclcpp_action REQUIRED) +find_package(moveit_common REQUIRED) +moveit_package() find_package(moveit_core REQUIRED) find_package(moveit_ros_move_group REQUIRED) find_package(moveit_task_constructor_msgs REQUIRED) find_package(pluginlib REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) add_library(${PROJECT_NAME} SHARED @@ -29,10 +29,10 @@ install(TARGETS ${PROJECT_NAME} ) pluginlib_export_plugin_description_file(moveit_ros_move_group capabilities_plugin_description.xml) -ament_export_dependencies(rclcpp_action) ament_export_dependencies(moveit_core) ament_export_dependencies(moveit_ros_move_group) ament_export_dependencies(moveit_task_constructor_msgs) ament_export_dependencies(pluginlib) +ament_export_dependencies(rclcpp_action) ament_export_dependencies(std_msgs) ament_package() diff --git a/capabilities/package.xml b/capabilities/package.xml index bdad2efa9..4f61ceffc 100644 --- a/capabilities/package.xml +++ b/capabilities/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_capabilities - 0.0.0 + 0.1.3 MoveGroupCapabilites to interact with MoveIt @@ -15,11 +15,11 @@ moveit_core moveit_ros_move_group moveit_ros_planning - rclcpp_action - pluginlib - std_msgs moveit_task_constructor_core moveit_task_constructor_msgs + pluginlib + rclcpp_action + std_msgs ament_cmake diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 2293cb139..7cde060c6 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -99,7 +99,7 @@ void ExecuteTaskSolutionCapability::initialize() { } void ExecuteTaskSolutionCapability::goalCallback( - const std::shared_ptr> goal_handle) { + const std::shared_ptr>& goal_handle) { auto result = std::make_shared(); const auto& goal = goal_handle->get_goal(); @@ -126,7 +126,7 @@ void ExecuteTaskSolutionCapability::goalCallback( } rclcpp_action::CancelResponse ExecuteTaskSolutionCapability::preemptCallback( - const std::shared_ptr> goal_handle) { + const std::shared_ptr>& /*goal_handle*/) { if (context_->plan_execution_) context_->plan_execution_->stop(); return rclcpp_action::CancelResponse::ACCEPT; @@ -142,16 +142,16 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr state = scene->getCurrentState(); } - plan.plan_components_.reserve(solution.sub_trajectory.size()); + plan.plan_components.reserve(solution.sub_trajectory.size()); for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) { const moveit_task_constructor_msgs::msg::SubTrajectory& sub_traj = solution.sub_trajectory[i]; - plan.plan_components_.emplace_back(); - plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back(); + plan.plan_components.emplace_back(); + plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components.back(); // define individual variable for use in closure below const std::string description = std::to_string(i + 1) + "/" + std::to_string(solution.sub_trajectory.size()); - exec_traj.description_ = description; + exec_traj.description = description; const moveit::core::JointModelGroup* group = nullptr; { @@ -173,8 +173,8 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr exec_traj.controller_names_ = sub_traj.execution_info.controller_names; /* TODO add action feedback and markers */ - exec_traj.effect_on_success_ = [this, sub_traj, - description](const plan_execution::ExecutableMotionPlan* /*plan*/) { + exec_traj.effect_on_success = [this, sub_traj, + description](const plan_execution::ExecutableMotionPlan* /*plan*/) { if (!moveit::core::isEmpty(sub_traj.scene_diff)) { RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description); return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff); @@ -194,5 +194,5 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr } // namespace move_group -#include -CLASS_LOADER_REGISTER_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability) +#include +PLUGINLIB_EXPORT_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability) diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h index 366347da7..3248b06a8 100644 --- a/capabilities/src/execute_task_solution_capability.h +++ b/capabilities/src/execute_task_solution_capability.h @@ -63,14 +63,14 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution, plan_execution::ExecutableMotionPlan& plan); - void goalCallback(const std::shared_ptr> goal_handle); + void goalCallback(const std::shared_ptr>& goal_handle); rclcpp_action::CancelResponse - preemptCallback(const std::shared_ptr> goal_handle); + preemptCallback(const std::shared_ptr>& goal_handle); /** Always accept the goal */ - rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& uuid, - const ExecuteTaskSolutionAction::Goal::ConstSharedPtr goal) const { + rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& /*uuid*/, + const ExecuteTaskSolutionAction::Goal::ConstSharedPtr& /*goal*/) const { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } diff --git a/core/CHANGELOG.rst b/core/CHANGELOG.rst new file mode 100644 index 000000000..ae0c53b37 --- /dev/null +++ b/core/CHANGELOG.rst @@ -0,0 +1,37 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_task_constructor_core +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.3 (2023-03-06) +------------------ +* MoveRelative: Allow backwards operation for joint-space delta (`#436 `_) +* ComputeIK: Limit collision checking to JMG (`#428 `_) +* Fix: Fetch pybind11 submodule if not yet present +* Contributors: Robert Haschke + +0.1.2 (2023-02-24) +------------------ +* Remove moveit/__init__.py during .deb builds to avoid installation conflicts +* MultiPlanner solver (`#429 `_): a planner that tries multiple planners in sequence + + * CartesianPath: Deprecate redundant property setters + * PlannerInterface: provide "timeout" property + * PlannerInterface: provide setters for properties +* JointInterpolation: fix timeout handling +* Contributors: Robert Haschke + +0.1.1 (2023-02-15) +------------------ +* Resort to MoveIt's python tools +* Provide ComputeIK.ik_frame as full PoseStamped +* Fixed build farm issues + + * Removed unused eigen_conversions includes + * Fixed odr compiler warning on Buster + * Fixed missing dependency declarations +* Contributors: Robert Haschke + +0.1.0 (2023-02-02) +------------------ +* Initial release +* Contributors: Michael Görner, Robert Haschke, Captain Yoshi, Christian Petersmeier, Henning Kayser, Jafar Abdi, Tyler Weaver diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index a7bcdfdd1..d10d2f5ba 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -5,6 +5,8 @@ find_package(ament_cmake REQUIRED) find_package(Boost REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(moveit_common REQUIRED) +moveit_package() find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) @@ -14,15 +16,17 @@ find_package(rviz_marker_tools REQUIRED) find_package(tf2_eigen REQUIRED) find_package(visualization_msgs REQUIRED) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) add_compile_options(-fvisibility-inlines-hidden) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) add_subdirectory(src) +# TODO: enable python wrapper +#add_subdirectory(python) add_subdirectory(test) install(DIRECTORY include/ DESTINATION include @@ -30,8 +34,7 @@ install(DIRECTORY include/ DESTINATION include pluginlib_export_plugin_description_file(${PROJECT_NAME} motion_planning_stages_plugin_description.xml) -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME} ${PROJECT_NAME}_stages) +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_export_dependencies(Boost) ament_export_dependencies(geometry_msgs) ament_export_dependencies(moveit_core) diff --git a/core/cmake/pybind11.cmake.in b/core/cmake/pybind11.cmake.in new file mode 100644 index 000000000..330fb73e5 --- /dev/null +++ b/core/cmake/pybind11.cmake.in @@ -0,0 +1,9 @@ +# pybind11 must use the ROS python version +set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION_STRING}) + +if(@INSTALLSPACE@) + include(${CMAKE_CURRENT_LIST_DIR}/pybind11Config.cmake) +else() + # in build space, directly include pybind11 directory + add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../pybind11 ${CMAKE_CURRENT_BINARY_DIR}/pybind11) +endif() diff --git a/core/doc/Doxyfile b/core/doc/Doxyfile new file mode 100644 index 000000000..269a6d3ed --- /dev/null +++ b/core/doc/Doxyfile @@ -0,0 +1,21 @@ +PROJECT_NAME = MTC +INPUT = ../include/ ../src/ +RECURSIVE = YES + +GENERATE_HTML = YES +GENERATE_LATEX = NO +GENERATE_XML = NO +HTML_OUTPUT = _cpp +XML_OUTPUT = _doxygenxml +XML_PROGRAMLISTING = YES + +ALIASES = "rst=\verbatim embed:rst" +ALIASES += "endrst=\endverbatim" + +QUIET = YES +WARNINGS = YES +WARN_IF_UNDOCUMENTED = NO + +EXCLUDE_SYMBOLS = *Private +EXCLUDE_SYMBOLS += class_ +EXCLUDE_SYMBOLS += declval* diff --git a/core/doc/_templates/custom-class-template.rst b/core/doc/_templates/custom-class-template.rst new file mode 100644 index 000000000..6e49ce80a --- /dev/null +++ b/core/doc/_templates/custom-class-template.rst @@ -0,0 +1,34 @@ +{{ fullname | escape | underline}} + +.. currentmodule:: {{ module }} + +.. autoclass:: {{ objname }} + :members: + :show-inheritance: + :inherited-members: + :special-members: __len__, __getitem__, __iter__, __call__, __add__, __mul__ + + {% block methods %} + {% if methods %} + .. rubric:: {{ _('Methods') }} + + .. autosummary:: + :nosignatures: + {% for item in methods %} + {%- if not item.startswith('_') %} + ~{{ name }}.{{ item }} + {%- endif -%} + {%- endfor %} + {% endif %} + {% endblock %} + + {% block attributes %} + {% if attributes %} + .. rubric:: {{ _('Attributes') }} + + .. autosummary:: + {% for item in attributes %} + ~{{ name }}.{{ item }} + {%- endfor %} + {% endif %} + {% endblock %} diff --git a/core/doc/_templates/custom-module-template.rst b/core/doc/_templates/custom-module-template.rst new file mode 100644 index 000000000..d066d0e4d --- /dev/null +++ b/core/doc/_templates/custom-module-template.rst @@ -0,0 +1,66 @@ +{{ fullname | escape | underline}} + +.. automodule:: {{ fullname }} + + {% block attributes %} + {% if attributes %} + .. rubric:: Module attributes + + .. autosummary:: + :toctree: + {% for item in attributes %} + {{ item }} + {%- endfor %} + {% endif %} + {% endblock %} + + {% block functions %} + {% if functions %} + .. rubric:: {{ _('Functions') }} + + .. autosummary:: + :toctree: + :nosignatures: + {% for item in functions %} + {{ item }} + {%- endfor %} + {% endif %} + {% endblock %} + + {% block classes %} + {% if classes %} + .. rubric:: {{ _('Classes') }} + + .. autosummary:: + :toctree: + :template: custom-class-template.rst + :nosignatures: + {% for item in classes %} + {{ item }} + {%- endfor %} + {% endif %} + {% endblock %} + + {% block exceptions %} + {% if exceptions %} + .. rubric:: {{ _('Exceptions') }} + + .. autosummary:: + :toctree: + {% for item in exceptions %} + {{ item }} + {%- endfor %} + {% endif %} + {% endblock %} + +{% block modules %} +{% if modules %} +.. autosummary:: + :toctree: + :template: custom-module-template.rst + :recursive: +{% for item in modules %} + {{ item }} +{%- endfor %} +{% endif %} +{% endblock %} diff --git a/core/doc/api.rst b/core/doc/api.rst new file mode 100644 index 000000000..4a0800b42 --- /dev/null +++ b/core/doc/api.rst @@ -0,0 +1,20 @@ +.. _sec-api: + +API reference +------------- + +`C++ <_static/index.html>`_ +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Python +^^^^^^ + +.. autosummary:: + :toctree: _autosummary + :caption: API + :recursive: + :template: custom-module-template.rst + + moveit.task_constructor + pymoveit_mtc.core + pymoveit_mtc.stages diff --git a/core/doc/basics.rst b/core/doc/basics.rst new file mode 100644 index 000000000..cd1e87297 --- /dev/null +++ b/core/doc/basics.rst @@ -0,0 +1,47 @@ +Basic Concepts +============== + +The fundamental idea of MTC is that complex motion planning problems can be composed into a set of simpler subproblems. The top-level planning problem is specified as a Task while all subproblems are specified by Stages. Stages can be arranged in any arbitrary order and hierarchy only limited by the individual stages types. The order in which stages can be arranged is restricted by the direction in which results are passed. There are three possible stage types w.r.t. their data flow: generators, propagators, and connector stages: + +.. glossary:: + + Generators + compute their results independently of their neighboring stages and pass them in both directions, backwards and forwards. Examples include pose generators, e.g. for grasping or placing, as well as ``ComputeIK``, which computes IK solutions for Cartesian targets. neighboring stages can continue processing from the generated states. The most important generator stage, however, is ``CurrentState``, which provides the current robot state as the starting point for a planning pipeline. + + Propagators + receive the result of *one* neighboring stage as input, plan towards a goal state, and then propagate their result to the opposite interface site. Propagating stages can receive their input on either interface, begin or end. The flow of information (forwards or backwards) is determined by the input interface of the stage. An example is a stage that computes a Cartesian path based on either a start or a goal state. + + Connectors + do not propagate any results, but rather attempt to bridge the gap between the resulting states of both its neighboring stages. It receives input states from both, the begin and end interface and attempts to connect them via a suitable motion plan. Obviously, any pair of input states needs to be *compatible*, i.e. their states (including collision and attached objects as well as joint poses) need to match except for those joints that are part of the given planning group. + +Processing starts from generator stages, expands via propagators, and finally connects partial solution sequences via connector stages. +Obviously, there exist limitations on how stages can be connected to each other. For example, two generator stages cannot occur in sequence as they would both attempt to *write* into their interfaces, but non of them is actually *reading*. Same applies for two connectors in a row: they would both attempt to read, while no stage is actually writing. +The compatibility of stages is automatically checked once before planning by ``Task::init()``. + +To compose a planning pipeline from multiple seemingly independent parts, for example grasping an object and placing it at a new location, one needs to break the typical linear pipeline structure: the place pose is another generator stage (additionally to the ``CurrentState`` stage we are starting from) serving as a seed for the placing sub solution. However, this stage is not completely independent from the grasping sub solution: it should continue where grasping left off, namely with the grasped object attached to the gripper. To convey this state information, the place pose generator should inherit from ``MonitoringGenerator``, which monitors the solutions of another stage in the pipeline in fast-forwards them for further processing in the dependent stage. + +In order to hierarchically organize planning pipelines and to allow for reuse of sub pipelines, e.g. for grasping or placing, one can encapsulate stages within various containers. +Stages without children are called primitive stages. We provide three types of containers: + +.. glossary:: + + Wrappers + encapsulate a single subordinate stage and modify or filter its results. For example, a filter stage that only accepts solutions of its child stage that satisfy a certain constraint can be realized as a wrapper. Another standard use of this type includes the IK wrapper stage, which generates inverse kinematics solutions based on planning scenes annotated with a pose target property. + + Serial containers + hold a sequence of subordinate stages and only consider end-to-end solutions as results. An example is a picking motion that consists of a sequence of coherent steps. + + Parallel containers + consider the solutions of all their children as feasible. Different sub types are available, namely: + + ``Alternative`` container + processes all children simultaneously (currently in a round-robin fashion). For example, one could plan a grasping sequence for a left and right arm in parallel if the actual choice of the arm doesn't matter for the task. + + ``Fallback`` container + processes their children sequentially: only if the current child has exhausted all its solution candidates (and didn't produce any feasible solution), the next child is considered. + Use this container, e.g. if you prefer grasping with the right arm, but allow falling back to the left if really needed. + + ``Merger`` container + processes their children simultaneously and combine their solutions into an joint solution. It is assumed that children operate on disjoint joint model groups, e.g. the arm and the hand, such that their solution trajectories can be executed in parallel after being merged. + +Stages not only support solving motion planning problems. They can also be used for all kinds of state transitions, as for instance modifying the planning scene. Combined with the possibility of using class inheritance it is possible to construct very complex behavior while only relying on a well-structured set of primitive stages. diff --git a/core/doc/concepts.rst b/core/doc/concepts.rst new file mode 100644 index 000000000..7465e47aa --- /dev/null +++ b/core/doc/concepts.rst @@ -0,0 +1,9 @@ +.. _sec-concepts: + +Concepts +------------ + +.. toctree:: + :maxdepth: 2 + + basics diff --git a/core/doc/conf.py b/core/doc/conf.py new file mode 100644 index 000000000..ce4f88d26 --- /dev/null +++ b/core/doc/conf.py @@ -0,0 +1,243 @@ +# Configuration file for the Sphinx documentation builder. +# +# This file does only contain a selection of the most common options. +# For a full list, refer to: http://www.sphinx-doc.org/en/master/config + + +from lxml import etree +import os +import subprocess +import sys +from pathlib import Path + +DIR = Path(__file__).parent.resolve() + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# sys.path.insert(0, os.path.abspath('.')) + +# -- General configuration ------------------------------------------------ + +# If your documentation needs a minimal Sphinx version, state it here. +# needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + "sphinx_copybutton", + "sphinx.ext.autodoc", + "sphinx.ext.autosummary", + "sphinx.ext.intersphinx", + "sphinx.ext.napoleon", + "sphinx.ext.extlinks", +] + +autosummary_generate = True +autoclass_content = "both" # Add __init__ doc (ie. params) to class summaries +html_show_sourcelink = False # Remove 'view source code' from top of page (for html, not python) +autodoc_inherit_docstrings = True # If no docstring, inherit from base class +set_type_checking_flag = True # Enable 'expensive' imports for sphinx_autodoc_typehints +add_module_names = False + +# Add any paths that contain templates here, relative to this directory. +templates_path = ["_templates"] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# source_suffix = ['.rst', '.md'] +source_suffix = ".rst" + +# The encoding of source files. +# source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = "index" + +# General information about the project. +project = "MoveIt Task Constructor" +author = "Michael Görner, Robert Haschke" + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# Read version from package.xml +xml = etree.parse("../package.xml") +version = str(xml.xpath("/package/version/text()")[0]) + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = "en" + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +# today = '' +# Else, today_fmt is used as the format for a strftime call. +# today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = [".build", "python/pybind11"] + +# The reST default role (used for this markup: `text`) to use for all +# documents. +# default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +# add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +# add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +# show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +# pygments_style = 'monokai' + +# A list of ignored prefixes for module index sorting. +# modindex_common_prefix = [] + +# If true, keep warnings as "system message" paragraphs in the built documents. +# keep_warnings = False + +# If true, `todo` and `todoList` produce output, else they produce nothing. +todo_include_todos = False + + +# -- Options for HTML output ---------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +html_theme = "furo" + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +# html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +# html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +# html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +# html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +# html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ["_cpp"] + +# Add any extra paths that contain custom files (such as robots.txt or +# .htaccess) here, relative to this directory. These files are copied +# directly to the root of the documentation. +# html_extra_path = [] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +# html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +# html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +# html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +# html_additional_pages = {} + +# If false, no module index is generated. +# html_domain_indices = True + +# If false, no index is generated. +# html_use_index = True + +# If true, the index is split into individual pages for each letter. +# html_split_index = False + +# If true, links to the reST sources are added to the pages. +# html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +# html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +# html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +# html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +# html_file_suffix = None + +# Language to be used for generating the HTML full-text search index. +# Sphinx supports the following languages: +# html_search_language = 'en' + +# A dictionary with options for the search language support, empty by default. +# Now only 'ja' uses this config value +# html_search_options = {'type': 'default'} + +# The name of a javascript file (relative to the configuration directory) that +# implements a search results scorer. If empty, the default will be used. +# html_search_scorer = 'scorer.js' + +# Output file base name for HTML help builder. +htmlhelp_basename = "mtcdoc" + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {"python": ("https://docs.python.org/3", None)} + +ros_distro = "noetic" +ros_docs = f"http://docs.ros.org/{ros_distro}/api" +extlinks = { + "rosdocs": (f"{ros_docs}/%s", "%s"), + "msgs": (f"{ros_docs}/moveit_task_constructor/html/msg/%s.html", "%s"), + "moveit_msgs": (f"{ros_docs}/moveit_msgs/html/msg/%s.html", "%s"), + "geometry_msgs": (f"{ros_docs}/geometry_msgs/html/msg/%s.html", "%s"), + "visualization_msgs": (f"{ros_docs}/visualization_msgs/html/msg/%s.html", "%s"), +} + + +def generate_doxygen_xml(app): + build_dir = os.path.join(app.confdir, ".build") + if not os.path.exists(build_dir): + os.mkdir(build_dir) + + print("Running doxygen") + try: + subprocess.call(["doxygen", "--version"]) + retcode = subprocess.call(["doxygen"], cwd=app.confdir) + if retcode < 0: + sys.stderr.write(f"doxygen error code: {-retcode}\n") + except OSError as e: + sys.stderr.write(f"doxygen execution failed: {e}\n") + + +def setup(app): + # Add hook for building doxygen xml when needed + # app.connect("builder-inited", generate_doxygen_xml) + pass diff --git a/core/doc/howto.rst b/core/doc/howto.rst new file mode 100644 index 000000000..b574a7a71 --- /dev/null +++ b/core/doc/howto.rst @@ -0,0 +1,301 @@ +.. _sec-howtoguides: + +How-To Guides +============= + +.. _subsec-howto-stage-usage: + +Stage Usage +----------- + +.. _subsubsec-howto-alternatives: + +Alternatives +^^^^^^^^^^^^ + +Using the ``alternatives`` stage, you can plan for multiple +execution paths. +Download the full example code here: :download:`Source <./../../demo/scripts/alternatives.py>` + +.. literalinclude:: ./../../demo/scripts/alternatives.py + :language: python + :start-after: [initAndConfigAlternatives] + :end-before: [initAndConfigAlternatives] + +.. _subsubsec-howto-fallbacks: + +Fallbacks +^^^^^^^^^ + +The ``fallbacks`` stage provides alternative motion planners +if planning fails with the primary one. +Download the full example code here: :download:`Source <./../../demo/scripts/fallbacks.py>` + +.. literalinclude:: ./../../demo/scripts/fallbacks.py + :language: python + :start-after: [initAndConfigFallbacks] + :end-before: [initAndConfigFallbacks] + +.. _subsubsec-howto-merger: + +Merger +^^^^^^ + +Plan and execute sequences in parallel using the ``merger`` stage. +Download the full example code here: :download:`Source <./../../demo/scripts/merger.py>` + +.. literalinclude:: ./../../demo/scripts/merger.py + :language: python + :start-after: [initAndConfigMerger] + :end-before: [initAndConfigMerger] + +.. _subsubsec-howto-connect: + +Connect +^^^^^^^ + +Connect two stages by finding a motion plan between them. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigConnect] + :end-before: [initAndConfigConnect] + +.. _subsubsec-howto-fix-collision-objects: + +FixCollisionObjects +^^^^^^^^^^^^^^^^^^^ + +Check for collisions and resolve them if applicable. +Download the full example code here: :download:`Source <./../../demo/scripts/fix_collision_objects.py>` + +.. literalinclude:: ./../../demo/scripts/fix_collision_objects.py + :language: python + :start-after: [initAndConfig] + :end-before: [initAndConfig] + +.. _subsubsec-howto-generate-place-pose: + +GeneratePlacePose +^^^^^^^^^^^^^^^^^^^ + +Sample feasible poses around an object pose. +Considers geometry of primitive object type. +Solutions can be used for inverse +kinematics calculations. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initCollisionObject] + :end-before: [initCollisionObject] + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigGeneratePlacePose] + :end-before: [initAndConfigGeneratePlacePose] + +.. _subsubsec-howto-generate-grasp-pose: + +GenerateGraspPose +^^^^^^^^^^^^^^^^^ + +Sample poses around an object pose by providing +sample density ``angle_delta``. +Solutions can be used for inverse kinematics +calculations. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigGenerateGraspPose] + :end-before: [initAndConfigGenerateGraspPose] + +.. _subsubsec-howto-generate-pose: + +GeneratePose +^^^^^^^^^^^^ + +Spawn a pose on new solutions of the monitored stage. +Download the full example code here: :download:`Source <./../../demo/scripts/generate_pose.py>` + +.. literalinclude:: ./../../demo/scripts/generate_pose.py + :language: python + :start-after: [initAndConfigGeneratePose] + :end-before: [initAndConfigGeneratePose] + +.. _subsubsec-howto-pick: + +Pick +^^^^ + +Wraps the task pipeline to execute a pick. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigPick] + :end-before: [initAndConfigPick] + +.. _subsubsec-howto-place: + +Place +^^^^^ + +Wraps the task pipeline to execute a pick. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigPlace] + :end-before: [initAndConfigPlace] + +.. _subsubsec-howto-simplegrasp: + +SimpleGrasp +^^^^^^^^^^^ + +Wraps the pose generation and inverse kinematics +computation for the pick pipeline. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigSimpleGrasp] + :end-before: [initAndConfigSimpleGrasp] + +.. _subsubsec-howto-simpleungrasp: + +SimpleUnGrasp +^^^^^^^^^^^^^ + +Wraps the pose generation and inverse kinematics +computation for the place pipeline. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigSimpleUnGrasp] + :end-before: [initAndConfigSimpleUnGrasp] + +.. _subsubsec-howto-modify-planning-scene: + +ModifyPlanningScene +^^^^^^^^^^^^^^^^^^^ + +Modify the planning scene. +Download the full example code here: :download:`Source <./../../demo/scripts/modify_planning_scene.py>` + +.. literalinclude:: ./../../demo/scripts/modify_planning_scene.py + :language: python + :start-after: [initAndConfigModifyPlanningScene] + :end-before: [initAndConfigModifyPlanningScene] + +.. _subsubsec-howto-fixed-state: + +FixedState +^^^^^^^^^^ + +Spawn a pre-defined state. +Download the full example code here: :download:`Source <./../../demo/scripts/fixed_state.py>` + +.. literalinclude:: ./../../demo/scripts/fixed_state.py + :language: python + :start-after: [initAndConfigFixedState] + :end-before: [initAndConfigFixedState] + +.. _subsubsec-howto-compute-ik: + +ComputeIK +^^^^^^^^^ + +Compute the inverse kinematics of the monitored stages' +solution. Be sure to correctly configure the ``target_pose`` +property to be derived from the monitored stage as shown +in the example. +Download the full example code here: :download:`Source <./../../demo/scripts/compute_ik.py>` + +.. literalinclude:: ./../../demo/scripts/compute_ik.py + :language: python + :start-after: [initAndConfigComputeIk] + :end-before: [initAndConfigComputeIk] + +.. _subsubsec-howto-move-to: + +MoveTo +^^^^^^ + +Use planners to compute a motion plan. +Download the full example code here: :download:`Source <../../demo/scripts/cartesian.py>` + +.. literalinclude:: ../../demo/scripts/cartesian.py + :language: python + :start-after: [initAndConfigMoveTo] + :end-before: [initAndConfigMoveTo] + +.. _subsubsec-howto-move-relative: + +MoveRelative +^^^^^^^^^^^^ + +Move along a relative offset. +Download the full example code here: :download:`Source <../../demo/scripts/cartesian.py>` + +.. literalinclude:: ../../demo/scripts/cartesian.py + :language: python + :start-after: [initAndConfigMoveRelative] + :end-before: [initAndConfigMoveRelative] + +.. _subsec-howto-stage-extension: + +Stage Extension +--------------- + +.. _subsubsec-howto-move-relative-extension: + +MoveRelative +^^^^^^^^^^^^ + +You may derive from this stage to extend its functionality. +``MoveRelative`` itself derives from the propagator stage that +alters solutions (i.e. computes a motion plan) when they are +passed through the stage. + +.. literalinclude:: ./../../core/python/test/rostest_trampoline.py + :language: python + :pyobject: PyMoveRelX + +.. _subsubsec-howto-generator-extension: + +Generator +^^^^^^^^^ + +Derive from the ``Generator`` stage to implement your own +logic in the compute function. + +.. literalinclude:: ./../../core/python/test/rostest_trampoline.py + :language: python + :pyobject: PyGenerator + +.. _subsubsec-howto-monitoring-generator-extension: + +MonitoringGenerator +^^^^^^^^^^^^^^^^^^^ + +Derive from the ``MonitoringGenerator`` stage to +implement your own logic in the compute function. +Use the monitoring generator instead of the normal +generator if you need to access solutions of the +monitored stage (e.g. computation of inverse kinematics). + +.. literalinclude:: ./../../core/python/test/rostest_trampoline.py + :language: python + :pyobject: PyMonitoringGenerator diff --git a/core/doc/index.rst b/core/doc/index.rst new file mode 100644 index 000000000..75f0c94af --- /dev/null +++ b/core/doc/index.rst @@ -0,0 +1,31 @@ +MoveIt Task Constructor (MTC) +============================= + +The Task Constructor framework provides a flexible and transparent way +to define and plan actions that consist of multiple interdependent subtasks. +It draws on the planning capabilities of MoveIt to solve individual subproblems +in black-box planning stages. +A common interface, based on MoveIt’s PlanningScene is used to pass solution +hypotheses between stages. The framework enables the hierarchical organization of +basic stages using containers, allowing for sequential as well as parallel compositions. +For more details, please refer to the associated `ICRA 2019 publication `_. + + +Organization of the documentation +--------------------------------- + +- :ref:`sec-tutorials` provide examples how to setup your task pipeline. + Start with :ref:`subsec-tut-firststeps` if you are new to MTC. +- :ref:`sec-concepts` discuss the architecture and terminology of MTC on a fairly high level. +- :ref:`sec-howtoguides` help solving specific problems and use cases. +- The :ref:`sec-api` provides quick access to available classes, functions, and their parameters. + +.. toctree:: + :maxdepth: 2 + :hidden: + + tutorials/index + concepts + howto + api + troubleshooting diff --git a/core/doc/requirements.txt b/core/doc/requirements.txt new file mode 100644 index 000000000..d1ca8780e --- /dev/null +++ b/core/doc/requirements.txt @@ -0,0 +1,4 @@ +furo +lxml +sphinx +sphinx-copybutton diff --git a/core/doc/troubleshooting.rst b/core/doc/troubleshooting.rst new file mode 100644 index 000000000..8ee9f0b90 --- /dev/null +++ b/core/doc/troubleshooting.rst @@ -0,0 +1,31 @@ +.. _sec-troubleshooting: + +Troubleshooting +=============== + +``Task::init()`` reports mismatching interfaces +----------------------------------------------- + +Before planning, the planning pipeline is checked for consistency. Each stage type has a specific flow interface, e.g. generator-type stages write into both, their begin and end interface, propagator-type stages read from one and write to the opposite, and connector-type stages read from both sides. Obviously these interfaces need to match. If they don't, an ``InitStageException`` is thrown. Per default, this is not very verbose, but you can use the following code snippet to get some more info: + + .. code-block:: c + + try { + task.plan(); + } catch (const InitStageException& e) { + std::cerr << e << std::endl; + throw; + } + +For example, the following pipeline: + +- ↕ current +- ⛓ connect +- ↑ moveTo + +throws the error: ``task pipeline: end interface (←) of 'moveto' does not match mine (→)``. + +The validation process runs sequentially through a ``SerialContainer``. Here, ``current``, as a generator stage is compatible to ``connect``, writing to the interface read by ``connect``. +``moveTo`` as a propagator can operate, in principle, forwards and backwards. By default, the operation direction is inferred automatically. Here, as ``connect`` requires a reading end-interface, moveTo should seemingly operate backwards. However, now the whole pipeline is incompatible to the enclosing container's interface: a task requires a generator-type interface as it generates solutions - there is no input interface to read from. Hence, the *reading* end interface (←) of ``moveto`` conflicts with a *writing* end interface of the task. + +Obviously, in a ``ParallelContainer`` all (direct) children need to share a common interface. diff --git a/core/doc/tutorials/cartesian.rst b/core/doc/tutorials/cartesian.rst new file mode 100644 index 000000000..dd350e69e --- /dev/null +++ b/core/doc/tutorials/cartesian.rst @@ -0,0 +1,86 @@ +.. _subsec-tut-cartesian: + +Cartesian +--------- + +The following example demonstrates how to compute a simple point to point motion +plan using the moveit task constructor. You can take a look at the full +source code here: +:download:`Source <../../../demo/scripts/cartesian.py>` + +First, lets make sure we specify the planning group and the +end effector that we want to use. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut1] + :end-before: [cartesianTut1] + +The moveit task constructor provides different planners. +We will use the ``CartesianPath`` and ``JointInterpolation`` +planners for this example. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut2] + :end-before: [cartesianTut2] + +Lets start by initializing a task and adding the current +planning scene state and robot state to it. +This will be the starting state for our motion plan. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut3] + :end-before: [cartesianTut3] + +To compute a relative motion in cartesian space, we can use +the ``MoveRelative`` stage. Specify the planning group and +frame relative to which you want to carry out the motion. +the relative direction can be specified by a ``Vector3Stamped`` +geometry message. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [initAndConfigMoveRelative] + :end-before: [initAndConfigMoveRelative] + +Similarly we can move along a different axis. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut4] + :end-before: [cartesianTut4] + +The ``MoveRelative`` stage also offers an interface to +``Twist`` messages, allowing to specify rotations. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut5] + :end-before: [cartesianTut5] + +Lastly, we can compute linear movements in cartesian space +by providing offsets in joint space. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut6] + :end-before: [cartesianTut6] + +If we want to specify goals instead of directions, +we can use the ``MoveTo`` stage. In the following example +we use simple joint interpolation to move the robot to +a named pose. the named pose is defined in the urdf of +the robot configuration. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [initAndConfigMoveTo] + :end-before: [initAndConfigMoveTo] + +Lastly, we invoke the planning mechanism that traverses +the task hierarchy for us and compute a valid motion plan. +At this point, when you run this script you are able to +inspect the solutions of the individual stages in the rviz +mtc panel. diff --git a/core/doc/tutorials/first-steps.rst b/core/doc/tutorials/first-steps.rst new file mode 100644 index 000000000..864750731 --- /dev/null +++ b/core/doc/tutorials/first-steps.rst @@ -0,0 +1,22 @@ +.. _subsec-tut-firststeps: + +First Steps +----------- + +The MoveIt Task Constructor package contains several basic examples and +a pick-and-place demo. For all demos you should launch the basic environment: + +.. code-block:: + + roslaunch moveit_task_constructor_demo demo.launch + +Subsequently, you can run the individual demos: + +.. code-block:: + + rosrun moveit_task_constructor_demo cartesian + rosrun moveit_task_constructor_demo modular + roslaunch moveit_task_constructor_demo pickplace.launch + +To inspect the task hierarchy, be sure that you selected the correct solution topic +in the reviz moveit task constructor plugin. diff --git a/core/doc/tutorials/index.rst b/core/doc/tutorials/index.rst new file mode 100644 index 000000000..d62f5c24d --- /dev/null +++ b/core/doc/tutorials/index.rst @@ -0,0 +1,15 @@ +.. _sec-tutorials: + +Tutorials +========= + +The following tutorials take you step by step through the implementation of fundamental examples +of the moveit task constructor. + +.. toctree:: + :caption: Tutorials + + first-steps + cartesian + properties + pick-and-place diff --git a/core/doc/tutorials/pick-and-place.rst b/core/doc/tutorials/pick-and-place.rst new file mode 100644 index 000000000..bcddd5449 --- /dev/null +++ b/core/doc/tutorials/pick-and-place.rst @@ -0,0 +1,138 @@ +.. _subsec-tut-pick-place: + +Pick and Place +-------------- + +The following tutorial demonstrates how you can use the moveit +task constructor to plan and carry out pick and place movements. + +First, lets specify the planning group and the +end effector that you want to use. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut1] + :end-before: [pickAndPlaceTut1] + +Next, we add the object that we want to displace to the +planning scene. To this end, make sure that the planning scene +not already contains such object. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut2] + :end-before: [pickAndPlaceTut2] + +At this point, we are ready to create the task hierarchy. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut3] + :end-before: [pickAndPlaceTut3] + +The pipeline planner encapsulates the moveit interface +to sampling-based geometric motion planners. + +.. tip:: + Planning does not proceed linearly from top to bottom. + Rather, it proceeds from the inside out. + Connect stages therefore compute a motion plan between + two previously calculated subordinate solutions. + For a clear visualization, inspect the rviz mtc panel. + +Lets connect the current robot state with the solutions of +the following stages. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut4] + :end-before: [pickAndPlaceTut4] + +To pick the object, we first need to know possible end effector +poses with which we can perform a successful grasp. +For this, we use a ``GenerateGraspPoseStage``. +which essentially spawns poses +with a given ``angle_delta`` in circular fashion around a center +point. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut5] + :end-before: [pickAndPlaceTut5] + +Next, we need to compute the inverse kinematics of the robot arm +for all previously sampled poses. This way we can +rule out solutions that are not feasible due to the robot geometry. +The ``simpleGrasp`` stage combines ik calculation with motion plan +generation for opening and closing the end effector, as well as attaching +the object to the robot an disabling collision. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut6] + :end-before: [pickAndPlaceTut6] + +Lastly, we can insert all the previous steps into the ``Pick`` +container stage. At this point we might also specify approach and +lift twists for the robot relative to the object we want to grasp. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut7] + :end-before: [pickAndPlaceTut7] + +Since all the previous stages were chained together via their +constructor arguments, we only need to add the top level ``Pick`` +stage to the task hierarchy. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut8] + :end-before: [pickAndPlaceTut8] + +Thats everything we need for picking an object! +Lets find a motion plan to place the object + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut9] + :end-before: [pickAndPlaceTut9] + +Similar to the picking procedure, we define the place task. +First, start with sampling place poses. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut10] + :end-before: [pickAndPlaceTut10] + +Next, wrap the inverse kinematics computation and gripper +movements. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut11] + :end-before: [pickAndPlaceTut11] + +Lastly, add place and retract motions and add the ``Place`` +stage to the task hierarchy. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut12] + :end-before: [pickAndPlaceTut12] + +Finally, compute solutions for the task hierarchy and delete +the planner instances. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut13] + :end-before: [pickAndPlaceTut13] + +At this point, you might inspect the task hierarchy in the mtc rviz +plugin. + +.. tip:: + Use the mtc rviz plugin to graphically inspect the solutions + of individual stages in the task hierarchy. diff --git a/core/doc/tutorials/properties.rst b/core/doc/tutorials/properties.rst new file mode 100644 index 000000000..d9b260ec2 --- /dev/null +++ b/core/doc/tutorials/properties.rst @@ -0,0 +1,127 @@ +.. _subsec-tut-properties: + +Properties +---------- + +Properties are named attributes of a stage. +They can be used to configure the stages behaviour +and control further substages. Lets take a closer +look at how to work with properties. + +Basic Operations with Properties +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Lets define a property and assign a description, as well as +a value to it. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut1] + :end-before: [propertyTut1] + +Notice that a property always has two values: the current value +and the default value. Before we use the property, we might want to +check if the current value defined. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut2] + :end-before: [propertyTut2] + +Now we are ready to safely retrieve the values of the proprty! + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut3] + :end-before: [propertyTut3] + +The Property Map +^^^^^^^^^^^^^^^^ + +Usually a stage comprises multiple properties. A stage +contains a single PropertyMap that acts as a container for +all properties associated to that stage. + +Lets first create a PropertyMap in isolation and initialize +some properties using a dict. As you can see, properties can be +of arbitrary type. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut4] + :end-before: [propertyTut4] + +Properties can also be initialized using a more pythonic way. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut5] + :end-before: [propertyTut5] + +There are two ways to retrieve properties back from the property map. +We might only be interested in in the value of the property: + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut6] + :end-before: [propertyTut6] + +Or we can obtain a reference to the whole property object. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut7] + :end-before: [propertyTut7] + +The PropertyMap class additionally provides an iterator that can be used in loops. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut8] + :end-before: [propertyTut8] + +Remember that wer initialized our PropertyMap by using a dict. In fact, you +can also use an existing PropertMap to copy over some properties. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut9] + :end-before: [propertyTut9] + +Accessing Properties of a Stage +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +You can obtain a reference to the the PropertyMap of a stage like so + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut10] + :end-before: [propertyTut10] + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut11] + :end-before: [propertyTut11] + + +As mentioned, each stage contains a PropertyMap. +Stages communicate to each other via their interfaces. +If you want to forward properties through these interfaces, +you can use the reference of a stages' property object. + +.. literalinclude:: ../../../demo/scripts/compute_ik.py + :language: python + :start-after: [propertyTut12] + :end-before: [propertyTut12] + +.. literalinclude:: ../../../demo/scripts/compute_ik.py + :language: python + :start-after: [propertyTut13] + :end-before: [propertyTut13] + +.. literalinclude:: ../../../demo/scripts/compute_ik.py + :language: python + :start-after: [propertyTut14] + :end-before: [propertyTut14] + +Take a look at the :ref:`How-To-Guides ` +for a full example of this. diff --git a/core/include/moveit/python/python_tools/ros_types.h b/core/include/moveit/python/python_tools/ros_types.h new file mode 100644 index 000000000..e058ea8bf --- /dev/null +++ b/core/include/moveit/python/python_tools/ros_types.h @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include + +/** Provide pybind11 type converters for ROS types */ + +namespace moveit { +namespace python { + +PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name); +PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const char* ros_msg_name); + +} // namespace python +} // namespace moveit + +namespace pybind11 { +namespace detail { + +/// Convert ros::Duration / ros::WallDuration into a float +template +struct DurationCaster +{ + // C++ -> Python + static handle cast(T&& src, return_value_policy /* policy */, handle /* parent */) { + return PyFloat_FromDouble(src.toSec()); + } + + // Python -> C++ + bool load(handle src, bool convert) { + if (hasattr(src, "to_sec")) { + value = T(src.attr("to_sec")().cast()); + } else if (convert) { + value = T(src.cast()); + } else + return false; + return true; + } + + PYBIND11_TYPE_CASTER(T, _("Duration")); +}; + +template <> +struct type_caster : DurationCaster +{}; + +/// Convert ROS message types (C++ <-> python) +template ::value>> +struct type_caster_ros_msg +{ + // C++ -> Python + static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */) { + // serialize src into (python) buffer + std::size_t size = ros::serialization::serializationLength(src); + object pbuffer = reinterpret_steal(PyBytes_FromStringAndSize(nullptr, size)); + ros::serialization::OStream stream(reinterpret_cast(PyBytes_AsString(pbuffer.ptr())), size); + ros::serialization::serialize(stream, src); + // deserialize python type from buffer + object msg = moveit::python::createMessage(ros::message_traits::DataType::value()); + msg.attr("deserialize")(pbuffer); + return msg.release(); + } + + // Python -> C++ + bool load(handle src, bool /*convert*/) { + if (!moveit::python::convertible(src, ros::message_traits::DataType::value())) + return false; + // serialize src into (python) buffer + object pstream = module::import("io").attr("BytesIO")(); + src.attr("serialize")(pstream); + object pbuffer = pstream.attr("getvalue")(); + // deserialize C++ type from buffer + char* cbuffer = nullptr; + Py_ssize_t size; + PyBytes_AsStringAndSize(pbuffer.ptr(), &cbuffer, &size); + ros::serialization::IStream cstream(const_cast(reinterpret_cast(cbuffer)), size); + ros::serialization::deserialize(cstream, value); + return true; + } + + PYBIND11_TYPE_CASTER(T, _()); +}; + +template +struct type_caster::value>> : type_caster_ros_msg +{}; + +} // namespace detail +} // namespace pybind11 diff --git a/core/include/moveit/python/task_constructor/properties.h b/core/include/moveit/python/task_constructor/properties.h new file mode 100644 index 000000000..a164f5530 --- /dev/null +++ b/core/include/moveit/python/task_constructor/properties.h @@ -0,0 +1,81 @@ +#pragma once + +#include +#include +#include +#include +#include + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap) + +namespace moveit { +namespace python { + +class PYBIND11_EXPORT PropertyConverterBase +{ +public: + using to_python_converter_function = pybind11::object (*)(const boost::any&); + using from_python_converter_function = boost::any (*)(const pybind11::object&); + +protected: + static bool insert(const std::type_index& type_index, const std::string& ros_msg_name, + to_python_converter_function to, from_python_converter_function from); +}; + +/// utility class to register C++ / Python converters for a property of type T +template +class PropertyConverter : protected PropertyConverterBase +{ +public: + PropertyConverter() { insert(typeid(T), rosMsgName(), &toPython, &fromPython); } + +private: + static pybind11::object toPython(const boost::any& value) { return pybind11::cast(boost::any_cast(value)); } + static boost::any fromPython(const pybind11::object& po) { return pybind11::cast(po); } + + template + typename std::enable_if::value, std::string>::type rosMsgName() { + return ros::message_traits::DataType::value(); + } + + template + typename std::enable_if::value, std::string>::type rosMsgName() { + return std::string(); + } +}; + +namespace properties { + +/** Extension for pybind11::class_ to allow convienient definition of properties + * + * New method property(const char* name) adds a property getter/setter. + */ + +template +class class_ : public pybind11::classh // NOLINT(readability-identifier-naming) +{ + using base_class_ = pybind11::classh; + +public: + // forward all constructors + using base_class_::classh; + + template + class_& property(const char* name, const Extra&... extra) { + PropertyConverter(); // register corresponding property converter + auto getter = [name](const type_& self) { + const moveit::task_constructor::PropertyMap& props = self.properties(); + return props.get(name); + }; + auto setter = [name](type_& self, const PropertyType& value) { + moveit::task_constructor::PropertyMap& props = self.properties(); + props.set(name, boost::any(value)); + }; + base_class_::def_property(name, getter, setter, pybind11::return_value_policy::reference_internal, extra...); + return *this; + } +}; +} // namespace properties +} // namespace python +} // namespace moveit diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index fab732a71..203a08e02 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -53,6 +53,7 @@ class ContainerBase : public Stage size_t numChildren() const; Stage* findChild(const std::string& name) const; + Stage* operator[](int index) const; /** Callback function type used by traverse functions * Receives currently visited Stage and current depth in hierarchy @@ -75,6 +76,7 @@ class ContainerBase : public Stage virtual bool canCompute() const = 0; virtual void compute() = 0; + void explainFailure(std::ostream& os) const override; /// called by a (direct) child when a new solution becomes available virtual void onNewSolution(const SolutionBase& s) = 0; @@ -150,6 +152,7 @@ class Alternatives : public ParallelContainerBase void onNewSolution(const SolutionBase& s) override; }; +class FallbacksPrivate; /** Plan for different alternatives in sequence. * * Try to find feasible solutions using first child. Only if this fails, @@ -158,17 +161,23 @@ class Alternatives : public ParallelContainerBase */ class Fallbacks : public ParallelContainerBase { - mutable Stage* active_child_ = nullptr; + inline void replaceImpl(); public: - Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {} + PRIVATE_CLASS(Fallbacks) + Fallbacks(const std::string& name = "fallbacks"); void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; - bool canCompute() const override; - void compute() override; +protected: + Fallbacks(FallbacksPrivate* impl); void onNewSolution(const SolutionBase& s) override; + +private: + // not needed, we directly use corresponding virtual methods of FallbacksPrivate + bool canCompute() const final { return false; } + void compute() final {} }; class MergerPrivate; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 3b58d0da1..c9fb61f2c 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -76,7 +76,7 @@ namespace task_constructor { class ContainerBasePrivate : public StagePrivate { friend class ContainerBase; - friend void swap(StagePrivate*& lhs, StagePrivate*& rhs); + friend class ConnectingPrivate; // needed propagate setStatus() in newState() public: using container_type = StagePrivate::container_type; @@ -131,10 +131,11 @@ class ContainerBasePrivate : public StagePrivate inline const auto& externalToInternalMap() const { return internal_external_.by(); } /// called by a (direct) child when a solution failed - void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to); + virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to); protected: ContainerBasePrivate(ContainerBase* me, const std::string& name); + ContainerBasePrivate& operator=(ContainerBasePrivate&& other); // Set child's push interfaces: allow pushing if child requires it. inline void setChildsPushBackwardInterface(StagePrivate* child) { @@ -148,14 +149,19 @@ class ContainerBasePrivate : public StagePrivate child->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } - /// Set ENABLED / PRUNED status of the solution tree starting from s into given direction + /// Set ENABLED/PRUNED status of a solution branch starting from target into the given direction template - void setStatus(const InterfaceState* s, InterfaceState::Status status); + void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, + InterfaceState::Status status); - /// copy external_state to a child's interface and remember the link in internal_external map + /// Copy external_state to a child's interface and remember the link in internal_external map template - void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); - /// lift solution from internal to external level + void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated); + // non-template version + void copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target, + Interface::UpdateFlags updated); + + /// Lift solution from internal to external level void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to); @@ -228,12 +234,91 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const; private: - /// callback for new externally received states + /// notify callback for new externally received interface states template - void onNewExternalState(Interface::iterator external, bool updated); + void propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated); + + // override to customize behavior on received interface states (default: propagateStateToAllChildren()) + virtual void initializeExternalInterfaces(); }; PIMPL_FUNCTIONS(ParallelContainerBase) +/* The Fallbacks container needs to implement different behaviour based on its interface. + * Thus, we implement 3 different classes: for Generator, Propagator, and Connect-like interfaces. + * FallbacksPrivate is the common base class for all of them, defining the common API + * to be used by the Fallbacks container. + * The actual interface-specific class is instantiated in initializeExternalInterfaces() + * resp. Fallbacks::replaceImpl() when the actual interface is known. + * The key difference between the 3 variants is how they advance to the next job. */ +class FallbacksPrivate : public ParallelContainerBasePrivate +{ +public: + FallbacksPrivate(Fallbacks* me, const std::string& name); + FallbacksPrivate(FallbacksPrivate&& other); + + void initializeExternalInterfaces() final; + void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; + + // virtual methods specific to each variant + virtual void onNewSolution(const SolutionBase& s); + virtual void reset() {} +}; +PIMPL_FUNCTIONS(Fallbacks) + +/* Class shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator, + which both have the notion of a currently active child stage */ +class FallbacksPrivateCommon : public FallbacksPrivate +{ +public: + FallbacksPrivateCommon(FallbacksPrivate&& other) : FallbacksPrivate(std::move(other)) {} + + /// Advance to next child + inline void nextChild(); + /// Advance to the next job, assuming that the current child is exhausted on the current job. + virtual bool nextJob() = 0; + + void reset() override; + bool canCompute() const override; + void compute() override; + + container_type::const_iterator current_; // currently active child +}; + +/// Fallbacks implementation for GENERATOR interface +struct FallbacksPrivateGenerator : FallbacksPrivateCommon +{ + FallbacksPrivateGenerator(FallbacksPrivate&& old); + bool nextJob() override; +}; + +/// Fallbacks implementation for FORWARD or BACKWARD interface +struct FallbacksPrivatePropagator : FallbacksPrivateCommon +{ + FallbacksPrivatePropagator(FallbacksPrivate&& old); + void reset() override; + void onNewSolution(const SolutionBase& s) override; + bool nextJob() override; + + Interface::Direction dir_; // propagation direction + Interface::iterator job_; // pointer to currently processed external state + bool job_has_solutions_; // flag indicating whether the current job generated solutions +}; + +/// Fallbacks implementation for CONNECT interface +struct FallbacksPrivateConnect : FallbacksPrivate +{ + FallbacksPrivateConnect(FallbacksPrivate&& old); + void reset() override; + bool canCompute() const override; + void compute() override; + void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; + + template + void propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated); + + mutable container_type::const_iterator active_; // child picked for compute() +}; + class WrapperBasePrivate : public ParallelContainerBasePrivate { friend class WrapperBase; diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index dbbc368e0..02320b4c3 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -40,6 +40,7 @@ #include #include +#include namespace moveit { namespace task_constructor { @@ -69,6 +70,14 @@ class CostTerm class TrajectoryCostTerm : public CostTerm { public: + enum class Mode + { + AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */, + START_INTERFACE, + END_INTERFACE, + TRAJECTORY + }; + double operator()(const SolutionSequence& s, std::string& comment) const override; double operator()(const WrappedSolution& s, std::string& comment) const override; }; @@ -113,15 +122,34 @@ class Constant : public CostTerm double cost; }; -/// trajectory length (interpolated between waypoints) +/// trajectory length with optional weighting for different joints class PathLength : public TrajectoryCostTerm { public: + /// By default, all joints are considered with same weight of 1.0 PathLength() = default; - PathLength(std::vector j) : joints{ std::move(j) } {}; + /// Limit measurements to given joint names + PathLength(std::vector joints); + /// Limit measurements to given joints and use given weighting + PathLength(std::map j) : joints(std::move(j)) {} double operator()(const SubTrajectory& s, std::string& comment) const override; - std::vector joints; + std::map joints; //< joint weights +}; + +/// (weighted) joint-space distance to reference pose +class DistanceToReference : public TrajectoryCostTerm +{ +public: + DistanceToReference(const moveit_msgs::msg::RobotState& ref, Mode m = Mode::AUTO, + std::map w = std::map()); + DistanceToReference(const std::map& ref, Mode m = Mode::AUTO, + std::map w = std::map()); + double operator()(const SubTrajectory& s, std::string& comment) const override; + + moveit_msgs::msg::RobotState reference; + std::map weights; + Mode mode; }; /// execution duration of the whole trajectory @@ -153,14 +181,6 @@ class LinkMotion : public TrajectoryCostTerm class Clearance : public TrajectoryCostTerm { public: - enum class Mode - { - AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */, - START_INTERFACE, - END_INTERFACE, - TRAJECTORY - }; - Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group", Mode mode = Mode::AUTO); bool with_world; diff --git a/core/include/moveit/task_constructor/introspection.h b/core/include/moveit/task_constructor/introspection.h index c42e0209a..10652d329 100644 --- a/core/include/moveit/task_constructor/introspection.h +++ b/core/include/moveit/task_constructor/introspection.h @@ -96,8 +96,8 @@ class Introspection void publishAllSolutions(bool wait = true); /// get solution - bool getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr req, - const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr res); + bool getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr& req, + const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr& res); /// retrieve id of given stage uint32_t stageId(const moveit::task_constructor::Stage* const s) const; diff --git a/core/include/moveit/task_constructor/merge.h b/core/include/moveit/task_constructor/merge.h index 7ac7eef5d..734418f1a 100644 --- a/core/include/moveit/task_constructor/merge.h +++ b/core/include/moveit/task_constructor/merge.h @@ -39,6 +39,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -57,6 +58,7 @@ moveit::core::JointModelGroup* merge(const std::vector& sub_trajectories, - const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group); + const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group, + const trajectory_processing::TimeParameterization& time_parameterization); } // namespace task_constructor } // namespace moveit diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h index 1f41353be..f48935070 100644 --- a/core/include/moveit/task_constructor/moveit_compat.h +++ b/core/include/moveit/task_constructor/moveit_compat.h @@ -39,12 +39,8 @@ #pragma once #include +#include #define MOVEIT_VERSION_GE(major, minor, patch) \ (MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \ major * 1'000'000 + minor * 1'000 + patch) - -// use object shape poses relative to a single object pose -#define MOVEIT_HAS_OBJECT_POSE 1 - -#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK 0 diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index c5f7cfaf9..8dfa3ea2b 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -56,8 +56,10 @@ class CartesianPath : public PlannerInterface void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); } void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); } - void setMaxVelocityScaling(double factor) { setProperty("max_velocity_scaling_factor", factor); } - void setMaxAccelerationScaling(double factor) { setProperty("max_acceleration_scaling_factor", factor); } + [[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off + void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } + [[deprecated("Replace with setMaxAccelerationScalingFactor")]] // clang-format off + void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } void init(const moveit::core::RobotModelConstPtr& robot_model) override; @@ -66,9 +68,11 @@ class CartesianPath : public PlannerInterface const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + + std::string getPlannerId() const override { return std::string("CartesianPath"); } }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index 9fa94113c..66df1e510 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -63,9 +63,11 @@ class JointInterpolationPlanner : public PlannerInterface const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + + std::string getPlannerId() const override { return std::string("JointInterpolationPlanner"); } }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h new file mode 100644 index 000000000..2ddb47f51 --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/multi_planner.h @@ -0,0 +1,77 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: meta planner, running multiple planners in parallel or sequence +*/ + +#pragma once + +#include +#include + +namespace moveit { +namespace task_constructor { +namespace solvers { + +MOVEIT_CLASS_FORWARD(MultiPlanner); + +/** A meta planner that runs multiple alternative planners in sequence and returns the first found solution. + * + * This is useful to sequence different planning strategies of increasing complexity, + * e.g. Cartesian or joint-space interpolation first, then OMPL, ... + * This is (slightly) different from the Fallbacks container, as the MultiPlanner directly applies its planners to each + * individual planning job. In contrast, the Fallbacks container first runs the active child to exhaustion before + * switching to the next child, which possibly applies a different planning strategy. + */ +class MultiPlanner : public PlannerInterface, public std::vector +{ +public: + using PlannerList = std::vector; + using PlannerList::PlannerList; // inherit all std::vector constructors + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + + bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; +}; +} // namespace solvers +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 412d0efa5..f024da201 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -32,13 +32,16 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Robert Haschke - Desc: plan using MoveIt's PlanningPipeline +/* Authors: Robert Haschke, Sebastian Jahr + Description: Solver that uses a set of MoveIt PlanningPipelines to solve a given planning problem. */ #pragma once #include +#include +#include +#include #include #include @@ -56,48 +59,139 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner); class PipelinePlanner : public PlannerInterface { public: - struct Specification - { - moveit::core::RobotModelConstPtr model; - std::string ns{ "ompl" }; - std::string pipeline{ "ompl" }; - std::string adapter_param{ "request_adapters" }; - }; - - static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, - const moveit::core::RobotModelConstPtr& model) { - Specification spec; - spec.model = model; - return create(node, spec); + /** Simple Constructor to use only a single pipeline + * \param [in] node ROS 2 node + * \param [in] pipeline_name Name of the planning pipeline to be used. This is also the assumed namespace where the + * parameters of this pipeline can be found \param [in] planner_id Planner id to be used for planning + */ + PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl", + const std::string& planner_id = ""); + + [[deprecated("Deprecated: Use new constructor implementations.")]] // clang-format off + PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& /*planning_pipeline*/){}; + + /** \brief Constructor + * \param [in] node ROS 2 node + * \param [in] pipeline_id_planner_id_map A map containing pairs of planning pipeline name and planner plugin name + * for the planning requests + * \param [in] planning_pipelines Optional: A map with the pipeline names and initialized corresponding planning + * pipelines + * \param [in] stopping_criterion_callback Callback function to stop parallel planning pipeline according to a user defined criteria + * \param [in] solution_selection_function Callback function to choose the best solution when multiple pipelines are used + */ + PipelinePlanner(const rclcpp::Node::SharedPtr& node, + const std::unordered_map& pipeline_id_planner_id_map, + const std::unordered_map& planning_pipelines = + std::unordered_map(), + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr, + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function = + &moveit::planning_pipeline_interfaces::getShortestSolution); + + [[deprecated("Replaced with setPlannerId(pipeline_name, planner_id)")]] // clang-format off + void setPlannerId(const std::string& /*planner*/) { /* Do nothing */ } - static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec); - - /** - * - * @param node used to load the parameters for the planning pipeline + /** \brief Set the planner id for a specific planning pipeline for the planning requests + * \param [in] pipeline_name Name of the planning pipeline for which the planner id is set + * \param [in] planner_id Name of the planner ID that should be used by the planning pipeline + * \return true if the pipeline exists and the corresponding ID is set successfully + */ + bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id); + + /** \brief Set stopping criterion function for parallel planning + * \param [in] stopping_criterion_callback New stopping criterion function that will be used + */ + void setStoppingCriterionFunction(const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback); + + /** \brief Set solution selection function for parallel planning + * \param [in] solution_selection_function New solution selection that will be used + */ + void setSolutionSelectionFunction(const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function); + + /** \brief This function is called when an MTC task that uses this solver is initialized. If no pipelines are + * configured when this function is invoked, the planning pipelines named in the 'pipeline_id_planner_id_map' are + * initialized with the given robot model. + * \param [in] robot_model A robot model that is used to initialize the + * planning pipelines of this solver */ - PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl"); - - PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline); - - void setPlannerId(const std::string& planner) { setProperty("planner", planner); } - void init(const moveit::core::RobotModelConstPtr& robot_model) override; + /** + * \brief Plan a trajectory from a planning scene 'from' to scene 'to' + * \param [in] from Start planning scene + * \param [in] to Goal planning scene (used to create goal constraints) + * \param [in] joint_model_group Group of joints for which this trajectory is created + * \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds + * \param [in] result Reference to the location where the created trajectory is stored if planning is successful + * \param [in] path_constraints Path contraints for the planning problem + * \return true If the solver found a successful solution for the given planning problem + */ bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, - const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const core::JointModelGroup* joint_model_group, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + /** \brief Plan a trajectory from a planning scene 'from' to a target pose with an offset + * \param [in] from Start planning scene + * \param [in] link Link for which a target pose is given + * \param [in] offset Offset to be applied to a given target pose + * \param [in] target Target pose + * \param [in] joint_model_group Group of joints for which this trajectory is created + * \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds + * \param [in] result Reference to the location where the created trajectory is stored if planning is successful + * \param [in] path_constraints Path contraints for the planning problem + * \return true If the solver found a successful solution for the given planning problem + */ bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* joint_model_group, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + /** + * \brief Get planner name + * \return Name of the last successful planner + */ + std::string getPlannerId() const override; + protected: - std::string pipeline_name_; - planning_pipeline::PlanningPipelinePtr planner_; + /** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by + * the public plan function after the goal constraints are generated. This function uses a predefined number of + * planning pipelines in parallel to solve the planning problem and choose the best (user-defined) solution. + * \param [in] planning_scene Scene for which the planning should be solved + * \param [in] joint_model_group + * Group of joints for which this trajectory is created + * \param [in] goal_constraints Set of constraints that need to + * be satisfied by the solution + * \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds + * \param [in] result Reference to the location where the created + * trajectory is stored if planning is successful + * \param [in] path_constraints Path contraints for the planning + * problem + * \return true If the solver found a successful solution for the given planning problem + */ + bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, + const moveit::core::JointModelGroup* joint_model_group, + const moveit_msgs::msg::Constraints& goal_constraints, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()); + rclcpp::Node::SharedPtr node_; + + std::string last_successful_planner_; + + /** \brief Map of pipeline names (ids) and their corresponding planner ids. The planning problem is solved for every + * pipeline-planner pair in this map. If no pipelines are passed via constructor argument, the pipeline names are + * used to initialize a set of planning pipelines. */ + std::unordered_map pipeline_id_planner_id_map_; + + /** \brief Map of pipelines names and planning pipelines. This map is used to quickly search for a requested motion + * planning pipeline when during plan(..) */ + std::unordered_map planning_pipelines_; + + moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_; + moveit::planning_pipeline_interfaces::SolutionSelectionFunction solution_selection_function_; + }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index f67649328..ff9948414 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -49,6 +49,9 @@ MOVEIT_CLASS_FORWARD(PlanningScene); namespace robot_trajectory { MOVEIT_CLASS_FORWARD(RobotTrajectory); } +namespace trajectory_processing { +MOVEIT_CLASS_FORWARD(TimeParameterization); +} namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(LinkModel); @@ -75,6 +78,12 @@ class PlannerInterface const PropertyMap& properties() const { return properties_; } void setProperty(const std::string& name, const boost::any& value) { properties_.set(name, value); } + void setTimeout(double timeout) { properties_.set("timeout", timeout); } + void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); } + void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); } + void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) { + properties_.set("time_parameterization", tp); + } virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0; @@ -84,11 +93,15 @@ class PlannerInterface robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; - /// plan trajectory from current robot state to Cartesian target + /// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; + + // get name of the planner + virtual std::string getPlannerId() const = 0; }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index be2fa7aa8..ad93a0a83 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -241,6 +241,8 @@ class Stage /// Should we generate failure solutions? Note: Always report a failure! bool storeFailures() const; + virtual void explainFailure(std::ostream& os) const; + /// Get the stage's property map PropertyMap& properties(); const PropertyMap& properties() const { return const_cast(this)->properties(); } @@ -300,8 +302,8 @@ class PropagatingEitherWay : public ComputeBase // Default implementations, using generic compute(). // Override if you want to use different code for FORWARD and BACKWARD directions. - virtual void computeForward(const InterfaceState& from) { computeGeneric(from); } - virtual void computeBackward(const InterfaceState& to) { computeGeneric(to); } + virtual void computeForward(const InterfaceState& from); + virtual void computeBackward(const InterfaceState& to); protected: // constructor for use in derived classes diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 0dbd9f693..55ae265f5 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -61,7 +61,6 @@ class StagePrivate { friend class Stage; friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); - friend void swap(StagePrivate*& lhs, StagePrivate*& rhs); public: /// container type used to store children @@ -100,17 +99,11 @@ class StagePrivate inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); } inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); } - /// direction-based access to pull/push interfaces - inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; } - inline InterfacePtr pushInterface(Interface::Direction dir) { - return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); - } - inline InterfaceConstPtr pullInterface(Interface::Direction dir) const { - return dir == Interface::FORWARD ? starts_ : ends_; - } - inline InterfaceConstPtr pushInterface(Interface::Direction dir) const { - return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); - } + /// direction-based access to pull interface + template + inline InterfacePtr pullInterface(); + // non-template version + inline InterfacePtr pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; } /// set parent of stage /// enforce only one parent exists @@ -165,6 +158,8 @@ class StagePrivate void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); protected: + StagePrivate& operator=(StagePrivate&& other); + // associated/owning Stage instance Stage* me_; @@ -205,6 +200,15 @@ class StagePrivate PIMPL_FUNCTIONS(Stage) std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); +template <> +inline InterfacePtr StagePrivate::pullInterface() { + return starts_; +} +template <> +inline InterfacePtr StagePrivate::pullInterface() { + return ends_; +} + template <> inline void StagePrivate::send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution) { @@ -301,6 +305,7 @@ PIMPL_FUNCTIONS(MonitoringGenerator) class ConnectingPrivate : public ComputeBasePrivate { friend class Connecting; + friend struct FallbacksPrivateConnect; public: struct StatePair : std::pair @@ -311,18 +316,15 @@ class ConnectingPrivate : public ComputeBasePrivate } static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB, const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) { - unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled(); // combine bits into two-digit binary number - unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled(); + bool lhs = lhsA.enabled() && lhsB.enabled(); + bool rhs = rhsA.enabled() && rhsB.enabled(); + if (lhs == rhs) // if enabled status is identical return lhsA + lhsB < rhsA + rhsB; // compare the sums of both contributions - // one of the states in each pair should be enabled - assert(lhs != 0b00 && rhs != 0b00); - // both states valid (b11) - if (lhs == 0b11) - return true; - if (rhs == 0b11) - return false; - return lhs < rhs; // disabled states in 1st component go before disabled states in 2nd component + + // sort both-enabled pairs first + static_assert(true > false, "Comparing enabled states requires true > false"); + return lhs > rhs; } }; @@ -334,7 +336,7 @@ class ConnectingPrivate : public ComputeBasePrivate // Check whether there are pending feasible states that could connect to source template - bool hasPendingOpposites(const InterfaceState* source) const; + bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const; std::ostream& printPendingPairs(std::ostream& os = std::cerr) const; @@ -343,9 +345,9 @@ class ConnectingPrivate : public ComputeBasePrivate template inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second); - // get informed when new start or end state becomes available + // notify callback to get informed about newly inserted (or updated) start or end states template - void newState(Interface::iterator it, bool updated); + void newState(Interface::iterator it, Interface::UpdateFlags updated); // ordered list of pending state pairs ordered pending; diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index c8cf883d5..94dfde045 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -73,7 +73,13 @@ class Connect : public Connecting WAYPOINTS = 1 }; - using GroupPlannerVector = std::vector >; + struct PlannerIdTrajectoryPair + { + std::string planner_name; + robot_trajectory::RobotTrajectoryConstPtr robot_trajectory_ptr; + }; + + using GroupPlannerVector = std::vector>; Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {}); void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) { @@ -85,10 +91,10 @@ class Connect : public Connecting void compute(const InterfaceState& from, const InterfaceState& to) override; protected: - SolutionSequencePtr makeSequential(const std::vector& sub_trajectories, + SolutionSequencePtr makeSequential(const std::vector& trajectory_planner_vector, const std::vector& intermediate_scenes, const InterfaceState& from, const InterfaceState& to); - SubTrajectoryPtr merge(const std::vector& sub_trajectories, + SubTrajectoryPtr merge(const std::vector& trajectory_planner_vector, const std::vector& intermediate_scenes, const moveit::core::RobotState& state); diff --git a/core/include/moveit/task_constructor/stages/fixed_state.h b/core/include/moveit/task_constructor/stages/fixed_state.h index 2ace2726a..adb3bb1c8 100644 --- a/core/include/moveit/task_constructor/stages/fixed_state.h +++ b/core/include/moveit/task_constructor/stages/fixed_state.h @@ -48,7 +48,7 @@ namespace stages { class FixedState : public Generator { public: - FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr = nullptr); + FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr scene = nullptr); void setState(const planning_scene::PlanningScenePtr& scene); void reset() override; diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index 1fc3b3cc8..bc5da2041 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -149,8 +149,9 @@ class ModifyPlanningScene : public PropagatingEitherWay protected: // apply stored modifications to scene - InterfaceState apply(const InterfaceState& from, bool invert); - void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::msg::CollisionObject& object); + std::pair apply(const InterfaceState& from, bool invert); + void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::msg::CollisionObject& object, + bool invert); void attachObjects(planning_scene::PlanningScene& scene, const std::pair>& pair, bool invert); void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert); diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index 107bac3b6..7be5f63be 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -66,10 +66,8 @@ class MoveTo : public PropagatingEitherWay void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); } void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); template - void setIKFrame(const T& p, const std::string& link) { - Eigen::Isometry3d pose; - pose = p; - setIKFrame(pose, link); + void setIKFrame(const T& pose, const std::string& link) { + setIKFrame(Eigen::Isometry3d(pose), link); } void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index 6fd9786b4..7fe83cd22 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -80,13 +80,12 @@ class SimpleGraspBase : public SerialContainer /// set properties of IK solver void setIKFrame(const geometry_msgs::msg::PoseStamped& transform) { setProperty("ik_frame", transform); } void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); + void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } + /// allow setting IK frame from any type T that converts to Eigen::Isometry3d template - void setIKFrame(const T& t, const std::string& link) { - Eigen::Isometry3d transform; - transform = t; - setIKFrame(transform, link); + void setIKFrame(const T& transform, const std::string& link) { + setIKFrame(Eigen::Isometry3d(transform), link); } - void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } void setMaxIKSolutions(uint32_t max_ik_solutions) { setProperty("max_ik_solutions", max_ik_solutions); } }; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index dc278dd93..5bff53d74 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -83,9 +84,11 @@ class InterfaceState enum Status { ENABLED, // state is actively considered during planning - PRUNED, // state is disabled because a required connected state failed - FAILED, // state that failed, causing the whole partial solution to be disabled + ARMED, // disabled state in a Connecting interface that will become re-enabled with a new opposite state + PRUNED, // disabled state on a pruned solution branch }; + static const char* colorForStatus(unsigned int s) { return STATUS_COLOR_[s]; } + /** InterfaceStates are ordered according to two values: * Depth of interlinked trajectory parts and accumulated trajectory costs along that path. * Preference ordering considers high-depth first and within same depth, minimal cost paths. @@ -101,8 +104,6 @@ class InterfaceState inline Status status() const { return std::get<0>(*this); } inline bool enabled() const { return std::get<0>(*this) == ENABLED; } - inline bool failed() const { return std::get<0>(*this) == FAILED; } - inline bool pruned() const { return std::get<0>(*this) == PRUNED; } inline unsigned int depth() const { return std::get<1>(*this); } inline double cost() const { return std::get<2>(*this); } @@ -129,6 +130,7 @@ class InterfaceState /// copy an existing InterfaceState, but not including incoming/outgoing trajectories InterfaceState(const InterfaceState& other); InterfaceState(InterfaceState&& other) = default; + InterfaceState& operator=(const InterfaceState& other) = default; inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; } inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; } @@ -139,15 +141,24 @@ class InterfaceState /// states are ordered by priority inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; } + inline const Priority& priority() const { return priority_; } + /// Update priority and call owner's notify() if possible + void updatePriority(const InterfaceState::Priority& priority); + /// Update status, but keep current priority + void updateStatus(Status status); + Interface* owner() const { return owner_; } private: // these methods should be only called by SolutionBase::set[Start|End]State() inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); } inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); } + // Set new priority without updating the owning interface (USE WITH CARE) + inline void setPriority(const Priority& prio) { priority_ = prio; } private: + static const char* STATUS_COLOR_[]; planning_scene::PlanningSceneConstPtr scene_; PropertyMap properties_; /// trajectories which are *timewise before* this state @@ -170,6 +181,7 @@ class Interface : public ordered class iterator : public base_type::iterator { public: + iterator() = default; iterator(base_type::iterator other) : base_type::iterator(other) {} InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); } @@ -191,10 +203,27 @@ class Interface : public ordered { FORWARD, BACKWARD, - START = FORWARD, - END = BACKWARD }; - using NotifyFunction = std::function; + enum Update + { + STATUS = 1 << 0, + PRIORITY = 1 << 1, + ALL = STATUS | PRIORITY, + }; + using UpdateFlags = utils::Flags; + using NotifyFunction = std::function; + + class DisableNotify + { + Interface& if_; + Interface::NotifyFunction old_; + + public: + DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); } + ~DisableNotify() { old_.swap(if_.notify_); } + }; + friend class DisableNotify; + Interface(const NotifyFunction& notify = NotifyFunction()); /// add a new InterfaceState @@ -205,9 +234,10 @@ class Interface : public ordered /// update state's priority (and call notify_ if it really has changed) void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority); + inline bool notifyEnabled() const { return static_cast(notify_); } private: - const NotifyFunction notify_; + NotifyFunction notify_; // restrict access to some functions to ensure consistency // (we need to set/unset InterfaceState::owner_) @@ -220,6 +250,17 @@ class Interface : public ordered std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio); std::ostream& operator<<(std::ostream& os, const Interface& interface); +std::ostream& operator<<(std::ostream& os, Interface::Direction dir); + +/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found. +template +size_t getIndex(const T& container, typename T::const_iterator search) { + size_t index = 1; + for (typename T::const_iterator it = container.begin(), end = container.end(); it != end; ++it, ++index) + if (it == search) + return index; + return 0; +} class CostTerm; class StagePrivate; @@ -234,8 +275,8 @@ class SolutionBase public: virtual ~SolutionBase() = default; - inline const InterfaceState* start() const { return start_; } - inline const InterfaceState* end() const { return end_; } + [[nodiscard]] inline const InterfaceState* start() const { return start_; } + [[nodiscard]] inline const InterfaceState* end() const { return end_; } /** Set the solution's start_state_ * @@ -257,23 +298,28 @@ class SolutionBase const_cast(state).addIncoming(this); } - inline const Stage* creator() const { return creator_; } + [[nodiscard]] inline const Stage* creator() const { return creator_; } void setCreator(Stage* creator); - inline double cost() const { return cost_; } + [[nodiscard]] inline double cost() const { return cost_; } void setCost(double cost); void markAsFailure(const std::string& msg = std::string()); - inline bool isFailure() const { return !std::isfinite(cost_); } + [[nodiscard]] inline bool isFailure() const { return !std::isfinite(cost_); } + + [[nodiscard]] const std::string& plannerId() const { return planner_id_; } + void setPlannerId(const std::string& planner_id) { planner_id_ = planner_id; } - const std::string& comment() const { return comment_; } + [[nodiscard]] const std::string& comment() const { return comment_; } void setComment(const std::string& comment) { comment_ = comment; } - auto& markers() { return markers_; } + [[nodiscard]] auto& markers() { return markers_; } const auto& markers() const { return markers_; } + /// convert solution to message + void toMsg(moveit_task_constructor_msgs::msg::Solution& solution, Introspection* introspection = nullptr) const; /// append this solution to Solution msg - virtual void fillMessage(moveit_task_constructor_msgs::msg::Solution& solution, - Introspection* introspection = nullptr) const = 0; + virtual void appendTo(moveit_task_constructor_msgs::msg::Solution& solution, + Introspection* introspection = nullptr) const = 0; void fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection = nullptr) const; /// required to dispatch to type-specific CostTerm methods via vtable @@ -283,14 +329,17 @@ class SolutionBase bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; } protected: - SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "") - : creator_(creator), cost_(cost), comment_(std::move(comment)) {} + SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = std::string(""), + std::string planner_id = std::string("")) + : creator_(creator), cost_(cost), planner_id_(std::move(planner_id)), comment_(std::move(comment)) {} private: // back-pointer to creating stage, allows to access sub-solutions Stage* creator_; // associated cost double cost_; + // name of the planner used to create this solution + std::string planner_id_; // comment for this solution, e.g. explanation of failure std::string comment_; // markers for this solution, e.g. target frame or collision indicators @@ -308,14 +357,14 @@ class SubTrajectory : public SolutionBase public: SubTrajectory( const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(), - double cost = 0.0, std::string comment = "") - : SolutionBase(nullptr, cost, std::move(comment)), trajectory_(trajectory) {} + double cost = 0.0, std::string comment = std::string(""), std::string planner_id = std::string("")) + : SolutionBase(nullptr, cost, std::move(comment), std::move(planner_id)), trajectory_(trajectory) {} robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; } - void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, - Introspection* introspection = nullptr) const override; + void appendTo(moveit_task_constructor_msgs::msg::Solution& msg, + Introspection* introspection = nullptr) const override; double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -348,7 +397,7 @@ class SolutionSequence : public SolutionBase void push_back(const SolutionBase& solution); /// append all subsolutions to solution - void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const override; + void appendTo(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const override; double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -379,8 +428,8 @@ class WrappedSolution : public SolutionBase : SolutionBase(creator, cost), wrapped_(wrapped) {} explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped) : WrappedSolution(creator, wrapped, wrapped->cost()) {} - void fillMessage(moveit_task_constructor_msgs::msg::Solution& solution, - Introspection* introspection = nullptr) const override; + void appendTo(moveit_task_constructor_msgs::msg::Solution& solution, + Introspection* introspection = nullptr) const override; double computeCost(const CostTerm& cost, std::string& comment) const override; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 04ca2ceb5..ad1327749 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -47,6 +47,7 @@ #include #include +#include #include @@ -74,6 +75,8 @@ class Task : protected WrapperBase { public: PRIVATE_CLASS(Task) + using WrapperBase::setCostTerm; + using WrapperBase::operator[]; Task(const std::string& ns = "", bool introspection = true, ContainerBase::pointer&& container = std::make_unique("task pipeline")); @@ -84,6 +87,9 @@ class Task : protected WrapperBase const std::string& name() const { return stages()->name(); } void setName(const std::string& name) { stages()->setName(name); } + Stage* findChild(const std::string& name) const { return stages()->findChild(name); } + Stage* operator[](int index) const { return stages()->operator[](index); } + const moveit::core::RobotModelConstPtr& getRobotModel() const; /// setting the robot model also resets the task void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model); @@ -119,15 +125,18 @@ class Task : protected WrapperBase void init(); /// reset, init scene (if not yet done), and init all stages, then start planning - bool plan(size_t max_solutions = 0); + moveit::core::MoveItErrorCode plan(size_t max_solutions = 0); /// interrupt current planning (or execution) void preempt(); /// execute solution, return the result - moveit_msgs::msg::MoveItErrorCodes execute(const SolutionBase& s); + moveit::core::MoveItErrorCode execute(const SolutionBase& s); /// print current task state (number of found solutions and propagated states) to std::cout void printState(std::ostream& os = std::cout) const; + /// print an explanation for a planning failure to os + void explainFailure(std::ostream& os = std::cout) const override; + size_t numSolutions() const { return solutions().size(); } const ordered& solutions() const { return stages()->solutions(); } const std::list& failures() const { return stages()->failures(); } diff --git a/core/include/moveit/task_constructor/task_p.h b/core/include/moveit/task_constructor/task_p.h index 327f16318..f0e6dc0a6 100644 --- a/core/include/moveit/task_constructor/task_p.h +++ b/core/include/moveit/task_constructor/task_p.h @@ -51,15 +51,14 @@ namespace task_constructor { class TaskPrivate : public WrapperBasePrivate { friend class Task; + TaskPrivate& operator=(TaskPrivate&& other); public: TaskPrivate(Task* me, const std::string& ns); + const std::string& ns() const { return ns_; } const ContainerBase* stages() const; -protected: - static void swap(StagePrivate*& lhs, StagePrivate*& rhs); - private: std::string ns_; robot_model_loader::RobotModelLoaderPtr robot_model_loader_; diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index 302d47804..fcc6f591c 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -140,11 +140,6 @@ class Flags Int i; }; -#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags; - -const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, - std::string frame); - bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, const moveit::core::JointModelGroup* jmg, SolutionBase& solution, const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame); diff --git a/core/package.xml b/core/package.xml index f3517c12c..3cb44239b 100644 --- a/core/package.xml +++ b/core/package.xml @@ -1,32 +1,40 @@ - + moveit_task_constructor_core - 0.0.0 + 0.1.3 MoveIt Task Pipeline + https://github.com/ros-planning/moveit_task_constructor + https://github.com/ros-planning/moveit_task_constructor + https://github.com/ros-planning/moveit_task_constructor/issues + BSD Michael Goerner Robert Haschke ament_cmake + ament_cmake_python - tf2_eigen geometry_msgs - rclcpp moveit_core moveit_ros_planning moveit_ros_planning_interface moveit_task_constructor_msgs - visualization_msgs + rclcpp rviz_marker_tools + tf2_eigen + visualization_msgs ament_cmake_gmock ament_cmake_gtest + ament_cmake_pytest + moveit_configs_utils - + launch_testing_ament_cmake + moveit_planners ament_cmake diff --git a/core/python/CMakeLists.txt b/core/python/CMakeLists.txt new file mode 100644 index 000000000..dcb988585 --- /dev/null +++ b/core/python/CMakeLists.txt @@ -0,0 +1,43 @@ +# We rely on pybind11's smart_holder branch imported pybind11 via git submodule + +find_package(ament_cmake_python REQUIRED) +find_package(Python3 COMPONENTS Interpreter Development) + +# Use minimum-size optimization for pybind11 bindings +add_compile_options("-Os") + +# configure pybind11 install for use by downstream packages in install space +set(PYBIND11_INSTALL ON CACHE INTERNAL "Install pybind11") +set(CMAKE_INSTALL_INCLUDEDIR include/moveit/python) +set(PYBIND11_CMAKECONFIG_INSTALL_DIR share/${PROJECT_NAME}/cmake + CACHE INTERNAL "install path for pybind11 cmake files") + +# source pybind11 folder, which exposes its targets and installs them +if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/pybind11/CMakeLists.txt") + message("Missing content of submodule pybind11: Use 'git clone --recurse-submodule' in future.\n" + "Checking out content automatically") + execute_process(COMMAND git submodule init WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) + execute_process(COMMAND git submodule update WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) +endif() +add_subdirectory(pybind11) + +# C++ wrapper code +add_subdirectory(bindings) + +ament_python_install_package("src") + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + set(_pytest_tests + test/test_mtc.py + test/rostest_mtc.py + ) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() +endif() diff --git a/core/python/bindings/CMakeLists.txt b/core/python/bindings/CMakeLists.txt new file mode 100644 index 000000000..d92cb8f5e --- /dev/null +++ b/core/python/bindings/CMakeLists.txt @@ -0,0 +1,42 @@ +# python tools support lib +set(TOOLS_LIB_NAME moveit_python_tools) +set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/python_tools) +add_library(${TOOLS_LIB_NAME} SHARED + ${INCLUDES}/ros_init.h + ${INCLUDES}/ros_types.h + src/ros_init.cpp + src/ros_types.cpp +) +target_include_directories(${TOOLS_LIB_NAME} + PUBLIC $ + PUBLIC $ +) +target_link_libraries(${TOOLS_LIB_NAME} PUBLIC pybind11::pybind11 ${Boost_LIBRARIES} rclcpp::rclcpp) + +install(TARGETS ${TOOLS_LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +# moveit.python_tools +pybind11_add_module(pymoveit_python_tools src/python_tools.cpp) +target_link_libraries(pymoveit_python_tools PRIVATE ${TOOLS_LIB_NAME}) + +# moveit.task_constructor +set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor) +pybind11_add_module(pymoveit_mtc + ${INCLUDES}/properties.h + + src/properties.cpp + src/solvers.cpp + src/core.cpp + src/stages.cpp + src/module.cpp +) +target_include_directories(pymoveit_mtc PUBLIC $) +target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${TOOLS_LIB_NAME}) + +# install python libs +install(TARGETS pymoveit_python_tools pymoveit_mtc + LIBRARY DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" +) diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp new file mode 100644 index 000000000..0ba74bab8 --- /dev/null +++ b/core/python/bindings/src/core.cpp @@ -0,0 +1,515 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "core.h" +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; +using namespace moveit::task_constructor; + +namespace moveit { +namespace python { + +namespace { + +// utility function to normalize index: negative indeces reference from the end +size_t normalize_index(size_t size, long index) { + if (index < 0) + index += size; + if (index >= long(size) || index < 0) + throw pybind11::index_error("Index out of range"); + return index; +} + +// implement operator[](index) +template +typename T::value_type get_item(const T& container, long index) { + auto it = container.begin(); + std::advance(it, normalize_index(container.size(), index)); + return *it; +} + +py::list getForwardedProperties(const Stage& self) { + py::list l; + for (const std::string& value : self.forwardedProperties()) + l.append(value); + return l; +} + +void setForwardedProperties(Stage& self, const py::object& names) { + std::set s; + try { + // handle string argument as single name + if (PyBytes_Check(names.ptr())) + s.emplace(names.cast()); + else // expect iterable otherwise + for (auto item : names) + s.emplace(item.cast()); + } catch (const py::cast_error& e) { + // manually translate cast_error to type error + PyErr_SetString(PyExc_TypeError, e.what()); + throw py::error_already_set(); + } + self.setForwardedProperties(s); +} + +} // anonymous namespace + +void export_core(pybind11::module& m) { + /// translate InitStageException into InitStageError + static py::exception init_stage_error(m, "InitStageError"); + /// provide extended error description for InitStageException + py::register_exception_translator([](std::exception_ptr p) { // NOLINT(performance-unnecessary-value-param) + try { + if (p) + std::rethrow_exception(p); + } catch (const InitStageException& e) { + std::stringstream message; + message << e; + init_stage_error(message.str().c_str()); + } + }); + + py::classh(m, "Solution", "Abstract base class for solutions of a stage") + .def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, "float: Cost associated with the solution") + .def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, + "str: Comment associated with the solution") + .def("markAsFailure", &SolutionBase::markAsFailure, "Mark the SubTrajectory as a failure", "comment"_a) + .def_property_readonly("isFailure", &SolutionBase::isFailure, + "bool: True if the trajectory is marked as a failure (read-only)") + .def_property_readonly("start", &SolutionBase::start, "InterfaceState: Start of the trajectory (read-only)") + .def_property_readonly("end", &SolutionBase::end, "InterfaceState: End of the trajectory (read-only)") + .def_property_readonly( + "markers", py::overload_cast<>(&SolutionBase::markers), + ":visualization_msgs:`Marker`: Markers to visualize important aspects of the trajectory (read-only)") + .def( + "toMsg", + [](const SolutionBase& self) { + moveit_task_constructor_msgs::msg::Solution msg; + self.toMsg(msg); + return msg; + }, + "Convert to the ROS message ``Solution``"); + + py::classh(m, "SubTrajectory", + "Solution trajectory connecting two InterfaceStates of a stage") + .def(py::init<>()) + .def_property("trajectory", &SubTrajectory::trajectory, &SubTrajectory::setTrajectory, + ":moveit_msgs:`RobotTrajectory`: Actual robot trajectory"); + + using Solutions = ordered; + py::classh(m, "Solutions", "Cost-ordered list of solutions") + .def("__len__", &Solutions::size) + .def("__getitem__", &get_item) + .def( + "__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); }, + py::keep_alive<0, 1>()); + + py::classh(m, "InterfaceState", + "Describes a potential start or goal state of a Stage. " + "It comprises a PlanningScene as well as a PropertyMap.") + .def(py::init(), "scene"_a) + .def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties), + "PropertyMap: PropertyMap of the state (read-only).") + .def_property_readonly("scene", &InterfaceState::scene, + "PlanningScene: PlanningScene of the state (read-only)."); + + py::classh(m, "MoveItErrorCode", "Encapsulates moveit error code message") + .def_readonly("val", &moveit::core::MoveItErrorCode::val, ":moveit_msgs:`MoveItErrorCodes`: error code") + .def(PYBIND11_BOOL_ATTR, + [](const moveit::core::MoveItErrorCode& err) { return pybind11::cast(static_cast(err)); }); + + py::classh(m, "CostTerm", "Base class for cost calculation in stages"); + auto tct = py::classh(m, "TrajectoryCostTerm", + "Base class for cost calculation of trajectories"); + py::enum_(tct, "Mode", "Specify which states are considered for collision checking") + .value("AUTO", TrajectoryCostTerm::Mode::AUTO, "TRAJECTORY (if available) or START_INTERFACE") + .value("START_INTERFACE", TrajectoryCostTerm::Mode::START_INTERFACE, "Only consider start state") + .value("END_INTERFACE", TrajectoryCostTerm::Mode::END_INTERFACE, "Only consider end state") + .value("TRAJECTORY", TrajectoryCostTerm::Mode::TRAJECTORY, "Consider whole trajectory"); + + py::classh(m, "PathLength", + "Computes joint-based path length along trajectory") + .def(py::init<>()) + .def(py::init>()) + .def(py::init>()); + py::classh(m, "DistanceToReference", + "Computes joint-based distance to reference pose") + .def(py::init>(), + "reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map()) + .def(py::init&, TrajectoryCostTerm::Mode, std::map>(), + "reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map()); + py::classh(m, "TrajectoryDuration", "Computes duration of trajectory") + .def(py::init<>()); + py::classh(m, "LinkMotion", + "Computes Cartesian path length of given link along trajectory") + .def(py::init(), "link_name"_a); + + py::classh(m, "Clearance", "Computes inverse distance to collision objects") + .def(py::init(), "with_world"_a = true, + "cumulative"_a = false, "group_property"_a = "group", "mode"_a = TrajectoryCostTerm::Mode::AUTO); + + auto stage = + properties::class_>(m, "Stage", "Abstract base class of all stages.") + .property("timeout", "float: Maximally allowed time [s] per computation step") + .property("marker_ns", "str: Namespace for any markers that are associated to the stage") + .def_property("forwarded_properties", getForwardedProperties, setForwardedProperties, + "list: set of properties forwarded from input to output InterfaceState") + .def_property("name", &Stage::name, &Stage::setName, "str: name of the stage displayed e.g. in rviz") + .def_property_readonly("properties", py::overload_cast<>(&Stage::properties), + "PropertyMap: PropertyMap of the stage (read-only)") + .def_property_readonly("solutions", &Stage::solutions, "Successful Solutions of the stage (read-only)") + .def_property_readonly("failures", &Stage::failures, "Solutions: Failed Solutions of the stage (read-only)") + .def("setCostTerm", &Stage::setCostTerm, + "Specify a CostTerm for calculation of stage costs") + .def( + "setCostTerm", [](Stage& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); }, + "Specify a function to calculate trajectory costs") + .def( + "setCostTerm", + [](Stage& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); }, + "Specify a function to calculate trajectory costs") + .def("reset", &Stage::reset, "Reset the Stage. Clears all solutions, interfaces and inherited properties") + .def("init", &Stage::init, + "Initialize the stage once before planning. " + "Will setup properties configured for initialization from parent.", + "robot_model"_a); + + py::enum_( + stage, "PropertyInitializerSource", + "OR-combinable flags defining a source to initialize a specific property from. " + "Used in :doc:`pymoveit_mtc.core.PropertyMap` ``configureInitFrom()``. ") + .value("PARENT", Stage::PARENT, "Inherit properties from parent stage") + .value("INTERFACE", Stage::INTERFACE, "Inherit properties from the input InterfaceState"); + + auto either_way = py::classh>( + m, "PropagatingEitherWay", "Base class for propagator-like stages") + .def(py::init(), "name"_a = std::string("PropagatingEitherWay")) + .def("restrictDirection", &PropagatingEitherWay::restrictDirection, + "Explicitly specify computation direction") + .def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward") + .def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward") + //.def("sendForward", &PropagatingEitherWay::sendForward) + //.def("sendBackward", &PropagatingEitherWay::sendBackward) + ; + + py::enum_(either_way, "Direction", "Propagation direction") + .value("AUTO", PropagatingEitherWay::AUTO) + .value("FORWARD", PropagatingEitherWay::FORWARD, "Propagating forwards from start to end") + .value("BACKWARD", PropagatingEitherWay::BACKWARD, "Propagating backwards from end to start"); + + py::classh>( + m, "PropagatingForward", "Base class for forward-propagating stages") + .def(py::init(), "name"_a = std::string("PropagatingForward")); + py::classh>( + m, "PropagatingBackward", "Base class for backward-propagating stages") + .def(py::init(), "name"_a = std::string("PropagatingBackward")); + + properties::class_>(m, "Generator", R"( + Base class for generator-like stages + + Derive from this stage to implement a custom generator stage that can produce new seed states w/o prior knowledge. + Implement the virtual methods as follows:: + + class MyGenerator(core.Generator): + """Implements a custom 'Generator' stage that produces maximally 3 solutions.""" + + def __init__(self, name="Generator"): + core.Generator.__init__(self, name) + self.reset() + + def init(self, robot_model): + self.ps = PlanningScene(robot_model) + + def reset(self): + core.Generator.reset(self) + + def canCompute(self): + return len(self.solutions) < 3 # maximally produce 3 solutions + + def compute(self): + self.spawn(core.InterfaceState(self.ps), cost=len(self.solutions)) + )") + .def(py::init(), "name"_a = std::string("Generator")) + .def("canCompute", &Generator::canCompute, "Return ``True`` if the stage can still produce solutions.") + .def("compute", &Generator::compute, "Compute an actual solution and ``spawn`` an ``InterfaceState``") + .def( + "spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); }, + "Spawn an ``InterfaceState`` to both, start and end interface", "state"_a, "cost"_a); + + properties::class_>(m, "MonitoringGenerator", R"( + Base class for monitoring generator stages + + To implement a generator stage that draws on some previously computed solution, you need to derive + from ``MonitoringGenerator`` - monitoring the solutions produced by another stage. + Each time, the monitored stage produces a new solution, the method ``onNewSolution()`` of the + MonitoringGenerator is called. Usually, you schedule this solution for later processing in ``compute()``:: + + class PyMonitoringGenerator(core.MonitoringGenerator): + """ Implements a custom 'MonitoringGenerator' stage.""" + + solution_multiplier = 2 + + def __init__(self, name="MonitoringGenerator"): + core.MonitoringGenerator.__init__(self, name) + self.reset() + + def reset(self): + core.MonitoringGenerator.reset(self) + self.pending = [] + + def onNewSolution(self, sol): + self.pending.append(sol) + + def canCompute(self): + return bool(self.pending) + + def compute(self): + # fetch first pending upstream solution ... + scene = self.pending.pop(0).end.scene + # ... and generate new solutions derived from it + for i in range(self.solution_multiplier): + self.spawn(core.InterfaceState(scene), i) + + Upon creation of the stage, assign the monitored stage as follows:: + + jointspace = core.JointInterpolationPlanner() + + task = core.Task() + current = stages.CurrentState("current") + task.add(current) + + connect = stages.Connect(planners=[('panda_arm', jointspace)]) + task.add(connect) + + mg = PyMonitoringGenerator("generator") + mg.setMonitoredStage(task["current"]) + task.add(mg) + )") + .def(py::init(), "name"_a = std::string("generator")) + .def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the monitored ``Stage``", "stage"_a) + .def("_onNewSolution", &PubMonitoringGenerator::onNewSolution); + + py::classh(m, "ContainerBase", R"( + Abstract base class for container stages + Containers allow encapsulation and reuse of planning functionality in a hierachical fashion. + You can iterate of the children of a container and access them by name.)") + .def( + "add", + [](ContainerBase& c, const py::args& args) { + for (auto it = args.begin(), end = args.end(); it != end; ++it) + c.add(it->cast()); + }, + "Insert a stage at the end of the current children list") + .def("insert", &ContainerBase::insert, "stage"_a, "before"_a = -1, + "Insert a stage before the given index into the children list") + .def("remove", py::overload_cast(&ContainerBase::remove), "Remove child stage by index", "pos"_a) + .def("remove", py::overload_cast(&ContainerBase::remove), "Remove child stage by instance", "child"_a) + .def("clear", &ContainerBase::clear, "Remove all stages from the container") + .def("__len__", &ContainerBase::numChildren) + .def( + "__getitem__", + [](const ContainerBase& c, const std::string& name) -> Stage* { + Stage* child = c.findChild(name); + if (!child) + throw py::index_error(); + return child; + }, + py::return_value_policy::reference_internal) + .def( + "__getitem__", + [](const ContainerBase& c, int idx) -> Stage* { + Stage* child = c[idx]; + if (!child) + throw py::index_error(); + return child; + }, + py::return_value_policy::reference_internal) + .def( + "__iter__", + [](const ContainerBase& c) { + const auto& children = c.pimpl()->children(); + return py::make_iterator(children.begin(), children.end()); + }, + py::keep_alive<0, 1>()) // keep container alive as long as iterator lives + ; + + py::classh(m, "SerialContainer", "Container implementing a linear planning sequence") + .def(py::init(), "name"_a = std::string("SerialContainer")); + + py::classh(m, "ParallelContainerBase", + "Abstract base class for parallel containers"); + + py::classh(m, "Alternatives", R"( + Plan for different alternatives in parallel. + Solutions of all children are considered simultaneously. + See :ref:`How-To-Guides ` for an example. + )") + .def(py::init(), "name"_a = std::string("Alternatives")); + + py::classh(m, "Fallbacks", R"( + Plan for different alternatives in sequence + Try to find feasible solutions using the children in sequence. The behaviour slightly differs for the indivual stage types: + + - Generator: Proceed to next child if currently active one exhausted its solution, i.e. returns ``canCompute() == False``. + - Propagator: Forward an incoming ``InterfaceState`` to the next child if the current one ultimately failed on it. + - Connect: Only ``Connect`` stages are supported. Pairs of ``InterfaceStates`` are forward to the next child on failure of the current child. + + See :ref:`How-To-Guides ` for an example. + )") + .def(py::init(), "name"_a = std::string("Fallbacks")); + + py::classh(m, "Merger", R"( + Plan for different sub tasks in parallel and eventually merge all sub solutions into a single trajectory + This requires all children to operate on disjoint ``JointModelGroups``. + + See :ref:`How-To-Guides ` for an example. + )") + .def(py::init(), "name"_a = std::string("merger")); + + py::classh(m, "WrapperBase", R"( + Base class for wrapping containers, which can be used to filter or modify solutions generated by the single child. + Implementations of this interface need to implement ``onNewSolution()`` to process a solution generated by the child. + The wrapper may reject the solution or create one or multiple derived solutions, potentially adapting the cost, + the trajectory and output ``InterfaceStates``. + )"); + + py::classh(m, "Task", R"(Root stage of a planning pipeline. + A task stage usually wraps a single container (by default ``SerialContainer``) stage. + The class provides methods to ``plan()`` for the configured pipeline and retrieve full solutions.)") + .def(py::init(), "ns"_a = std::string(), "introspection"_a = true) + .def(py::init(), "ns"_a = std::string(), + "introspection"_a = true, "container"_a) + + .def_property_readonly("properties", py::overload_cast<>(&Task::properties), + "PropertyMap: PropertyMap of the stage (read-only)") + .def_property_readonly("solutions", &Task::solutions, "Successful Solutions of the stage (read-only)") + .def_property_readonly("failures", &Task::failures, "Solutions: Failed Solutions of the stage (read-only)") + .def_property("name", &Task::name, &Task::setName, "str: name of the task displayed e.g. in rviz") + + .def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description", + "Load robot model from given ROS parameter") + .def("getRobotModel", &Task::getRobotModel) + .def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true, + "Enable publishing intermediate results for inspection in ``rviz``") + .def("clear", &Task::clear, "Reset the stage task (and all its stages)") + .def( + "add", + [](Task& t, const py::args& args) { + for (auto it = args.begin(), end = args.end(); it != end; ++it) + t.add(it->cast()); + }, + "Append stage(s) to the task's top-level container") + .def("insert", &Task::insert, "stage"_a, "before"_a = -1, "Insert stage before given index") + .def("__len__", [](const Task& t) { t.stages()->numChildren(); }) + .def( + "__getitem__", + [](const Task& t, const std::string& name) -> Stage* { + Stage* child = t.stages()->findChild(name); + if (!child) + throw py::index_error(); + return child; + }, + py::return_value_policy::reference_internal) + .def( + "__getitem__", + [](const Task& t, int idx) -> Stage* { + Stage* child = t.stages()->operator[](idx); + if (!child) + throw py::index_error(); + return child; + }, + py::return_value_policy::reference_internal) + .def( + "__iter__", + [](const Task& t) { + const auto& children = t.stages()->pimpl()->children(); + return py::make_iterator(children.begin(), children.end()); + }, + py::keep_alive<0, 1>()) // keep container alive as long as iterator lives + .def( + "setCostTerm", [](Task& self, const CostTermConstPtr& c) { self.setCostTerm(c); }, + "Specify a CostTerm for calculation of stage costs") + .def( + "setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); }, + "Specify a function to calculate trajectory costs") + .def( + "setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); }, + "Specify a function to calculate trajectory costs") + .def("reset", &Task::reset, "Reset task (and all its stages)") + .def("init", py::overload_cast<>(&Task::init), "Initialize the task (and all its stages)") + .def("plan", &Task::plan, "max_solutions"_a = 0, R"( + Reset, init, and plan. Planning is limited to ``max_allowed_solutions``. + Returns if planning was successful.)") + .def("preempt", &Task::preempt, "Interrupt current planning (or execution)") + .def( + "publish", + [](Task& self, const SolutionBasePtr& solution) { self.introspection().publishSolution(*solution); }, + "solution"_a, "Publish the given solution to the ROS topic ``solution``") + .def_static( + "execute", + [](const SolutionBasePtr& solution) { + using namespace moveit::planning_interface; + PlanningSceneInterface psi; + MoveGroupInterface mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]); + + MoveGroupInterface::Plan plan; + moveit_task_constructor_msgs::msg::Solution serialized; + solution->toMsg(serialized); + + for (const moveit_task_constructor_msgs::msg::SubTrajectory& traj : serialized.sub_trajectory) { + if (!traj.trajectory.joint_trajectory.points.empty()) { + plan.trajectory_ = traj.trajectory; + if (!mgi.execute(plan)) { + ROS_ERROR("Execution failed! Aborting!"); + return; + } + } + psi.applyPlanningScene(traj.scene_diff); + } + ROS_INFO("Executed successfully"); + }, + "solution"_a, "Send given solution to ``move_group`` node for execution"); +} +} // namespace python +} // namespace moveit diff --git a/core/python/bindings/src/core.h b/core/python/bindings/src/core.h new file mode 100644 index 000000000..9be7a4de6 --- /dev/null +++ b/core/python/bindings/src/core.h @@ -0,0 +1,142 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +/** Trampoline classes to allow inheritance in Python (overriding virtual functions) */ + +namespace moveit { +namespace task_constructor { + +class Task; +namespace solvers { +class PlannerInterface; +} + +template +class PyStage : public Stage, public pybind11::trampoline_self_life_support +{ +public: + using Stage::Stage; + + void init(const moveit::core::RobotModelConstPtr& robot_model) override { + PYBIND11_OVERRIDE(void, Stage, init, robot_model); + } + void reset() override { PYBIND11_OVERRIDE(void, Stage, reset, ); } +}; + +template +class PyGenerator : public PyStage +{ +public: + using PyStage::PyStage; + bool canCompute() const override { PYBIND11_OVERRIDE_PURE(bool, Generator, canCompute, ); } + void compute() override { PYBIND11_OVERRIDE_PURE(void, Generator, compute, ); } +}; + +template +class PyMonitoringGenerator : public PyGenerator +{ +public: + using PyGenerator::PyGenerator; + void onNewSolution(const SolutionBase& s) override { + // pass solution as pointer to trigger passing by reference + PYBIND11_OVERRIDE_PURE(void, MonitoringGenerator, onNewSolution, &s); + } +}; + +// Helper class to expose protected member function onNewSolution +// https://pybind11.readthedocs.io/en/stable/advanced/classes.html#binding-protected-member-functions +class PubMonitoringGenerator : public MonitoringGenerator +{ +public: + using MonitoringGenerator::onNewSolution; +}; + +template +class PyPropagatingEitherWay : public PyStage +{ +public: + using PyStage::PyStage; + void computeForward(const InterfaceState& from_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_PURE(void, PropagatingEitherWay, computeForward, &from_state); + } + void computeBackward(const InterfaceState& to_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_PURE(void, PropagatingEitherWay, computeBackward, &to_state); + } +}; + +} // namespace task_constructor +} // namespace moveit + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::solvers::PlannerInterface) + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SolutionBase) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SubTrajectory) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(ordered) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::InterfaceState) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::core::MoveItErrorCode) + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::CostTerm) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::TrajectoryCostTerm) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::PathLength) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::DistanceToReference) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::TrajectoryDuration) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::LinkMotion) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::Clearance) + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Stage) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingEitherWay) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingForward) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingBackward) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Generator) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::MonitoringGenerator) + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::ContainerBase) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SerialContainer) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::ParallelContainerBase) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Alternatives) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Fallbacks) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Merger) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::WrapperBase) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Task) diff --git a/core/python/bindings/src/module.cpp b/core/python/bindings/src/module.cpp new file mode 100644 index 000000000..a79771136 --- /dev/null +++ b/core/python/bindings/src/module.cpp @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +namespace moveit { +namespace python { + +void export_properties(pybind11::module& m); +void export_solvers(pybind11::module& m); +void export_core(pybind11::module& m); +void export_stages(pybind11::module& m); + +} // namespace python +} // namespace moveit + +PYBIND11_MODULE(pymoveit_mtc, m) { + auto msub = m.def_submodule("core", "Provides wrappers for core C++ classes. " + "**Import as** :doc:`moveit.task_constructor.core`."); + + moveit::python::export_properties(msub); + moveit::python::export_solvers(msub); + moveit::python::export_core(msub); + + msub = m.def_submodule("stages", "Provides wrappers of standard MTC stages. " + "**Import as** :doc:`moveit.task_constructor.stages`."); + moveit::python::export_stages(msub); +} diff --git a/core/python/bindings/src/properties.cpp b/core/python/bindings/src/properties.cpp new file mode 100644 index 000000000..e53d7a293 --- /dev/null +++ b/core/python/bindings/src/properties.cpp @@ -0,0 +1,239 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include + +namespace py = pybind11; +using namespace py::literals; +using namespace moveit::task_constructor; + +namespace moveit { +namespace python { +namespace { + +class PropertyConverterRegistry +{ + struct Entry + { + PropertyConverterBase::to_python_converter_function to_; + PropertyConverterBase::from_python_converter_function from_; + }; + // map from type_index to corresponding converter functions + typedef std::map RegistryMap; + RegistryMap types_; + // map from ros-msg-names to entry in types_ + using RosMsgTypeNameMap = std::map; + RosMsgTypeNameMap msg_names_; + +public: + PropertyConverterRegistry(); + + inline bool insert(const std::type_index& type_index, const std::string& ros_msg_name, + PropertyConverterBase::to_python_converter_function to, + PropertyConverterBase::from_python_converter_function from); + + static py::object toPython(const boost::any& value); + + static boost::any fromPython(const py::object& bpo); +}; +static PropertyConverterRegistry REGISTRY_SINGLETON; + +PropertyConverterRegistry::PropertyConverterRegistry() { + // register property converters + PropertyConverter(); + PropertyConverter(); + PropertyConverter(); + PropertyConverter(); + PropertyConverter(); + PropertyConverter(); + PropertyConverter(); + PropertyConverter>(); + PropertyConverter>(); +} + +bool PropertyConverterRegistry::insert(const std::type_index& type_index, const std::string& ros_msg_name, + PropertyConverterBase::to_python_converter_function to, + PropertyConverterBase::from_python_converter_function from) { + auto it_inserted = types_.insert(std::make_pair(type_index, Entry{ to, from })); + if (!it_inserted.second) + return false; + + if (!ros_msg_name.empty()) // is this a ROS msg type? + msg_names_.insert(std::make_pair(ros_msg_name, it_inserted.first)); + + return true; +} + +py::object PropertyConverterRegistry::toPython(const boost::any& value) { + if (value.empty()) + return py::object(); + + auto it = REGISTRY_SINGLETON.types_.find(value.type()); + if (it == REGISTRY_SINGLETON.types_.end()) { + std::string msg("No Python -> C++ conversion for: "); + msg += boost::core::demangle(value.type().name()); + PyErr_SetString(PyExc_TypeError, msg.c_str()); + throw py::error_already_set(); + } + + return it->second.to_(value); +} + +std::string rosMsgName(PyObject* object) { + py::object o = py::reinterpret_borrow(object); + try { + return o.attr("_type").cast(); + } catch (const py::error_already_set&) { + // change error to TypeError + std::string msg = o.attr("__class__").attr("__name__").cast(); + msg += " is not a ROS message type"; + PyErr_SetString(PyExc_TypeError, msg.c_str()); + throw py::error_already_set(); + } +} + +boost::any PropertyConverterRegistry::fromPython(const py::object& po) { + PyObject* o = po.ptr(); + + if (PyBool_Check(o)) + return (o == Py_True); +#if PY_MAJOR_VERSION >= 3 + if (PyLong_Check(o)) + return PyLong_AS_LONG(o); +#else + if (PyInt_Check(o)) + return PyInt_AS_LONG(o); +#endif + if (PyFloat_Check(o)) + return PyFloat_AS_DOUBLE(o); +#if PY_MAJOR_VERSION >= 3 + if (PyUnicode_Check(o)) +#else + if (PyString_Check(o)) +#endif + return py::cast(o); + + const std::string& ros_msg_name = rosMsgName(o); + auto it = REGISTRY_SINGLETON.msg_names_.find(ros_msg_name); + if (it == REGISTRY_SINGLETON.msg_names_.end()) { + std::string msg("No Python -> C++ conversion for: "); + msg += ros_msg_name; + PyErr_SetString(PyExc_TypeError, msg.c_str()); + throw py::error_already_set(); + } + + return it->second->second.from_(po); +} + +} // end anonymous namespace + +bool PropertyConverterBase::insert(const std::type_index& type_index, const std::string& ros_msg_name, + moveit::python::PropertyConverterBase::to_python_converter_function to, + moveit::python::PropertyConverterBase::from_python_converter_function from) { + return REGISTRY_SINGLETON.insert(type_index, ros_msg_name, to, from); +} + +void export_properties(py::module& m) { + // clang-format off + py::classh(m, "Property", "Holds an arbitrarily typed value and a default value") + .def(py::init<>()) + .def("setValue", [](Property& self, const py::object& value) + { self.setValue(PropertyConverterRegistry::fromPython(value)); }, + "Set current and default value.", "value"_a) + .def("setCurrentValue", [](Property& self, const py::object& value) + { self.setCurrentValue(PropertyConverterRegistry::fromPython(value)); }, + "Set the current value only, w/o touching the default.", "value"_a) + .def("value", [](const Property& self) + { return PropertyConverterRegistry::toPython(self.value()); }, "Retrieve the stored value.") + .def("defaultValue", [](const Property& self) + { return PropertyConverterRegistry::toPython(self.defaultValue()); }, + "Retrieve the default value.") + .def("reset", &Property::reset, "Reset the value to the stored default.") + .def("defined", &Property::defined, "Was a (non-default) value stored?") + .def("description", &Property::description, "Retrive the property description string") + .def("setDescription", &Property::setDescription, + "Set the property's description", "desc"_a); + + py::classh(m, "PropertyMap", "Dictionary of named :doc:`properties `") + .def(py::init<>()) + .def("__bool__", [](const PropertyMap& self) { return self.begin() == self.end();}) + .def("__iter__", [](PropertyMap& self) { return py::make_key_iterator(self.begin(), self.end()); }, + py::keep_alive<0, 1>()) // Essential: keep list alive while iterator exists + .def("items", [](const PropertyMap& self) { return py::make_iterator(self.begin(), self.end()); }, + py::keep_alive<0, 1>(), "Retrieve an iterator over the items of the dictionary.") + .def("__contains__", [](const PropertyMap& self, const std::string &key) { return self.hasProperty(key); }) + .def("property", [](PropertyMap& self, const std::string& key) + { return self.property(key); }, py::return_value_policy::reference_internal, R"( + Retrieve the property instance for the given key. + This is in contrast to ``map[key]``, which returns ``map.property(key).value()``.)", + "key"_a) + .def("__getitem__", [](const PropertyMap& self, const std::string& key) + { return PropertyConverterRegistry::toPython(self.get(key)); }) + .def("__setitem__", [](PropertyMap& self, const std::string& key, const py::object& value) + { self.set(key, PropertyConverterRegistry::fromPython(value)); }) + .def("reset", &PropertyMap::reset, "Reset all properties to their default values") + .def("update", [](PropertyMap& self, const py::dict& values) { + for (auto it = values.begin(), end = values.end(); it != end; ++it) { + self.set(it->first.cast(), + PropertyConverterRegistry::fromPython(py::reinterpret_borrow(it->second))); + } + }, "Update property values from another dictionary", "values"_a) + .def("configureInitFrom", [](PropertyMap& self, Property::SourceFlags sources, const py::list& names) { + std::set s; + for (auto& item : names) + s.insert(item.cast()); + self.configureInitFrom(sources, s); + }, "Configure initialization of listed (or all) properties from given source(s).", + "sources"_a, "names"_a = py::list()) + .def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name) { + self.exposeTo(other, name, name); + }, "Declare ``named`` property in ``other`` PropertyMap - using same name.", + "other"_a, "name"_a) + .def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name, const std::string& other_name) { + self.exposeTo(other, name, other_name); + }, "Declare ``named`` property in ``other`` PropertyMap - using ``other_name``.", + "other"_a, "name"_a, "other_name"_a) + .def("exposeTo", [](PropertyMap& self, PropertyMap& other, const py::list& names) { + std::set s; + for (auto& item : names) + s.insert(item.cast()); + self.exposeTo(other, s); + }, "Declare `all` ``named`` properties in ``other`` PropertyMap - using the same names.", + "other"_a, "names"_a) + ; + // clang-format on +} +} // namespace python +} // namespace moveit diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp new file mode 100644 index 000000000..8c9b086d1 --- /dev/null +++ b/core/python/bindings/src/solvers.cpp @@ -0,0 +1,119 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; +using namespace moveit::task_constructor; +using namespace moveit::task_constructor::solvers; + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(PlannerInterface) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(PipelinePlanner) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(JointInterpolationPlanner) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(CartesianPath) + +namespace moveit { +namespace python { + +void export_solvers(py::module& m) { + properties::class_(m, "PlannerInterface", "Abstract base class for planning algorithms") + .property("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]") + .property("max_acceleration_scaling_factor", + "float: Reduce the maximum acceleration by scaling between (0,1]") + .def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties), + py::return_value_policy::reference_internal, "Properties of the planner"); + + properties::class_(m, "PipelinePlanner", + R"(Plan using MoveIt's ``PlanningPipeline`` + :: + + from moveit.task_constructor import core + + # Create and configure a planner instance + pipelinePlanner = core.PipelinePlanner() + pipelinePlanner.planner = 'PRMkConfigDefault' + pipelinePlanner.num_planning_attempts = 10 + )") + .property("planner", "str: Planner ID") + .property("num_planning_attempts", "int: Number of planning attempts") + .property( + "workspace_parameters", + ":moveit_msgs:`WorkspaceParameters`: Specifies workspace box to be used for Cartesian sampling") + .property("goal_joint_tolerance", "float: Tolerance for reaching joint goals") + .property("goal_position_tolerance", "float: Tolerance for reaching position goals") + .property("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals") + .def(py::init(), "pipeline"_a = std::string("ompl")); + + properties::class_( + m, "JointInterpolationPlanner", + R"(Perform linear interpolation between joint space poses. + Fails on collision along the interpolation path. There is no obstacle avoidance. :: + + from moveit.task_constructor import core + + # Instantiate joint-space interpolation planner + jointPlanner = core.JointInterpolationPlanner() + jointPlanner.max_step = 0.1 + )") + .property("max_step", "float: Limit any (single) joint change between two waypoints to this amount") + .def(py::init<>()); + + properties::class_(m, "CartesianPath", R"( + Perform linear interpolation between Cartesian poses. + Fails on collision along the interpolation path. There is no obstacle avoidance. :: + + from moveit.task_constructor import core + + # Instantiate Cartesian-space interpolation planner + cartesianPlanner = core.CartesianPath() + cartesianPlanner.step_size = 0.01 + cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold. + )") + .property("step_size", "float: Limit the Cartesian displacement between consecutive waypoints " + "In contrast to joint-space interpolation, the Cartesian planner can also " + "succeed when only a fraction of the linear path was feasible.") + .property( + "jump_threshold", + "float: Limit joint displacement between consecutive waypoints, thus preventing jumps in joint space. " + "This values specifies the fraction of mean acceptable joint motion per step.") + .property("min_fraction", "float: Fraction of overall distance required to succeed.") + .def(py::init<>()); +} +} // namespace python +} // namespace moveit diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp new file mode 100644 index 000000000..082e78865 --- /dev/null +++ b/core/python/bindings/src/stages.cpp @@ -0,0 +1,538 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "stages.h" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; +using namespace moveit::task_constructor; +using namespace moveit::task_constructor::stages; + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(ModifyPlanningScene) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(CurrentState) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixedState) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(ComputeIK) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(MoveTo) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(MoveRelative) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(Connect) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixCollisionObjects) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(GenerateGraspPose) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePlacePose) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePose) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(Pick) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(Place) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGraspBase) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGrasp) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleUnGrasp) + +namespace moveit { +namespace python { + +namespace { + +/// extract from python argument a vector, where arg maybe a single T or a list of Ts +template +std::vector elementOrList(const py::object& arg) { + try { + return std::vector{ arg.cast() }; + } catch (const py::cast_error&) { + return arg.cast>(); + } +} + +} // anonymous namespace + +void export_stages(pybind11::module& m) { + // clang-format off + properties::class_(m, "ModifyPlanningScene", R"( + Apply modifications to the PlanningScene w/o moving the robot + + This stage takes the incoming planning scene and applies previously scheduled changes to it, for example: + + * Modify allowed collision matrix, enabling or disabling collision pairs + * Attach or detach objects to robot links + * Add or remove objects + + For an example, see :ref:`How-To-Guides `. + )") + .def(py::init(), "name"_a = std::string("modify planning scene")) + .def("attachObject", &ModifyPlanningScene::attachObject, "Attach an object to a robot link", "name"_a, "link"_a) + .def("detachObject", &ModifyPlanningScene::detachObject, "Detach an object from a robot link", "name"_a, "link"_a) + .def("attachObjects", [](ModifyPlanningScene& self, const py::object& names, + const std::string& attach_link, bool attach) { + self.attachObjects(elementOrList(names), attach_link, attach); + }, "Attach multiple objects to a robot link", "names"_a, "attach_link"_a, "attach"_a = true) + .def("detachObjects", [](ModifyPlanningScene& self, const py::object& names, + const std::string& attach_link) { + self.attachObjects(elementOrList(names), attach_link, false); + }, "Detach multiple objects from a robot link", "names"_a, "attach_link"_a) + .def("allowCollisions", [](ModifyPlanningScene& self, const std::string& object, bool allow) {self.allowCollisions(object, allow);}, + "Allow or disable all collisions involving the given object", "object"_a, "enable_collision"_a = true) + .def("allowCollisions", [](ModifyPlanningScene& self, + const py::object& first, const py::object& second, bool enable_collision) { + self.allowCollisions(elementOrList(first), elementOrList(second), enable_collision); + }, "Allow or disable collisions between links and objects", "first"_a, "second"_a, "enable_collision"_a = true) + .def("addObject", &ModifyPlanningScene::addObject, R"( + Add a CollisionObject_ to the planning scene + + .. _CollisionObject: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/CollisionObject.html + + )", "collision_object"_a) + .def("removeObject", &ModifyPlanningScene::removeObject, + "Remove a CollisionObject_ from the planning scene", "name"_a) + ; + + properties::class_(m, "CurrentState", R"( + Fetch the current PlanningScene / real robot state via the ``get_planning_scene`` service. + Usually, this stage should be used *once* at the beginning of your pipeline to start from the current state. + + .. literalinclude:: ./../../../demo/scripts/current_state.py + :language: python + + )") + .def(py::init(), "name"_a = std::string("current state")); + + properties::class_(m, "FixedState", R"( + Spawn a pre-defined PlanningScene state. + + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``FixedState`` stage. + )") + .def("setState", &FixedState::setState, R"( + Use a planning scene pointer to specify which state the Fixed State + stage should have. + )", "scene"_a) + .def(py::init(), "name"_a = std::string("fixed state")); + +#if 0 + .def("setState", [](FixedState& stage, const moveit_msg::PlanningScene& scene_msg) { + // TODO: How to initialize the PlanningScene? + planning_scene::PlanningScenePtr scene; + scene->setPlanningSceneMsg(scene_msg); + stage.setState(scene); + }) +#endif + ; + + properties::class_(m, "ComputeIK", R"( + Wrapper for any pose generator stage to compute the inverse + kinematics for a pose in Cartesian space. + + The wrapper reads a ``target_pose`` from the interface state of + solutions provided by the wrapped stage. This cartesian pose + (``PoseStamped`` msg) is used as a goal pose for inverse + kinematics. + + Usually, the end effector's parent link or the group's tip link + is used as the inverse kinematics frame, which should be + moved to the goal frame. However, any other inverse kinematics + frame can be defined (which is linked to the tip of the group). + + Properties of the internally received ``InterfaceState`` can be + forwarded to the newly generated, externally exposed ``InterfaceState``. + + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``ComputeIK`` stage. + )") + .property("eef", R"( + str: Specify which end effector of the active planning group + should be used. + )") + .property("group", R"( + str: Specify which planning group + should be used. + )") + .property("default_pose", R"( + str: Default joint pose of the active group + (defines cost of the inverse kinematics). + )") + .property("max_ik_solutions", R"( + int: Set the maximum number of inverse + kinematic solutions thats should be generated. + )") + .property("ignore_collisions", R"( + bool: Specify if collisions with other members of + the planning scene are allowed. + )") + .property("ik_frame", R"( + PoseStamped_: Specify the frame with respect + to which the inverse kinematics + should be calculated. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .property("target_pose", R"( + PoseStamped_: Specify the pose on which + the inverse kinematics should be + calculated on. Since this property should + almost always be set + in the Interface State which is sent by the child, + if possible, avoid setting it manually. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + // methods of base class py::class_ need to be called last! + .def(py::init(), "name"_a, "stage"_a); + + properties::class_>(m, "MoveTo", R"( + Compute a trajectory between the robot state from the + interface state of the preceeding stage and a specified + goal. + + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``MoveTo`` stage. + )") + .property("group", R"( + str: Planning group which should be utilized for planning and execution. + )") + .property("ik_frame", R"( + PoseStamped_: IK reference frame for the goal pose + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + + )") + .property("path_constraints", R"( + Constraints_: Set path constraints via the corresponding moveit message type + + .. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html + )") + .def(py::init(), "name"_a, "planner"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move link to a given PoseStamped_ + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )", "goal"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move link to given PointStamped_, keeping current orientation + + .. _PointStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PointStamped.html + )", "goal"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move joints specified in RobotState_ to their target values + + .. _RobotState: https://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/RobotState.html + )", "goal"_a) + .def("setGoal", py::overload_cast&>(&MoveTo::setGoal), R"( + Move joints by name to their mapped target value provided by dict goal argument + )", "goal"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move joint model group to given named pose provided as a str argument + )", "goal"_a); + + properties::class_>(m, "MoveRelative", R"( + Perform a Cartesian motion relative to some link. + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``MoveRelative`` stage. + To implement your own propagtor logic on top of the ``MoveRelative`` class' functionality, + you may derive from the stage. Take a look at the corresponding + :ref:`How-To-Guide `. + )") + .property("group", R"( + str: Planning group which should be utilized for planning and execution. + )") + .property("ik_frame", R"( + PoseStamped_: IK reference frame for the goal pose. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .property("min_distance", "float: Set the minimum distance to move") + .property("max_distance", "float: Set the maximum distance to move") + .property("path_constraints", R"( + Constraints_: These are the path constraints. + + .. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html + )") + .def(py::init(), "name"_a, "planner"_a) + .def("setDirection", py::overload_cast(&MoveRelative::setDirection), R"( + Perform twist motion on specified link. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "twist"_a) + .def("setDirection", py::overload_cast(&MoveRelative::setDirection), R"( + Translate link along given direction. + + .. _Vector3Stamped: https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Vector3Stamped.html + )", "direction"_a) + .def("setDirection", py::overload_cast&>(&MoveRelative::setDirection), R"( + Move specified joint variables by given amount. + )", "joint_deltas"_a); + + py::enum_(m, "MergeMode", R"( + Define the merge strategy to use when performing planning operations + with e.g. the connect stage. + )") + .value("SEQUENTIAL", stages::Connect::MergeMode::SEQUENTIAL, "Store sequential trajectories") + .value("WAYPOINTS", stages::Connect::MergeMode::WAYPOINTS, "Join trajectories by their waypoints"); + PropertyConverter(); + + properties::class_(m, "Connect", R"( + Connect arbitrary InterfaceStates by motion planning. + You can specify the planning groups and the planners you + want to utilize. + + The states may differ in various planning groups. + To connect both states, the planners provided for + individual sub groups are applied in the specified order. + Each planner only plan for joints within the corresponding + planning group. Finally, an attempt is made to merge the + sub trajectories of individual planning results. + If this fails, the sequential planning result is returned. + + For an example, see :ref:`How-To-Guides `. + )") + .def(py::init(), + "name"_a = std::string("connect"), "planners"_a); + + properties::class_(m, "FixCollisionObjects", R"( + Test for collisions and find a correction for applicable objects. + Move the objects out of the way along the correction direction. + + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``FixCollisionObjects`` stage. + )") + .property("max_penetration", R"( + float: Cutoff length up to which collision objects get fixed. + )") + .def(py::init(), "name"_a = std::string("fix collisions")); + + properties::class_(m, "GeneratePlacePose", R"( + GeneratePlacePose stage derives from monitoring generator and generates poses + for the place pipeline. Notice that whilst GenerateGraspPose spawns poses with an + ``angle_delta`` intervall, GeneratePlacePose samples a fixed amount, which is dependent + on the objects shape. + + Take a look at the :ref:`How-To-Guides ` + for a snippet that demonstrates usage of the `GeneratePlacePose` stage. + )") + .property("object", R"( + str: Name of the object in the planning scene, attached to the robot which should be placed + )") + .property("eef", "str: Name of the end effector that should be used for grasping") + .property("pose", R"( + PoseStamped_: The pose where the object should be placed, i.e. states should be sampled + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .def(py::init(), "name"_a = std::string("Generate Place Pose")); + + + properties::class_(m, "GenerateGraspPose", R"( + GenerateGraspPose stage derives from monitoring generator and can + be used to generate poses for grasping. Set the desired attributes + of the grasp using the stages properties. + Take a look at the :ref:`How-To-Guides ` + for a snippet that demonstrates usage of the `GenerateGraspPose` stage. + )") + .property("object", R"( + str: Name of the Object in the planning scene, which should be grasped + )") + .property("eef", R"( + str: Name of the end effector that should be used for grasping + )") + .property("pregrasp", "str: Name of the pre-grasp pose") + .property("grasp", "str: Name of the grasp pose") + .property("angle_delta", R"( + float: Angular step distance in rad with which positions around the object are sampled. + )") + .def(py::init(), "name"_a = std::string("Generate Grasp Pose")); + + properties::class_(m, "GeneratePose", R"( + Monitoring generator stage which can be used to generate a pose, based on solutions provided + by the monitored stage. + + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``GeneratePose`` stage. + )") + .property("pose", R"( + PoseStamped_: Set the pose, which should be spawned on each new solution of the monitored stage. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .def(py::init(), "name"_a); + + properties::class_(m, "Pick", R"( + The Pick stage is a specialization of the PickPlaceBase class, which + wraps the pipeline to pick or place an object with a given end effector. + + Picking consist of the following sub stages: + + - Linearly approaching the object along an approach direction/twist "grasp" end effector posture + - Attach the object + - Lift along a given direction/twist + + The end effector postures corresponding to pre-grasp and grasp as well + as the end effector's cartesian pose needs to be provided by an external + grasp stage. + + Take a look at the :ref:`Pick and Place Tutorial ` for an in-depth look, + as well as the :ref:`How-To Guide ` for a minimal implementation + of a task hierarchy that makes use of the + ``Pick`` stage. + )") + .property("object", "str: Name of object to pick") + .property("eef", "str: The End effector name") + .property("eef_frame", "str: Name of the end effector frame") + .property("eef_group", "str: Joint model group of the end effector") + .property("eef_parent_group", "str: Joint model group of the eef's parent") + .def(py::init(), "grasp_generator"_a, + "name"_a = std::string("pick")) + .def("setApproachMotion", &Pick::setApproachMotion, R"( + The approaching motion towards the grasping state is represented + by a twist message. + Additionally specify the minimum and maximum allowed distances to travel. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "motion"_a, "min_distance"_a, "max_distance"_a) + .def("setLiftMotion", py::overload_cast(&Pick::setLiftMotion), R"( + The lifting motion away from the grasping state is represented by a twist message. + Additionally specify the minimum and maximum allowed distances to travel. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "motion"_a, "min_distance"_a, "max_distance"_a) + .def("setLiftMotion", py::overload_cast&>(&Pick::setLiftMotion), R"( + The lifting motion away from the grasping state is represented by its destination as joint-value pairs + )", "place"_a); + + properties::class_(m, "Place", R"( + The Place stage is a specialization of the PickPlaceBase class, which + wraps the pipeline to pick or place an object with a given end effector. + + Placing consist of the inverse order of stages: + + - Place down along a given direction + - Detach the object + - Linearly retract end effector + + The end effector postures corresponding to pre-grasp and grasp as well + as the end effector's Cartesian pose needs to be provided by an external + grasp stage. + + Take a look at the :ref:`Pick and Place Tutorial ` for an in-depth look, + as well as the :ref:`How-To Guide ` for a minimal implementation + of a task hierarchy that makes use of the + ``Place`` stage. + )") + .property("object", "str: Name of object to pick") + .property("eef", "str: The End effector name") + .property("eef_frame", "str: Name of the end effector frame") + .property("eef_group", "str: Joint model group of the end effector") + .property("eef_parent_group", "str: Joint model group of the eef's parent") + .def("setRetractMotion", &Place::setRetractMotion, R"( + The retract motion towards the final state is represented + by a Twist_ message. Additionally specify the minimum and + maximum allowed distances to travel. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "motion"_a, "min_distance"_a, "max_distance"_a) + .def("setPlaceMotion", py::overload_cast(&Place::setPlaceMotion), R"( + The object-placing motion towards the final state is represented by a twist message. + Additionally specify the minimum and maximum allowed distances to travel. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "motion"_a, "min_distance"_a, "max_distance"_a ) + .def("setPlaceMotion", py::overload_cast&>(&Place::setPlaceMotion), R"( + The placing motion to the final state is represented by its destination as joint-value pairs + )", "joints"_a ) + .def(py::init(), "place_generator"_a, + "name"_a = std::string("place")); + + properties::class_(m, "SimpleGraspBase", "Abstract base class for grasping and releasing") + .property("eef", "str: The end effector of the robot") + .property("object", "str: The object to grasp (Must be present in the planning scene)") + .property("ik_frame", R"( + PoseStamped_: Set the frame for which the inverse kinematics is calculated + with respect to each pose generated by the pose_generator. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .def("setIKFrame", &SimpleGraspBase::setIKFrame, R"( + Set the frame as a PoseStamped_ for which the inverse kinematics are calculated with respect to + each pose generated by the pose_generator. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )", "transform"_a) + .def("setIKFrame", &SimpleGraspBase::setIKFrame, R"( + Set the frame as a PoseStamped_ for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )", "pose"_a, "link"_a) + .def("setIKFrame", &SimpleGraspBase::setIKFrame, R"( + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + )", "link"_a) + .def("setMaxIKSolutions", &SimpleGraspBase::setMaxIKSolutions, R"( + Set the maximum number of inverse kinematics solutions that should be computed. + )", "max_ik_solutions"_a) + ; + properties::class_(m, "SimpleGrasp", R"( + Specialization of SimpleGraspBase to realize grasping. + + Take a look at the :ref:`Pick and Place Tutorial ` for an in-depth look, + as well as the :ref:`How-To Guide ` for a minimal implementation + of a task hierarchy that makes use of the + ``SimpleGrasp`` stage. + )") + .def(py::init(), "pose_generator"_a, + "name"_a = std::string("grasp")); + + properties::class_(m, "SimpleUnGrasp", R"( + Specialization of SimpleGraspBase to realize ungrasping + + Take a look at the :ref:`Pick and Place Tutorial ` for an in-depth look, + as well as the :ref:`How-To Guide ` for a minimal implementation + of a task hierarchy that makes use of the + ``SimpleUnGrasp`` stage. + )") + .property("pregrasp", "str: Name of the pre-grasp pose") + .property("grasp", "str: Name of the grasp pose") + .def(py::init(), "pose_generator"_a, + "name"_a = std::string("ungrasp")); +} +} // namespace python +} // namespace moveit diff --git a/core/python/bindings/src/stages.h b/core/python/bindings/src/stages.h new file mode 100644 index 000000000..c8acddf9e --- /dev/null +++ b/core/python/bindings/src/stages.h @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "core.h" + +#include +#include + +/** Trampoline classes to allow inheritance in Python (overriding virtual functions) */ + +namespace moveit { +namespace task_constructor { +namespace stages { + +template +class PyMoveTo : public PyPropagatingEitherWay +{ +public: + using PyPropagatingEitherWay::PyPropagatingEitherWay; + void computeForward(const InterfaceState& from_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_IMPL(void, T, "computeForward", &from_state); + return T::computeForward(from_state); + } + void computeBackward(const InterfaceState& to_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_IMPL(void, T, "computeBackward", &to_state); + return T::computeBackward(to_state); + } +}; + +template +class PyMoveRelative : public PyPropagatingEitherWay +{ +public: + using PyPropagatingEitherWay::PyPropagatingEitherWay; + void computeForward(const InterfaceState& from_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_IMPL(void, T, "computeForward", &from_state); + return T::computeForward(from_state); + } + void computeBackward(const InterfaceState& to_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_IMPL(void, T, "computeBackward", &to_state); + return T::computeBackward(to_state); + } +}; + +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/python/pybind11 b/core/python/pybind11 new file mode 160000 index 000000000..4070a64f8 --- /dev/null +++ b/core/python/pybind11 @@ -0,0 +1 @@ +Subproject commit 4070a64f867c60842476f74a2212110ea5e05230 diff --git a/core/python/src/__init__.py b/core/python/src/__init__.py new file mode 100644 index 000000000..1e2408706 --- /dev/null +++ b/core/python/src/__init__.py @@ -0,0 +1,2 @@ +# https://packaging.python.org/guides/packaging-namespace-packages/#pkgutil-style-namespace-packages +__path__ = __import__("pkgutil").extend_path(__path__, __name__) diff --git a/core/python/src/python_tools/__init__.py b/core/python/src/python_tools/__init__.py new file mode 100644 index 000000000..c6b08468f --- /dev/null +++ b/core/python/src/python_tools/__init__.py @@ -0,0 +1 @@ +from pymoveit_python_tools import * diff --git a/core/python/src/task_constructor/__init__.py b/core/python/src/task_constructor/__init__.py new file mode 100644 index 000000000..4b1b82a93 --- /dev/null +++ b/core/python/src/task_constructor/__init__.py @@ -0,0 +1,3 @@ +from . import core, stages + +__doc__ = "top-level module of MoveIt Task constructor" diff --git a/core/python/src/task_constructor/core.py b/core/python/src/task_constructor/core.py new file mode 100644 index 000000000..899074f32 --- /dev/null +++ b/core/python/src/task_constructor/core.py @@ -0,0 +1,3 @@ +from pymoveit_mtc.core import * + +__doc__ = "Provides wrappers for :doc:`core C++ classes `." diff --git a/core/python/src/task_constructor/stages.py b/core/python/src/task_constructor/stages.py new file mode 100644 index 000000000..744e20a01 --- /dev/null +++ b/core/python/src/task_constructor/stages.py @@ -0,0 +1,3 @@ +from pymoveit_mtc.stages import * + +__doc__ = "Provides wrappers for :doc:`stage C++ classes `." diff --git a/core/python/test/rostest_mps.py b/core/python/test/rostest_mps.py new file mode 100755 index 000000000..2c2551248 --- /dev/null +++ b/core/python/test/rostest_mps.py @@ -0,0 +1,117 @@ +#! /usr/bin/env python + +from __future__ import print_function +import unittest +import rostest +from moveit_commander.roscpp_initializer import roscpp_initialize +from moveit_commander import PlanningSceneInterface +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped + + +def make_pose(x, y, z): + pose = PoseStamped() + pose.header.frame_id = "base_link" + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + pose.pose.orientation.w = 1.0 + return pose + + +class TestModifyPlanningScene(unittest.TestCase): + PLANNING_GROUP = "manipulator" + + def setUp(self): + super(TestModifyPlanningScene, self).setUp() + self.psi = PlanningSceneInterface(synchronous=True) + self.make_box = self.psi._PlanningSceneInterface__make_box + # insert a box to collide with + self.psi.add_box("box", make_pose(0.8, 0.25, 1.25), [0.2, 0.2, 0.2]) + self.task = task = core.Task() + task.add(stages.CurrentState("current")) + + def insertMove(self, position=-1): + # colliding motion + move = stages.MoveRelative("move", core.JointInterpolationPlanner()) + move.group = self.PLANNING_GROUP + move.setDirection({"joint_1": 0.3}) + self.task.insert(move, position) + + def test_collision(self): + self.insertMove() + self.assertFalse(self.task.plan()) + + def test_allow_collision_list(self): + mps = stages.ModifyPlanningScene("allow specific collisions for box") + mps.allowCollisions("box", ["link_4", "link_5", "link_6"], True) + self.task.add(mps) + self.insertMove() + self.assertTrue(self.task.plan()) + + def test_allow_collision_all(self): + # insert an extra collision object that is unknown to ACM + self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]) + # attach box to end effector + self.psi.attach_box("link_6", "box") + self.insertMove() + self.assertFalse(self.task.plan()) + + # allow all collisions for attached "box" object + mps = stages.ModifyPlanningScene("allow all collisions for box") + mps.allowCollisions("box", True) + self.task.insert(mps, 1) + self.assertTrue(self.task.plan()) + # restore original state + self.psi.remove_attached_object("link_6", "box") + self.psi.remove_world_object("block") + + def test_fw_add_object(self): + mps = stages.ModifyPlanningScene("addObject(block)") + mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])) + self.task.add(mps) + self.insertMove() + self.assertFalse(self.task.plan()) + + def test_fw_remove_object(self): + mps = stages.ModifyPlanningScene("removeObject(box)") + mps.removeObject("box") + self.task.insert(mps, 1) + self.assertTrue(self.task.plan()) + s = self.task.solutions[0].toMsg() + self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box") + + def DISABLED_test_bw_add_object(self): + # add object to move_group's planning scene + self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]) + + # backward operation will actually remove the object + mps = stages.ModifyPlanningScene("addObject(block) backwards") + mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])) + self.task.insert(mps, 0) + self.insertMove(0) + self.assertTrue(self.task.plan()) + + self.psi.remove_world_object("block") # restore original state + + s = self.task.solutions[0].toMsg() + + # block shouldn't be in start scene + objects = [o.id for o in s.start_scene.world.collision_objects] + self.assertTrue(objects == ["box"]) + + # only addObject(block) should add it + objects = [o.id for o in s.sub_trajectory[1].scene_diff.world.collision_objects] + self.assertTrue(objects == ["block", "box"]) + + def DISABLED_test_bw_remove_object(self): + mps = stages.ModifyPlanningScene("removeObject(box) backwards") + mps.removeObject("box") + self.task.insert(mps, 0) + self.insertMove(0) + self.assertFalse(self.task.plan()) + + +if __name__ == "__main__": + roscpp_initialize("test_mtc") + rostest.rosrun("mtc", "mps", TestModifyPlanningScene) diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py new file mode 100755 index 000000000..958066ad6 --- /dev/null +++ b/core/python/test/rostest_mtc.py @@ -0,0 +1,59 @@ +#! /usr/bin/env python + +from __future__ import print_function +import unittest +import rostest +from moveit_commander.roscpp_initializer import roscpp_initialize +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped, Pose +from geometry_msgs.msg import Vector3Stamped, Vector3 +from std_msgs.msg import Header +import rospy + + +class Test(unittest.TestCase): + PLANNING_GROUP = "manipulator" + + def test_MoveAndExecute(self): + moveRel = stages.MoveRelative("moveRel", core.JointInterpolationPlanner()) + moveRel.group = self.PLANNING_GROUP + moveRel.setDirection({"joint_1": 0.2, "joint_2": 0.4}) + + moveTo = stages.MoveTo("moveTo", core.JointInterpolationPlanner()) + moveTo.group = self.PLANNING_GROUP + moveTo.setGoal("all-zeros") + + task = core.Task() + task.add(stages.CurrentState("current"), moveRel, moveTo) + + self.assertTrue(task.plan()) + self.assertEqual(len(task.solutions), 1) + task.execute(task.solutions[0]) + + def test_Merger(self): + cartesian = core.CartesianPath() + + def createDisplacement(group, displacement): + move = stages.MoveRelative("displace", cartesian) + move.group = group + move.ik_frame = PoseStamped(header=Header(frame_id="tool0")) + dir = Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(*displacement)) + move.setDirection(dir) + move.restrictDirection(stages.MoveRelative.Direction.FORWARD) + return move + + task = core.Task() + task.add(stages.CurrentState("current")) + merger = core.Merger("merger") + merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0])) + merger.insert(createDisplacement(self.PLANNING_GROUP, [0.2, 0, 0])) + task.add(merger) + + task.enableIntrospection() + task.init() + self.assertFalse(task.plan()) + + +if __name__ == "__main__": + roscpp_initialize("test_mtc") + rostest.rosrun("mtc", "base", Test) diff --git a/core/python/test/rostest_mtc.test b/core/python/test/rostest_mtc.test new file mode 100644 index 000000000..0c34d9a51 --- /dev/null +++ b/core/python/test/rostest_mtc.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py new file mode 100755 index 000000000..562adf7b7 --- /dev/null +++ b/core/python/test/rostest_trampoline.py @@ -0,0 +1,135 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from __future__ import print_function +import unittest +import rostest +from moveit_commander.roscpp_initializer import roscpp_initialize +from moveit.task_constructor import core, stages +from moveit.core.planning_scene import PlanningScene +from geometry_msgs.msg import Vector3Stamped, Vector3, PoseStamped +from std_msgs.msg import Header + +PLANNING_GROUP = "manipulator" + +pybind11_versions = [ + k for k in __builtins__.__dict__.keys() if k.startswith("__pybind11_internals_v") +] +incompatible_pybind11_msg = "MoveIt and MTC use incompatible pybind11 versions: " + "\n- ".join( + pybind11_versions +) + + +class PyGenerator(core.Generator): + """Implements a custom 'Generator' stage.""" + + max_calls = 3 + + def __init__(self, name="Generator"): + core.Generator.__init__(self, name) + self.reset() + + def init(self, robot_model): + self.ps = PlanningScene(robot_model) + + def reset(self): + core.Generator.reset(self) + self.num = self.max_calls + + def canCompute(self): + return self.num > 0 + + def compute(self): + self.num = self.num - 1 + self.spawn(core.InterfaceState(self.ps), self.num) + + +class PyMonitoringGenerator(core.MonitoringGenerator): + """Implements a custom 'MonitoringGenerator' stage.""" + + solution_multiplier = 2 + + def __init__(self, name="MonitoringGenerator"): + core.MonitoringGenerator.__init__(self, name) + self.reset() + + def reset(self): + core.MonitoringGenerator.reset(self) + self.upstream_solutions = list() + + def onNewSolution(self, sol): + self.upstream_solutions.append(sol) + + def canCompute(self): + return bool(self.upstream_solutions) + + def compute(self): + scene = self.upstream_solutions.pop(0).end.scene + for i in range(self.solution_multiplier): + self.spawn(core.InterfaceState(scene), i) + + +class PyMoveRelX(stages.MoveRelative): + """Implements a custom propagator stage.""" + + def __init__(self, x, planner, name="Move ±x"): + stages.MoveRelative.__init__(self, name, planner) + self.group = PLANNING_GROUP + self.ik_frame = PoseStamped(header=Header(frame_id="tool0")) + self.setDirection( + Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(x, 0, 0)) + ) + + def computeForward(self, from_state): + return stages.MoveRelative.computeForward(self, from_state) + + def computeBackward(self, to_state): + return stages.MoveRelative.computeBackward(self, to_state) + + +class TestTrampolines(unittest.TestCase): + def setUp(self): + self.cartesian = core.CartesianPath() + self.jointspace = core.JointInterpolationPlanner() + + def create(self, *stages): + task = core.Task() + task.enableIntrospection() + for stage in stages: + task.add(stage) + return task + + def plan(self, task, expected_solutions=None, wait=False): + task.plan() + if expected_solutions is not None: + self.assertEqual(len(task.solutions), expected_solutions) + if wait: + input("Waiting for any key (allows inspection in rviz)") + + @unittest.skipIf(len(pybind11_versions) > 1, incompatible_pybind11_msg) + def test_generator(self): + task = self.create(PyGenerator()) + self.plan(task, expected_solutions=PyGenerator.max_calls) + + @unittest.skipIf(len(pybind11_versions) > 1, incompatible_pybind11_msg) + def test_monitoring_generator(self): + task = self.create( + stages.CurrentState("current"), + stages.Connect(planners=[(PLANNING_GROUP, self.jointspace)]), + PyMonitoringGenerator("generator"), + ) + task["generator"].setMonitoredStage(task["current"]) + self.plan(task, expected_solutions=PyMonitoringGenerator.solution_multiplier) + + def test_propagator(self): + task = self.create( + PyMoveRelX(-0.2, self.cartesian), + stages.CurrentState(), + PyMoveRelX(+0.2, self.cartesian), + ) + self.plan(task, expected_solutions=1) + + +if __name__ == "__main__": + roscpp_initialize("test_mtc") + rostest.rosrun("mtc", "trampoline", TestTrampolines) diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py new file mode 100644 index 000000000..19cd843b0 --- /dev/null +++ b/core/python/test/test_mtc.py @@ -0,0 +1,347 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from __future__ import print_function +import unittest, sys +from geometry_msgs.msg import Pose, PoseStamped, PointStamped, TwistStamped, Vector3Stamped +from moveit_msgs.msg import RobotState, Constraints, MotionPlanRequest +from moveit.task_constructor import core, stages + + +class TestPropertyMap(unittest.TestCase): + def __init__(self, *args, **kwargs): + super(TestPropertyMap, self).__init__(*args, **kwargs) + self.props = core.PropertyMap() + + def _check(self, name, value): + self.props[name] = value + self.assertEqual(self.props[name], value) + + def test_assign(self): + self._check("double", 3.14) + self._check("long", 42) + self._check("long", 13) + self._check("bool", True) + self._check("bool", False) + self._check("string", "anything") + self._check("pose", PoseStamped()) + # MotionPlanRequest is not registered as property type and should raise + self.assertRaises(TypeError, self._check, "request", MotionPlanRequest()) + + def test_assign_in_reference(self): + planner = core.PipelinePlanner() + props = planner.properties + + props["goal_joint_tolerance"] = 3.14 + self.assertEqual(props["goal_joint_tolerance"], 3.14) + self.assertEqual(planner.goal_joint_tolerance, 3.14) + + planner.goal_joint_tolerance = 2.71 + self.assertEqual(props["goal_joint_tolerance"], 2.71) + + props["planner"] = "planner" + self.assertEqual(props["planner"], "planner") + self.assertEqual(planner.planner, "planner") + + props["double"] = 3.14 + a = props + props["double"] = 2.71 + self.assertEqual(a["double"], 2.71) + + planner.planner = "other" + self.assertEqual(props["planner"], "other") + self.assertEqual(planner.planner, "other") + + del planner + # We can still access props, because actual destruction of planner is delayed + self.assertEqual(props["goal_joint_tolerance"], 2.71) + self.assertEqual(props["planner"], "other") + + def test_iter(self): + # assign values so we can iterate over them + self.props["double"] = 3.14 + self.props["bool"] = True + keys = [v for v in self.props] + self.assertEqual(len(keys), 2) + items = [(k, v) for (k, v) in self.props.items()] + self.assertEqual(keys, [k for (k, v) in items]) + + def test_update(self): + self.props["double"] = 3.14 + self.props.update({"double": 2.72, "bool": True}) + self.props.update({}) + self.assertEqual(self.props["double"], 2.72) + self.assertEqual(self.props["bool"], True) + + def test_expose(self): + self.props["double"] = 3.14 + + other = core.PropertyMap() + self.props.exposeTo(other, "double") + self.assertEqual(other["double"], self.props["double"]) + + self.props.exposeTo(other, "double", "float") + self.assertEqual(other["float"], self.props["double"]) + + +class TestModifyPlanningScene(unittest.TestCase): + def __init__(self, *args, **kwargs): + super(TestModifyPlanningScene, self).__init__(*args, **kwargs) + self.mps = stages.ModifyPlanningScene("mps") + + def test_attach_objects_invalid_args(self): + for value in [None, 1, 1.5, {}]: + self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link") + self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link", True) + self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link", False) + + def test_attach_objects_valid_args(self): + self.mps.attachObject("object", "link") + self.mps.detachObject("object", "link") + + self.mps.attachObjects("object", "link") + self.mps.detachObjects("object", "link") + self.mps.attachObjects("object", "link", True) + self.mps.attachObjects("object", "link", False) + + self.mps.attachObjects([], "link") + self.mps.attachObjects(["object"], "link") + self.mps.attachObjects(["object1", "object2", "object3"], "link") + + def test_allow_collisions(self): + self.mps.allowCollisions("first", "second") + self.mps.allowCollisions("first", "second", True) + self.mps.allowCollisions("first", "second", False) + + self.mps.allowCollisions(["first"], ["second"]) + + +class TestStages(unittest.TestCase): + def __init__(self, *args, **kwargs): + super(TestStages, self).__init__(*args, **kwargs) + self.planner = core.PipelinePlanner() + + def _check(self, stage, name, value): + self._check_assign(stage, name, value) + self._check_invalid_args(stage, name, type(value)) + + def _check_assign(self, stage, name, value): + setattr(stage, name, value) + self.assertEqual(getattr(stage, name), value) + + def _check_invalid_args(self, stage, name, target_type): + """Check some basic types to raise an ArgumentError when assigned""" + for value in [None, 1, 1.0, "string", [], {}, set()]: + try: + target_type(value) + continue # ignore values that are implicitly convertible to target_type + except: + pass + + try: + setattr(stage, name, value) + except TypeError: + pass + except: + msg = "Assigning {} did raise wrong exception: {}" + self.fail(msg.format(value, sys.exc_info()[0])) + else: + if value == "string" and target_type is PoseStamped: + continue # string is convertible to PoseStamped + msg = "Assigning {} did not raise an exception, result: {}" + self.fail(msg.format(value, getattr(stage, name))) + + def test_CurrentState(self): + stage = stages.CurrentState("current") + + def test_FixedState(self): + stage = stages.FixedState("fixed") + + def test_ComputeIK(self): + generator_stage = stages.GeneratePose("generator") + stage = stages.ComputeIK("IK", generator_stage) + + self._check(stage, "timeout", 0.5) + self._check(stage, "eef", "eef") + self._check(stage, "group", "group") + self._check(stage, "default_pose", "default_pose") + self._check(stage, "max_ik_solutions", 1) + self.assertRaises(TypeError, self._check_assign, stage, "max_ik_solutions", -1) + self._check(stage, "ignore_collisions", False) + self._check(stage, "ignore_collisions", True) + self._check(stage, "ik_frame", PoseStamped()) + self._check(stage, "target_pose", PoseStamped()) + self._check(stage, "forwarded_properties", ["name1", "name2", "name3"]) + stage.forwarded_properties = "name" + self.assertRaises(TypeError, self._check_assign, stage, "forwarded_properties", [1, 2]) + + def test_MoveTo(self): + stage = stages.MoveTo("move", self.planner) + + self._check(stage, "group", "group") + self._check(stage, "ik_frame", PoseStamped()) + self._check(stage, "path_constraints", Constraints()) + stage.setGoal(PoseStamped()) + stage.setGoal(PointStamped()) + stage.setGoal(RobotState()) + stage.setGoal("named pose") + stage.setGoal(dict(joint1=1.0, joint2=2.0)) + self._check(stage, "path_constraints", Constraints()) + + def test_MoveRelative(self): + stage = stages.MoveRelative("move", self.planner) + + self._check(stage, "group", "group") + self._check(stage, "ik_frame", PoseStamped()) + self._check(stage, "min_distance", 0.5) + self._check(stage, "max_distance", 0.25) + self._check(stage, "path_constraints", Constraints()) + stage.setDirection(TwistStamped()) + stage.setDirection(Vector3Stamped()) + stage.setDirection({"joint": 0.1}) + + def test_Connect(self): + planner = core.PipelinePlanner() + stage = stages.Connect("connect", [("group1", planner), ("group2", planner)]) + + def test_FixCollisionObjects(self): + stage = stages.FixCollisionObjects("collision") + + self._check(stage, "max_penetration", 0.5) + + def test_GenerateGraspPose(self): + stage = stages.GenerateGraspPose("generate_grasp_pose") + + self._check(stage, "eef", "eef") + self._check(stage, "pregrasp", "pregrasp") + self._check(stage, "object", "object") + self._check(stage, "angle_delta", 0.5) + + def test_GeneratePose(self): + stage = stages.GeneratePose("generate_pose") + + self._check(stage, "pose", PoseStamped()) + + def test_Pick(self): + generator_stage = stages.GeneratePose("generator") + stage = stages.Pick(generator_stage, "pick") + + self._check(stage, "object", "object") + self._check(stage, "eef", "eef") + self._check(stage, "eef_frame", "eef_frame") + self._check(stage, "eef_group", "eef_group") + self._check(stage, "eef_parent_group", "eef_parent_group") + + def test_Place(self): + generator_stage = stages.GeneratePose("generator") + stage = stages.Place(generator_stage, "place") + + self._check(stage, "object", "object") + self._check(stage, "eef", "eef") + self._check(stage, "eef_frame", "eef_frame") + self._check(stage, "eef_group", "eef_group") + self._check(stage, "eef_parent_group", "eef_parent_group") + + def test_SimpleGrasp(self): + stage = stages.SimpleGrasp(stages.GenerateGraspPose("grasp")) + + self._check(stage, "eef", "eef") + self._check(stage, "object", "object") + + def test_SimpleUnGrasp(self): + stage = stages.SimpleUnGrasp(stages.GenerateGraspPose("ungrasp")) + + self._check(stage, "eef", "eef") + self._check(stage, "object", "object") + + def test_PropertyMaps(self): + for name in dir(stages): + if name.startswith("__") or name.endswith("__"): + continue + + stage = getattr(stages, name) + try: + props = stage().properties + except: + continue + + try: + for p in props: + pass + except Exception as ex: + print("error in class {}: {}".format(stage, ex)) + raise + + def test_CostTerm(self): + stage = stages.CurrentState() + weights = {"joint_{}".format(i + 1): 1.0 for i in range(6)} + costs = core.PathLength(weights) + stage.setCostTerm(costs) + + +class BaseTestCases: + class ContainerTest(unittest.TestCase): + def __init__(self, ContainerType, *args, **kwargs): + super(BaseTestCases.ContainerTest, self).__init__(*args, **kwargs) + self.ContainerType = ContainerType + self.container = container = ContainerType() + container.add(stages.CurrentState("1")) + container.add(stages.CurrentState("2")) + container.add(stages.CurrentState("3")) + + def test_move(self): + container = self.ContainerType() + stage = stages.CurrentState() + container.add(stage) + with self.assertRaises(ValueError): + stage.name + + def test_access_by_name(self): + with self.assertRaises(IndexError): + self.container["unknown"] + + child = self.container["2"] + self.assertEqual(child.name, "2") + + def test_access_by_iterator(self): + self.assertEqual([child.name for child in self.container], ["1", "2", "3"]) + + def test_access_by_index(self): + self.assertEqual(self.container[0].name, "1") + self.assertEqual(self.container[1].name, "2") + self.assertEqual(self.container[-1].name, "3") + self.assertEqual(self.container[-2].name, "2") + with self.assertRaises(IndexError): + self.container[3] + with self.assertRaises(IndexError): + self.container[-4] + + +class TestSerial(BaseTestCases.ContainerTest): + def __init__(self, *args, **kwargs): + super(TestSerial, self).__init__(core.SerialContainer, *args, **kwargs) + + +class TestTask(BaseTestCases.ContainerTest): + def __init__(self, *args, **kwargs): + super(TestTask, self).__init__(core.Task, *args, **kwargs) + + def test(self): + task = core.Task() + current = stages.CurrentState("current") + self.assertEqual(current.name, "current") + current.timeout = 1.23 + self.assertEqual(current.timeout, 1.23) + + task.add(current) + + # ownership of current was passed to task + with self.assertRaises(ValueError): + current.name + + task.add(stages.Connect("connect", [])) + task.add(stages.FixedState()) + + +if __name__ == "__main__": + unittest.main() diff --git a/core/rosdoc.yaml b/core/rosdoc.yaml new file mode 100644 index 000000000..2448c5a73 --- /dev/null +++ b/core/rosdoc.yaml @@ -0,0 +1,10 @@ +- builder: sphinx + name: Python API + output_dir: python + sphinx_root_dir: doc +- builder: doxygen + name: C++ API + output_dir: cpp + file_patterns: "*.c *.cpp *.h *.cc *.hh *.dox" + exclude_patterns: "*/core/python/pybind11/* */core/python/bindings/* */test/* " + exclude_symbols: "*Private class_ declval*" diff --git a/core/setup.py b/core/setup.py new file mode 100644 index 000000000..cd88b3320 --- /dev/null +++ b/core/setup.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python + +from setuptools import find_packages, setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + # list of packages to setup + packages=find_packages("python/src"), + # specify location of root ("") package dir + package_dir={"": "python/src"}, +) +dist = setup(**d) + +# Remove moveit/__init__.py when building .deb packages +# Otherwise, the installation procedure will complain about conflicting files (with moveit_core) +try: + # installation dir (.../lib/python3/dist-packages) + libdir = dist.command_obj["install_lib"].install_dir + if "/debian/ros-" in libdir and "moveit-task-constructor-core/opt/ros/" in libdir: + import os + import shutil + + os.remove(os.path.join(libdir, "moveit", "__init__.py")) + shutil.rmtree(os.path.join(libdir, "moveit", "__pycache__")) +except AttributeError as e: + pass diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 68333667a..6f65c0bb9 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(${PROJECT_NAME} SHARED ${PROJECT_INCLUDE}/solvers/cartesian_path.h ${PROJECT_INCLUDE}/solvers/joint_interpolation.h ${PROJECT_INCLUDE}/solvers/pipeline_planner.h + ${PROJECT_INCLUDE}/solvers/multi_planner.h container.cpp cost_terms.cpp @@ -32,27 +33,29 @@ add_library(${PROJECT_NAME} SHARED solvers/planner_interface.cpp solvers/cartesian_path.cpp - solvers/pipeline_planner.cpp solvers/joint_interpolation.cpp + solvers/pipeline_planner.cpp + solvers/multi_planner.cpp ) ament_target_dependencies(${PROJECT_NAME} - moveit_core - geometry_msgs - moveit_ros_planning - moveit_ros_planning_interface - moveit_task_constructor_msgs - rclcpp - rviz_marker_tools - visualization_msgs + moveit_core + geometry_msgs + moveit_ros_planning + moveit_ros_planning_interface + moveit_task_constructor_msgs + rclcpp + rviz_marker_tools + visualization_msgs ) target_include_directories(${PROJECT_NAME} PUBLIC $ - PUBLIC $ + $ + $ ) add_subdirectory(stages) install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets ARCHIVE DESTINATION lib LIBRARY DESTINATION lib) diff --git a/core/src/container.cpp b/core/src/container.cpp index e565b01d9..677e80165 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -38,7 +38,9 @@ #include #include #include +#include +#include #include #include @@ -49,16 +51,72 @@ #include using namespace std::placeholders; +using namespace trajectory_processing; namespace moveit { namespace task_constructor { +// for debugging of how children interfaces evolve over time +__attribute__((unused)) // silent unused-function warning +static void +printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator, + std::ostream& os = std::cerr) { + static unsigned int id = 0; + const unsigned int width = 10; // indentation of name + os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' '; + if (success) + os << ++id << ' '; + if (const auto conn = dynamic_cast(creator.pimpl())) + conn->printPendingPairs(os); + os << std::endl; + + for (const auto& child : container.children()) { + auto cimpl = child->pimpl(); + os << std::setw(width) << std::left << child->name(); + if (!cimpl->starts() && !cimpl->ends()) + os << "↕ " << std::endl; + if (cimpl->starts()) + os << "↓ " << *child->pimpl()->starts() << std::endl; + if (cimpl->starts() && cimpl->ends()) + os << std::setw(width) << " "; + if (cimpl->ends()) + os << "↑ " << *child->pimpl()->ends() << std::endl; + } +} + ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) , required_interface_(UNKNOWN) , pending_backward_(new Interface) , pending_forward_(new Interface) {} +ContainerBasePrivate& ContainerBasePrivate::operator=(ContainerBasePrivate&& other) { + assert(internal_external_.empty() && other.internal_external_.empty()); + + // move StagePrivate members + this->StagePrivate::operator=(std::move(other)); + + // swapping of container members needed to maintain valid pending_* interfaces + // and children (e.g. for TaskPrivate) + required_interface_ = other.required_interface_; + std::swap(pending_backward_, other.pending_backward_); + std::swap(pending_forward_, other.pending_forward_); + std::swap(children_, other.children_); + + // redirect all children's parent pointers to the new parent + auto reparent_children = [](ContainerBasePrivate& self) { + for (auto it = self.children_.begin(), end = self.children_.end(); it != end; ++it) { + auto cimpl = (*it)->pimpl(); + cimpl->unparent(); + cimpl->setParent(static_cast(self.me_)); + cimpl->setParentPosition(it); + } + }; + reparent_children(*this); + reparent_children(other); + return *this; +} + ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const { if (!for_insert && index < 0) --index; @@ -106,66 +164,38 @@ void ContainerBasePrivate::compute() { static_cast(me_)->compute(); } -void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "'" << child.name() << "' generated a failure"); - switch (child.pimpl()->interfaceFlags()) { - case GENERATE: - // just ignore: the pair of (new) states isn't known to us anyway - // TODO: If child is a container, from and to might have associated solutions already! - break; - - case PROPAGATE_FORWARDS: // mark from as failed (backwards) - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch"); - setStatus(from, InterfaceState::Status::FAILED); - break; - case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch"); - setStatus(to, InterfaceState::Status::FAILED); - break; - - case CONNECT: - if (const Connecting* conn = dynamic_cast(&child)) { - auto cimpl = conn->pimpl(); - if (!cimpl->hasPendingOpposites(from)) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch"); - setStatus(from, InterfaceState::Status::FAILED); - } - if (!cimpl->hasPendingOpposites(to)) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune forward branch"); - setStatus(to, InterfaceState::Status::FAILED); - } - } - break; - } - // printChildrenInterfaces(*this, false, child); -} - template -void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) { - if (s->priority().status() == status) +void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, + InterfaceState::Status status) { + if (status != InterfaceState::Status::ENABLED && creator) { + if (const auto* conn = dynamic_cast(creator)) { + auto cimpl = conn->pimpl(); + // if creator is a Connecting stage and target has enabled opposite states (other than source) + if (cimpl->hasPendingOpposites(source, target)) + return; // don't prune + } + } + if (target->priority().status() == status) return; // nothing changing - // if we should disable the state, only do so when there is no enabled alternative path - if (status == InterfaceState::PRUNED) { + // Skip disabling the state, if there are alternative enabled solutions + if (status != InterfaceState::ENABLED) { auto solution_is_enabled = [](auto&& solution) { return state()>(*solution)->priority().enabled(); }; - const auto& alternatives = trajectories()>(*s); + const auto& alternatives = trajectories()>(*target); auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled); if (alternative_path != alternatives.cend()) return; } // actually enable/disable the state - if (s->owner()) { - s->owner()->updatePriority(const_cast(s), InterfaceState::Priority(s->priority(), status)); - } else { - const_cast(s)->priority_ = InterfaceState::Priority(s->priority(), status); - } + const_cast(target)->updateStatus(status); - // if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface - if (parent() && trajectories(*s).empty()) { - auto external{ internalToExternalMap().find(s) }; + // if possible (i.e. if target has an external counterpart), escalate setStatus to external interface + if (parent() && trajectories(*target).empty()) { + // TODO: This was coded with SerialContainer in mind. Not sure, it works for ParallelContainers + auto external{ internalToExternalMap().find(target) }; if (external != internalToExternalMap().end()) { // do we have an external state? // only escalate if there is no other *enabled* internal state connected to the same external one // all internal states linked to external @@ -173,37 +203,75 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); }; auto other_path{ std::find_if(internals.first, internals.second, is_enabled) }; if (other_path == internals.second) - parent()->pimpl()->setStatus(external->get(), status); + parent()->pimpl()->setStatus(nullptr, nullptr, external->get(), status); return; } } // To break symmetry between both ends of a partial solution sequence that gets disabled, - // we mark the first state with FAILED and all other states down the tree only with PRUNED. - // This allows us to re-enable the FAILED side, while not (yet) consider the PRUNED states again, + // we mark the first state with ARMED and all other states down the tree with PRUNED. + // This allows us to re-enable the ARMED state, but not the PRUNED states, // when new states arrive in a Connecting stage. - // All PRUNED states are only re-enabled if the FAILED state actually gets connected. - // For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221 - if (status == InterfaceState::Status::FAILED) - status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED + // For details, https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + if (status == InterfaceState::Status::ARMED) + status = InterfaceState::Status::PRUNED; // only the first state is marked as ARMED // traverse solution tree - for (const SolutionBase* successor : trajectories(*s)) - setStatus(state(*successor), status); + for (const SolutionBase* successor : trajectories(*target)) + setStatus(successor->creator(), target, state(*successor), status); } -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); +// recursively update state priorities along solution path template -void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { - if (updated) { - auto internals{ externalToInternalMap().equal_range(&*external) }; - for (auto& i = internals.first; i != internals.second; ++i) { - setStatus(i->second, external->priority().status()); - } +inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) { + InterfaceState::Priority priority(prio, s.priority().status()); + if (s.priority() == priority) return; + const_cast(s).updatePriority(priority); + for (const SolutionBase* successor : trajectories(s)) + updateStatePrios(*state(*successor), prio); +} + +void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "'" << child.name() << "' generated a failure"); + switch (child.pimpl()->interfaceFlags()) { + case GENERATE: + // just ignore: the pair of (new) states isn't known to us anyway + // TODO: If child is a container, from and to might have associated solutions already! + break; + + case PROPAGATE_FORWARDS: // mark from as failed (backwards) + setStatus(nullptr, nullptr, from, InterfaceState::Status::PRUNED); + break; + case PROPAGATE_BACKWARDS: // mark to as failed (forwards) + setStatus(nullptr, nullptr, to, InterfaceState::Status::PRUNED); + break; + + case CONNECT: + setStatus(&child, to, from, InterfaceState::Status::ARMED); + setStatus(&child, from, to, InterfaceState::Status::ARMED); + break; } + // printChildrenInterfaces(*this, false, child); +} +template +void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, + Interface::UpdateFlags updated) { + if (updated) { + auto prio = external->priority(); + auto internals = externalToInternalMap().equal_range(&*external); + + if (updated.testFlag(Interface::Update::STATUS)) { // propagate external status updates to internal copies + for (auto& i = internals.first; i != internals.second; ++i) + setStatus(nullptr, nullptr, i->second, prio.status()); + } else if (updated.testFlag(Interface::Update::PRIORITY)) { + for (auto& i = internals.first; i != internals.second; ++i) + updateStatePrios()>(*i->second, prio); + } else + assert(false); // Expecting either STATUS or PRIORITY updates, not both! + return; + } // create a clone of external state within target interface (child's starts() or ends()) auto internal = states_.insert(states_.end(), InterfaceState(*external)); target->add(*internal); @@ -211,6 +279,14 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa internalToExternalMap().insert(std::make_pair(&*internal, &*external)); } +void ContainerBasePrivate::copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target, + Interface::UpdateFlags updated) { + if (dir == Interface::FORWARD) + copyState(external, target, updated); + else + copyState(external, target, updated); +} + void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to) { computeCost(*internal_from, *internal_to, *solution); @@ -266,6 +342,12 @@ Stage* ContainerBase::findChild(const std::string& name) const { return nullptr; } +Stage* ContainerBase::operator[](int index) const { + auto impl = pimpl(); + auto it = impl->childByIndex(index, false); + return it != impl->children().end() ? it->get() : nullptr; +} + bool ContainerBase::traverseChildren(const ContainerBase::StageCallback& processor) const { return pimpl()->traverseStages(processor, 0, 1); } @@ -369,6 +451,20 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) { throw errors; } +void ContainerBase::explainFailure(std::ostream& os) const { + for (const auto& stage : pimpl()->children()) { + if (!stage->solutions().empty()) + continue; // skip deeper traversal, this stage produced solutions + if (stage->numFailures()) { + os << stage->name() << " (0/" << stage->numFailures() << ")"; + stage->explainFailure(os); + os << std::endl; + break; + } + stage->explainFailure(os); // recursively process children + } +} + std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { ContainerBase::StageCallback processor = [&os](const Stage& stage, unsigned int depth) -> bool { os << std::string(2 * depth, ' ') << *stage.pimpl() << std::endl; @@ -378,31 +474,6 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { return os; } -// for debugging of how children interfaces evolve over time -static void printChildrenInterfaces(const ContainerBase& container, bool success, const Stage& creator, - std::ostream& os = std::cerr) { - static unsigned int id = 0; - const unsigned int width = 10; // indentation of name - os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' '; - if (success) - os << ++id << ' '; - if (const Connecting* conn = dynamic_cast(&creator)) - conn->pimpl()->printPendingPairs(os); - os << std::endl; - - for (const auto& child : container.pimpl()->children()) { - auto cimpl = child->pimpl(); - os << std::setw(width) << std::left << child->name(); - if (!cimpl->starts() && !cimpl->ends()) - os << "↕ " << std::endl; - if (cimpl->starts()) - os << "↓ " << *child->pimpl()->starts() << std::endl; - if (cimpl->starts() && cimpl->ends()) - os << std::setw(width) << " "; - if (cimpl->ends()) - os << "↑ " << *child->pimpl()->ends() << std::endl; - } -} /** Collect all partial solution sequences originating from start into given direction */ template struct SolutionCollector @@ -435,20 +506,6 @@ struct SolutionCollector SolutionSequence::container_type trace; }; -inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) { - if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list - state->owner()->updatePriority(const_cast(state), - // update depth + cost, but keep current status - InterfaceState::Priority(prio, state->priority().status())); -} - -template -inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path, - const InterfaceState::Priority& prio) { - for (const SolutionBase* solution : partial_solution_path) - updateStatePrio(state(*solution), prio); -} - void SerialContainer::onNewSolution(const SolutionBase& current) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("SerialContainer"), "'" << this->name() << "' received solution of child stage '" @@ -457,10 +514,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // failures should never trigger this callback assert(!current.isFailure()); - // states of solution must be active, otherwise this would not have been computed - assert(current.start()->priority().enabled()); - assert(current.end()->priority().enabled()); - auto impl = pimpl(); const Stage* creator = current.creator(); auto& children = impl->children(); @@ -498,14 +551,12 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { } if (prio.depth() > 1) { // update state priorities along the whole partial solution path - updateStatePrio(current.start(), prio); - updateStatePrio(current.end(), prio); - updateStatePrios(in.first, prio); - updateStatePrios(out.first, prio); + updateStatePrios(*current.start(), prio); + updateStatePrios(*current.end(), prio); } } } - // printChildrenInterfaces(*this, true, *current.creator()); + // printChildrenInterfaces(*this->pimpl(), true, *current.creator()); // finally, store + announce new solutions to external interface for (const auto& solution : sorted) @@ -570,9 +621,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { validateInterface(*first.pimpl(), expected); // connect first child's (start) pull interface if (const InterfacePtr& target = first.pimpl()->starts()) - starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) { + starts_ = std::make_shared([this, target](Interface::iterator it, Interface::UpdateFlags updated) { this->copyState(it, target, updated); - })); + }); } catch (InitStageException& e) { exceptions.append(e); } @@ -583,6 +634,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { StagePrivate* child_impl = (**it).pimpl(); StagePrivate* previous_impl = (**previous_it).pimpl(); child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK); + child_impl = (**it).pimpl(); // re-assign as pimpl_ pointer of a Fallback container will change! connect(*previous_impl, *child_impl); } catch (InitStageException& e) { exceptions.append(e); @@ -595,9 +647,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { validateInterface(*last.pimpl(), expected); // connect last child's (end) pull interface if (const InterfacePtr& target = last.pimpl()->ends()) - ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) { + ends_ = std::make_shared([this, target](Interface::iterator it, Interface::UpdateFlags updated) { this->copyState(it, target, updated); - })); + }); } catch (InitStageException& e) { exceptions.append(e); } @@ -613,7 +665,7 @@ void SerialContainerPrivate::validateConnectivity() const { ContainerBasePrivate::validateConnectivity(); InterfaceFlags mine = interfaceFlags(); - // check that input / output interface of first / last child matches this' resp. interface + // check that input/output interface of first/last child matches this' resp. interface validateInterface(*children().front()->pimpl(), mine); validateInterface(*children().back()->pimpl(), mine); @@ -625,7 +677,7 @@ void SerialContainerPrivate::validateConnectivity() const { const StagePrivate* const cur_impl = **cur; InterfaceFlags required = cur_impl->interfaceFlags(); - // get iterators to prev / next stage in sequence + // get iterators to prev/next stage in sequence auto prev = cur; --prev; auto next = cur; @@ -653,12 +705,12 @@ bool SerialContainer::canCompute() const { void SerialContainer::compute() { for (const auto& stage : pimpl()->children()) { - if (!stage->pimpl()->canCompute()) - continue; - stage->pimpl()->runCompute(); + if (stage->pimpl()->canCompute()) + stage->pimpl()->runCompute(); } } + ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name) : ContainerBasePrivate(me, name) {} @@ -676,8 +728,8 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { child_impl->resolveInterface(expected); validateInterfaces(*child_impl, expected, first); // initialize push connections of children according to their demands - setChildsPushForwardInterface(child_impl); setChildsPushBackwardInterface(child_impl); + setChildsPushForwardInterface(child_impl); first = false; } catch (InitStageException& e) { exceptions.append(e); @@ -688,17 +740,21 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { if (exceptions) throw exceptions; - // States received by the container need to be copied to all children's pull interfaces. - if (expected & READS_START) - starts().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(external, updated); - })); - if (expected & READS_END) - ends().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(external, updated); - })); - required_interface_ = expected; + + initializeExternalInterfaces(); +} + +void ParallelContainerBasePrivate::initializeExternalInterfaces() { + // States received by the container need to be copied to all children's pull interfaces. + if (requiredInterface() & READS_START) + starts() = std::make_shared([this](Interface::iterator external, Interface::UpdateFlags updated) { + this->propagateStateToAllChildren(external, updated); + }); + if (requiredInterface() & READS_END) + ends() = std::make_shared([this](Interface::iterator external, Interface::UpdateFlags updated) { + this->propagateStateToAllChildren(external, updated); + }); } void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external, @@ -725,7 +781,7 @@ void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, void ParallelContainerBasePrivate::validateConnectivity() const { InterfaceFlags my_interface = interfaceFlags(); - // check that input / output interfaces of all children are handled by my interface + // check that input/output interfaces of all children are handled by my interface for (const auto& child : children()) validateInterfaces(*child->pimpl(), my_interface); @@ -733,9 +789,9 @@ void ParallelContainerBasePrivate::validateConnectivity() const { } template -void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) { +void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated) { for (const Stage::pointer& stage : children()) - copyState(external, stage->pimpl()->pullInterface(dir), updated); + copyState(external, stage->pimpl()->pullInterface(), updated); } ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {} @@ -806,44 +862,241 @@ void Alternatives::onNewSolution(const SolutionBase& s) { liftSolution(s); } +Fallbacks::Fallbacks(const std::string& name) : Fallbacks(new FallbacksPrivate(this, name)) {} + +Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {} + void Fallbacks::reset() { - active_child_ = nullptr; ParallelContainerBase::reset(); + pimpl()->reset(); } void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { ParallelContainerBase::init(robot_model); - active_child_ = pimpl()->children().front().get(); + pimpl()->reset(); } -bool Fallbacks::canCompute() const { - while (active_child_) { - StagePrivate* child = active_child_->pimpl(); - if (child->canCompute()) - return true; +void Fallbacks::onNewSolution(const SolutionBase& s) { + pimpl()->onNewSolution(s); +} - // active child failed, continue with next - auto next = child->it(); - ++next; - if (next == pimpl()->children().end()) - active_child_ = nullptr; +inline void Fallbacks::replaceImpl() { + FallbacksPrivate *impl = pimpl(); + switch (pimpl()->requiredInterface()) { + case GENERATE: + impl = new FallbacksPrivateGenerator(std::move(*impl)); + break; + case PROPAGATE_FORWARDS: + case PROPAGATE_BACKWARDS: + impl = new FallbacksPrivatePropagator(std::move(*impl)); + break; + case CONNECT: + // For now, we only support Connecting children + for (const auto& child : impl->children()) + if (!dynamic_cast(child.get())) + throw std::runtime_error("CONNECT-like interface is only supported for Connecting children"); + impl = new FallbacksPrivateConnect(std::move(*impl)); + break; + } + delete pimpl_; + pimpl_ = impl; +} + +FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) + : ParallelContainerBasePrivate(me, name) {} + +FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other) +: ParallelContainerBasePrivate(static_cast(other.me()), "") { + // move contents of other + this->ParallelContainerBasePrivate::operator=(std::move(other)); +} + +void FallbacksPrivate::initializeExternalInterfaces() { + // Here we know the final interface of the container (and all its children) + // Thus replace, this pimpl() with a new interface-specific one: + static_cast(me())->replaceImpl(); +} + +void FallbacksPrivate::onNewSolution(const SolutionBase& s) { + // printChildrenInterfaces(*this, true, *s.creator()); + static_cast(me())->liftSolution(s); +} + +void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /*from*/, const InterfaceState* /*to*/) { + // This override is deliberately empty. + // The method prunes solution paths when a child failed to find a valid solution for it, + // but in Fallbacks the next child might still yield a successful solution + // Thus pruning must only occur once the last child is exhausted (inside computePropagate) + // printChildrenInterfaces(*this, false, child); + (void)child; +} + +void FallbacksPrivateCommon::reset() { + current_ = children().begin(); +} + +bool FallbacksPrivateCommon::canCompute() const { + while(current_ != children().end() && // not completely exhausted + !(*current_)->pimpl()->canCompute()) // but current child cannot compute + return const_cast(this)->nextJob(); // advance to next job + + // return value: current child is well defined and thus can compute? + return current_ != children().end(); +} + +void FallbacksPrivateCommon::compute() { + (*current_)->pimpl()->runCompute(); +} + +inline void FallbacksPrivateCommon::nextChild() { + if (std::next(current_) != children().end()) + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Fallbacks"), "Child '" << (*current_)->name() << "' failed, trying next one."); + ++current_; // advance to next child +} + +FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) + : FallbacksPrivateCommon(std::move(old)) { FallbacksPrivateCommon::reset(); } + +bool FallbacksPrivateGenerator::nextJob() { + assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); + + // don't advance to next child when we already produced solutions + if (!solutions_.empty()) { + current_ = children().end(); // indicate that we are exhausted + return false; + } + + do { nextChild(); } + while (current_ != children().end() && !(*current_)->pimpl()->canCompute()); + + // return value shall indicate current_->canCompute() + return current_ != children().end(); +} + + +FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) + : FallbacksPrivateCommon(std::move(old)) { + switch (requiredInterface()) { + case PROPAGATE_FORWARDS: + dir_ = Interface::FORWARD; + starts() = std::make_shared(); + break; + case PROPAGATE_BACKWARDS: + dir_ = Interface::BACKWARD; + ends() = std::make_shared(); + break; + default: + assert(false); + } + FallbacksPrivatePropagator::reset(); +} + +void FallbacksPrivatePropagator::reset() { + FallbacksPrivateCommon::reset(); + job_ = pullInterface(dir_)->end(); // indicate fresh start + job_has_solutions_ = false; +} + +void FallbacksPrivatePropagator::onNewSolution(const SolutionBase& s) { + job_has_solutions_ = true; + FallbacksPrivateCommon::onNewSolution(s); +} + +bool FallbacksPrivatePropagator::nextJob() { + assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); + const auto jobs = pullInterface(dir_); + + if (job_ != jobs->end()) { // current job exists, but is exhausted on current child + if (!job_has_solutions_) // job didn't produce solutions -> feed to next child + nextChild(); else - active_child_ = next->get(); + current_ = children().end(); // indicate that this job is exhausted on all children } - return false; + job_has_solutions_ = false; + + if (current_ == children().end()) { // all children processed the job_ + if (job_ != jobs->end()) { + jobs->remove(job_); // we don't need the job in our interface list anymore + job_ = jobs->end(); // indicate that we need to fetch a new job + } + current_ = children().begin(); // start next job with first child again + } + + // pick next job if needed and possible + if (job_ == jobs->end()) { // need to pick next job + if (!jobs->empty() && jobs->front()->priority().enabled()) + job_ = jobs->begin(); + else + return false; // no more jobs available + } + + // When arriving here, we have a valid job_ and a current_ child to feed it. Let's do that. + copyState(dir_, job_, (*current_)->pimpl()->pullInterface(dir_), Interface::UpdateFlags()); + return true; } -void Fallbacks::compute() { - if (!active_child_) - return; - active_child_->pimpl()->runCompute(); +FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old) + : FallbacksPrivate(std::move(old)) { + starts_ = std::make_shared( + std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); + ends_ = std::make_shared( + std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); + + FallbacksPrivateConnect::reset(); } -void Fallbacks::onNewSolution(const SolutionBase& s) { - liftSolution(s); +void FallbacksPrivateConnect::reset() { + active_ = children().end(); +} + +template +void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated) { + copyState(external, children().front()->pimpl()->pullInterface(dir), updated); + // As we use the Interface* from the first child for all children (we just populate their pending lists) + // there is no need to explicitly propagate state updates to other children. } +bool FallbacksPrivateConnect::canCompute() const { + for (auto it=children().begin(), end=children().end(); it!=end; ++it) + if ((*it)->pimpl()->canCompute()) { + active_ = it; + return true; + } + active_ = children().end(); + return false; +} + +void FallbacksPrivateConnect::compute() { + // Alternatively, we could also compute() all children that canCompute() + assert(active_ != children().end()); + (*active_)->pimpl()->runCompute(); +} + +void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { + // expect failure to be reported from active child + assert(active_ != children().end() && active_->get() == &child); (void)child; + // ... thus we can use std::next(active_) to find the next child + auto next = std::next(active_); + + // NOLINTNEXTLINE(readability-identifier-naming) + auto findIteratorFor = [](const InterfaceState* state, const Interface& interface) { + auto it = std::find(interface.begin(), interface.end(), state); + assert(it != interface.end()); + return it; + }; + + if (next != children().end()) { // pass job to next child + auto next_con = static_cast(const_cast((*next)->pimpl())); + auto first_con = static_cast(children().front()->pimpl()); + auto from_it = findIteratorFor(from, *first_con->starts()); + auto to_it = findIteratorFor(to, *first_con->ends()); + next_con->pending.insert(std::make_pair(from_it, to_it)); + } else // or report failure to parent + parent()->pimpl()->onNewFailure(*me(), from, to); +} + + MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} void MergerPrivate::resolveInterface(InterfaceFlags expected) { @@ -863,7 +1116,10 @@ void MergerPrivate::resolveInterface(InterfaceFlags expected) { } } -Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {} +Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) { + properties().declare("time_parameterization", + std::make_shared()); +} void Merger::reset() { ParallelContainerBase::reset(); @@ -1016,7 +1272,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, moveit::core::JointModelGroup* jmg = jmg_merged_.get(); robot_trajectory::RobotTrajectoryPtr merged; try { - merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); + auto timing = me_->properties().get("time_parameterization"); + merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg, *timing); } catch (const std::runtime_error& e) { SubTrajectory t; t.markAsFailure(); diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 363bbb096..edbcf747b 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -36,11 +36,11 @@ #include #include -#include #include #include #include +#include #include @@ -107,17 +107,23 @@ double Constant::operator()(const WrappedSolution& /*s*/, std::string& /*comment return cost; } +PathLength::PathLength(std::vector joints) { + for (auto& j : joints) + this->joints.emplace(std::move(j), 1.0); +} + double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) const { const auto& traj = s.trajectory(); if (traj == nullptr || traj->getWayPointCount() == 0) return 0.0; - std::vector joint_models; - joint_models.reserve(joints.size()); + std::map weights; const auto& first_waypoint = traj->getWayPoint(0); - for (auto& joint : joints) { - joint_models.push_back(first_waypoint.getJointModel(joint)); + for (auto& joint_weight : joints) { + const moveit::core::JointModel* jm = first_waypoint.getJointModel(joint_weight.first); + if (jm) + weights.emplace(jm, joint_weight.second); } double path_length{ 0.0 }; @@ -127,14 +133,67 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) if (joints.empty()) { path_length += last.distance(curr); } else { - for (const auto& model : joint_models) { - path_length += last.distance(curr, model); + for (const auto& item : weights) { + path_length += item.second * last.distance(curr, item.first); } } } return path_length; } +DistanceToReference::DistanceToReference(const moveit_msgs::msg::RobotState& ref, Mode m, + std::map w) + : reference(ref), weights(std::move(w)), mode(m) {} + +DistanceToReference::DistanceToReference(const std::map& ref, Mode m, + std::map w) + : weights(std::move(w)), mode(m) { + reference.joint_state.name.reserve(ref.size()); + reference.joint_state.position.reserve(ref.size()); + + for (auto& item : ref) { + reference.joint_state.name.push_back(item.first); + reference.joint_state.position.push_back(item.second); + } + reference.is_diff = true; +} + +double DistanceToReference::operator()(const SubTrajectory& s, std::string& /*comment*/) const { + const auto& state = (mode == Mode::END_INTERFACE) ? s.end() : s.start(); + const auto& traj = s.trajectory(); + + moveit::core::RobotState ref_state = state->scene()->getCurrentState(); + moveit::core::robotStateMsgToRobotState(reference, ref_state, false); + + std::map w; + for (auto& item : weights) { + const moveit::core::JointModel* jm = ref_state.getJointModel(item.first); + if (jm) + w.emplace(jm, item.second); + } + + auto distance = [this, &ref_state, &w](const moveit::core::RobotState& state) { + if (weights.empty()) { + return ref_state.distance(state); + } else { + double accumulated = 0.0; + for (const auto& item : w) + accumulated += item.second * ref_state.distance(state, item.first); + return accumulated; + } + }; + + if (mode == Mode::START_INTERFACE || mode == Mode::END_INTERFACE || (mode == Mode::AUTO && (traj == nullptr))) { + return distance(state->scene()->getCurrentState()); + } else { + double accumulated = 0.0; + for (size_t i = 0; i < traj->getWayPointCount(); ++i) + accumulated += distance(traj->getWayPoint(i)); + accumulated /= traj->getWayPointCount(); + return accumulated; + } +} + double TrajectoryDuration::operator()(const SubTrajectory& s, std::string& /*comment*/) const { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } @@ -185,8 +244,8 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const auto& state_properties{ state->properties() }; auto& stage_properties{ s.creator()->properties() }; request.group_name = state_properties.hasProperty(group_property) ? - state_properties.get(group_property) : - stage_properties.get(group_property); + state_properties.get(group_property) : + stage_properties.get(group_property); // look at all forbidden collisions involving group_name request.enableGroup(state->scene()->getRobotModel()); diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 8692a5747..589bf57ad 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -48,17 +48,21 @@ #include #include +#include namespace moveit { namespace task_constructor { namespace { std::string getTaskId(const TaskPrivate* task) { + static const std::string ALLOWED = "_/"; std::ostringstream oss; char our_hostname[256] = { 0 }; gethostname(our_hostname, sizeof(our_hostname) - 1); - // Hostname could have `-` as a character but this is an invalid character in ROS so we replace it with `_` - std::replace(std::begin(our_hostname), std::end(our_hostname), '-', '_'); + // Replace all invalid ROS-name chars with an underscore + std::replace_if( + our_hostname, our_hostname + strlen(our_hostname), + [](const char ch) { return !rcutils_isalnum_no_locale(ch) && ALLOWED.find(ch) == std::string::npos; }, '_'); oss << our_hostname << "_" << getpid() << "_" << reinterpret_cast(task); return oss.str(); } @@ -173,7 +177,7 @@ void Introspection::publishTaskDescription() { void Introspection::publishTaskState() { ::moveit_task_constructor_msgs::msg::TaskStatistics msg; - impl->task_statistics_publisher_->publish(fillTaskStatistics(msg)); + impl->task_statistics_publisher_->publish(fillTaskStatistics(msg)); // NOLINT(clang-analyzer-cplusplus.Move) } void Introspection::reset() { @@ -186,9 +190,7 @@ void Introspection::registerSolution(const SolutionBase& s) { } void Introspection::fillSolution(moveit_task_constructor_msgs::msg::Solution& msg, const SolutionBase& s) { - s.fillMessage(msg, this); - s.start()->scene()->getPlanningSceneMsg(msg.start_scene); - + s.toMsg(msg, this); msg.task_id = impl->task_id_; } @@ -218,8 +220,8 @@ const SolutionBase* Introspection::solutionFromId(uint id) const { return it->second; } -bool Introspection::getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr req, - const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr res) { +bool Introspection::getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr& req, + const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr& res) { const SolutionBase* solution = solutionFromId(req->solution_id); if (!solution) return false; diff --git a/core/src/marker_tools.cpp b/core/src/marker_tools.cpp index 865122f57..0ec7ea4ab 100644 --- a/core/src/marker_tools.cpp +++ b/core/src/marker_tools.cpp @@ -123,6 +123,8 @@ void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCa auto element_handler = [&](const T& element) { if (element && element->geometry) { createGeometryMarker(m, *element->geometry, element->origin, materialColor(*model, materialName(*element))); + if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0) + return; // skip zero-size marker m.pose = rviz_marker_tools::composePoses(robot_state.getGlobalLinkTransform(name), m.pose); callback(m, name); valid_found = true; diff --git a/core/src/merge.cpp b/core/src/merge.cpp index f43a00aac..e02acb56f 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -36,7 +36,6 @@ /* Authors: Luca Lach, Robert Haschke */ #include -#include #include #include @@ -106,7 +105,8 @@ moveit::core::JointModelGroup* merge(const std::vector& sub_trajectories, - const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) { + const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group, + const trajectory_processing::TimeParameterization& time_parameterization) { if (sub_trajectories.size() <= 1) throw std::runtime_error("Expected multiple sub solutions"); @@ -166,8 +166,7 @@ merge(const std::vector& sub_trajecto } // add timing - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*merged_traj, 1.0, 1.0); + time_parameterization.computeTimeStamps(*merged_traj, 1.0, 1.0); return merged_traj; } } // namespace task_constructor diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 12b2bca8c..4eea5278b 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -40,6 +40,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -73,7 +74,9 @@ class PropertyTypeRegistry const Entry& entry(const std::type_index& type_index) const { auto it = types_.find(type_index); if (it == types_.end()) { - RCLCPP_ERROR_STREAM(LOGGER, "Unregistered type: " << type_index.name()); + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + RCLCPP_WARN_STREAM_THROTTLE(LOGGER, steady_clock, 10'000, + "Unregistered property type: " << boost::core::demangle(type_index.name())); return dummy_; } return it->second; diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 059e83487..07eadef7f 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -37,12 +37,13 @@ */ #include -#include - #include -#include +#include +#include #include +using namespace trajectory_processing; + namespace moveit { namespace task_constructor { namespace solvers { @@ -54,6 +55,10 @@ CartesianPath::CartesianPath() { p.declare("step_size", 0.01, "step size between consecutive waypoints"); p.declare("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"); p.declare("min_fraction", 1.0, "fraction of motion required for success"); + p.declare("kinematics_options", kinematics::KinematicsQueryOptions(), + "KinematicsQueryOptions to pass to CartesianInterpolator"); + p.declare("kinematics_cost_fn", kinematics::KinematicsBase::IKCostFn(), + "Cost function to pass to IK solver"); } void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {} @@ -69,11 +74,13 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, } // reach pose of forward kinematics - return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result, path_constraints); + return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg, + std::min(timeout, properties().get("timeout")), result, path_constraints); } bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { const auto& props = properties(); @@ -94,16 +101,18 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true, moveit::core::MaxEEFStep(props.get("step_size")), - moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid); + moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid, + props.get("kinematics_options"), + props.get("kinematics_cost_fn"), offset); assert(!trajectory.empty()); // there should be at least the start state result = std::make_shared(sandbox_scene->getRobotModel(), jmg); for (const auto& waypoint : trajectory) result->addSuffixWayPoint(waypoint, 0.0); - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + auto timing = props.get("time_parameterization"); + timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); return achieved_fraction >= props.get("min_fraction"); } diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 0729b6f34..f7b717c77 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include @@ -48,9 +48,13 @@ namespace solvers { static const auto LOGGER = rclcpp::get_logger("JointInterpolationPlanner"); +using namespace trajectory_processing; + JointInterpolationPlanner::JointInterpolationPlanner() { auto& p = properties(); p.declare("max_step", 0.1, "max joint step"); + // allow passing max_effort to GripperCommand actions via + p.declare("max_effort", "max_effort for GripperCommand actions"); } void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {} @@ -91,20 +95,34 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr if (from->isStateColliding(to_state, jmg->getName())) return false; - // add timing, TODO: use a generic method to add timing via plugins - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + auto timing = props.get("time_parameterization"); + timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); + + // set max_effort on first and last waypoint (first, because we might reverse the trajectory) + const auto& max_effort = properties().get("max_effort"); + if (!max_effort.empty()) { + double effort = boost::any_cast(max_effort); + for (const auto* jm : jmg->getActiveJointModels()) { + if (jm->getVariableCount() != 1) + continue; + result->getFirstWayPointPtr()->dropAccelerations(); + result->getFirstWayPointPtr()->setJointEfforts(jm, &effort); + result->getLastWayPointPtr()->dropAccelerations(); + result->getLastWayPointPtr()->setJointEfforts(jm, &effort); + } + } return true; } bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, + const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { - const auto start_time = std::chrono::steady_clock::now(); + timeout = std::min(timeout, properties().get("timeout")); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration>(timeout); auto to{ from->diff() }; @@ -118,7 +136,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr return to->isStateValid(*robot_state, constraints, jmg->getName()); } }; - if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) { + if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) { // TODO(v4hn): planners need a way to add feedback to failing plans // in case of an invalid solution feedback should include unwanted collisions or violated constraints RCLCPP_WARN(LOGGER, "IK failed for pose target"); @@ -126,8 +144,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr } to->getCurrentStateNonConst().update(); - timeout = std::chrono::duration(std::chrono::steady_clock::now() - start_time).count(); - if (timeout <= 0.0) + if (std::chrono::steady_clock::now() >= deadline) return false; return plan(from, to, jmg, timeout, result, path_constraints); diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/multi_planner.cpp new file mode 100644 index 000000000..cfb90ab5b --- /dev/null +++ b/core/src/solvers/multi_planner.cpp @@ -0,0 +1,98 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: generate and validate a straight-line Cartesian path +*/ + +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace solvers { + +void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) { + for (const auto& p : *this) + p->init(robot_model); +} + +bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { + double remaining_time = std::min(timeout, properties().get("timeout")); + auto start_time = std::chrono::steady_clock::now(); + + for (const auto& p : *this) { + if (remaining_time < 0) + return false; // timeout + if (result) + result->clear(); + if (p->plan(from, to, jmg, remaining_time, result, path_constraints)) + return true; + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; +} + +bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { + double remaining_time = std::min(timeout, properties().get("timeout")); + auto start_time = std::chrono::steady_clock::now(); + + for (const auto& p : *this) { + if (remaining_time < 0) + return false; // timeout + if (result) + result->clear(); + if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints)) + return true; + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; +} +} // namespace solvers +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 4575a6f00..d658951b6 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -53,158 +53,182 @@ namespace moveit { namespace task_constructor { namespace solvers { -struct PlannerCache -{ - using PlannerID = std::tuple; - using PlannerMap = std::map >; - using ModelList = std::list, PlannerMap> >; - ModelList cache_; - - PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) { - // find model in cache_ and remove expired entries while doing so - ModelList::iterator model_it = cache_.begin(); - while (model_it != cache_.end()) { - if (model_it->first.expired()) { - model_it = cache_.erase(model_it); - continue; - } - if (model_it->first.lock() == model) - break; - ++model_it; - } - if (model_it == cache_.end()) // if not found, create a new PlannerMap for this model - model_it = cache_.insert(cache_.begin(), std::make_pair(model, PlannerMap())); - - return model_it->second.insert(std::make_pair(id, PlannerMap::mapped_type())).first->second; - } -}; - -planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Node::SharedPtr& node, - const PipelinePlanner::Specification& spec) { - static PlannerCache cache; - - static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin"; - - std::string pipeline_ns = spec.ns; - const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME; - // fallback to old structure for pipeline parameters in MoveIt - if (!node->has_parameter(parameter_name)) { - node->declare_parameter(parameter_name, rclcpp::ParameterType::PARAMETER_STRING); - } - if (std::string parameter; !node->get_parameter(parameter_name, parameter)) { - RCLCPP_WARN(node->get_logger(), "Failed to find '%s.%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME, - "Attempting to load pipeline from old parameter structure. Please update your MoveIt config."); - pipeline_ns = "move_group"; +PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name, + const std::string& planner_id) + : PipelinePlanner(node, [&]() { + std::unordered_map pipeline_id_planner_id_map; + pipeline_id_planner_id_map[pipeline_name] = planner_id; + return pipeline_id_planner_id_map; + }()) {} + +PipelinePlanner::PipelinePlanner( + const rclcpp::Node::SharedPtr& node, const std::unordered_map& pipeline_id_planner_id_map, + const std::unordered_map& planning_pipelines, + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback, + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) + : node_(node) + , last_successful_planner_("") + , pipeline_id_planner_id_map_(pipeline_id_planner_id_map) + , stopping_criterion_callback_(stopping_criterion_callback) + , solution_selection_function_(solution_selection_function) { + // If the pipeline name - pipeline map is passed as constructor argument, use it. Otherwise, it will be created in + // the init(..) function + if (!planning_pipelines.empty()) { + planning_pipelines_ = planning_pipelines; } + // Declare properties of the MotionPlanRequest + properties().declare("num_planning_attempts", 1u, "number of planning attempts"); + properties().declare( + "workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?"); + + properties().declare("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals"); + properties().declare("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); + properties().declare("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals"); + // Declare properties that configure the planning pipeline + properties().declare>( + "pipeline_id_planner_id_map", std::unordered_map(), + "Set of pipelines and planners used for planning"); +} - PlannerCache::PlannerID id(pipeline_ns, spec.adapter_param); - - std::weak_ptr& entry = cache.retrieve(spec.model, id); - planning_pipeline::PlanningPipelinePtr planner = entry.lock(); - if (!planner) { - // create new entry - planner = std::make_shared(spec.model, node, pipeline_ns, - PLUGIN_PARAMETER_NAME, spec.adapter_param); - // store in cache - entry = planner; +bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std::string& planner_id) { + // Only set ID if pipeline exists. It is not possible to create new pipelines with this command. + if (pipeline_id_planner_id_map_.count(pipeline_name) == 0) { + RCLCPP_ERROR(node_->get_logger(), + "PipelinePlanner does not have a pipeline called '%s'. Cannot set pipeline ID '%s'", + pipeline_name.c_str(), planner_id.c_str()); + return false; } - return planner; + pipeline_id_planner_id_map_[pipeline_name] = planner_id; + return true; } -PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name) - : pipeline_name_{ pipeline_name }, node_(node) { - auto& p = properties(); - p.declare("planner", "", "planner id"); - - p.declare("num_planning_attempts", 1u, "number of planning attempts"); - p.declare("workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), - "allowed workspace of mobile base?"); - - p.declare("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals"); - p.declare("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); - p.declare("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals"); - - p.declare("display_motion_plans", false, - "publish generated solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC); - p.declare("publish_planning_requests", false, - "publish motion planning requests on topic " + - planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC); +void PipelinePlanner::setStoppingCriterionFunction( + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_function) { + stopping_criterion_callback_ = stopping_criterion_function; } - -PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline) - : PipelinePlanner(rclcpp::Node::SharedPtr()) { - planner_ = planning_pipeline; +void PipelinePlanner::setSolutionSelectionFunction( + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) { + solution_selection_function_ = solution_selection_function; } void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { - if (!planner_) { - Specification spec; - spec.model = robot_model; - spec.pipeline = pipeline_name_; - planner_ = create(node_, spec); - } else if (robot_model != planner_->getRobotModel()) { - throw std::runtime_error( - "The robot model of the planning pipeline isn't the same as the task's robot model -- " - "use Task::setRobotModel for setting the robot model when using custom planning pipeline"); + // If no planning pipelines exist, create them based on the pipeline names provided in pipeline_id_planner_id_map_. + // The assumption here is that all parameters required by the planning pipeline can be found in a namespace that + // equals the pipeline name. + if (planning_pipelines_.empty()) { + planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap( + [&]() { + // Create pipeline name vector from the keys of pipeline_id_planner_id_map_ + if (pipeline_id_planner_id_map_.empty()) { + throw std::runtime_error("Cannot initialize PipelinePlanner: No planning pipeline was provided and " + "pipeline_id_planner_id_map_ is empty!"); + } + + std::vector pipeline_names; + for (const auto& pipeline_name_planner_id_pair : pipeline_id_planner_id_map_) { + pipeline_names.push_back(pipeline_name_planner_id_pair.first); + } + return pipeline_names; + }(), + robot_model, node_); } - planner_->displayComputedMotionPlans(properties().get("display_motion_plans")); - planner_->publishReceivedRequests(properties().get("publish_planning_requests")); -} -void initMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& req, const PropertyMap& p, - const moveit::core::JointModelGroup* jmg, double timeout) { - req.group_name = jmg->getName(); - req.planner_id = p.get("planner"); - req.allowed_planning_time = timeout; - req.start_state.is_diff = true; // we don't specify an extra start state - - req.num_planning_attempts = p.get("num_planning_attempts"); - req.max_velocity_scaling_factor = p.get("max_velocity_scaling_factor"); - req.max_acceleration_scaling_factor = p.get("max_acceleration_scaling_factor"); - req.workspace_parameters = p.get("workspace_parameters"); + // Check if it is still empty + if (planning_pipelines_.empty()) { + throw std::runtime_error( + "Cannot initialize PipelinePlanner: Could not create any valid entries for planning pipeline maps!"); + } } bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* joint_model_group, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { - const auto& props = properties(); - moveit_msgs::msg::MotionPlanRequest req; - initMotionPlanRequest(req, props, jmg, timeout); - - req.goal_constraints.resize(1); - req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(to->getCurrentState(), jmg, - props.get("goal_joint_tolerance")); - req.path_constraints = path_constraints; - - ::planning_interface::MotionPlanResponse res; - bool success = planner_->generatePlan(from, req, res); - result = res.trajectory_; - return success; + // Construct goal constraints from the goal planning scene + const auto goal_constraints = kinematic_constraints::constructGoalConstraints( + to->getCurrentState(), joint_model_group, properties().get("goal_joint_tolerance")); + return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints); } bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen, + const moveit::core::JointModelGroup* joint_model_group, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { - const auto& props = properties(); - moveit_msgs::msg::MotionPlanRequest req; - initMotionPlanRequest(req, props, jmg, timeout); + last_successful_planner_.clear(); + // Construct a Cartesian target pose from the given target transform and offset geometry_msgs::msg::PoseStamped target; target.header.frame_id = from->getPlanningFrame(); - target.pose = tf2::toMsg(target_eigen); - - req.goal_constraints.resize(1); - req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( - link.getName(), target, props.get("goal_position_tolerance"), - props.get("goal_orientation_tolerance")); - req.path_constraints = path_constraints; - - ::planning_interface::MotionPlanResponse res; - bool success = planner_->generatePlan(from, req, res); - result = res.trajectory_; - return success; + target.pose = tf2::toMsg(target_eigen * offset.inverse()); + + const auto goal_constraints = kinematic_constraints::constructGoalConstraints( + link.getName(), target, properties().get("goal_position_tolerance"), + properties().get("goal_orientation_tolerance")); + + return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints); +} + +bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, + const moveit::core::JointModelGroup* joint_model_group, + const moveit_msgs::msg::Constraints& goal_constraints, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { + // Create a request for every planning pipeline that should run in parallel + std::vector requests; + requests.reserve(pipeline_id_planner_id_map_.size()); + + auto const property_pipeline_id_planner_id_map = + properties().get>("pipeline_id_planner_id_map"); + for (auto const& pipeline_id_planner_id_pair : + (!property_pipeline_id_planner_id_map.empty() ? property_pipeline_id_planner_id_map : + pipeline_id_planner_id_map_)) { + // Check that requested pipeline exists and skip it if it doesn't exist + if (planning_pipelines_.find(pipeline_id_planner_id_pair.first) == planning_pipelines_.end()) { + RCLCPP_WARN( + node_->get_logger(), + "Pipeline '%s' is not available of this PipelineSolver instance. Skipping a request for this pipeline.", + pipeline_id_planner_id_pair.first.c_str()); + continue; + } + // Create MotionPlanRequest for pipeline + moveit_msgs::msg::MotionPlanRequest request; + request.pipeline_id = pipeline_id_planner_id_pair.first; + request.group_name = joint_model_group->getName(); + request.planner_id = pipeline_id_planner_id_pair.second; + request.allowed_planning_time = timeout; + request.start_state.is_diff = true; // we don't specify an extra start state + request.num_planning_attempts = properties().get("num_planning_attempts"); + request.max_velocity_scaling_factor = properties().get("max_velocity_scaling_factor"); + request.max_acceleration_scaling_factor = properties().get("max_acceleration_scaling_factor"); + request.workspace_parameters = properties().get("workspace_parameters"); + request.goal_constraints.resize(1); + request.goal_constraints.at(0) = goal_constraints; + request.path_constraints = path_constraints; + requests.push_back(request); + } + + // Run planning pipelines in parallel to create a vector of responses. If a solution selection function is provided, + // planWithParallelPipelines will return a vector with the single best solution + std::vector<::planning_interface::MotionPlanResponse> responses = + moveit::planning_pipeline_interfaces::planWithParallelPipelines( + requests, planning_scene, planning_pipelines_, stopping_criterion_callback_, solution_selection_function_); + + // If solutions exist and the first one is successful + if (!responses.empty()) { + auto const solution = responses.at(0); + if (solution) { + // Choose the first solution trajectory as response + result = solution.trajectory; + last_successful_planner_ = solution.planner_id; + return bool(result); + } + } + return false; +} +std::string PipelinePlanner::getPlannerId() const { + return last_successful_planner_.empty() ? std::string("Unknown") : last_successful_planner_; } } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index 23e314733..a58b110da 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -37,6 +37,9 @@ */ #include +#include + +using namespace trajectory_processing; namespace moveit { namespace task_constructor { @@ -44,8 +47,10 @@ namespace solvers { PlannerInterface::PlannerInterface() { auto& p = properties(); + p.declare("timeout", std::numeric_limits::infinity(), "timeout for planner (s)"); p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); + p.declare("time_parameterization", std::make_shared()); } } // namespace solvers } // namespace task_constructor diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 2bf12c855..a79e1d50f 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -38,7 +38,6 @@ #include #include #include -#include #include @@ -106,6 +105,33 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name) , parent_{ nullptr } , introspection_{ nullptr } {} +StagePrivate& StagePrivate::operator=(StagePrivate&& other) { + assert(typeid(*this) == typeid(other)); + + assert(states_.empty() && other.states_.empty()); + assert((!starts_ || starts_->empty()) && (!other.starts_ || other.starts_->empty())); + assert((!ends_ || ends_->empty()) && (!other.ends_ || other.ends_->empty())); + assert(solutions_.empty() && other.solutions_.empty()); + assert(failures_.empty() && other.failures_.empty()); + + // me_ must not be changed! + name_ = std::move(other.name_); + properties_ = std::move(other.properties_); + cost_term_ = std::move(other.cost_term_); + solution_cbs_ = std::move(other.solution_cbs_); + + starts_ = std::move(other.starts_); + ends_ = std::move(other.ends_); + prev_ends_ = std::move(other.prev_ends_); + next_starts_ = std::move(other.next_starts_); + + parent_ = other.parent_; + it_ = other.it_; + other.unparent(); + + return *this; +} + InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags f; if (starts()) @@ -318,6 +344,7 @@ void Stage::reset() { impl->next_starts_.reset(); // reset inherited properties impl->properties_.reset(); + impl->total_compute_time_ = std::chrono::duration::zero(); } void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { @@ -403,6 +430,11 @@ bool Stage::storeFailures() const { return pimpl()->storeFailures(); } +void Stage::explainFailure(std::ostream& os) const { + if (!failures().empty()) + os << ": " << failures().front()->comment(); +} + PropertyMap& Stage::properties() { return pimpl()->properties_; } @@ -485,14 +517,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction case PropagatingEitherWay::FORWARD: required_interface_ = PROPAGATE_FORWARDS; if (!starts_) // keep existing interface if possible - starts_.reset(new Interface()); + starts_ = std::make_shared(); ends_.reset(); return; case PropagatingEitherWay::BACKWARD: required_interface_ = PROPAGATE_BACKWARDS; starts_.reset(); if (!ends_) // keep existing interface if possible - ends_.reset(new Interface()); + ends_ = std::make_shared(); return; case PropagatingEitherWay::AUTO: required_interface_ = UNKNOWN; @@ -591,6 +623,14 @@ template void PropagatingEitherWay::send(const InterfaceStat template void PropagatingEitherWay::send(const InterfaceState& start, InterfaceState&& end, SubTrajectory&& trajectory); +void PropagatingEitherWay::computeForward(const InterfaceState& from) { + computeGeneric(from); +} + +void PropagatingEitherWay::computeBackward(const InterfaceState& to) { + computeGeneric(to); +} + template void PropagatingEitherWay::computeGeneric(const InterfaceState& start) { planning_scene::PlanningScenePtr end; @@ -690,10 +730,10 @@ void MonitoringGeneratorPrivate::solutionCB(const SolutionBase& s) { } ConnectingPrivate::ConnectingPrivate(Connecting* me, const std::string& name) : ComputeBasePrivate(me, name) { - starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, - std::placeholders::_2))); - ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, - std::placeholders::_2))); + starts_ = std::make_shared(std::bind(&ConnectingPrivate::newState, this, + std::placeholders::_1, std::placeholders::_2)); + ends_ = std::make_shared( + std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, std::placeholders::_2)); } InterfaceFlags ConnectingPrivate::requiredInterface() const { @@ -711,56 +751,108 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In return StatePair(second, first); } -template -void ConnectingPrivate::newState(Interface::iterator it, bool updated) { - if (updated) { // many pairs might be affected: resort - if (it->priority().pruned()) - // remove all pending pairs involving this state - pending.remove_if([it](const StatePair& p) { return std::get()>(p) == it; }); - else - // TODO(v4hn): If a state becomes reenabled, this skips all previously removed pairs, right? - pending.sort(); +template +void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) { + auto parent_pimpl = parent()->pimpl(); + // disable current interface to break loop (jumping back and forth between both interfaces) + // this will be checked by notifyEnabled() below + Interface::DisableNotify disable_source_interface(*pullInterface()); + if (updated) { + if (updated.testFlag(Interface::STATUS) && // only perform these costly operations if needed + pullInterface()>()->notifyEnabled()) // suppressing recursive loop? + { + // If status has changed, propagate the update to the opposite side + auto status = it->priority().status(); + if (status == InterfaceState::Status::PRUNED) // PRUNED becomes ARMED on opposite side + status = InterfaceState::Status::ARMED; // (only for pending state pairs) + + for (const auto& candidate : this->pending) { + if (std::get()>(candidate) != it) // only consider pairs with source state == state + continue; + auto oit = std::get(candidate); // opposite target state + auto ostatus = oit->priority().status(); + if (ostatus != status) { + if (status != InterfaceState::Status::ENABLED) { + // quicker check for hasPendingOpposites(): search in it->owner() for an enabled alternative + bool cancel = false; // if found, cancel propagation of new status + for (const auto alternative : *it->owner()) + if ((cancel = alternative->priority().enabled())) + break; + if (cancel) + continue; + } + // pass creator=nullptr to skip hasPendingOpposites() check + parent_pimpl->setStatus()>(nullptr, nullptr, &*oit, status); + } + } + } + + // many pairs will have changed priorities: resort pending list + pending.sort(); } else { // new state: insert all pairs with other interface assert(it->priority().enabled()); // new solutions are feasible, aren't they? - InterfacePtr other_interface = pullInterface(other); + InterfacePtr other_interface = pullInterface(); + bool have_enabled_opposites = false; for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { - // Don't re-enable states that are marked DISABLED - if (static_cast(me_)->compatible(*it, *oit)) { - // re-enable the opposing state oit if its status is FAILED - if (oit->priority().status() == InterfaceState::Status::FAILED) - oit->owner()->updatePriority(&*oit, - InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED)); - pending.insert(make_pair(it, oit)); - } + if (!static_cast(me_)->compatible(*it, *oit)) + continue; + + // re-enable the opposing state oit (and its associated solution branch) if its status is ARMED + // https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + if (oit->priority().status() == InterfaceState::Status::ARMED) + parent_pimpl->setStatus()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED); + if (oit->priority().enabled()) + have_enabled_opposites = true; + + // Remember all pending states, regardless of their status! + pending.insert(make_pair(it, oit)); } + if (!have_enabled_opposites) // prune new state and associated branch if necessary + // pass creator=nullptr to skip hasPendingOpposites() check as we did this here already + parent_pimpl->setStatus(nullptr, nullptr, &*it, InterfaceState::Status::ARMED); } - // std::cerr << name_ << ": "; - // printPendingPairs(std::cerr); - // std::cerr << std::endl; +#if 0 + auto& os = std::cerr; + for (auto d : { Interface::FORWARD, Interface::BACKWARD }) { + if (d == Interface::FORWARD) + os << " " << std::setw(10) << std::left << this->name(); + else + os << std::setw(12) << std::right << ""; + if (dir != d) + os << (updated ? " !" : " +"); + else + os << " "; + os << d << " " << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl; + } + os << std::setw(15) << " "; + printPendingPairs(os) << std::endl; +#endif } -// Check whether there are pending feasible states that could connect to source. -// If not, we exhausted all solution candidates for source and thus should mark it as failure. +// Check whether there are pending feasible states (other than source) that could connect to target. +// If not, we exhausted all solution candidates for target and thus should mark it as failure. template -inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const { +inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const { for (const auto& candidate : this->pending) { - static_assert(Interface::FORWARD == 0, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); - const auto src = std::get(candidate); - static_assert(Interface::BACKWARD == 1, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); - const auto tgt = std::get()>(candidate); + static_assert(Interface::FORWARD == 0 && Interface::BACKWARD == 1, + "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); + const InterfaceState* src = &*std::get(candidate); + const InterfaceState* tgt = &*std::get()>(candidate); - if (&*src == source && tgt->priority().enabled()) + if (tgt == target && src != source && src->priority().enabled()) return true; // early stopping when only infeasible pairs are to come - if (!std::get<0>(candidate)->priority().enabled()) + if (!std::get<0>(candidate)->priority().enabled() || !std::get<1>(candidate)->priority().enabled()) break; } return false; } // explicitly instantiate templates for both directions -template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const; -template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const; +template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* start, + const InterfaceState* end) const; +template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* end, + const InterfaceState* start) const; bool ConnectingPrivate::canCompute() const { // Do we still have feasible pending state pairs? @@ -777,24 +869,15 @@ void ConnectingPrivate::compute() { } std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const { - static const char* red = "\033[31m"; - static const char* reset = "\033[m"; + const char* reset = InterfaceState::colorForStatus(3); for (const auto& candidate : pending) { - if (!candidate.first->priority().enabled() || !candidate.second->priority().enabled()) - os << " " << red; - // find indeces of InterfaceState pointers in start/end Interfaces - unsigned int first = 0, second = 0; - std::find_if(starts()->begin(), starts()->end(), [&](const InterfaceState* s) { - ++first; - return &*candidate.first == s; - }); - std::find_if(ends()->begin(), ends()->end(), [&](const InterfaceState* s) { - ++second; - return &*candidate.second == s; - }); - os << first << ":" << second << " "; + size_t first = getIndex(*starts(), candidate.first); + size_t second = getIndex(*ends(), candidate.second); + os << InterfaceState::colorForStatus(candidate.first->priority().status()) << first << reset << ":" + << InterfaceState::colorForStatus(candidate.second->priority().status()) << second << reset << " "; } - os << reset; + if (pending.empty()) + os << "---"; return os; } static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connecting"); @@ -826,12 +909,10 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta return false; } -#if MOVEIT_HAS_OBJECT_POSE if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) { RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object pose: " << from_object_name); return false; // transforms do not match } -#endif if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) { RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object shapes: " << from_object_name); @@ -878,15 +959,9 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta return false; // shapes not matching } -#if MOVEIT_HAS_OBJECT_POSE auto from_it = from_object->getShapePosesInLinkFrame().cbegin(); auto from_end = from_object->getShapePosesInLinkFrame().cend(); auto to_it = to_object->getShapePosesInLinkFrame().cbegin(); -#else - auto from_it = from_object->getFixedTransforms().cbegin(); - auto from_end = from_object->getFixedTransforms().cend(); - auto to_it = to_object->getFixedTransforms().cbegin(); -#endif for (; from_it != from_end; ++from_it, ++to_it) if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { RCLCPP_DEBUG_STREAM(LOGGER, name() diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 050e94014..8da8786fd 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -41,18 +41,18 @@ add_library(${PROJECT_NAME}_stages SHARED ) target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME}) ament_target_dependencies(${PROJECT_NAME}_stages - moveit_core - geometry_msgs - moveit_ros_planning - moveit_ros_planning_interface - moveit_task_constructor_msgs - rclcpp - tf2_eigen - visualization_msgs + moveit_core + geometry_msgs + moveit_ros_planning + moveit_ros_planning_interface + moveit_task_constructor_msgs + rclcpp + tf2_eigen + visualization_msgs ) add_library(${PROJECT_NAME}_stage_plugins SHARED - plugins.cpp + plugins.cpp ) target_link_libraries(${PROJECT_NAME}_stage_plugins ${PROJECT_NAME}_stages) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index b16990009..463603122 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include @@ -90,7 +89,15 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& } // found IK solutions -using IKSolutions = std::vector>; + +struct IKSolution +{ + std::vector joint_positions; + bool feasible; + collision_detection::Contact contact; +}; + +using IKSolutions = std::vector; namespace { @@ -98,7 +105,7 @@ namespace { // TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene, moveit::core::RobotState& robot_state, Eigen::Isometry3d pose, - const moveit::core::LinkModel* link, + const moveit::core::LinkModel* link, const moveit::core::JointModelGroup* jmg = nullptr, collision_detection::CollisionResult* collision_result = nullptr) { // consider all rigidly connected parent links as well const moveit::core::LinkModel* parent = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link); @@ -129,6 +136,8 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce collision_detection::CollisionRequest req; collision_detection::CollisionResult result; req.contacts = (collision_result != nullptr); + if (jmg) + req.group_name = jmg->getName(); collision_detection::CollisionResult& res = collision_result ? *collision_result : result; scene->checkCollision(req, res, robot_state, acm); return res.collision; @@ -282,7 +291,7 @@ void ComputeIK::compute() { if (value.empty()) { // property undefined // determine IK link from eef/group if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) : - jmg->getOnlyOneEndEffectorTip())) { + jmg->getOnlyOneEndEffectorTip())) { RCLCPP_WARN_STREAM(LOGGER, "Failed to derive IK target link"); return; } @@ -299,7 +308,7 @@ void ComputeIK::compute() { } ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose; - link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id); + link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id); // transform target pose such that ik frame will reach there if link does target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName()); @@ -309,26 +318,28 @@ void ComputeIK::compute() { collision_detection::CollisionResult collisions; moveit::core::RobotState sandbox_state{ scene->getCurrentState() }; bool colliding = - !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions); + !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions); - // markers used for failures - std::deque failure_markers; // frames at target pose and ik frame - rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame"); - rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame"); + std::deque frame_markers; + rviz_marker_tools::appendFrame(frame_markers, target_pose_msg, 0.1, "target frame"); + rviz_marker_tools::appendFrame(frame_markers, ik_pose_msg, 0.1, "ik frame"); + // end-effector markers + std::deque eef_markers; // visualize placed end-effector - auto appender = [&failure_markers](visualization_msgs::msg::Marker& marker, const std::string& /*name*/) { + auto appender = [&eef_markers](visualization_msgs::msg::Marker& marker, const std::string& /*name*/) { marker.ns = "ik target"; marker.color.a *= 0.5; - failure_markers.push_back(marker); + eef_markers.push_back(marker); }; const auto& links_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link) ->getParentJointModel() ->getDescendantLinkModels(); if (colliding) { SubTrajectory solution; + std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); generateCollisionMarkers(sandbox_state, appender, links_to_visualize); - std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers())); + std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers())); solution.markAsFailure(); // TODO: visualize collisions solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", ")); @@ -356,14 +367,24 @@ void ComputeIK::compute() { &ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* joint_positions) { for (const auto& sol : ik_solutions) { - if (jmg->distance(joint_positions, sol.data()) < min_solution_distance) + if (jmg->distance(joint_positions, sol.joint_positions.data()) < min_solution_distance) return false; // too close to already found solution } state->setJointGroupPositions(jmg, joint_positions); ik_solutions.emplace_back(); - state->copyJointGroupPositions(jmg, ik_solutions.back()); - - return ignore_collisions || !scene->isStateColliding(*state, jmg->getName()); + auto& solution{ ik_solutions.back() }; + state->copyJointGroupPositions(jmg, solution.joint_positions); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.contacts = true; + req.max_contacts = 1; + req.group_name = jmg->getName(); + scene->checkCollision(req, res, *state); + solution.feasible = ignore_collisions || !res.collision; + if (!res.contacts.empty()) { + solution.contact = res.contacts.begin()->second.front(); + } + return solution.feasible; }; uint32_t max_ik_solutions = props.get("max_ik_solutions"); @@ -372,8 +393,10 @@ void ComputeIK::compute() { double remaining_time = timeout(); auto start_time = std::chrono::steady_clock::now(); while (ik_solutions.size() < max_ik_solutions && remaining_time > 0) { - if (tried_current_state_as_seed) + if (tried_current_state_as_seed) { sandbox_state.setToRandomPositions(jmg); + sandbox_state.update(); + } tried_current_state_as_seed = true; size_t previous = ik_solutions.size(); @@ -389,24 +412,28 @@ void ComputeIK::compute() { planning_scene::PlanningScenePtr solution_scene = scene->diff(); SubTrajectory solution; solution.setComment(s.comment()); + std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); - // frames at target pose and ik frame - rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame"); - rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame"); - - if (succeeded && i + 1 == ik_solutions.size()) + if (ik_solutions[i].feasible) // compute cost as distance to compare_pose - solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); - else // found an IK solution, but this was not valid - solution.markAsFailure(); - + solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data())); + else { // found an IK solution, but this was not valid + std::stringstream ss; + ss << "Collision between '" << ik_solutions[i].contact.body_name_1 << "' and '" + << ik_solutions[i].contact.body_name_2 << "'"; + solution.markAsFailure(ss.str()); + } // set scene's robot state moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); - solution_state.setJointGroupPositions(jmg, ik_solutions[i].data()); + solution_state.setJointGroupPositions(jmg, ik_solutions[i].joint_positions.data()); solution_state.update(); InterfaceState state(solution_scene); forwardProperties(*s.start(), state); + + // ik target link placement + std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers())); + spawn(std::move(state), std::move(solution)); } @@ -423,9 +450,17 @@ void ComputeIK::compute() { solution.markAsFailure(); solution.setComment(s.comment() + " no IK found"); + std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); // ik target link placement - std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers())); + std_msgs::msg::ColorRGBA tint_color; + tint_color.r = 1.0; + tint_color.g = 0.0; + tint_color.b = 0.0; + tint_color.a = 0.5; + for (auto& marker : eef_markers) + marker.color = tint_color; + std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers())); spawn(InterfaceState(scene), std::move(solution)); } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 0fac2feb2..d4591f5c6 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -38,9 +38,12 @@ #include #include +#include + #include +#include -#include +using namespace trajectory_processing; namespace moveit { namespace task_constructor { @@ -56,6 +59,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : p.declare("merge_mode", WAYPOINTS, "merge mode"); p.declare("path_constraints", moveit_msgs::msg::Constraints(), "constraints to maintain during trajectory"); + properties().declare("merge_time_parameterization", + std::make_shared()); } void Connect::reset() { @@ -136,7 +141,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { const auto& path_constraints = props.get("path_constraints"); const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState(); - std::vector sub_trajectories; + std::vector trajectory_planner_vector; std::vector intermediate_scenes; planning_scene::PlanningSceneConstPtr start = from.scene(); @@ -156,7 +161,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { robot_trajectory::RobotTrajectoryPtr trajectory; success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); - sub_trajectories.push_back(trajectory); // include failed trajectory + trajectory_planner_vector.push_back(PlannerIdTrajectoryPair({ pair.second->getPlannerId(), trajectory })); if (!success) break; @@ -167,20 +172,21 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { SolutionBasePtr solution; if (success && mode != SEQUENTIAL) // try to merge - solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState()); + solution = merge(trajectory_planner_vector, intermediate_scenes, from.scene()->getCurrentState()); if (!solution) // success == false or merging failed: store sequentially - solution = makeSequential(sub_trajectories, intermediate_scenes, from, to); + solution = makeSequential(trajectory_planner_vector, intermediate_scenes, from, to); if (!success) // error during sequential planning solution->markAsFailure(); + connect(from, to, solution); } SolutionSequencePtr -Connect::makeSequential(const std::vector& sub_trajectories, +Connect::makeSequential(const std::vector& trajectory_planner_vector, const std::vector& intermediate_scenes, const InterfaceState& from, const InterfaceState& to) { - assert(!sub_trajectories.empty()); - assert(sub_trajectories.size() + 1 == intermediate_scenes.size()); + assert(!trajectory_planner_vector.empty()); + assert(trajectory_planner_vector.size() + 1 == intermediate_scenes.size()); /* We need to decouple the sequence of subsolutions, created here, from the external from and to states. Hence, we create new interface states for all subsolutions. */ @@ -188,18 +194,19 @@ Connect::makeSequential(const std::vectorsetCreator(this); - if (!sub) // a null RobotTrajectoryPtr indicates a failure + if (!pair.robot_trajectory_ptr) // a null RobotTrajectoryPtr indicates a failure inserted->markAsFailure(); // push back solution pointer sub_solutions.push_back(&*inserted); // create a new end state, either from intermediate or final planning scene planning_scene::PlanningSceneConstPtr end_ps = - (sub_solutions.size() < sub_trajectories.size()) ? *++scene_it : to.scene(); + (sub_solutions.size() < trajectory_planner_vector.size()) ? *++scene_it : to.scene(); const InterfaceState* end = &*states_.insert(states_.end(), InterfaceState(end_ps)); // provide newly created start/end states @@ -212,25 +219,50 @@ Connect::makeSequential(const std::vector(std::move(sub_solutions)); } -SubTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, +SubTrajectoryPtr Connect::merge(const std::vector& trajectory_planner_vector, const std::vector& intermediate_scenes, const moveit::core::RobotState& state) { // no need to merge if there is only a single sub trajectory - if (sub_trajectories.size() == 1) - return std::make_shared(sub_trajectories[0]); + if (trajectory_planner_vector.size() == 1) + return std::make_shared(trajectory_planner_vector.at(0).robot_trajectory_ptr, 0.0, std::string(""), + trajectory_planner_vector.at(0).planner_name); auto jmg = merged_jmg_.get(); assert(jmg); - robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg); - if (!trajectory) + auto timing = properties().get("merge_time_parameterization"); + robot_trajectory::RobotTrajectoryPtr merged_trajectory = task_constructor::merge( + [&]() { + // Move trajectories into single vector + std::vector robot_traj_vector; + robot_traj_vector.reserve(trajectory_planner_vector.size()); + + // Copy second elements of robot planner vector into separate vector + std::transform(begin(trajectory_planner_vector), end(trajectory_planner_vector), + std::back_inserter(robot_traj_vector), + [](auto const& pair) { return pair.robot_trajectory_ptr; }); + return robot_traj_vector; + }(), + state, jmg, *timing); + + // check merged trajectory is empty or has collisions + if (!merged_trajectory || + !intermediate_scenes.front()->isPathValid(*merged_trajectory, + properties().get("path_constraints"))) { return SubTrajectoryPtr(); + } - // check merged trajectory for collisions - if (!intermediate_scenes.front()->isPathValid(*trajectory, - properties().get("path_constraints"))) - return SubTrajectoryPtr(); + // Copy first elements of robot planner vector into separate vector + std::vector planner_names; + planner_names.reserve(trajectory_planner_vector.size()); + std::transform(begin(trajectory_planner_vector), end(trajectory_planner_vector), std::back_inserter(planner_names), + [](auto const& pair) { return pair.planner_name; }); - return std::make_shared(trajectory); + // Create a sequence of planner names + std::string planner_name_sequence; + for (auto const& name : planner_names) { + planner_name_sequence += std::string(", " + name); + } + return std::make_shared(merged_trajectory, 0.0, std::string(""), planner_name_sequence); } } // namespace stages } // namespace task_constructor diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp index ca48fb1b6..f03b37adb 100644 --- a/core/src/stages/current_state.cpp +++ b/core/src/stages/current_state.cpp @@ -35,6 +35,8 @@ /* Authors: Michael Goerner, Luca Lach, Robert Haschke */ +#include + #include #include #include @@ -47,13 +49,16 @@ namespace moveit { namespace task_constructor { namespace stages { +using namespace std::chrono_literals; +constexpr std::chrono::duration DEFAULT_TIMEOUT = 3s; + static const rclcpp::Logger LOGGER = rclcpp::get_logger("CurrentState"); CurrentState::CurrentState(const std::string& name) : Generator(name) { auto& p = properties(); Property& timeout = p.property("timeout"); timeout.setDescription("max time to wait for get_planning_scene service"); - timeout.setValue(-1.0); + timeout.setValue(DEFAULT_TIMEOUT.count()); } void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model) { diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index bfbff1fb7..840b700be 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -38,7 +38,6 @@ #include #include -#include #include #include diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index 71ad60ad7..0c8ebaade 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -44,7 +44,8 @@ namespace task_constructor { namespace stages { FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene) - : Generator(name), scene_(scene) { + : Generator(name), scene_(std::move(scene)) { + properties().declare("ignore_collisions", false); setCostTerm(std::make_unique(0.0)); } @@ -62,7 +63,12 @@ bool FixedState::canCompute() const { } void FixedState::compute() { - spawn(InterfaceState(scene_), 0.0); + SubTrajectory trajectory; + if (!properties().get("ignore_collisions") && scene_->isStateColliding()) { + trajectory.markAsFailure("in collision"); + } + + spawn(InterfaceState(scene_), std::move(trajectory)); ran_ = true; } } // namespace stages diff --git a/core/src/stages/generate_place_pose.cpp b/core/src/stages/generate_place_pose.cpp index 56dd2bdea..42a3205ca 100644 --- a/core/src/stages/generate_place_pose.cpp +++ b/core/src/stages/generate_place_pose.cpp @@ -60,7 +60,7 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("GeneratePlacePose"); GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(name) { auto& p = properties(); p.declare("object"); - p.declare("ik_frame"); + p.declare("allow_z_flip", false, "allow placing objects upside down"); } void GeneratePlacePose::onNewSolution(const SolutionBase& s) { @@ -68,11 +68,14 @@ void GeneratePlacePose::onNewSolution(const SolutionBase& s) { const auto& props = properties(); const std::string& object = props.get("object"); + bool frame_found = false; + const moveit::core::LinkModel* link = nullptr; + scene->getCurrentState().getFrameInfo(object, link, frame_found); std::string msg; - if (!scene->getCurrentState().hasAttachedBody(object)) - msg = "'" + object + "' is not an attached object"; - if (scene->getCurrentState().getAttachedBody(object)->getShapes().empty()) - msg = "'" + object + "' has no associated shapes"; + if (!frame_found) + msg = "frame '" + object + "' is not known"; + if (!link) + msg = "frame '" + object + "' is not attached to the robot"; if (!msg.empty()) { if (storeFailures()) { InterfaceState state(scene); @@ -97,26 +100,21 @@ void GeneratePlacePose::compute() { const moveit::core::RobotState& robot_state = scene->getCurrentState(); const auto& props = properties(); - const moveit::core::AttachedBody* object = robot_state.getAttachedBody(props.get("object")); - // current object_pose w.r.t. planning frame - const Eigen::Isometry3d& orig_object_pose = object->getGlobalCollisionBodyTransforms()[0]; + const std::string& frame_id = props.get("object"); + geometry_msgs::msg::PoseStamped ik_frame; + ik_frame.header.frame_id = frame_id; + ik_frame.pose = tf2::toMsg(Eigen::Isometry3d::Identity()); + const moveit::core::AttachedBody* object = robot_state.getAttachedBody(frame_id); const geometry_msgs::msg::PoseStamped& pose_msg = props.get("pose"); Eigen::Isometry3d target_pose; tf2::fromMsg(pose_msg.pose, target_pose); // target pose w.r.t. planning frame scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose); - const geometry_msgs::msg::PoseStamped& ik_frame_msg = props.get("ik_frame"); - Eigen::Isometry3d ik_frame; - tf2::fromMsg(ik_frame_msg.pose, ik_frame); - ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame; - Eigen::Isometry3d object_to_ik = orig_object_pose.inverse() * ik_frame; - // spawn the nominal target object pose, considering flip about z and rotations about z-axis - auto spawner = [&s, &scene, &object_to_ik, this](const Eigen::Isometry3d& nominal, uint z_flips, - uint z_rotations = 10) { - for (uint flip = 0; flip < z_flips; ++flip) { + auto spawner = [&s, &scene, &ik_frame, this](const Eigen::Isometry3d& nominal, uint z_flips, uint z_rotations = 10) { + for (uint flip = 0; flip <= z_flips; ++flip) { // flip about object's x-axis Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX()); for (uint i = 0; i < z_rotations; ++i) { @@ -129,11 +127,12 @@ void GeneratePlacePose::compute() { // target ik_frame's pose w.r.t. planning frame geometry_msgs::msg::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = scene->getPlanningFrame(); - target_pose_msg.pose = tf2::toMsg(object * object_to_ik); + target_pose_msg.pose = tf2::toMsg(object); InterfaceState state(scene); forwardProperties(*s.end(), state); // forward properties from inner solutions state.properties().set("target_pose", target_pose_msg); + state.properties().set("ik_frame", ik_frame); SubTrajectory trajectory; trajectory.setCost(0.0); @@ -144,20 +143,20 @@ void GeneratePlacePose::compute() { } }; - if (object->getShapes().size() == 1) { + uint z_flips = props.get("allow_z_flip") ? 1 : 0; + if (object && object->getShapes().size() == 1) { switch (object->getShapes()[0]->type) { case shapes::CYLINDER: - spawner(target_pose, 2); + spawner(target_pose, z_flips); return; case shapes::BOX: { // consider 180/90 degree rotations about z axis const double* dims = static_cast(*object->getShapes()[0]).size; - spawner(target_pose, 2, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2); + spawner(target_pose, z_flips, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2); return; } case shapes::SPHERE: // keep original orientation and rotate about world's z - target_pose.linear() = orig_object_pose.linear(); - spawner(target_pose, 1); + spawner(target_pose, z_flips); return; default: break; diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index bea2fee21..9f00cc9bc 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -72,7 +72,9 @@ void GeneratePose::compute() { if (upstream_solutions_.empty()) return; - planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); + const SolutionBase& s = *upstream_solutions_.pop(); + planning_scene::PlanningSceneConstPtr scene = s.end()->scene()->diff(); + geometry_msgs::msg::PoseStamped target_pose = properties().get("pose"); if (target_pose.header.frame_id.empty()) target_pose.header.frame_id = scene->getPlanningFrame(); @@ -82,6 +84,7 @@ void GeneratePose::compute() { } InterfaceState state(scene); + forwardProperties(*s.end(), state); // forward registered properties from received solution state.properties().set("target_pose", target_pose); SubTrajectory trajectory; diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index fc9701649..ae0cc0e15 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -86,11 +86,13 @@ void ModifyPlanningScene::allowCollisions(const std::string& first, const moveit } void ModifyPlanningScene::computeForward(const InterfaceState& from) { - sendForward(from, apply(from, false), SubTrajectory()); + auto result = apply(from, false); + sendForward(from, std::move(result.first), std::move(result.second)); } void ModifyPlanningScene::computeBackward(const InterfaceState& to) { - sendBackward(apply(to, true), to, SubTrajectory()); + auto result = apply(to, true); + sendBackward(std::move(result.first), to, std::move(result.second)); } void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene, @@ -100,8 +102,8 @@ void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene, bool attach = pair.second.second; if (invert) attach = !attach; - obj.object.operation = - attach ? (int8_t)moveit_msgs::msg::CollisionObject::ADD : (int8_t)moveit_msgs::msg::CollisionObject::REMOVE; + obj.object.operation = attach ? static_cast(moveit_msgs::msg::CollisionObject::ADD) : + static_cast(moveit_msgs::msg::CollisionObject::REMOVE); for (const std::string& name : pair.second.first) { obj.object.id = name; scene.processAttachedCollisionObjectMsg(obj); @@ -113,38 +115,55 @@ void ModifyPlanningScene::allowCollisions(planning_scene::PlanningScene& scene, collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst(); bool allow = invert ? !pairs.allow : pairs.allow; if (pairs.second.empty()) { - for (const auto& name : pairs.first) + for (const auto& name : pairs.first) { + acm.setDefaultEntry(name, allow); acm.setEntry(name, allow); + } } else acm.setEntry(pairs.first, pairs.second, allow); } // invert indicates, whether to detach instead of attach (and vice versa) // as well as to forbid instead of allow collision (and vice versa) -InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert) { +std::pair ModifyPlanningScene::apply(const InterfaceState& from, bool invert) { planning_scene::PlanningScenePtr scene = from.scene()->diff(); - InterfaceState result(scene); - // add/remove objects - for (const auto& collision_object : collision_objects_) - processCollisionObject(*scene, collision_object); - - // attach/detach objects - for (const auto& pair : attach_objects_) - attachObjects(*scene, pair, invert); - - // allow/forbid collisions - for (const auto& pairs : collision_matrix_edits_) - allowCollisions(*scene, pairs, invert); - - if (callback_) - callback_(scene, properties()); - - return result; + InterfaceState state(scene); + SubTrajectory traj; + try { + // add/remove objects + for (auto& collision_object : collision_objects_) + processCollisionObject(*scene, collision_object, invert); + + // attach/detach objects + for (const auto& pair : attach_objects_) + attachObjects(*scene, pair, invert); + + // allow/forbid collisions + for (const auto& pairs : collision_matrix_edits_) + allowCollisions(*scene, pairs, invert); + + if (callback_) + callback_(scene, properties()); + } catch (const std::exception& e) { + traj.markAsFailure(e.what()); + } + return std::make_pair(state, traj); } void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene, - const moveit_msgs::msg::CollisionObject& object) { + const moveit_msgs::msg::CollisionObject& object, bool invert) { + const auto op = object.operation; + if (invert) { + if (op == moveit_msgs::msg::CollisionObject::ADD) + // (temporarily) change operation to REMOVE to revert adding the object + const_cast(object).operation = moveit_msgs::msg::CollisionObject::REMOVE; + else if (op == moveit_msgs::msg::CollisionObject::REMOVE) + throw std::runtime_error("cannot apply removeObject() backwards"); + } + scene.processCollisionObjectMsg(object); + // restore previous operation (for next call) + const_cast(object).operation = op; } } // namespace stages } // namespace task_constructor diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 23b7a9dc3..62450fa77 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -85,11 +85,12 @@ void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model) { planner_->init(robot_model); } -static bool getJointStateFromOffset(const boost::any& direction, const moveit::core::JointModelGroup* jmg, - moveit::core::RobotState& robot_state) { +static bool getJointStateFromOffset(const boost::any& direction, const Interface::Direction dir, + const moveit::core::JointModelGroup* jmg, moveit::core::RobotState& robot_state) { try { const auto& accepted = jmg->getActiveJointModels(); const auto& joints = boost::any_cast>(direction); + double sign = dir == Interface::Direction::FORWARD ? +1.0 : -1.0; for (const auto& j : joints) { auto jm = robot_state.getRobotModel()->getJointModel(j.first); if (!jm || std::find(accepted.begin(), accepted.end(), jm) == accepted.end()) @@ -98,7 +99,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c if (jm->getVariableCount() != 1) throw std::runtime_error("Cannot plan for multi-variable joint '" + j.first + "'"); auto index = jm->getFirstVariableIndex(); - robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second); + robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + sign * j.second); robot_state.enforceBounds(jm); } robot_state.update(); @@ -110,8 +111,9 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c return false; } +// Create an arrow marker from start_pose to reached_pose, split into a red and green part based on achieved distance static void visualizePlan(std::deque& markers, Interface::Direction dir, bool success, - const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose, + const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) { double linear_norm = linear.norm(); @@ -120,7 +122,7 @@ static void visualizePlan(std::deque& markers, auto quat_cylinder = quat_target * Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY()); // link position before planning; reached link position after planning; target link position - Eigen::Vector3d pos_link = link_pose.translation(); + Eigen::Vector3d pos_start = start_pose.translation(); Eigen::Vector3d pos_reached = reached_pose.translation(); Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0); @@ -130,7 +132,7 @@ static void visualizePlan(std::deque& markers, if (dir == Interface::FORWARD) { if (success) { // valid part: green arrow - rviz_marker_tools::makeArrow(m, pos_link, pos_reached, 0.1 * linear_norm); + rviz_marker_tools::makeArrow(m, pos_start, pos_reached, 0.1 * linear_norm); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); markers.push_back(m); } else { @@ -145,14 +147,14 @@ static void visualizePlan(std::deque& markers, rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); // position half-way between pos_link and pos_reached - m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_link + pos_reached) }); + m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_start + pos_reached) }); m.pose.orientation = tf2::toMsg(quat_cylinder); markers.push_back(m); } } else { // valid part: green arrow // head length according to above comment - rviz_marker_tools::makeArrow(m, pos_reached, pos_link, 0.1 * linear_norm, 0.23 * linear_norm); + rviz_marker_tools::makeArrow(m, pos_reached, pos_start, 0.1 * linear_norm, 0.23 * linear_norm); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); markers.push_back(m); if (!success) { @@ -194,9 +196,10 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; - if (getJointStateFromOffset(direction, jmg, scene->getCurrentStateNonConst())) { + if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + solution.setPlannerId(planner_->getPlannerId()); } else { // Cartesian targets require an IK reference frame const moveit::core::LinkModel* link; @@ -209,10 +212,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning Eigen::Vector3d linear; // linear translation Eigen::Vector3d angular; // angular rotation double linear_norm = 0.0, angular_norm = 0.0; - Eigen::Isometry3d target_eigen; - Eigen::Isometry3d link_pose = - scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success try { // try to extract Twist const geometry_msgs::msg::TwistStamped& target = boost::any_cast(direction); @@ -246,13 +246,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning angular *= -1.0; } - // compute absolute transform for link + // compute target transform for ik_frame applying motion transform of twist + // linear+angular are expressed w.r.t. model frame and thus we need left-multiplication linear = frame_pose.linear() * linear; angular = frame_pose.linear() * angular; - target_eigen = link_pose; - target_eigen.linear() = - target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular); - target_eigen.translation() += linear; + auto R = Eigen::AngleAxisd(angular_norm, angular); // NOLINT(readability-identifier-naming) + auto p = ik_pose_world.translation(); + target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world); goto COMPUTE; } catch (const boost::bad_any_cast&) { /* continue with Vector */ } @@ -274,50 +274,53 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (dir == Interface::BACKWARD) linear *= -1.0; - // compute absolute transform for link + // compute target transform for ik_frame applying delta transform of twist linear = frame_pose.linear() * linear; - target_eigen = link_pose; - target_eigen.translation() += linear; + target_eigen = Eigen::Translation3d(linear) * ik_pose_world; } catch (const boost::bad_any_cast&) { solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name()); return false; } COMPUTE: - // transform target pose such that ik frame will reach there if link does - target_eigen = target_eigen * scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; - - success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); - - moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); - reached_state->updateLinkTransforms(); - const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link); - - double distance = 0.0; - if (use_rotation_distance) { - Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose()); - distance = rotation.angle(); - } else - distance = (reached_pose.translation() - link_pose.translation()).norm(); - - // min_distance reached? - if (min_distance > 0.0) { - success = distance >= min_distance; - if (!success) { - char msg[100]; - snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance); - solution.setComment(msg); + // offset from link to ik_frame + const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; + + success = + planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + solution.setPlannerId(planner_->getPlannerId()); + + if (robot_trajectory) { // the following requires a robot_trajectory returned from planning + moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); + reached_state->updateLinkTransforms(); + const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; + + double distance = 0.0; + if (use_rotation_distance) { + Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose()); + distance = rotation.angle(); + } else + distance = (reached_pose.translation() - ik_pose_world.translation()).norm(); + + // min_distance reached? + if (min_distance > 0.0) { + success = distance >= min_distance; + if (!success) { + char msg[100]; + snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance); + solution.setComment(msg); + } + } else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case + success = true; + } else if (!success) + solution.setComment("failed to move full distance"); + + // visualize plan + auto ns = props.get("marker_ns"); + if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance + visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose, + linear, distance); } - } else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case - success = true; - } else if (!success) - solution.setComment("failed to move full distance"); - - // visualize plan - auto ns = props.get("marker_ns"); - if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance - visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), link_pose, reached_pose, linear, - distance); } } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 6d4027c94..1a44a197a 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -41,7 +41,6 @@ #include #include -#include #include #include @@ -210,6 +209,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + solution.setPlannerId(planner_->getPlannerId()); } else { // Cartesian goal // Where to go? Eigen::Isometry3d target; @@ -237,11 +237,12 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP add_frame(target, "target frame"); add_frame(ik_pose_world, "ik frame"); - // transform target pose such that ik frame will reach there if link does - target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); + // offset from link to ik_frame + Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; // plan to Cartesian target - success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints); + success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints); + solution.setPlannerId(planner_->getPlannerId()); } // store result diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 63c897169..6a2e9987a 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -36,8 +36,11 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na return pose; }; + const auto& forwarded_props = grasp_stage->forwardedProperties(); + { auto approach = std::make_unique(forward ? "approach object" : "retract", cartesian_solver_); + approach->setForwardedProperties(forwarded_props); PropertyMap& p = approach->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("ik_frame").configureInitFrom(Stage::PARENT, init_ik_frame); @@ -52,6 +55,7 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na { auto lift = std::make_unique(forward ? "lift object" : "place object", cartesian_solver_); + lift->setForwardedProperties(forwarded_props); PropertyMap& p = lift->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("ik_frame").configureInitFrom(Stage::PARENT, init_ik_frame); diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 303718ca0..0bbf40190 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -64,6 +64,7 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name) : SerialContainer(name void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) { // properties provided by the grasp generator via its Interface or its PropertyMap const std::set& grasp_prop_names = { "object", "eef", "pregrasp", "grasp" }; + this->setForwardedProperties(grasp_prop_names); // insert children at end / front, i.e. normal or reverse order int insertion_position = forward ? -1 : (generator ? 1 : 0); @@ -131,8 +132,8 @@ void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) { attach->setCallback([forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p) { const std::string& eef = p.get("eef"); moveit_msgs::msg::AttachedCollisionObject obj; - obj.object.operation = forward ? (int8_t)moveit_msgs::msg::CollisionObject::ADD : - (int8_t)moveit_msgs::msg::CollisionObject::REMOVE; + obj.object.operation = forward ? static_cast(moveit_msgs::msg::CollisionObject::ADD) : + static_cast(moveit_msgs::msg::CollisionObject::REMOVE); obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; obj.object.id = p.get("object"); scene->processAttachedCollisionObjectMsg(obj); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 921289fb1..d5b0db4db 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -84,6 +84,21 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) return cost() < other.cost(); } +void InterfaceState::updatePriority(const InterfaceState::Priority& priority) { + // Never overwrite ARMED with PRUNED + if (priority.status() == InterfaceState::Status::PRUNED && priority_.status() == InterfaceState::Status::ARMED) + return; + + if (owner()) { + owner()->updatePriority(this, priority); + } else { + setPriority(priority); + } +} +void InterfaceState::updateStatus(Status status) { + updatePriority(InterfaceState::Priority(priority_, status)); +} + Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {} // Announce a new InterfaceState @@ -117,7 +132,7 @@ void Interface::add(InterfaceState& state) { moveFrom(it, container); // and finally call notify callback if (notify_) - notify_(it, false); + notify_(it, UpdateFlags()); } Interface::container_type Interface::remove(iterator it) { @@ -128,15 +143,23 @@ Interface::container_type Interface::remove(iterator it) { } void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) { - if (priority == state->priority()) + const auto old_prio = state->priority(); + if (priority == old_prio) return; // nothing to do auto it = std::find(begin(), end(), state); // find iterator to state assert(it != end()); // state should be part of this interface + state->priority_ = priority; // update priority update(it); // update position in ordered list - if (notify_) - notify_(it, true); // notify callback + + if (notify_) { + UpdateFlags updated(Update::ALL); + if (old_prio.status() == priority.status()) + updated &= ~STATUS; + + notify_(it, updated); // notify callback + } } std::ostream& operator<<(std::ostream& os, const Interface& interface) { @@ -146,15 +169,20 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) { os << istate->priority() << " "; return os; } +const char* InterfaceState::STATUS_COLOR_[] = { + "\033[32m", // ENABLED - green + "\033[33m", // ARMED - yellow + "\033[31m", // PRUNED - red + "\033[m" // reset +}; std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) { // maps InterfaceState::Status values to output (color-changing) prefix - static const char* prefix[] = { - "\033[32me:", // ENABLED - green - "\033[33md:", // PRUNED - yellow - "\033[31mf:", // FAILED - red - }; - static const char* color_reset = "\033[m"; - os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset; + os << InterfaceState::colorForStatus(prio.status()) << prio.depth() << ":" << prio.cost() + << InterfaceState::colorForStatus(3); + return os; +} +std::ostream& operator<<(std::ostream& os, Interface::Direction dir) { + os << (dir == Interface::FORWARD ? "↓" : "↑"); return os; } @@ -169,14 +197,25 @@ void SolutionBase::setCost(double cost) { void SolutionBase::markAsFailure(const std::string& msg) { setCost(std::numeric_limits::infinity()); - if (!msg.empty()) - setComment(msg + "\n" + comment()); + if (!msg.empty()) { + std::stringstream ss; + ss << msg; + if (!comment().empty()) + ss << "\n" << comment(); + setComment(ss.str()); + } +} + +void SolutionBase::toMsg(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const { + appendTo(msg, introspection); + start()->scene()->getPlanningSceneMsg(msg.start_scene); } void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection) const { info.id = introspection ? introspection->solutionId(*this) : 0; info.cost = this->cost(); info.comment = this->comment(); + info.planner_id = this->plannerId(); const Introspection* ci = introspection; info.stage_id = ci ? ci->stageId(this->creator()) : 0; @@ -185,7 +224,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& inf std::copy(markers.begin(), markers.end(), info.markers.begin()); } -void SubTrajectory::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const { +void SubTrajectory::appendTo(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const { msg.sub_trajectory.emplace_back(); moveit_task_constructor_msgs::msg::SubTrajectory& t = msg.sub_trajectory.back(); SolutionBase::fillInfo(t.info, introspection); @@ -208,8 +247,7 @@ void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } -void SolutionSequence::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, - Introspection* introspection) const { +void SolutionSequence::appendTo(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const { moveit_task_constructor_msgs::msg::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); @@ -231,7 +269,7 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size()); for (const SolutionBase* s : subsolutions_) { size_t current = msg.sub_trajectory.size(); - s->fillMessage(msg, introspection); + s->appendTo(msg, introspection); // zero IDs of sub solutions with same creator as this if (s->creator() == this->creator()) { @@ -248,9 +286,9 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co return f(*this, comment); } -void WrappedSolution::fillMessage(moveit_task_constructor_msgs::msg::Solution& solution, - Introspection* introspection) const { - wrapped_->fillMessage(solution, introspection); +void WrappedSolution::appendTo(moveit_task_constructor_msgs::msg::Solution& solution, + Introspection* introspection) const { + wrapped_->appendTo(solution, introspection); // prepend this solutions info as a SubSolution msg moveit_task_constructor_msgs::msg::SubSolution sub_msg; diff --git a/core/src/task.cpp b/core/src/task.cpp index 9d1694fce..4811c6fa2 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -76,30 +76,16 @@ namespace task_constructor { TaskPrivate::TaskPrivate(Task* me, const std::string& ns) : WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {} -void swap(StagePrivate*& lhs, StagePrivate*& rhs) { - // It only makes sense to swap pimpl instances of a Task! - // However, due to member protection rules, we can only implement it here - assert(typeid(lhs) == typeid(rhs)); - - // swap instances - ::std::swap(lhs, rhs); - // as well as their me_ pointers - ::std::swap(lhs->me_, rhs->me_); - - // and redirect the parent pointers of children to new parents - auto& lhs_children = static_cast(lhs)->children_; - for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it) { - (*it)->pimpl()->unparent(); - (*it)->pimpl()->setParent(static_cast(lhs->me_)); - (*it)->pimpl()->setParentPosition(it); - } - - auto& rhs_children = static_cast(rhs)->children_; - for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it) { - (*it)->pimpl()->unparent(); - (*it)->pimpl()->setParent(static_cast(rhs->me_)); - (*it)->pimpl()->setParentPosition(it); - } +TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) { + this->WrapperBasePrivate::operator=(std::move(other)); + ns_ = std::move(other.ns_); + robot_model_ = std::move(other.robot_model_); + robot_model_loader_ = std::move(other.robot_model_loader_); + task_cbs_ = std::move(other.task_cbs_); + // Ensure same introspection status, but keep the existing introspection instance, + // which stores this task pointer and includes it in its task_id_ + static_cast(me_)->enableIntrospection(static_cast(other.introspection_)); + return *this; } const ContainerBase* TaskPrivate::stages() const { @@ -124,7 +110,7 @@ Task::Task(Task&& other) // NOLINT(performance-noexcept-move-constructor) Task& Task::operator=(Task&& other) { // NOLINT(performance-noexcept-move-constructor) clear(); // remove all stages of current task - swap(this->pimpl_, other.pimpl_); + *static_cast(pimpl_) = std::move(*static_cast(other.pimpl_)); return *this; } @@ -227,17 +213,17 @@ void Task::init() { stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE })); // provide introspection instance to all stages - impl->setIntrospection(impl->introspection_.get()); + auto* introspection = impl->introspection_.get(); impl->traverseStages( - [impl](Stage& stage, int /*depth*/) { - stage.pimpl()->setIntrospection(impl->introspection_.get()); + [introspection](Stage& stage, int /*depth*/) { + stage.pimpl()->setIntrospection(introspection); return true; }, 1, UINT_MAX); // first time publish task - if (impl->introspection_) - impl->introspection_->publishTaskDescription(); + if (introspection) + introspection->publishTaskDescription(); } bool Task::canCompute() const { @@ -248,30 +234,40 @@ void Task::compute() { stages()->pimpl()->runCompute(); } -bool Task::plan(size_t max_solutions) { +moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { auto impl = pimpl(); init(); + // Print state and return success if there are solutions otherwise the input error_code + const auto success_or = [this](const int32_t error_code) -> int32_t { + if (numSolutions() > 0) + return moveit::core::MoveItErrorCode::SUCCESS; + printState(); + explainFailure(); + return error_code; + }; impl->preempt_requested_ = false; const double available_time = timeout(); const auto start_time = std::chrono::steady_clock::now(); - while (!impl->preempt_requested_ && canCompute() && (max_solutions == 0 || numSolutions() < max_solutions) && - std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() < available_time) { + while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) { + if (impl->preempt_requested_) + return success_or(moveit::core::MoveItErrorCode::PREEMPTED); + if (std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() >= available_time) + return success_or(moveit::core::MoveItErrorCode::TIMED_OUT); compute(); for (const auto& cb : impl->task_cbs_) cb(*this); if (impl->introspection_) impl->introspection_->publishTaskState(); - } - printState(); - return numSolutions() > 0; + }; + return success_or(moveit::core::MoveItErrorCode::PLANNING_FAILED); } void Task::preempt() { pimpl()->preempt_requested_ = true; } -moveit_msgs::msg::MoveItErrorCodes Task::execute(const SolutionBase& s) { +moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { // Add random ID to prevent warnings about multiple publishers within the same node rclcpp::NodeOptions options; options.arguments( @@ -283,8 +279,7 @@ moveit_msgs::msg::MoveItErrorCodes Task::execute(const SolutionBase& s) { ac->wait_for_action_server(); moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal; - s.fillMessage(goal.solution, pimpl()->introspection_.get()); - s.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene); + s.toMsg(goal.solution, pimpl()->introspection_.get()); moveit_msgs::msg::MoveItErrorCodes error_code; error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; @@ -294,7 +289,7 @@ moveit_msgs::msg::MoveItErrorCodes Task::execute(const SolutionBase& s) { return error_code; } - auto goal_handle = goal_handle_future.get(); + const auto& goal_handle = goal_handle_future.get(); if (!goal_handle) { RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); return error_code; @@ -353,5 +348,10 @@ const core::RobotModelConstPtr& Task::getRobotModel() const { void Task::printState(std::ostream& os) const { os << *stages(); } + +void Task::explainFailure(std::ostream& os) const { + os << "Failing stage(s):\n"; + stages()->explainFailure(os); +} } // namespace task_constructor } // namespace moveit diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 979e228df..8cfc480e7 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -44,7 +44,6 @@ #include #include -#include #include #include @@ -52,22 +51,6 @@ namespace moveit { namespace task_constructor { namespace utils { -const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, - std::string frame) { -#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK - return state.getRigidlyConnectedParentLinkModel(frame); -#else - const moveit::core::LinkModel* link{ nullptr }; - - if (state.hasAttachedBody(frame)) { - link = state.getAttachedBody(frame)->getAttachedLink(); - } else if (state.getRobotModel()->hasLinkModel(frame)) - link = state.getLinkModel(frame); - - return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link); -#endif -} - bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, const moveit::core::JointModelGroup* jmg, SolutionBase& solution, const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) { @@ -90,23 +73,27 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); } else { auto ik_pose_msg = boost::any_cast(property.value()); - if (ik_pose_msg.header.frame_id.empty()) { - if (!(robot_link = get_tip())) { - solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); - return false; - } - tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); - tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame; - } else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) { - robot_link = getRigidlyConnectedParentLinkModel(scene.getCurrentState(), ik_pose_msg.header.frame_id); - tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); - tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame; - } else { + tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); + + robot_link = nullptr; + bool found = false; + auto ref_frame = scene.getCurrentState().getFrameInfo(ik_pose_msg.header.frame_id, robot_link, found); + if (!found && !ik_pose_msg.header.frame_id.empty()) { std::stringstream ss; ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'"; solution.markAsFailure(ss.str()); return false; } + if (!robot_link) + robot_link = get_tip(); + if (!robot_link) { + solution.markAsFailure("ik_frame doesn't specify a link frame"); + return false; + } else if (!found) { // use robot link's frame as reference by default + ref_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); + } + + tip_in_global_frame = ref_frame * tip_in_global_frame; } return true; diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 99eb6097b..69fe81160 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -6,7 +6,7 @@ if (BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_gmock REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) add_library(gtest_utils SHARED gtest_value_printers.cpp models.cpp stage_mockups.cpp) @@ -24,15 +24,13 @@ if (BUILD_TESTING) string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME}) # Configure build target if(TEST_FILE) # Add launch test if .launch.py file was specified - ament_add_gtest_executable(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) - add_launch_test(${TEST_FILE} - ARGS "test_binary:=$") - else() - if("${TYPE}" STREQUAL "gtest") - ament_add_gtest(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) - elseif("${TYPE}" STREQUAL "gmock") - ament_add_gmock(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) - endif() + ament_add_gtest_executable(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) + add_launch_test(${TEST_FILE} TARGET ${TEST_NAME} + ARGS "test_binary:=$") + elseif("${TYPE}" STREQUAL "gtest") + ament_add_gtest(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) + elseif("${TYPE}" STREQUAL "gmock") + ament_add_gmock(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) endif() target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) endmacro() @@ -55,6 +53,8 @@ if (BUILD_TESTING) mtc_add_gmock(test_interface_state.cpp) mtc_add_gtest(test_move_to.cpp move_to.launch.py) + mtc_add_gtest(test_move_relative.cpp move_to.launch.py) + mtc_add_gtest(test_pipeline_planner.cpp) # building these integration tests works without moveit config packages ament_add_gtest_executable(pick_ur5 pick_ur5.cpp) diff --git a/core/test/move_to.launch.py b/core/test/move_to.launch.py index 103a20cff..023283bea 100644 --- a/core/test/move_to.launch.py +++ b/core/test/move_to.launch.py @@ -1,73 +1,23 @@ import unittest -import os -import yaml +import launch_testing +import pytest from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -import launch_testing -from launch_testing.asserts import assertExitCodes -from launch_testing.util import KeepAliveProc -from launch_testing.actions import ReadyToTest -from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node - -import pytest - -import xacro - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from moveit_configs_utils import MoveItConfigsBuilder @pytest.mark.launch_test def generate_test_description(): - # planning_context - robot_description_config = xacro.process_file( - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "panda.urdf.xacro", - ) + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) - robot_description = {"robot_description": robot_description_config.toxml()} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" - ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - - robot_description_planning = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) - } test_exec = Node( executable=[ @@ -75,10 +25,10 @@ def generate_test_description(): ], output="screen", parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - robot_description_planning, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, ], ) diff --git a/core/test/pick_pa10.cpp b/core/test/pick_pa10.cpp index 367372400..4ee7b9336 100644 --- a/core/test/pick_pa10.cpp +++ b/core/test/pick_pa10.cpp @@ -47,8 +47,7 @@ TEST(PA10, pick) { t.setProperty("eef", std::string("la_tool_mount")); t.setProperty("gripper", std::string("left_hand")); - auto pipeline = std::make_shared(node); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); auto cartesian = std::make_shared(); Stage* initial_stage = nullptr; diff --git a/core/test/pick_pr2.cpp b/core/test/pick_pr2.cpp index 2554097b5..c8a31c792 100644 --- a/core/test/pick_pr2.cpp +++ b/core/test/pick_pr2.cpp @@ -40,8 +40,7 @@ TEST(PR2, pick) { auto node = rclcpp::Node::make_shared("pr2"); // planner used for connect - auto pipeline = std::make_shared(node); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); // connect to pick stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } }; auto connect = std::make_unique("connect", planners); diff --git a/core/test/pick_ur5.cpp b/core/test/pick_ur5.cpp index 87d17e87e..d268bdc15 100644 --- a/core/test/pick_ur5.cpp +++ b/core/test/pick_ur5.cpp @@ -42,8 +42,7 @@ TEST(UR5, pick) { auto node = rclcpp::Node::make_shared("ur5"); // planner used for connect - auto pipeline = std::make_shared(node); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); // connect to pick stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } }; auto connect = std::make_unique("connect", planners); diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 4eaa143bd..1a7a9de4c 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -79,20 +79,27 @@ TEST(ContainerBase, positionForInsert) { /* TODO: remove interface as it returns raw pointers */ TEST(ContainerBase, findChild) { - SerialContainer s; + auto s = std::make_unique(); Stage *a, *b, *c1, *d; - s.add(Stage::pointer(a = new NamedStage("a"))); - s.add(Stage::pointer(b = new NamedStage("b"))); - s.add(Stage::pointer(c1 = new NamedStage("c"))); + s->add(Stage::pointer(a = new NamedStage("a"))); + s->add(Stage::pointer(b = new NamedStage("b"))); + s->add(Stage::pointer(c1 = new NamedStage("c"))); auto sub = ContainerBase::pointer(new SerialContainer("c")); sub->add(Stage::pointer(d = new NamedStage("d"))); - s.add(std::move(sub)); - - EXPECT_EQ(s.findChild("a"), a); - EXPECT_EQ(s.findChild("b"), b); - EXPECT_EQ(s.findChild("c"), c1); - EXPECT_EQ(s.findChild("d"), nullptr); - EXPECT_EQ(s.findChild("c/d"), d); + s->add(std::move(sub)); + + EXPECT_EQ(s->findChild("a"), a); + EXPECT_EQ(s->findChild("b"), b); + EXPECT_EQ(s->findChild("c"), c1); + EXPECT_EQ(s->findChild("d"), nullptr); + EXPECT_EQ(s->findChild("c/d"), d); + + Task t("", false, std::move(s)); + EXPECT_EQ(t.findChild("a"), a); + EXPECT_EQ(t.findChild("b"), b); + EXPECT_EQ(t.findChild("c"), c1); + EXPECT_EQ(t.findChild("d"), nullptr); + EXPECT_EQ(t.findChild("c/d"), d); } template @@ -586,10 +593,6 @@ TEST(Task, move) { Task t2 = std::move(t1); EXPECT_EQ(t2.stages()->numChildren(), 2u); EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move) - - t1 = std::move(t2); - EXPECT_EQ(t1.stages()->numChildren(), 2u); - EXPECT_EQ(t2.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move) } TEST(Task, reuse) { @@ -655,7 +658,7 @@ TEST(Task, timeout) { // zero timeout fails t.reset(); t.setTimeout(0.0); - EXPECT_FALSE(t.plan()); + EXPECT_EQ(t.plan(), moveit::core::MoveItErrorCode::TIMED_OUT); // time for 1 solution t.reset(); diff --git a/core/test/test_cost_queue.cpp b/core/test/test_cost_queue.cpp index 0f4a0433f..1d6657048 100644 --- a/core/test/test_cost_queue.cpp +++ b/core/test/test_cost_queue.cpp @@ -5,6 +5,10 @@ #include #include +#ifndef TYPED_TEST_SUITE +#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) +#endif + namespace mtc = moveit::task_constructor; // type-trait functions for OrderedTest @@ -62,11 +66,7 @@ class ValueOrPointeeLessTest : public ::testing::Test }; // set of template types to test for using TypeInstances = ::testing::Types; -#ifdef TYPED_TEST_SUITE TYPED_TEST_SUITE(ValueOrPointeeLessTest, TypeInstances); -#else -TYPED_TEST_CASE(ValueOrPointeeLessTest, TypeInstances); -#endif TYPED_TEST(ValueOrPointeeLessTest, less) { EXPECT_TRUE(this->less(2, 3)); EXPECT_FALSE(this->less(1, 1)); @@ -105,11 +105,7 @@ class OrderedTest : public ::testing::Test, public ordered SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \ this->pushAndValidate(cost, __VA_ARGS__); \ } -#ifdef TYPED_TEST_SUITE TYPED_TEST_SUITE(OrderedTest, TypeInstances); -#else -TYPED_TEST_CASE(OrderedTest, TypeInstances); -#endif TYPED_TEST(OrderedTest, sorting) { pushAndValidate(2, { 2 }); pushAndValidate(1, { 1, 2 }); diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index d3f7e9068..cf2d70504 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -18,7 +18,7 @@ using namespace moveit::task_constructor; using FallbacksFixtureGenerator = TaskTestBase; -TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) { +TEST_F(FallbacksFixtureGenerator, stayWithFirstSuccessful) { auto fallback = std::make_unique("Fallbacks"); fallback->add(std::make_unique(PredefinedCosts::single(INF))); fallback->add(std::make_unique(PredefinedCosts::single(1.0))); @@ -56,7 +56,7 @@ TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) { EXPECT_EQ(t.solutions().size(), 0u); } -TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) { +TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStageOnly) { t.add(std::make_unique()); auto fallbacks = std::make_unique("Fallbacks"); @@ -68,7 +68,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) { EXPECT_EQ(t.numSolutions(), 1u); } -TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutionOnly) { +TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStagePerSolutionOnly) { t.add(std::make_unique(PredefinedCosts({ 2.0, 1.0 }))); // duplicate generator solutions with resulting costs: 4, 2 | 3, 1 t.add(std::make_unique(PredefinedCosts({ 2.0, 0.0, 2.0, 0.0 }), 2)); @@ -79,10 +79,11 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutio t.add(std::move(fallbacks)); EXPECT_TRUE(t.plan()); - EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 211, 222)); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 212, 221)); } -TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) { +// requires individual job control in Fallbacks's children +TEST_F(FallbacksFixturePropagate, DISABLED_updateSolutionOrder) { t.add(std::make_unique(PredefinedCosts({ 10.0, 0.0 }))); t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); // available solutions (sorted) in individual runs of fallbacks: 1 | 11, 2 | 2, 11 @@ -101,7 +102,8 @@ TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) { EXPECT_COSTS(t.solutions(), testing::ElementsAre(2)); // expecting less costly solution as result } -TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) { +// requires individual job control in Fallbacks's children +TEST_F(FallbacksFixturePropagate, DISABLED_multipleActivePendingStates) { t.add(std::make_unique(PredefinedCosts({ 2.0, 1.0, 3.0 }))); // use a fallback container to delay computation: the 1st child never succeeds, but only the 2nd auto inner = std::make_unique("Inner"); @@ -118,7 +120,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) { // check that first solution is not marked as pruned } -TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) { +TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions) { t.add(std::make_unique()); auto fallback = std::make_unique("Fallbacks"); @@ -130,7 +132,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) { EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0)); } -TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) { +TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions2) { t.add(std::make_unique()); auto fallback = std::make_unique("Fallbacks"); @@ -142,7 +144,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) { EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0)); } -TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) { +TEST_F(FallbacksFixturePropagate, activeChildReset) { t.add(std::make_unique(PredefinedCosts({ 1.0, INF, 3.0 }))); auto fallbacks = std::make_unique("Fallbacks"); @@ -160,18 +162,18 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) { using FallbacksFixtureConnect = TaskTestBase; -TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) { +TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); auto fallbacks = std::make_unique("Fallbacks"); - fallbacks->add(std::make_unique(PredefinedCosts::constant(0.0))); + fallbacks->add(std::make_unique(PredefinedCosts({ 0.0, 0.0, INF, 0.0 }))); fallbacks->add(std::make_unique(PredefinedCosts::constant(100.0))); t.add(std::move(fallbacks)); t.add(std::make_unique(PredefinedCosts({ 10.0, 20.0 }))); EXPECT_TRUE(t.plan()); - EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 21, 22)); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 22, 121)); } int main(int argc, char** argv) { diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 9ca6af53f..65aeee36f 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -19,7 +19,7 @@ TEST(InterfaceStatePriority, compare) { EXPECT_TRUE(Prio(1, 42) < Prio(0, 0)); EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger - auto dstart = InterfaceState::Status::FAILED; + auto dstart = InterfaceState::Status::ARMED; EXPECT_TRUE(Prio(0, 0, dstart) == Prio(0, 0, dstart)); EXPECT_TRUE(Prio(1, 0, dstart) < Prio(0, 0, dstart)); EXPECT_TRUE(Prio(1, 42, dstart) < Prio(0, 0, dstart)); @@ -68,7 +68,7 @@ TEST(Interface, update) { i.updatePriority(*i.rbegin(), Prio(5, 0.0)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); - i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::FAILED)); + i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::ARMED)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 })); } @@ -87,8 +87,11 @@ TEST(StatePairs, compare) { EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0))); auto good = InterfaceState::Status::ENABLED; - auto bad = InterfaceState::Status::FAILED; - EXPECT_TRUE(pair(good, good) < pair(good, bad)); - EXPECT_TRUE(pair(good, good) < pair(bad, good)); - EXPECT_TRUE(pair(bad, good) < pair(good, bad)); + auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good)); + ASSERT_TRUE(good_good > pair(good, good)); // a bad status should reverse this relation + for (auto bad : { InterfaceState::Status::ARMED, InterfaceState::Status::PRUNED }) { + EXPECT_TRUE(good_good < pair(bad, good)); + EXPECT_TRUE(good_good < pair(good, bad)); + EXPECT_TRUE(good_good < pair(bad, bad)); + } } diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp new file mode 100644 index 000000000..5a36457f9 --- /dev/null +++ b/core/test/test_move_relative.cpp @@ -0,0 +1,137 @@ +#include "models.h" + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +using namespace moveit::task_constructor; +using namespace planning_scene; +using namespace moveit::core; + +constexpr double TAU{ 2 * M_PI }; +constexpr double EPS{ 5e-5 }; + +// provide a basic test fixture that prepares a Task +struct PandaMoveRelative : public testing::Test +{ + Task t; + stages::MoveRelative* move; + PlanningScenePtr scene; + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_relative"); + + const JointModelGroup* group; + + PandaMoveRelative() { + t.setRobotModel(loadModel(node)); + + group = t.getRobotModel()->getJointModelGroup("panda_arm"); + + scene = std::make_shared(t.getRobotModel()); + scene->getCurrentStateNonConst().setToDefaultValues(); + scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); + t.add(std::make_unique("start", scene)); + + auto move_relative = std::make_unique("move", std::make_shared()); + move_relative->setGroup(group->getName()); + move = move_relative.get(); + t.add(std::move(move_relative)); + } +}; + +moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string& id) { + moveit_msgs::msg::AttachedCollisionObject aco; + aco.link_name = "panda_hand"; + aco.object.header.frame_id = aco.link_name; + aco.object.operation = aco.object.ADD; + aco.object.id = id; + aco.object.primitives.resize(1, [] { + shape_msgs::msg::SolidPrimitive p; + p.type = p.SPHERE; + p.dimensions.resize(1); + p.dimensions[p.SPHERE_RADIUS] = 0.01; + return p; + }()); + + geometry_msgs::msg::Pose p; + p.position.x = 0.1; + p.orientation.w = 1.0; + aco.object.pose = p; + return aco; +} + +void expect_const_position(const SolutionBaseConstPtr& solution, const std::string& tip, + const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) { + const robot_trajectory::RobotTrajectory& t = *std::dynamic_pointer_cast(solution)->trajectory(); + const Eigen::Vector3d start_position = (t.getFirstWayPoint().getFrameTransform(tip) * offset).translation(); + for (size_t i = 0; i < t.getWayPointCount(); ++i) { + const Eigen::Vector3d position = (t.getWayPoint(i).getFrameTransform(tip) * offset).translation(); + ASSERT_TRUE(start_position.isApprox(position, EPS)) + << "Rotation must maintain position\n" + << i << ": " << start_position.transpose() << " != " << position.transpose(); + } +} + +#define EXPECT_CONST_POSITION(...) \ + { \ + SCOPED_TRACE("expect_constant_position(" #__VA_ARGS__ ")"); \ + expect_const_position(__VA_ARGS__); \ + } + +TEST_F(PandaMoveRelative, cartesianRotateEEF) { + move->setDirection([] { + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = TAU / 8.0; + return twist; + }()); + + ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), group->getOnlyOneEndEffectorTip()->getName()); +} + +TEST_F(PandaMoveRelative, cartesianCircular) { + const std::string tip = "panda_hand"; + auto offset = Eigen::Translation3d(0, 0, 0.1); + move->setIKFrame(offset, tip); + move->setDirection([] { + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.x = TAU / 4.0; + return twist; + }()); + + ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), tip, Eigen::Isometry3d(offset)); +} + +TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { + const std::string attached_object{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object)); + move->setIKFrame(attached_object); + + move->setDirection([] { + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = TAU / 8.0; + return twist; + }()); + + ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), attached_object); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 78fa8ee33..807bcb9e0 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -1,10 +1,10 @@ #include "models.h" +#include "stage_mockups.h" #include #include #include #include -#include #include @@ -77,7 +77,8 @@ TEST_F(PandaMoveTo, stateTarget) { EXPECT_ONE_SOLUTION; } -geometry_msgs::msg::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) { +geometry_msgs::msg::PoseStamped getFramePoseOfNamedState(RobotState state, const std::string& pose, + const std::string& frame) { state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose); auto frame_eigen{ state.getFrameTransform(frame) }; geometry_msgs::msg::PoseStamped p; @@ -103,9 +104,9 @@ TEST_F(PandaMoveTo, poseTarget) { } TEST_F(PandaMoveTo, poseIKFrameLinkTarget) { - const std::string IK_FRAME{ "panda_hand" }; - move_to->setIKFrame(IK_FRAME); - move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); + const std::string ik_frame{ "panda_hand" }; + move_to->setIKFrame(ik_frame); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ik_frame)); EXPECT_ONE_SOLUTION; } @@ -122,48 +123,63 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string p.dimensions[p.SPHERE_RADIUS] = 0.01; return p; }()); - aco.object.primitive_poses.resize(1); -#if MOVEIT_HAS_OBJECT_POSE aco.object.pose.position.z = 0.2; aco.object.pose.orientation.w = 1.0; -#else - aco.object.primitive_poses[0].position.z = 0.2; - aco.object.primitive_poses[0].orientation.w = 1.0; -#endif -#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK - // If we don't have this, we also don't have subframe support aco.object.subframe_names.resize(1, "subframe"); aco.object.subframe_poses.resize(1, [] { geometry_msgs::msg::Pose p; p.orientation.w = 1.0; return p; }()); -#endif return aco; } TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) { - const std::string ATTACHED_OBJECT{ "attached_object" }; - scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + const std::string attached_object{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object)); - move_to->setIKFrame(ATTACHED_OBJECT); - move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ATTACHED_OBJECT)); + move_to->setIKFrame(attached_object); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", attached_object)); EXPECT_ONE_SOLUTION; } -#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK -// If we don't have this, we also don't have subframe support TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { - const std::string ATTACHED_OBJECT{ "attached_object" }; - const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" }; + const std::string attached_object{ "attached_object" }; + const std::string ik_frame{ attached_object + "/subframe" }; - scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object)); - move_to->setIKFrame(IK_FRAME); - move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); + move_to->setIKFrame(ik_frame); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ik_frame)); EXPECT_ONE_SOLUTION; } -#endif + +// https://github.com/ros-planning/moveit_task_constructor/pull/371 +TEST(Task, taskMoveConstructor) { + auto create_task = [] { + moveit::core::RobotModelConstPtr robot_model = getModel(); + Task t("first"); + t.setRobotModel(robot_model); + auto ref = new stages::FixedState("fixed"); + auto scene = std::make_shared(t.getRobotModel()); + ref->setState(scene); + + t.add(Stage::pointer(ref)); + t.add(std::make_unique()); + t.add(std::make_unique(ref)); + return t; + }; + + Task t; + t = create_task(); + + try { + t.init(); + EXPECT_TRUE(t.plan(1)); + } catch (const InitStageException& e) { + ADD_FAILURE() << "InitStageException:" << std::endl << e << t; + } +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/core/test/test_pipeline_planner.cpp b/core/test/test_pipeline_planner.cpp new file mode 100644 index 000000000..6f19a18b8 --- /dev/null +++ b/core/test/test_pipeline_planner.cpp @@ -0,0 +1,65 @@ +#include "models.h" + +#include +#include + +#include + +using namespace moveit::task_constructor; + +// Test fixture for PipelinePlanner +struct PipelinePlannerTest : public testing::Test +{ + PipelinePlannerTest() { + node->declare_parameter("STOMP.planning_plugin", "stomp_moveit/StompPlanner"); + }; + const rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("test_pipeline_planner"); + const moveit::core::RobotModelPtr robot_model = getModel(); +}; + +TEST_F(PipelinePlannerTest, testInitialization) { + // GIVEN a valid robot model, ROS node and PipelinePlanner + auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp"); + // WHEN a PipelinePlanner instance is initialized + // THEN it does not throw + EXPECT_NO_THROW(pipeline_planner.init(robot_model)); + EXPECT_EQ(pipeline_planner.getPlannerId(), "Unknown"); +} + +TEST_F(PipelinePlannerTest, testWithoutPlanningPipelines) { + // GIVEN a PipelinePlanner instance without planning pipelines + std::unordered_map empty_pipeline_id_planner_id_map; + auto pipeline_planner = solvers::PipelinePlanner(node, empty_pipeline_id_planner_id_map); + // WHEN a PipelinePlanner instance is initialized + // THEN it does not throw + EXPECT_THROW(pipeline_planner.init(robot_model), std::runtime_error); +} + +TEST_F(PipelinePlannerTest, testValidPlan) { + // GIVEN an initialized PipelinePlanner + auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp"); + pipeline_planner.init(robot_model); + // WHEN a solution for a valid request is requested + auto scene = std::make_shared(robot_model); + robot_trajectory::RobotTrajectoryPtr result = + std::make_shared(robot_model, robot_model->getJointModelGroup("group")); + // THEN it returns true + EXPECT_TRUE(pipeline_planner.plan(scene, scene, robot_model->getJointModelGroup("group"), 1.0, result)); + EXPECT_EQ(pipeline_planner.getPlannerId(), "stomp"); +} + +TEST_F(PipelinePlannerTest, testInvalidPipelineID) { + // GIVEN a valid initialized PipelinePlanner instance + auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp"); + pipeline_planner.init(robot_model); + // WHEN the planner ID for a non-existing planning pipeline is set + // THEN setPlannerID returns false + EXPECT_FALSE(pipeline_planner.setPlannerId("CHOMP", "stomp")); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 5da29cdd1..b26ac5277 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -8,6 +8,10 @@ #include +#ifndef TYPED_TEST_SUITE +#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) +#endif + using namespace moveit::task_constructor; using Pruning = TaskTestBase; @@ -40,7 +44,54 @@ TEST_F(Pruning, PruningMultiForward) { EXPECT_EQ((*t.solutions().begin())->cost(), 0u); } +// The 2nd failing FW attempt would prune the path through CON, +// but shouldn't because there exist two more GEN2 solutions +TEST_F(Pruning, NoPruningIfAlternativesExist) { + add(t, new GeneratorMockup(PredefinedCosts({ 0.0 }))); + add(t, new ConnectMockup()); + add(t, new GeneratorMockup(std::list{ 0, 10, 20, 30 }, 2)); + add(t, new ForwardMockup({ INF, INF, 0.0, INF })); + + t.plan(); + + EXPECT_EQ(t.solutions().size(), 1u); +} + +TEST_F(Pruning, ConnectReactivatesPrunedPaths) { + add(t, new BackwardMockup); + add(t, new GeneratorMockup({ 0 })); + add(t, new ConnectMockup()); + // the solution here should re-activate the initially pruned backward path + add(t, new GeneratorMockup({ 0 })); + + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.solutions().size(), 1u); +} + +// same as before, but wrapping Connect into a container +template +struct PruningContainerTests : public Pruning +{ + void test() { + add(t, new BackwardMockup); + add(t, new GeneratorMockup({ 0 })); + auto c = new T(); + add(*c, new ConnectMockup()); + add(t, c); + add(t, new GeneratorMockup({ 0 })); + + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.solutions().size(), 1u); + } +}; +using ContainerTypes = ::testing::Types; // TODO: fails for Fallbacks! +TYPED_TEST_SUITE(PruningContainerTests, ContainerTypes); +TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) { + this->test(); +} + TEST_F(Pruning, ConnectConnectForward) { + add(t, new BackwardMockup()); add(t, new GeneratorMockup()); auto c1 = add(t, new ConnectMockup({ INF, 0, 0 })); // 1st attempt is a failue add(t, new GeneratorMockup({ 0, 10, 20 })); @@ -62,6 +113,7 @@ TEST_F(Pruning, ConnectConnectForward) { } TEST_F(Pruning, ConnectConnectBackward) { + add(t, new BackwardMockup()); add(t, new GeneratorMockup({ 1, 2, 3 })); auto c1 = add(t, new ConnectMockup()); add(t, new BackwardMockup()); @@ -141,3 +193,17 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) { // the failure in one branch of Alternatives must not prune computing back EXPECT_EQ(back->runs_, 1u); } + +TEST_F(Pruning, TwoConnects) { + add(t, new GeneratorMockup({ 0 })); + add(t, new ForwardMockup({ INF })); + add(t, new ConnectMockup()); + + add(t, new GeneratorMockup()); + add(t, new ConnectMockup()); + + add(t, new GeneratorMockup()); + add(t, new ForwardMockup()); + + EXPECT_FALSE(t.plan()); +} diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 976219bed..25a938b57 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -21,7 +21,7 @@ struct StandaloneGeneratorMockup : public GeneratorMockup InterfacePtr next; StandaloneGeneratorMockup(std::initializer_list&& costs) - : StandaloneGeneratorMockup{ PredefinedCosts{ std::move(costs), true } } {} + : StandaloneGeneratorMockup{ PredefinedCosts{ costs, true } } {} StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true }) : GeneratorMockup{ std::move(costs) } { @@ -120,8 +120,8 @@ void spawnObject(PlanningScene& scene, const std::string& name, int type, void attachObject(PlanningScene& scene, const std::string& object, const std::string& link, bool attach) { moveit_msgs::msg::AttachedCollisionObject obj; obj.link_name = link; - obj.object.operation = - attach ? (int8_t)moveit_msgs::msg::CollisionObject::ADD : (int8_t)moveit_msgs::msg::CollisionObject::REMOVE; + obj.object.operation = attach ? static_cast(moveit_msgs::msg::CollisionObject::ADD) : + static_cast(moveit_msgs::msg::CollisionObject::REMOVE); obj.object.id = object; scene.processAttachedCollisionObjectMsg(obj); } diff --git a/demo/CHANGELOG.rst b/demo/CHANGELOG.rst new file mode 100644 index 000000000..56da3339c --- /dev/null +++ b/demo/CHANGELOG.rst @@ -0,0 +1,28 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_task_constructor_demo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.3 (2023-03-06) +------------------ +* Use const reference instead of reference for ros::NodeHandle (`#437 `_) +* Contributors: Robert Haschke + +0.1.2 (2023-02-24) +------------------ +* CartesianPath: Deprecate redundant property setters (`#429 `_) +* Contributors: Robert Haschke + +0.1.1 (2023-02-15) +------------------ +* Resort to MoveIt's python tools + * Provide ComputeIK.ik_frame as full PoseStamped +* Fixed build farm issues + * Fixed missing dependency declarations +* pick_place_task: monitor last state before Connect + ... to prune solutions as much as possible +* Contributors: Robert Haschke + +0.1.0 (2023-02-02) +------------------ +* Initial release +* Contributors: Michael Görner, Robert Haschke diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 442dd22b6..ea171691e 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -1,32 +1,32 @@ cmake_minimum_required(VERSION 3.5) project(moveit_task_constructor_demo) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(moveit_common REQUIRED) +moveit_package() find_package(moveit_core REQUIRED) -find_package(moveit_task_constructor_core REQUIRED) -find_package(moveit_task_constructor_msgs REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) +find_package(moveit_task_constructor_core REQUIRED) +find_package(moveit_task_constructor_msgs REQUIRED) +find_package(rclcpp REQUIRED) find_package(tf2_eigen REQUIRED) -find_package(rosparam_shortcuts REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - moveit_core - moveit_task_constructor_core - moveit_task_constructor_msgs - moveit_ros_planning - moveit_ros_planning_interface - tf2_eigen - rosparam_shortcuts) + moveit_core + moveit_ros_planning + moveit_ros_planning_interface + moveit_task_constructor_core + moveit_task_constructor_msgs + rclcpp + tf2_eigen +) add_library(${PROJECT_NAME}_pick_place_task SHARED src/pick_place_task.cpp) ament_target_dependencies(${PROJECT_NAME}_pick_place_task ${THIS_PACKAGE_INCLUDE_DEPENDS}) @@ -40,7 +40,12 @@ include_directories(include) # declare a demo consisting of a single cpp file function(demo name) add_executable(${PROJECT_NAME}_${name} src/${name}.cpp) - ament_target_dependencies(${PROJECT_NAME}_${name} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + set(parameter_filename ${name}_parameters) + if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/src/${parameter_filename}.yaml) + generate_parameter_library(${parameter_filename} src/${parameter_filename}.yaml) + target_link_libraries(${PROJECT_NAME}_${name} ${parameter_filename}) + endif() + ament_target_dependencies(${PROJECT_NAME}_${name} ${THIS_PACKAGE_INCLUDE_DEPENDS}) set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "") install(TARGETS ${PROJECT_NAME}_${name} ARCHIVE DESTINATION lib @@ -53,12 +58,14 @@ demo(cartesian) demo(modular) demo(alternative_path_costs) demo(ik_clearance_cost) +demo(fallbacks_move_to) demo(pick_place_demo) +target_link_libraries(${PROJECT_NAME}_pick_place_task pick_place_demo_parameters) target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task) install(DIRECTORY launch config - DESTINATION share/${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME} ) # TODO: Port tests diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index cbb452270..da76c2fae 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -35,7 +35,7 @@ */ // ROS -#include +#include // MoveIt #include @@ -57,12 +57,7 @@ #include #include #include - -#if __has_include() -#include -#else -#include -#endif +#include "pick_place_demo_parameters.hpp" #pragma once @@ -70,56 +65,22 @@ namespace moveit_task_constructor_demo { using namespace moveit::task_constructor; // prepare a demo environment from ROS parameters under node -void setupDemoScene(const rclcpp::Node::SharedPtr& node); +void setupDemoScene(const pick_place_task_demo::Params& params); class PickPlaceTask { public: - PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& pnh); + PickPlaceTask(const std::string& task_name); ~PickPlaceTask() = default; - bool init(); + bool init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params); - bool plan(); + bool plan(const std::size_t max_solutions); bool execute(); private: - void loadParameters(); - - rclcpp::Node::SharedPtr node_; - std::string task_name_; moveit::task_constructor::TaskPtr task_; - - // planning group properties - std::string arm_group_name_; - std::string eef_name_; - std::string hand_group_name_; - std::string hand_frame_; - - // object + surface - std::vector support_surfaces_; - std::string object_reference_frame_; - std::string surface_link_; - std::string object_name_; - std::string world_frame_; - std::vector object_dimensions_; - - // Predefined pose targets - std::string hand_open_pose_; - std::string hand_close_pose_; - std::string arm_home_pose_; - - // Pick metrics - Eigen::Isometry3d grasp_frame_transform_; - double approach_object_min_dist_; - double approach_object_max_dist_; - double lift_object_min_dist_; - double lift_object_max_dist_; - - // Place metrics - geometry_msgs::msg::Pose place_pose_; - double place_surface_offset_; }; } // namespace moveit_task_constructor_demo diff --git a/demo/launch/alternative_path_costs.launch.py b/demo/launch/alternative_path_costs.launch.py index e72e8ceb9..4e465dd49 100644 --- a/demo/launch/alternative_path_costs.launch.py +++ b/demo/launch/alternative_path_costs.launch.py @@ -1,72 +1,25 @@ -import os -import yaml from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder def generate_launch_description(): - # Get URDF and SRDF - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" - ) - - # Planning Functionality - ompl_planning_pipeline_config = { - "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, - } - } - ompl_planning_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines(pipelines=["ompl"]) + .to_moveit_configs() ) - ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) cartesian_task = Node( package="moveit_task_constructor_demo", executable="alternative_path_costs", output="screen", parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - ompl_planning_pipeline_config, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, ], ) diff --git a/demo/launch/cartesian.launch.py b/demo/launch/cartesian.launch.py index 8773ece61..fe2db9ac0 100644 --- a/demo/launch/cartesian.launch.py +++ b/demo/launch/cartesian.launch.py @@ -1,55 +1,24 @@ -import os -import yaml from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder def generate_launch_description(): - # Get URDF and SRDF - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) cartesian_task = Node( package="moveit_task_constructor_demo", executable="cartesian", output="screen", - parameters=[robot_description, robot_description_semantic, kinematics_yaml], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], ) return LaunchDescription([cartesian_task]) diff --git a/demo/launch/demo.launch.py b/demo/launch/demo.launch.py index 2756703a9..84072d8d1 100644 --- a/demo/launch/demo.launch.py +++ b/demo/launch/demo.launch.py @@ -1,96 +1,23 @@ import os -import yaml + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ExecuteProcess from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -import xacro - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder def generate_launch_description(): - # planning_context - robot_description_config = xacro.process_file( - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "panda.urdf.xacro", - ) - ) - robot_description = {"robot_description": robot_description_config.toxml()} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" - ) - - # Planning Functionality - ompl_planning_pipeline_config = { - "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, - } - } - ompl_planning_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .planning_pipelines(pipelines=["ompl"]) + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .to_moveit_configs() ) - ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) - - # Trajectory Execution Functionality - moveit_simple_controllers_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml" - ) - moveit_controllers = { - "moveit_simple_controller_manager": moveit_simple_controllers_yaml, - "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", - } - - trajectory_execution = { - "moveit_manage_controllers": True, - "trajectory_execution.allowed_execution_duration_scaling": 1.2, - "trajectory_execution.allowed_goal_duration_margin": 0.5, - "trajectory_execution.allowed_start_tolerance": 0.01, - } - - planning_scene_monitor_parameters = { - "publish_planning_scene": True, - "publish_geometry_updates": True, - "publish_state_updates": True, - "publish_transforms_updates": True, - } # Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation - move_group_capabilities = { - "capabilities": "move_group/ExecuteTaskSolutionCapability" - } + move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"} # Start the actual move_group node/action server run_move_group_node = Node( @@ -98,13 +25,7 @@ def generate_launch_description(): executable="move_group", output="screen", parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - ompl_planning_pipeline_config, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters, + moveit_config.to_dict(), move_group_capabilities, ], ) @@ -120,10 +41,10 @@ def generate_launch_description(): output="log", arguments=["-d", rviz_config_file], parameters=[ - robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, ], ) @@ -142,23 +63,20 @@ def generate_launch_description(): executable="robot_state_publisher", name="robot_state_publisher", output="both", - parameters=[robot_description], + parameters=[moveit_config.robot_description], ) # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, ros2_controllers_path], - output={ - "stdout": "screen", - "stderr": "screen", - }, + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="both", ) # Load controllers @@ -170,7 +88,7 @@ def generate_launch_description(): ]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="screen", ) diff --git a/demo/launch/fallbacks_move_to.launch.py b/demo/launch/fallbacks_move_to.launch.py new file mode 100644 index 000000000..9bd20936b --- /dev/null +++ b/demo/launch/fallbacks_move_to.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"]) + .to_moveit_configs() + ) + + print(moveit_config.planning_pipelines) + fallbacks_move_to_task = Node( + package="moveit_task_constructor_demo", + executable="fallbacks_move_to", + output="screen", + parameters=[ + moveit_config.cartesian_limits, + moveit_config.joint_limits, + moveit_config.planning_pipelines, + moveit_config.robot_description, + moveit_config.robot_description_kinematics, + moveit_config.robot_description_semantic, + ], + ) + + return LaunchDescription([fallbacks_move_to_task]) diff --git a/demo/launch/ik_clearance_cost.launch.py b/demo/launch/ik_clearance_cost.launch.py index e97f2a8ba..9d2b826db 100644 --- a/demo/launch/ik_clearance_cost.launch.py +++ b/demo/launch/ik_clearance_cost.launch.py @@ -1,55 +1,24 @@ -import os -import yaml from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder def generate_launch_description(): - # Get URDF and SRDF - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) cartesian_task = Node( package="moveit_task_constructor_demo", executable="ik_clearance_cost", output="screen", - parameters=[robot_description, robot_description_semantic, kinematics_yaml], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], ) return LaunchDescription([cartesian_task]) diff --git a/demo/launch/modular.launch.py b/demo/launch/modular.launch.py index 673dfe97e..a4bd7bb2e 100644 --- a/demo/launch/modular.launch.py +++ b/demo/launch/modular.launch.py @@ -1,55 +1,24 @@ -import os -import yaml from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder def generate_launch_description(): - # Get URDF and SRDF - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) modular_task = Node( package="moveit_task_constructor_demo", executable="modular", output="screen", - parameters=[robot_description, robot_description_semantic, kinematics_yaml], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], ) return LaunchDescription([modular_task]) diff --git a/demo/launch/pickplace.launch.py b/demo/launch/pickplace.launch.py index ebf16e91a..d332dbdb4 100644 --- a/demo/launch/pickplace.launch.py +++ b/demo/launch/pickplace.launch.py @@ -1,62 +1,19 @@ import os -import yaml + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder def generate_launch_description(): - # Get URDF and SRDF - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" - ) - - # Planning Functionality - ompl_planning_pipeline_config = { - "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, - } - } - ompl_planning_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .planning_pipelines(pipelines=["ompl"]) + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .to_moveit_configs() ) - ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) pick_place_demo = Node( package="moveit_task_constructor_demo", @@ -68,10 +25,11 @@ def generate_launch_description(): "config", "panda_config.yaml", ), - robot_description, - robot_description_semantic, - kinematics_yaml, - ompl_planning_pipeline_config, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, ], ) diff --git a/demo/package.xml b/demo/package.xml index e0578b942..d872cf1c9 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -1,22 +1,24 @@ moveit_task_constructor_demo - 0.0.1 + 0.1.3 demo tasks illustrating various capabilities of MTC. - simon Goldstein + Robert Haschke + Simon Goldstein Henning Kayser - Henning Kayser + Robert Haschke BSD ament_cmake - moveit_task_constructor_core - moveit_ros_planning_interface + generate_parameter_library moveit_core - rosparam_shortcuts - moveit_task_constructor_capabilities + moveit_ros_planning_interface + moveit_task_constructor_core + moveit_configs_utils moveit_resources_panda_moveit_config + moveit_task_constructor_capabilities ament_cmake diff --git a/demo/scripts/alternatives.py b/demo/scripts/alternatives.py new file mode 100755 index 000000000..7bdfa20e4 --- /dev/null +++ b/demo/scripts/alternatives.py @@ -0,0 +1,66 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit_commander.roscpp_initializer import roscpp_initialize +import time + +roscpp_initialize("mtc_tutorial_alternatives") + +# Use the joint interpolation planner +jointPlanner = core.JointInterpolationPlanner() + +# Create a task +task = core.Task() + +# Start from current robot state +currentState = stages.CurrentState("current state") + +# Add the current state to the task hierarchy +task.add(currentState) + +# [initAndConfigAlternatives] +# The alternatives stage supports multiple execution paths +alternatives = core.Alternatives("Alternatives") + +# goal 1 +goalConfig1 = { + "panda_joint1": 1.0, + "panda_joint2": -1.0, + "panda_joint3": 0.0, + "panda_joint4": -2.5, + "panda_joint5": 1.0, + "panda_joint6": 1.0, + "panda_joint7": 1.0, +} + +# goal 2 +goalConfig2 = { + "panda_joint1": -3.0, + "panda_joint2": -1.0, + "panda_joint3": 0.0, + "panda_joint4": -2.0, + "panda_joint5": 1.0, + "panda_joint6": 2.0, + "panda_joint7": 0.5, +} + +# First motion plan to compare +moveTo1 = stages.MoveTo("Move To Goal Configuration 1", jointPlanner) +moveTo1.group = "panda_arm" +moveTo1.setGoal(goalConfig1) +alternatives.insert(moveTo1) + +# Second motion plan to compare +moveTo2 = stages.MoveTo("Move To Goal Configuration 2", jointPlanner) +moveTo2.group = "panda_arm" +moveTo2.setGoal(goalConfig2) +alternatives.insert(moveTo2) + +# Add the alternatives stage to the task hierarchy +task.add(alternatives) +# [initAndConfigAlternatives] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py new file mode 100755 index 000000000..bd304de59 --- /dev/null +++ b/demo/scripts/cartesian.py @@ -0,0 +1,74 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from std_msgs.msg import Header +from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3 +from moveit.task_constructor import core, stages +from math import pi +import time + +from moveit_commander.roscpp_initializer import roscpp_initialize + +roscpp_initialize("mtc_tutorial") + +# [cartesianTut1] +group = "panda_arm" +# [cartesianTut1] + +# [cartesianTut2] +# Cartesian and joint-space interpolation planners +cartesian = core.CartesianPath() +jointspace = core.JointInterpolationPlanner() +# [cartesianTut2] + +# [cartesianTut3] +task = core.Task() + +# start from current robot state +task.add(stages.CurrentState("current state")) +# [cartesianTut3] + +# [initAndConfigMoveRelative] +# move along x +move = stages.MoveRelative("x +0.2", cartesian) +move.group = group +header = Header(frame_id="world") +move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0))) +task.add(move) +# [initAndConfigMoveRelative] + +# [cartesianTut4] +# move along y +move = stages.MoveRelative("y -0.3", cartesian) +move.group = group +move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0))) +task.add(move) +# [cartesianTut4] + +# [cartesianTut5] +# rotate about z +move = stages.MoveRelative("rz +45°", cartesian) +move.group = group +move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0, 0, pi / 4.0)))) +task.add(move) +# [cartesianTut5] + +# [cartesianTut6] +# Cartesian motion, defined as joint-space offset +move = stages.MoveRelative("joint offset", cartesian) +move.group = group +move.setDirection(dict(panda_joint1=pi / 6, panda_joint3=-pi / 6)) +task.add(move) +# [cartesianTut6] + +# [initAndConfigMoveTo] +# moveTo named posture, using joint-space interplation +move = stages.MoveTo("moveTo ready", jointspace) +move.group = group +move.setGoal("ready") +task.add(move) +# [initAndConfigMoveTo] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(100) diff --git a/demo/scripts/compute_ik.py b/demo/scripts/compute_ik.py new file mode 100755 index 000000000..e910991e5 --- /dev/null +++ b/demo/scripts/compute_ik.py @@ -0,0 +1,59 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped, Pose, Vector3 +from std_msgs.msg import Header +import time + +from moveit_commander.roscpp_initializer import roscpp_initialize + +roscpp_initialize("mtc_tutorial_compute_ik") + +# Specify the planning group +group = "panda_arm" + +# Create a task +task = core.Task() + +# Add a stage to retrieve the current state +task.add(stages.CurrentState("current state")) + +# Add a planning stage connecting the generator stages +planner = core.PipelinePlanner() # create default planning pipeline +task.add(stages.Connect("connect", [(group, planner)])) # operate on group +del planner # Delete PipelinePlanner when not explicitly needed anymore + +# [propertyTut12] +# Add a Cartesian pose generator +generator = stages.GeneratePose("cartesian pose") +# [propertyTut12] +# Inherit PlanningScene state from "current state" stage +generator.setMonitoredStage(task["current state"]) +# Configure target pose +# [propertyTut13] +pose = Pose(position=Vector3(z=0.2)) +generator.pose = PoseStamped(header=Header(frame_id="panda_link8"), pose=pose) +# [propertyTut13] + +# [initAndConfigComputeIk] +# Wrap Cartesian generator into a ComputeIK stage to yield a joint pose +computeIK = stages.ComputeIK("compute IK", generator) +computeIK.group = group # Use the group-specific IK solver +# Which end-effector frame should reach the target? +computeIK.ik_frame = PoseStamped(header=Header(frame_id="panda_link8")) +computeIK.max_ik_solutions = 4 # Limit the number of IK solutions +# [propertyTut14] +props = computeIK.properties +# derive target_pose from child's solution +props.configureInitFrom(core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"]) +# [propertyTut14] + +# Add the stage to the task hierarchy +task.add(computeIK) +# [initAndConfigComputeIk] + +if task.plan(): + task.publish(task.solutions[0]) + +time.sleep(1) # sleep some time to allow C++ threads to publish their messages diff --git a/demo/scripts/current_state.py b/demo/scripts/current_state.py new file mode 100755 index 000000000..e8011dbe4 --- /dev/null +++ b/demo/scripts/current_state.py @@ -0,0 +1,21 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit_commander.roscpp_initializer import roscpp_initialize +import time + +roscpp_initialize("mtc_tutorial_current_state") + +# Create a task +task = core.Task() + +# Get the current robot state +currentState = stages.CurrentState("current state") + +# Add the stage to the task hierarchy +task.add(currentState) + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/fallbacks.py b/demo/scripts/fallbacks.py new file mode 100755 index 000000000..f99016c2c --- /dev/null +++ b/demo/scripts/fallbacks.py @@ -0,0 +1,44 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit_commander.roscpp_initializer import roscpp_initialize +import time + +roscpp_initialize("mtc_tutorial_fallbacks") + +# use cartesian and joint interpolation planners +cartesianPlanner = core.CartesianPath() +jointPlanner = core.JointInterpolationPlanner() + +# initialize the mtc task +task = core.Task() + +# add the current planning scene state to the task hierarchy +currentState = stages.CurrentState("Current State") +task.add(currentState) + +# [initAndConfigFallbacks] +# create a fallback container to fall back to a different planner +# if motion generation fails with the primary one +fallbacks = core.Fallbacks("Fallbacks") + +# primary motion plan +moveTo1 = stages.MoveTo("Move To Goal Configuration 1", cartesianPlanner) +moveTo1.group = "panda_arm" +moveTo1.setGoal("extended") +fallbacks.insert(moveTo1) + +# fallback motion plan +moveTo2 = stages.MoveTo("Move To Goal Configuration 2", jointPlanner) +moveTo2.group = "panda_arm" +moveTo2.setGoal("extended") +fallbacks.insert(moveTo2) + +# add the fallback container to the task hierarchy +task.add(fallbacks) +# [initAndConfigFallbacks] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/fix_collision_objects.py b/demo/scripts/fix_collision_objects.py new file mode 100755 index 000000000..3fdd14e11 --- /dev/null +++ b/demo/scripts/fix_collision_objects.py @@ -0,0 +1,29 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit_commander.roscpp_initializer import roscpp_initialize +import time + +roscpp_initialize("mtc_tutorial_current_state") + +# Create a task +task = core.Task() + +# Add the current state to the task hierarchy +task.add(stages.CurrentState("current state")) + +# [initAndConfig] +# check for collisions and find corrections +fixCollisionObjects = stages.FixCollisionObjects("FixCollisionObjects") + +# cut off length for collision fixing +fixCollisionObjects.max_penetration = 0.01 + +# Add the stage to the task hierarchy +task.add(fixCollisionObjects) +# [initAndConfig] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/fixed_state.py b/demo/scripts/fixed_state.py new file mode 100755 index 000000000..7ada93eb8 --- /dev/null +++ b/demo/scripts/fixed_state.py @@ -0,0 +1,33 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.core import planning_scene +from moveit.task_constructor import core, stages +from moveit_commander.roscpp_initializer import roscpp_initialize +from moveit.core.planning_scene import PlanningScene +import time + +roscpp_initialize("mtc_tutorial_current_state") + + +# Create a task +task = core.Task() + +# [initAndConfigFixedState] +# Initialize a PlanningScene for use in a FixedState stage +task.loadRobotModel() # load the robot model (usually done in init()) +planningScene = PlanningScene(task.getRobotModel()) + +# Create a FixedState stage and pass the created PlanningScene as its state +fixedState = stages.FixedState("fixed state") +fixedState.setState(planningScene) + +# Add the stage to the task hierarchy +task.add(fixedState) +# [initAndConfigFixedState] + +if task.plan(): + task.publish(task.solutions[0]) + +del planningScene # Avoid ClassLoader warning by destroying the RobotModel +time.sleep(1) diff --git a/demo/scripts/generate_pose.py b/demo/scripts/generate_pose.py new file mode 100755 index 000000000..ce8aa705c --- /dev/null +++ b/demo/scripts/generate_pose.py @@ -0,0 +1,51 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped +from moveit_commander.roscpp_initializer import roscpp_initialize +import time + +roscpp_initialize("mtc_tutorial_compute_ik") + +# Specify the planning group +group = "panda_arm" + +# Create a task +task = core.Task() + +# Get the current robot state +currentState = stages.CurrentState("current state") +task.add(currentState) + +# Create a planner instance that is used to connect +# the current state to the grasp approach pose +pipelinePlanner = core.PipelinePlanner() +pipelinePlanner.planner = "RRTConnectkConfigDefault" +planners = [(group, pipelinePlanner)] + +# Connect the two stages +connect = stages.Connect("connect1", planners) +connect.properties.configureInitFrom(core.Stage.PropertyInitializerSource.PARENT) +task.add(connect) + +# [initAndConfigGeneratePose] +# create an example pose wrt. the origin of the +# panda arm link8 +pose = PoseStamped() +pose.header.frame_id = "panda_link8" + +# Calculate the inverse kinematics for the current robot state +generatePose = stages.GeneratePose("generate pose") + +# spwan a pose whenever there is a solution of the monitored stage +generatePose.setMonitoredStage(task["current state"]) +generatePose.pose = pose + +# Add the stage to the task hierarchy +task.add(generatePose) +# [initAndConfigGeneratePose] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/merger.py b/demo/scripts/merger.py new file mode 100755 index 000000000..f2d261391 --- /dev/null +++ b/demo/scripts/merger.py @@ -0,0 +1,42 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit_commander.roscpp_initializer import roscpp_initialize +import time + +roscpp_initialize("mtc_tutorial_merger") + +# use the joint interpolation planner +planner = core.JointInterpolationPlanner() + +# the task will contain our stages +task = core.Task() + +# start from current robot state +currentState = stages.CurrentState("current state") +task.add(currentState) + +# [initAndConfigMerger] +# the merger plans for two parallel execution paths +merger = core.Merger("Merger") + +# first simultaneous execution +moveTo1 = stages.MoveTo("Move To Home", planner) +moveTo1.group = "hand" +moveTo1.setGoal("close") +merger.insert(moveTo1) + +# second simultaneous execution +moveTo2 = stages.MoveTo("Move To Ready", planner) +moveTo2.group = "panda_arm" +moveTo2.setGoal("extended") +merger.insert(moveTo2) + +# add the merger stage to the task hierarchy +task.add(merger) +# [initAndConfigMerger] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/modify_planning_scene.py b/demo/scripts/modify_planning_scene.py new file mode 100755 index 000000000..ef5385da3 --- /dev/null +++ b/demo/scripts/modify_planning_scene.py @@ -0,0 +1,49 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit_msgs.msg import CollisionObject +from shape_msgs.msg import SolidPrimitive +from geometry_msgs.msg import PoseStamped +from moveit_commander.roscpp_initializer import roscpp_initialize +import time + +roscpp_initialize("mtc_tutorial_modify_planning_scene") + +# Create a task +task = core.Task() + +# Add the current state to the task hierarchy +task.add(stages.CurrentState("current state")) + +# [initAndConfigModifyPlanningScene] +# Specify object parameters +object_name = "grasp_object" +object_radius = 0.02 + +objectPose = PoseStamped() +objectPose.header.frame_id = "world" +objectPose.pose.orientation.x = 1.0 +objectPose.pose.position.x = 0.30702 +objectPose.pose.position.y = 0.0 +objectPose.pose.position.z = 0.285 + +object = CollisionObject() +object.header.frame_id = "world" +object.id = object_name +sphere = SolidPrimitive() +sphere.type = sphere.SPHERE +sphere.dimensions.insert(sphere.SPHERE_RADIUS, object_radius) + +object.primitives.append(sphere) +object.primitive_poses.append(objectPose.pose) +object.operation = object.ADD + +modifyPlanningScene = stages.ModifyPlanningScene("modify planning scene") +modifyPlanningScene.addObject(object) +task.add(modifyPlanningScene) +# [initAndConfigModifyPlanningScene] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py new file mode 100755 index 000000000..e71af604e --- /dev/null +++ b/demo/scripts/pickplace.py @@ -0,0 +1,173 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit_commander.roscpp_initializer import roscpp_initialize +from moveit.task_constructor import core, stages +from moveit_commander import PlanningSceneInterface +from geometry_msgs.msg import PoseStamped, TwistStamped +import time + +roscpp_initialize("pickplace") + +# [pickAndPlaceTut1] +# Specify robot parameters +arm = "panda_arm" +eef = "hand" +# [pickAndPlaceTut1] + +# [pickAndPlaceTut2] +# Specify object parameters +object_name = "grasp_object" +object_radius = 0.02 + +# Start with a clear planning scene +psi = PlanningSceneInterface(synchronous=True) +psi.remove_world_object() + +# [initCollisionObject] +# Grasp object properties +objectPose = PoseStamped() +objectPose.header.frame_id = "world" +objectPose.pose.orientation.x = 1.0 +objectPose.pose.position.x = 0.30702 +objectPose.pose.position.y = 0.0 +objectPose.pose.position.z = 0.285 +# [initCollisionObject] + +# Add the grasp object to the planning scene +psi.add_box(object_name, objectPose, size=[0.1, 0.05, 0.03]) +# [pickAndPlaceTut2] + +# [pickAndPlaceTut3] +# Create a task +task = core.Task("PandaPickPipelineExample") +task.enableIntrospection() +# [pickAndPlaceTut3] + +# [pickAndPlaceTut4] +# Start with the current state +task.add(stages.CurrentState("current")) + +# [initAndConfigConnect] +# Create a planner instance that is used to connect +# the current state to the grasp approach pose +pipeline = core.PipelinePlanner() +pipeline.planner = "RRTConnectkConfigDefault" +planners = [(arm, pipeline)] + +# Connect the two stages +task.add(stages.Connect("connect1", planners)) +# [initAndConfigConnect] +# [pickAndPlaceTut4] + +# [pickAndPlaceTut5] +# [initAndConfigGenerateGraspPose] +# The grasp generator spawns a set of possible grasp poses around the object +grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose") +grasp_generator.angle_delta = 0.2 +grasp_generator.pregrasp = "open" +grasp_generator.grasp = "close" +grasp_generator.setMonitoredStage(task["current"]) # Generate solutions for all initial states +# [initAndConfigGenerateGraspPose] +# [pickAndPlaceTut5] + +# [pickAndPlaceTut6] +# [initAndConfigSimpleGrasp] +# SimpleGrasp container encapsulates IK calculation of arm pose as well as finger closing +simpleGrasp = stages.SimpleGrasp(grasp_generator, "Grasp") +# Set frame for IK calculation in the center between the fingers +ik_frame = PoseStamped() +ik_frame.header.frame_id = "panda_hand" +ik_frame.pose.position.z = 0.1034 +simpleGrasp.setIKFrame(ik_frame) +# [initAndConfigSimpleGrasp] +# [pickAndPlaceTut6] + +# [pickAndPlaceTut7] +# [initAndConfigPick] +# Pick container comprises approaching, grasping (using SimpleGrasp stage), and lifting of object +pick = stages.Pick(simpleGrasp, "Pick") +pick.eef = eef +pick.object = object_name + +# Twist to approach the object +approach = TwistStamped() +approach.header.frame_id = "world" +approach.twist.linear.z = -1.0 +pick.setApproachMotion(approach, 0.03, 0.1) + +# Twist to lift the object +lift = TwistStamped() +lift.header.frame_id = "panda_hand" +lift.twist.linear.z = -1.0 +pick.setLiftMotion(lift, 0.03, 0.1) +# [pickAndPlaceTut7] + +# [pickAndPlaceTut8] +# Add the pick stage to the task's stage hierarchy +task.add(pick) +# [initAndConfigPick] +# [pickAndPlaceTut8] + +# [pickAndPlaceTut9] +# Connect the Pick stage with the following Place stage +task.add(stages.Connect("connect2", planners)) +# [pickAndPlaceTut9] + +# [pickAndPlaceTut10] +# [initAndConfigGeneratePlacePose] +# Define the pose that the object should have after placing +placePose = objectPose +placePose.pose.position.y += 0.2 # shift object by 20cm along y axis + +# Generate Cartesian place poses for the object +place_generator = stages.GeneratePlacePose("Generate Place Pose") +place_generator.setMonitoredStage(task["Pick"]) +place_generator.object = object_name +place_generator.pose = placePose +# [initAndConfigGeneratePlacePose] +# [pickAndPlaceTut10] + +# [initAndConfigSimpleUnGrasp] +# The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose +# [pickAndPlaceTut11] +simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp") +# [pickAndPlaceTut11] + +# [pickAndPlaceTut12] +# [initAndConfigPlace] +# Place container comprises placing, ungrasping, and retracting +place = stages.Place(simpleUnGrasp, "Place") +place.eef = eef +place.object = object_name +place.eef_frame = "panda_link8" +# [initAndConfigSimpleUnGrasp] + +# Twist to retract from the object +retract = TwistStamped() +retract.header.frame_id = "world" +retract.twist.linear.z = 1.0 +place.setRetractMotion(retract, 0.03, 0.1) + +# Twist to place the object +placeMotion = TwistStamped() +placeMotion.header.frame_id = "panda_hand" +placeMotion.twist.linear.z = 1.0 +place.setPlaceMotion(placeMotion, 0.03, 0.1) + +# Add the place pipeline to the task's hierarchy +task.add(place) +# [initAndConfigPlace] +# [pickAndPlaceTut12] + +# [pickAndPlaceTut13] +if task.plan(): + task.publish(task.solutions[0]) + +# avoid ClassLoader warning +del pipeline +del planners +# [pickAndPlaceTut13] + +# Prevent the program from exiting, giving you the opportunity to inspect solutions in rviz +time.sleep(3600) diff --git a/demo/scripts/properties.py b/demo/scripts/properties.py new file mode 100644 index 000000000..cffc0e0fc --- /dev/null +++ b/demo/scripts/properties.py @@ -0,0 +1,100 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped +import time + +from moveit_commander.roscpp_initializer import roscpp_initialize + +roscpp_initialize("mtc_tutorial") + +# Create a task container +task = core.Task() + +# [propertyTut10] +# Create a current state to capture the current planning scene state +currentState = stages.CurrentState("Current State") +# [propertyTut10] + +# [propertyTut1] +# Create a property +p = core.Property() + +# Set a descriptive string to describe the properties function +p.setDescription("Foo Property") +# [propertyTut1] + +# Set the current and the default value +p.setValue("Bar") + +# [propertyTut2] +# Check if the property is defined +assert p.defined() +# [propertyTut2] + +# [propertyTut3] +# Retrieve the stored value +print(p.value()) + +# Retrieve the default value +print(p.defaultValue()) + +# Retrieve the description +print(p.description()) +# [propertyTut3] + +# [propertyTut4] +# Create a property map +pm = core.PropertyMap() +props = {"prop1": "test", "prop2": 21, "prop3": PoseStamped(), "prop4": 5.4} +pm.update(props) +# [propertyTut4] + +# [propertyTut5] +# Add a property to the property map using the pythonic way +pm["prop5"] = 2 +# [propertyTut5] + +# [propertyTut6] +# Return the value of a property +print(pm["prop5"]) +# [propertyTut6] + +# [propertyTut7] +# Return the underlying property object +p2 = pm.property("prop5") +# [propertyTut7] + +# [propertyTut8] +# Iterate through all the values in the property map +print("\n") +for i in pm: + print(i, "\t\t", pm[i]) +print("\n") +# [propertyTut8] + +# [propertyTut9] +# A new property map can also be configured using an existing one +# You can also only use a subset of the properties that should be configured. +pm2 = core.PropertyMap() +pm.exposeTo(pm2, ["prop2", "prop4"]) +# [propertyTut9] + +# Lets test that by printing out our properties +for i in pm2: + print(i, "\t\t", pm2[i]) +print("\n") + +# [propertyTut11] +# Access the property map of the stage +props = currentState.properties +# [propertyTut11] + +# Add the stage to the task hierarchy +task.add(currentState) + +if task.plan(): + task.publish(task.solutions[0]) + +time.sleep(100) diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp new file mode 100644 index 000000000..a95ef42ab --- /dev/null +++ b/demo/src/fallbacks_move_to.cpp @@ -0,0 +1,131 @@ +#include + +#include +#include + +#include +#include +#include +#include +#include + +constexpr double TAU = 2 * M_PI; + +using namespace moveit::task_constructor; + +/** CurrentState -> Fallbacks( MoveTo, MoveTo, MoveTo )*/ +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("mtc_tutorial"); + std::thread spinning_thread([node] { rclcpp::spin(node); }); + + // setup Task + Task t; + t.loadRobotModel(node); + const moveit::core::RobotModelConstPtr robot{ t.getRobotModel() }; + + assert(robot->getName() == "panda"); + + // setup solvers + auto cartesian = std::make_shared(); + cartesian->setJumpThreshold(2.0); + + const auto ptp = [&node]() { + auto pp{ std::make_shared(node, "pilz_industrial_motion_planner", "PTP") }; + return pp; + }(); + + const auto rrtconnect = [&node]() { + auto pp{ std::make_shared(node, "ompl", "RRTConnectkConfigDefault") }; + return pp; + }(); + + // target state for Task + std::map target_state; + robot->getJointModelGroup("panda_arm")->getVariableDefaultPositions("ready", target_state); + target_state["panda_joint1"] = +TAU / 8; + + // define initial scenes + auto initial_scene{ std::make_shared(robot) }; + initial_scene->getCurrentStateNonConst().setToDefaultValues(robot->getJointModelGroup("panda_arm"), "ready"); + + auto initial_alternatives = std::make_unique("initial states"); + + { + // can reach target with Cartesian motion + auto fixed{ std::make_unique("current state") }; + auto scene{ initial_scene->diff() }; + scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } }); + fixed->setState(scene); + initial_alternatives->add(std::move(fixed)); + } + { + // Cartesian motion to target is impossible, but PTP is collision-free + auto fixed{ std::make_unique("current state") }; + auto scene{ initial_scene->diff() }; + scene->getCurrentStateNonConst().setVariablePositions({ + { "panda_joint1", +TAU / 8 }, + { "panda_joint4", 0 }, + }); + fixed->setState(scene); + initial_alternatives->add(std::move(fixed)); + } + { + // Cartesian and PTP motion to target would be in collision + auto fixed = std::make_unique("current state"); + auto scene{ initial_scene->diff() }; + scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } }); + scene->processCollisionObjectMsg([]() { + moveit_msgs::msg::CollisionObject co; + co.id = "box"; + co.header.frame_id = "panda_link0"; + co.operation = co.ADD; + co.pose = []() { + geometry_msgs::msg::Pose p; + p.position.x = 0.3; + p.position.y = 0.0; + p.position.z = 0.64 / 2; + p.orientation.w = 1.0; + return p; + }(); + co.primitives.push_back([]() { + shape_msgs::msg::SolidPrimitive sp; + sp.type = sp.BOX; + sp.dimensions = { 0.2, 0.05, 0.64 }; + return sp; + }()); + return co; + }()); + fixed->setState(scene); + initial_alternatives->add(std::move(fixed)); + } + + t.add(std::move(initial_alternatives)); + + // fallbacks to reach target_state + auto fallbacks = std::make_unique("move to other side"); + + auto add_to_fallbacks{ [&](auto& solver, auto& name) { + auto move_to = std::make_unique(name, solver); + move_to->setGroup("panda_arm"); + move_to->setGoal(target_state); + fallbacks->add(std::move(move_to)); + } }; + add_to_fallbacks(cartesian, "Cartesian path"); + add_to_fallbacks(ptp, "PTP path"); + add_to_fallbacks(rrtconnect, "RRT path"); + + t.add(std::move(fallbacks)); + + try { + std::cout << t << std::endl; + t.plan(); + } catch (const InitStageException& e) { + std::cout << e << std::endl; + } + + // keep alive for interactive inspection in rviz + spinning_thread.join(); + + return 0; +} diff --git a/demo/src/ik_clearance_cost.cpp b/demo/src/ik_clearance_cost.cpp index 9429368cc..3c2b80ce7 100644 --- a/demo/src/ik_clearance_cost.cpp +++ b/demo/src/ik_clearance_cost.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include "ik_clearance_cost_parameters.hpp" using namespace moveit::task_constructor; @@ -18,6 +18,9 @@ int main(int argc, char** argv) { auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo"); std::thread spinning_thread([node] { rclcpp::spin(node); }); + const auto param_listener = std::make_shared(node); + const auto params = param_listener->get_params(); + Task t; t.stages()->setName("clearance IK"); @@ -46,6 +49,7 @@ int main(int argc, char** argv) { auto initial = std::make_unique(); initial->setState(scene); + initial->properties().set("ignore_collisions", true); auto ik = std::make_unique(); ik->insert(std::move(initial)); @@ -55,10 +59,8 @@ int main(int argc, char** argv) { ik->setMaxIKSolutions(100); auto cl_cost{ std::make_unique() }; - cl_cost->cumulative = false; - rosparam_shortcuts::get(node, "cumulative", cl_cost->cumulative); // sum up pairwise distances? - cl_cost->with_world = true; - rosparam_shortcuts::get(node, "with_world", cl_cost->cumulative); // consider distance to world objects? + cl_cost->cumulative = params.cumulative; + cl_cost->with_world = params.with_world; ik->setCostTerm(std::move(cl_cost)); t.add(std::move(ik)); diff --git a/demo/src/ik_clearance_cost_parameters.yaml b/demo/src/ik_clearance_cost_parameters.yaml new file mode 100644 index 000000000..2b51c8754 --- /dev/null +++ b/demo/src/ik_clearance_cost_parameters.yaml @@ -0,0 +1,9 @@ +ik_clearance_cost_demo: + cumulative: + type: bool + default_value: false + read_only: true + with_world: + type: bool + default_value: true + read_only: true diff --git a/demo/src/pick_place_demo.cpp b/demo/src/pick_place_demo.cpp index 14a560d8d..7c45133aa 100644 --- a/demo/src/pick_place_demo.cpp +++ b/demo/src/pick_place_demo.cpp @@ -40,7 +40,7 @@ // MTC pick/place demo implementation #include -#include +#include "pick_place_demo_parameters.hpp" static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo"); @@ -51,18 +51,20 @@ int main(int argc, char** argv) { auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo", node_options); std::thread spinning_thread([node] { rclcpp::spin(node); }); - moveit_task_constructor_demo::setupDemoScene(node); + const auto param_listener = std::make_shared(node); + const auto params = param_listener->get_params(); + moveit_task_constructor_demo::setupDemoScene(params); // Construct and run pick/place task - moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", node); - if (!pick_place_task.init()) { + moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task"); + if (!pick_place_task.init(node, params)) { RCLCPP_INFO(LOGGER, "Initialization failed"); return 1; } - if (pick_place_task.plan()) { + if (pick_place_task.plan(params.max_solutions)) { RCLCPP_INFO(LOGGER, "Planning succeded"); - if (bool execute = false; rosparam_shortcuts::get(node, "execute", execute) && execute) { + if (params.execute) { pick_place_task.execute(); RCLCPP_INFO(LOGGER, "Execution complete"); } else { diff --git a/demo/src/pick_place_demo_parameters.yaml b/demo/src/pick_place_demo_parameters.yaml new file mode 100644 index 000000000..a05c52702 --- /dev/null +++ b/demo/src/pick_place_demo_parameters.yaml @@ -0,0 +1,97 @@ +pick_place_task_demo: + execute: + type: bool + default_value: false + table_name: + type: string + validation: + not_empty<>: [] + table_reference_frame: + type: string + validation: + not_empty<>: [] + table_dimensions: + type: double_array + validation: + fixed_size<>: [3] + table_pose: + type: double_array + validation: + fixed_size<>: [6] + object_name: + type: string + validation: + not_empty<>: [] + object_reference_frame: + type: string + validation: + not_empty<>: [] + object_dimensions: + type: double_array + validation: + fixed_size<>: [2] + object_pose: + type: double_array + validation: + fixed_size<>: [6] + spawn_table: + type: bool + default_value: true + max_solutions: + type: int + default_value: 10 + arm_group_name: + type: string + validation: + not_empty<>: [] + eef_name: + type: string + validation: + not_empty<>: [] + hand_group_name: + type: string + validation: + not_empty<>: [] + hand_frame: + type: string + validation: + not_empty<>: [] + hand_open_pose: + type: string + validation: + not_empty<>: [] + hand_close_pose: + type: string + validation: + not_empty<>: [] + arm_home_pose: + type: string + validation: + not_empty<>: [] + # Scene frames + world_frame: + type: string + validation: + not_empty<>: [] + surface_link: + type: string + validation: + not_empty<>: [] + grasp_frame_transform: + type: double_array + validation: + fixed_size<>: [6] + place_pose: + type: double_array + validation: + fixed_size<>: [6] + place_surface_offset: + type: double + approach_object_min_dist: + type: double + approach_object_max_dist: + type: double + lift_object_min_dist: + type: double + lift_object_max_dist: + type: double diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 43eee00cb..71c340f76 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -34,11 +34,26 @@ Desc: A demo to show MoveIt Task Constructor in action */ +#include #include -#include +#include +#include +#include "pick_place_demo_parameters.hpp" static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo"); +namespace { +Eigen::Isometry3d vectorToEigen(const std::vector& values) { + return Eigen::Translation3d(values[0], values[1], values[2]) * + Eigen::AngleAxisd(values[3], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(values[4], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(values[5], Eigen::Vector3d::UnitZ()); +} +geometry_msgs::msg::Pose vectorToPose(const std::vector& values) { + return tf2::toMsg(vectorToEigen(values)); +}; +} // namespace + namespace moveit_task_constructor_demo { void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, @@ -47,106 +62,46 @@ void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, throw std::runtime_error("Failed to spawn object: " + object.id); } -moveit_msgs::msg::CollisionObject createTable(const rclcpp::Node::SharedPtr& node) { - std::string table_name, table_reference_frame; - std::vector table_dimensions; - geometry_msgs::msg::Pose pose; - std::size_t errors = 0; - errors += !rosparam_shortcuts::get(node, "table_name", table_name); - errors += !rosparam_shortcuts::get(node, "table_reference_frame", table_reference_frame); - errors += !rosparam_shortcuts::get(node, "table_dimensions", table_dimensions); - errors += !rosparam_shortcuts::get(node, "table_pose", pose); - rosparam_shortcuts::shutdownIfError(errors); - +moveit_msgs::msg::CollisionObject createTable(const pick_place_task_demo::Params& params) { + geometry_msgs::msg::Pose pose = vectorToPose(params.table_pose); moveit_msgs::msg::CollisionObject object; - object.id = table_name; - object.header.frame_id = table_reference_frame; + object.id = params.table_name; + object.header.frame_id = params.table_reference_frame; object.primitives.resize(1); object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; - object.primitives[0].dimensions = { table_dimensions.at(0), table_dimensions.at(1), table_dimensions.at(2) }; - pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world + object.primitives[0].dimensions = { params.table_dimensions.at(0), params.table_dimensions.at(1), + params.table_dimensions.at(2) }; + pose.position.z -= 0.5 * params.table_dimensions[2]; // align surface with world object.primitive_poses.push_back(pose); return object; } -moveit_msgs::msg::CollisionObject createObject(const rclcpp::Node::SharedPtr& node) { - std::string object_name, object_reference_frame; - std::vector object_dimensions; - geometry_msgs::msg::Pose pose; - std::size_t error = 0; - error += !rosparam_shortcuts::get(node, "object_name", object_name); - error += !rosparam_shortcuts::get(node, "object_reference_frame", object_reference_frame); - error += !rosparam_shortcuts::get(node, "object_dimensions", object_dimensions); - error += !rosparam_shortcuts::get(node, "object_pose", pose); - rosparam_shortcuts::shutdownIfError(error); - +moveit_msgs::msg::CollisionObject createObject(const pick_place_task_demo::Params& params) { + geometry_msgs::msg::Pose pose = vectorToPose(params.object_pose); moveit_msgs::msg::CollisionObject object; - object.id = object_name; - object.header.frame_id = object_reference_frame; + object.id = params.object_name; + object.header.frame_id = params.object_reference_frame; object.primitives.resize(1); object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; - object.primitives[0].dimensions = { object_dimensions.at(0), object_dimensions.at(1) }; - pose.position.z += 0.5 * object_dimensions[0]; + object.primitives[0].dimensions = { params.object_dimensions.at(0), params.object_dimensions.at(1) }; + pose.position.z += 0.5 * params.object_dimensions[0]; object.primitive_poses.push_back(pose); return object; } -void setupDemoScene(const rclcpp::Node::SharedPtr& node) { +void setupDemoScene(const pick_place_task_demo::Params& params) { // Add table and object to planning scene rclcpp::sleep_for(std::chrono::microseconds(100)); // Wait for ApplyPlanningScene service moveit::planning_interface::PlanningSceneInterface psi; - if (bool spawn_table = true; rosparam_shortcuts::get(node, "spawn_table", spawn_table) && spawn_table) - spawnObject(psi, createTable(node)); - spawnObject(psi, createObject(node)); + if (params.spawn_table) + spawnObject(psi, createTable(params)); + spawnObject(psi, createObject(params)); } -PickPlaceTask::PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& node) - : node_(node), task_name_(task_name) { - loadParameters(); -} +PickPlaceTask::PickPlaceTask(const std::string& task_name) : task_name_(task_name) {} -void PickPlaceTask::loadParameters() { - /**************************************************** - * * - * Load Parameters * - * * - ***************************************************/ - RCLCPP_INFO(LOGGER, "Loading task parameters"); - - // Planning group properties - size_t errors = 0; - errors += !rosparam_shortcuts::get(node_, "arm_group_name", arm_group_name_); - errors += !rosparam_shortcuts::get(node_, "hand_group_name", hand_group_name_); - errors += !rosparam_shortcuts::get(node_, "eef_name", eef_name_); - errors += !rosparam_shortcuts::get(node_, "hand_frame", hand_frame_); - errors += !rosparam_shortcuts::get(node_, "world_frame", world_frame_); - errors += !rosparam_shortcuts::get(node_, "grasp_frame_transform", grasp_frame_transform_); - - // Predefined pose targets - errors += !rosparam_shortcuts::get(node_, "hand_open_pose", hand_open_pose_); - errors += !rosparam_shortcuts::get(node_, "hand_close_pose", hand_close_pose_); - errors += !rosparam_shortcuts::get(node_, "arm_home_pose", arm_home_pose_); - - // Target object - errors += !rosparam_shortcuts::get(node_, "object_name", object_name_); - errors += !rosparam_shortcuts::get(node_, "object_dimensions", object_dimensions_); - errors += !rosparam_shortcuts::get(node_, "object_reference_frame", object_reference_frame_); - errors += !rosparam_shortcuts::get(node_, "surface_link", surface_link_); - support_surfaces_ = { surface_link_ }; - - // Pick/Place metrics - errors += !rosparam_shortcuts::get(node_, "approach_object_min_dist", approach_object_min_dist_); - errors += !rosparam_shortcuts::get(node_, "approach_object_max_dist", approach_object_max_dist_); - errors += !rosparam_shortcuts::get(node_, "lift_object_min_dist", lift_object_min_dist_); - errors += !rosparam_shortcuts::get(node_, "lift_object_max_dist", lift_object_max_dist_); - errors += !rosparam_shortcuts::get(node_, "place_surface_offset", place_surface_offset_); - errors += !rosparam_shortcuts::get(node_, "place_pose", place_pose_); - rosparam_shortcuts::shutdownIfError(errors); -} - -bool PickPlaceTask::init() { +bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params) { RCLCPP_INFO(LOGGER, "Initializing task pipeline"); - const std::string object = object_name_; // Reset ROS introspection before constructing the new object // TODO(v4hn): global storage for Introspection services to enable one-liner @@ -155,46 +110,43 @@ bool PickPlaceTask::init() { Task& t = *task_; t.stages()->setName(task_name_); - t.loadRobotModel(node_); + t.loadRobotModel(node); // Sampling planner - auto sampling_planner = std::make_shared(node_); + auto sampling_planner = std::make_shared(node); sampling_planner->setProperty("goal_joint_tolerance", 1e-5); // Cartesian planner auto cartesian_planner = std::make_shared(); - cartesian_planner->setMaxVelocityScaling(1.0); - cartesian_planner->setMaxAccelerationScaling(1.0); + cartesian_planner->setMaxVelocityScalingFactor(1.0); + cartesian_planner->setMaxAccelerationScalingFactor(1.0); cartesian_planner->setStepSize(.01); // Set task properties - t.setProperty("group", arm_group_name_); - t.setProperty("eef", eef_name_); - t.setProperty("hand", hand_group_name_); - t.setProperty("hand_grasping_frame", hand_frame_); - t.setProperty("ik_frame", hand_frame_); + t.setProperty("group", params.arm_group_name); + t.setProperty("eef", params.eef_name); + t.setProperty("hand", params.hand_group_name); + t.setProperty("hand_grasping_frame", params.hand_frame); + t.setProperty("ik_frame", params.hand_frame); /**************************************************** * * * Current State * * * ***************************************************/ - Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator { auto current_state = std::make_unique("current state"); // Verify that object is not attached auto applicability_filter = std::make_unique("applicability test", std::move(current_state)); - applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) { + applicability_filter->setPredicate([object = params.object_name](const SolutionBase& s, std::string& comment) { if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) { comment = "object with id '" + object + "' is already attached and cannot be picked"; return false; } return true; }); - - current_state_ptr = applicability_filter.get(); t.add(std::move(applicability_filter)); } @@ -203,10 +155,12 @@ bool PickPlaceTask::init() { * Open Hand * * * ***************************************************/ + Stage* initial_state_ptr = nullptr; { // Open Hand auto stage = std::make_unique("open hand", sampling_planner); - stage->setGroup(hand_group_name_); - stage->setGoal(hand_open_pose_); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_open_pose); + initial_state_ptr = stage.get(); // remember start state for monitoring grasp pose generator t.add(std::move(stage)); } @@ -217,7 +171,7 @@ bool PickPlaceTask::init() { ***************************************************/ { // Move-to pre-grasp auto stage = std::make_unique( - "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); + "move to pick", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } }); stage->setTimeout(5.0); stage->properties().configureInitFrom(Stage::PARENT); t.add(std::move(stage)); @@ -228,7 +182,7 @@ bool PickPlaceTask::init() { * Pick Object * * * ***************************************************/ - Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator + Stage* pick_stage_ptr = nullptr; { auto grasp = std::make_unique("pick object"); t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); @@ -240,13 +194,13 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("approach object", cartesian_planner); stage->properties().set("marker_ns", "approach_object"); - stage->properties().set("link", hand_frame_); + stage->properties().set("link", params.hand_frame); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); + stage->setMinMaxDistance(params.approach_object_min_dist, params.approach_object_max_dist); // Set hand forward direction geometry_msgs::msg::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; + vec.header.frame_id = params.hand_frame; vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); @@ -260,16 +214,16 @@ bool PickPlaceTask::init() { auto stage = std::make_unique("generate grasp pose"); stage->properties().configureInitFrom(Stage::PARENT); stage->properties().set("marker_ns", "grasp_pose"); - stage->setPreGraspPose(hand_open_pose_); - stage->setObject(object); + stage->setPreGraspPose(params.hand_open_pose); + stage->setObject(params.object_name); stage->setAngleDelta(M_PI / 12); - stage->setMonitoredStage(current_state_ptr); // Hook into current state + stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions // Compute IK auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); wrapper->setMaxIKSolutions(8); wrapper->setMinSolutionDistance(1.0); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); + wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame); wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); grasp->insert(std::move(wrapper)); @@ -281,7 +235,8 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("allow collision (hand,object)"); stage->allowCollisions( - object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), + params.object_name, + t.getRobotModel()->getJointModelGroup(params.hand_group_name)->getLinkModelNamesWithCollisionGeometry(), true); grasp->insert(std::move(stage)); } @@ -291,8 +246,8 @@ bool PickPlaceTask::init() { ***************************************************/ { auto stage = std::make_unique("close hand", sampling_planner); - stage->setGroup(hand_group_name_); - stage->setGoal(hand_close_pose_); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_close_pose); grasp->insert(std::move(stage)); } @@ -301,8 +256,7 @@ bool PickPlaceTask::init() { ***************************************************/ { auto stage = std::make_unique("attach object"); - stage->attachObject(object, hand_frame_); - attach_object_stage = stage.get(); + stage->attachObject(params.object_name, params.hand_frame); grasp->insert(std::move(stage)); } @@ -311,7 +265,7 @@ bool PickPlaceTask::init() { ***************************************************/ { auto stage = std::make_unique("allow collision (object,support)"); - stage->allowCollisions({ object }, support_surfaces_, true); + stage->allowCollisions({ params.object_name }, { params.surface_link }, true); grasp->insert(std::move(stage)); } @@ -321,13 +275,13 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("lift object", cartesian_planner); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_); - stage->setIKFrame(hand_frame_); + stage->setMinMaxDistance(params.lift_object_min_dist, params.lift_object_max_dist); + stage->setIKFrame(params.hand_frame); stage->properties().set("marker_ns", "lift_object"); // Set upward direction geometry_msgs::msg::Vector3Stamped vec; - vec.header.frame_id = world_frame_; + vec.header.frame_id = params.world_frame; vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); @@ -338,10 +292,12 @@ bool PickPlaceTask::init() { ***************************************************/ { auto stage = std::make_unique("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surfaces_, false); + stage->allowCollisions({ params.object_name }, { params.surface_link }, false); grasp->insert(std::move(stage)); } + pick_stage_ptr = grasp.get(); // remember for monitoring place pose generator + // Add grasp container to task t.add(std::move(grasp)); } @@ -353,7 +309,7 @@ bool PickPlaceTask::init() { *****************************************************/ { auto stage = std::make_unique( - "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); + "move to place", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } }); stage->setTimeout(5.0); stage->properties().configureInitFrom(Stage::PARENT); t.add(std::move(stage)); @@ -375,13 +331,13 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("lower object", cartesian_planner); stage->properties().set("marker_ns", "lower_object"); - stage->properties().set("link", hand_frame_); + stage->properties().set("link", params.hand_frame); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->setMinMaxDistance(.03, .13); // Set downward direction geometry_msgs::msg::Vector3Stamped vec; - vec.header.frame_id = world_frame_; + vec.header.frame_id = params.world_frame; vec.vector.z = -1.0; stage->setDirection(vec); place->insert(std::move(stage)); @@ -395,20 +351,20 @@ bool PickPlaceTask::init() { auto stage = std::make_unique("generate place pose"); stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); stage->properties().set("marker_ns", "place_pose"); - stage->setObject(object); + stage->setObject(params.object_name); // Set target pose geometry_msgs::msg::PoseStamped p; - p.header.frame_id = object_reference_frame_; - p.pose = place_pose_; - p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; + p.header.frame_id = params.object_reference_frame; + p.pose = vectorToPose(params.place_pose); + p.pose.position.z += 0.5 * params.object_dimensions[0] + params.place_surface_offset; stage->setPose(p); - stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage + stage->setMonitoredStage(pick_stage_ptr); // hook into successful pick solutions // Compute IK auto wrapper = std::make_unique("place pose IK", std::move(stage)); wrapper->setMaxIKSolutions(2); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); + wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame); wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); place->insert(std::move(wrapper)); @@ -419,8 +375,8 @@ bool PickPlaceTask::init() { *****************************************************/ { auto stage = std::make_unique("open hand", sampling_planner); - stage->setGroup(hand_group_name_); - stage->setGoal(hand_open_pose_); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_open_pose); place->insert(std::move(stage)); } @@ -429,7 +385,8 @@ bool PickPlaceTask::init() { *****************************************************/ { auto stage = std::make_unique("forbid collision (hand,object)"); - stage->allowCollisions(object_name_, *t.getRobotModel()->getJointModelGroup(hand_group_name_), false); + stage->allowCollisions(params.object_name, *t.getRobotModel()->getJointModelGroup(params.hand_group_name), + false); place->insert(std::move(stage)); } @@ -438,7 +395,7 @@ bool PickPlaceTask::init() { *****************************************************/ { auto stage = std::make_unique("detach object"); - stage->detachObject(object_name_, hand_frame_); + stage->detachObject(params.object_name, params.hand_frame); place->insert(std::move(stage)); } @@ -449,10 +406,10 @@ bool PickPlaceTask::init() { auto stage = std::make_unique("retreat after place", cartesian_planner); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->setMinMaxDistance(.12, .25); - stage->setIKFrame(hand_frame_); + stage->setIKFrame(params.hand_frame); stage->properties().set("marker_ns", "retreat"); geometry_msgs::msg::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; + vec.header.frame_id = params.hand_frame; vec.vector.z = -1.0; stage->setDirection(vec); place->insert(std::move(stage)); @@ -470,7 +427,7 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("move home", sampling_planner); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setGoal(arm_home_pose_); + stage->setGoal(params.arm_home_pose); stage->restrictDirection(stages::MoveTo::FORWARD); t.add(std::move(stage)); } @@ -486,12 +443,10 @@ bool PickPlaceTask::init() { return true; } -bool PickPlaceTask::plan() { +bool PickPlaceTask::plan(const std::size_t max_solutions) { RCLCPP_INFO(LOGGER, "Start searching for task solutions"); - int max_solutions = 10; - rosparam_shortcuts::get(node_, "max_solutions", max_solutions); - return task_->plan(max_solutions); + return static_cast(task_->plan(max_solutions)); } bool PickPlaceTask::execute() { @@ -502,8 +457,8 @@ bool PickPlaceTask::execute() { // // If you want to inspect the goal message, use this instead: // actionlib::SimpleActionClient // execute("execute_task_solution", true); execute.waitForServer(); - // moveit_task_constructor_msgs::msg::ExecuteTaskSolutionGoal execute_goal; - // task_->solutions().front()->fillMessage(execute_goal.solution); + // moveit_task_constructor_msgs::msg::ExecuteTaskSolution::Goal execute_goal; + // task_->solutions().front()->toMsg(execute_goal.solution); // execute.sendGoalAndWait(execute_goal); // execute_result = execute.getResult()->error_code; diff --git a/demo/test/CMakeLists.txt b/demo/test/CMakeLists.txt index fc5402414..6e5529a73 100644 --- a/demo/test/CMakeLists.txt +++ b/demo/test/CMakeLists.txt @@ -3,9 +3,8 @@ ############# ## Add gtest based cpp test target and link libraries -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - - add_rostest_gtest(pick_place_test pick_place.test pick_place_test.cpp) - target_link_libraries(pick_place_test ${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES}) +if (BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(pick_place_test pick_place_test.cpp) + target_link_libraries(pick_place_test ${PROJECT_NAME}_pick_place_task) endif() diff --git a/doc/MIGRATION_GUIDE.md b/doc/MIGRATION_GUIDE.md deleted file mode 100644 index c208ee2fa..000000000 --- a/doc/MIGRATION_GUIDE.md +++ /dev/null @@ -1,6 +0,0 @@ -# Migration Guide from ROS1 - -* Default C++ standard set to 17 -* CMake version set to 3.5 -* PipelinePlanner's constructor now have a node `rclcpp::Node::SharedPtr` as an argument to load the `PlanningPipeline`'s parameters -* `Task::loadRobotModel` have a node as a parameter and the user have to call loadRobotModel explicitly otherwise init will throw an exception diff --git a/msgs/CHANGELOG.rst b/msgs/CHANGELOG.rst new file mode 100644 index 000000000..945dd4b93 --- /dev/null +++ b/msgs/CHANGELOG.rst @@ -0,0 +1,17 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_task_constructor_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.3 (2023-03-06) +------------------ + +0.1.2 (2023-02-24) +------------------ + +0.1.1 (2023-02-15) +------------------ + +0.1.0 (2023-02-02) +------------------ +* Initial release +* Contributors: Michael Görner, Robert Haschke diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index 86c991753..9e56f27ae 100644 --- a/msgs/CMakeLists.txt +++ b/msgs/CMakeLists.txt @@ -3,8 +3,8 @@ project(moveit_task_constructor_msgs) find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) -find_package(rosidl_default_generators REQUIRED) find_package(moveit_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) find_package(visualization_msgs REQUIRED) # ROS messages, services and actions @@ -32,11 +32,11 @@ set(action_files rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} - ${action_files} + ${action_files} DEPENDENCIES - builtin_interfaces - moveit_msgs - visualization_msgs + builtin_interfaces + moveit_msgs + visualization_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/msgs/msg/SolutionInfo.msg b/msgs/msg/SolutionInfo.msg index c3346ab53..5689205c2 100644 --- a/msgs/msg/SolutionInfo.msg +++ b/msgs/msg/SolutionInfo.msg @@ -10,5 +10,8 @@ string comment # id of stage that created this trajectory uint32 stage_id +# name of the planner that created this solution +string planner_id + # markers, e.g. providing additional hints or illustrating failure visualization_msgs/Marker[] markers diff --git a/msgs/package.xml b/msgs/package.xml index b9ff68c86..207af1c0a 100644 --- a/msgs/package.xml +++ b/msgs/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_msgs - 0.0.0 + 0.1.3 Messages for MoveIt Task Pipeline BSD diff --git a/rviz_marker_tools/CHANGELOG.rst b/rviz_marker_tools/CHANGELOG.rst new file mode 100644 index 000000000..584c508a2 --- /dev/null +++ b/rviz_marker_tools/CHANGELOG.rst @@ -0,0 +1,19 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rviz_marker_tools +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.3 (2023-03-06) +------------------ + +0.1.2 (2023-02-24) +------------------ +* Fix marker creation: allow zero scale for geometric shapes (`#430 `_) +* Contributors: Robert Haschke + +0.1.1 (2023-02-15) +------------------ + +0.1.0 (2023-02-02) +------------------ +* Initial release +* Contributors: Robert Haschke, Michael Görner diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt index 19b67eaa7..f10d3e8af 100644 --- a/rviz_marker_tools/CMakeLists.txt +++ b/rviz_marker_tools/CMakeLists.txt @@ -1,32 +1,36 @@ cmake_minimum_required(VERSION 3.5) project(rviz_marker_tools) -find_package(geometry_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rviz2 REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(tf2_eigen REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(moveit_common REQUIRED) +moveit_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + eigen3_cmake_module + Eigen3 + geometry_msgs + rclcpp + rviz2 + tf2_eigen + urdfdom_headers + visualization_msgs +) + +foreach(dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${dependency} REQUIRED) +endforeach() if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 17) endif() -set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}) - -set(HEADERS - ${PROJECT_INCLUDE}/marker_creation.h -) - add_library(${PROJECT_NAME} SHARED - ${HEADERS} src/marker_creation.cpp ) -ament_target_dependencies(${PROJECT_NAME} visualization_msgs geometry_msgs Eigen3) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_include_directories(${PROJECT_NAME} - PUBLIC + PUBLIC $ $ ) @@ -34,17 +38,10 @@ target_include_directories(${PROJECT_NAME} install(DIRECTORY include/ DESTINATION include) install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) -ament_export_dependencies(eigen3_cmake_module) -ament_export_dependencies(Eigen3) -ament_export_dependencies(geometry_msgs) -ament_export_dependencies(visualization_msgs) -ament_export_dependencies(rclcpp) -ament_export_dependencies(rviz2) -ament_export_dependencies(tf2_eigen) + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) + +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml index c53924e02..54f32e818 100644 --- a/rviz_marker_tools/package.xml +++ b/rviz_marker_tools/package.xml @@ -1,9 +1,10 @@ rviz_marker_tools - 0.0.0 + 0.1.3 Tools for marker creation / handling BSD + Robert Haschke Robert Haschke ament_cmake @@ -11,11 +12,13 @@ eigen3_cmake_module eigen - visualization_msgs geometry_msgs + moveit_common rclcpp rviz2 tf2_eigen + urdfdom_headers + visualization_msgs ament_cmake diff --git a/rviz_marker_tools/src/marker_creation.cpp b/rviz_marker_tools/src/marker_creation.cpp index 7221899de..3307bfd62 100644 --- a/rviz_marker_tools/src/marker_creation.cpp +++ b/rviz_marker_tools/src/marker_creation.cpp @@ -165,12 +165,6 @@ void prepareMarker(vm::msg::Marker& m, int marker_type) { m.points.clear(); m.colors.clear(); - // ensure valid scale - if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0) { - m.scale.x = 1.0; - m.scale.y = 1.0; - m.scale.z = 1.0; - } // ensure non-null orientation if (m.pose.orientation.w == 0 && m.pose.orientation.x == 0 && m.pose.orientation.y == 0 && m.pose.orientation.z == 0) m.pose.orientation.w = 1.0; @@ -195,6 +189,7 @@ vm::msg::Marker& makeXYPlane(vm::msg::Marker& m) { p[3].y = -1.0; p[3].z = 0.0; + m.scale.x = m.scale.y = m.scale.z = 1.0; prepareMarker(m, vm::msg::Marker::TRIANGLE_LIST); m.points.push_back(p[0]); m.points.push_back(p[1]); @@ -224,6 +219,7 @@ vm::msg::Marker& makeYZPlane(vm::msg::Marker& m) { /// create a cone of given angle along the x-axis vm::msg::Marker makeCone(double angle, vm::msg::Marker& m) { + m.scale.x = m.scale.y = m.scale.z = 1.0; prepareMarker(m, vm::msg::Marker::TRIANGLE_LIST); geometry_msgs::msg::Point p[3]; p[0].x = p[0].y = p[0].z = 0.0; @@ -303,6 +299,7 @@ vm::msg::Marker& makeArrow(vm::msg::Marker& m, double scale, bool tip_at_origin) } vm::msg::Marker& makeText(vm::msg::Marker& m, const std::string& text) { + m.scale.x = m.scale.y = m.scale.z = 1.0; prepareMarker(m, vm::msg::Marker::TEXT_VIEW_FACING); m.text = text; return m; diff --git a/visualization/CHANGELOG.rst b/visualization/CHANGELOG.rst new file mode 100644 index 000000000..5c492add0 --- /dev/null +++ b/visualization/CHANGELOG.rst @@ -0,0 +1,19 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_task_constructor_visualization +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.3 (2023-03-06) +------------------ + +0.1.2 (2023-02-24) +------------------ + +0.1.1 (2023-02-15) +------------------ +* Remove unused eigen_conversions includes +* Contributors: Robert Haschke + +0.1.0 (2023-02-02) +------------------ +* Initial release +* Contributors: Robert Haschke, Michael Görner diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index a1d692d2e..50e849561 100644 --- a/visualization/CMakeLists.txt +++ b/visualization/CMakeLists.txt @@ -3,6 +3,8 @@ project(moveit_task_constructor_visualization) find_package(ament_cmake REQUIRED) find_package(Boost REQUIRED) +find_package(moveit_common REQUIRED) +moveit_package() find_package(moveit_core REQUIRED) find_package(moveit_ros_visualization REQUIRED) find_package(moveit_task_constructor_core REQUIRED) @@ -27,7 +29,7 @@ set(CMAKE_AUTOMOC ON) set(CMAKE_AUTORCC ON) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 17) endif() add_subdirectory(visualization_tools) diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt index d1351ab27..f65d69862 100644 --- a/visualization/motion_planning_tasks/properties/CMakeLists.txt +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -23,8 +23,8 @@ target_include_directories(${MOVEIT_LIB_NAME} PRIVATE ${YAML_INCLUDE_DIRS} ) ament_target_dependencies(${MOVEIT_LIB_NAME} - moveit_task_constructor_core - rviz_common + moveit_task_constructor_core + rviz_common ) install(TARGETS ${MOVEIT_LIB_NAME} diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 5d6079441..3c3af2a10 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -149,7 +149,7 @@ void PropertyFactory::addRemainingProperties(rviz_common::properties::Property* } #ifndef HAVE_YAML -rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, +rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/, const std::string& description, const std::string& value, rviz_common::properties::Property* old) { diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index bfe4be299..ddd923200 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -99,7 +99,7 @@ QModelIndex LocalTaskModel::index(int row, int column, const QModelIndex& parent Q_ASSERT(dynamic_cast(node(parent))); ContainerBase* p = static_cast(node(parent)); - if (!p || row < 0 || (size_t)row >= p->numChildren()) + if (!p || row < 0 || static_cast(row) >= p->numChildren()) return QModelIndex(); Node* child = nullptr; @@ -149,7 +149,7 @@ QVariant LocalTaskModel::data(const QModelIndex& index, int role) const { case 0: return QString::fromStdString(n->name()); case 1: - return (uint)n->solutions().size(); + return static_cast(n->solutions().size()); case 2: return 0; } @@ -180,7 +180,7 @@ bool LocalTaskModel::removeRows(int row, int count, const QModelIndex& parent) { Q_ASSERT(dynamic_cast(node(parent))); ContainerBase* c = static_cast(node(parent)); - if (row < 0 || (size_t)row + count > c->numChildren()) + if (row < 0 || static_cast(row + count) > c->numChildren()) return false; beginRemoveRows(parent, row, row + count - 1); diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index de4057b4d..613c58ceb 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -159,7 +159,7 @@ RemoteTaskModel::Node* RemoteTaskModel::node(const QModelIndex& index) const { // internal pointer refers to parent node Node* parent = static_cast(index.internalPointer()); - Q_ASSERT(index.row() >= 0 && (size_t)index.row() < parent->children_.size()); + Q_ASSERT(index.row() >= 0 && static_cast(index.row()) < parent->children_.size()); return parent->children_.at(index.row()).get(); } @@ -218,7 +218,7 @@ QModelIndex RemoteTaskModel::index(int row, int column, const QModelIndex& paren return QModelIndex(); Node* p = node(parent); - if (!p || row < 0 || (size_t)row >= p->children_.size()) + if (!p || row < 0 || static_cast(row) >= p->children_.size()) return QModelIndex(); p->children_[row]->node_flags_ |= WAS_VISITED; diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 53d716aa9..582ec462b 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -80,7 +80,7 @@ TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_d task_solution_topic_property_ = new rviz_common::properties::RosTopicProperty( "Task Solution Topic", "", rosidl_generator_traits::data_type(), - "The topic on which task solutions (moveit_msgs::Solution messages) are received", this, + "The topic on which task solutions (moveit_msgs::msg::Solution messages) are received", this, SLOT(changedTaskSolutionTopic()), this); trajectory_visual_.reset(new TaskSolutionVisualization(this, this)); @@ -198,7 +198,7 @@ void TaskDisplay::changedRobotDescription() { loadRobotModel(); } -void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr msg) { +void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr& msg) { setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK"); requestPanel(); task_list_model_->processTaskDescriptionMessage(*msg, base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id); @@ -222,12 +222,12 @@ void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::msg::Tas } } -void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr msg) { +void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr& msg) { setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK"); task_list_model_->processTaskStatisticsMessage(*msg); } -void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr msg) { +void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr& msg) { setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK"); try { const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg); diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index f91587c2c..e1b5015fc 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -116,9 +116,9 @@ private Q_SLOTS: void onTasksRemoved(const QModelIndex& parent, int first, int last); void onTaskDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight); - void taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr msg); - void taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr msg); - void taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr msg); + void taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr& msg); + void taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr& msg); + void taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr& msg); protected: /** @brief A Node which is registered with the main executor (used in the "update" thread). diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 21a542612..3cd646690 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -295,7 +295,7 @@ void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_m DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg) { auto it = remote_tasks_.find(msg.task_id); if (it == remote_tasks_.cend()) - return DisplaySolutionPtr(); // unkown task + return DisplaySolutionPtr(); // unknown task RemoteTaskModel* remote_task = it->second; if (!remote_task) diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 3c829f4fb..356f947bd 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -91,8 +91,13 @@ TaskPanel::TaskPanel(QWidget* parent) : rviz_common::Panel(parent), d_ptr(new Ta Q_D(TaskPanel); // sync checked tool button with displayed widget +#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0) + connect(d->tool_buttons_group, static_cast(&QButtonGroup::idClicked), d->stackedWidget, + [d](int index) { d->stackedWidget->setCurrentIndex(index); }); +#else connect(d->tool_buttons_group, static_cast(&QButtonGroup::buttonClicked), d->stackedWidget, [d](int index) { d->stackedWidget->setCurrentIndex(index); }); +#endif connect(d->stackedWidget, &QStackedWidget::currentChanged, d->tool_buttons_group, [d](int index) { d->tool_buttons_group->button(index)->setChecked(true); }); @@ -176,7 +181,7 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) { tool_buttons_group = new QButtonGroup(panel); tool_buttons_group->setExclusive(true); button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); - button_show_stage_dock_widget->setToolTip(QStringLiteral("Show available stages")); + button_show_stage_dock_widget->setToolTip("Show available stages"); property_root = new rviz_common::properties::Property("Global Settings"); } @@ -235,7 +240,7 @@ TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view) { meta_model->setMimeTypes({ factory->mimeType() }); tasks_view->setModel(meta_model); QObject::connect(meta_model, SIGNAL(rowsInserted(QModelIndex, int, int)), q_ptr, - SLOT(_q_configureInsertedModels(QModelIndex, int, int))); + SLOT(configureInsertedModels(QModelIndex, int, int))); tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection); tasks_view->setAcceptDrops(true); @@ -271,7 +276,7 @@ void TaskViewPrivate::configureExistingModels() { configureTaskListModel(meta_model->getTaskListModel(meta_model->index(row, 0)).first); } -void TaskViewPrivate::_q_configureInsertedModels(const QModelIndex& parent, int first, int last) { +void TaskViewPrivate::configureInsertedModels(const QModelIndex& parent, int first, int last) { if (parent.isValid() && !parent.parent().isValid()) { // top-level task items inserted int expand = q_ptr->initial_task_expand->getOptionInt(); for (int row = first; row <= last; ++row) { @@ -551,7 +556,7 @@ void TaskView::onExecCurrentSolution() const { RCLCPP_ERROR(LOGGER, "send goal call failed"); return; } - auto goal_handle = goal_handle_future.get(); + const auto& goal_handle = goal_handle_future.get(); if (!goal_handle) RCLCPP_ERROR(LOGGER, "Goal was rejected by server"); } diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index efd95d263..68c6462d9 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -158,7 +158,7 @@ protected Q_SLOTS: void onOldTaskHandlingChanged(); private: - Q_PRIVATE_SLOT(d_ptr, void _q_configureInsertedModels(QModelIndex, int, int)); + Q_PRIVATE_SLOT(d_ptr, void configureInsertedModels(QModelIndex, int, int)); Q_SIGNALS: void oldTaskHandlingChanged(int old_task_handling); diff --git a/visualization/motion_planning_tasks/src/task_panel_p.h b/visualization/motion_planning_tasks/src/task_panel_p.h index 2d9885a7f..1246ce334 100644 --- a/visualization/motion_planning_tasks/src/task_panel_p.h +++ b/visualization/motion_planning_tasks/src/task_panel_p.h @@ -82,9 +82,8 @@ class TaskViewPrivate : public Ui_TaskView void configureTaskListModel(TaskListModel* model); /// configure all TaskListModels that were already created when TaskView gets instantiated void configureExistingModels(); - // NOLINTNEXTLINE(readability-identifier-naming) /// configure newly inserted models - void _q_configureInsertedModels(const QModelIndex& parent, int first, int last); + void configureInsertedModels(const QModelIndex& parent, int first, int last); /// unlock locked_display_ if given display is different void lock(TaskDisplay* display); diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 53f625ada..2e0721609 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -44,6 +44,7 @@ #include #include #include +#include using namespace moveit::task_constructor; @@ -224,12 +225,19 @@ TEST_F(TaskListModelTest, deletion) { // process deleteLater() events QCoreApplication::sendPostedEvents(nullptr, QEvent::DeferredDelete); // as m is owned by model, m should be destroyed - // EXPECT_EQ(num_deletes, 1); // TODO: event is not processed, missing event loop? + EXPECT_EQ(num_deletes, 1); EXPECT_EQ(model.rowCount(), 0); } int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); - return RUN_ALL_TESTS(); + QCoreApplication app(argc, argv); + // https://bugs.llvm.org/show_bug.cgi?id=40367 + // NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks) + QTimer::singleShot(0, [&]() { + ::testing::InitGoogleTest(&argc, argv); + auto test_result = RUN_ALL_TESTS(); + app.exit(test_result); + }); + return app.exec(); } diff --git a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp index a8453d8ae..a8c63f642 100644 --- a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp @@ -415,11 +415,11 @@ bool FlatMergeProxyModel::insertModel(QAbstractItemModel* model, int pos) { return false; // all models must have same column count // limit pos to range [0, modelCount()] - if (pos > 0 && pos > (int)modelCount()) + if (pos > 0 && pos > static_cast(modelCount())) pos = modelCount(); if (pos < 0) pos = modelCount() + std::max(pos + 1, -modelCount()); - Q_ASSERT(pos >= 0 && pos <= (int)modelCount()); + Q_ASSERT(pos >= 0 && pos <= static_cast(modelCount())); auto it = d_ptr->data_.begin(); std::advance(it, pos); @@ -467,6 +467,7 @@ std::pair FlatMergeProxyModel::getModel(const const QModelIndex& src_index = d_ptr->mapToSource(index, data); Q_ASSERT(data); + // NOLINTNEXTLINE(clang-analyzer-core.NonNullParamChecker) return std::make_pair(data->model_, src_index); } @@ -480,9 +481,9 @@ bool FlatMergeProxyModel::removeModel(int pos) { pos = modelCount() + pos + 1; if (pos < 0) return false; - if (pos >= (int)modelCount()) + if (pos >= static_cast(modelCount())) return false; - Q_ASSERT(pos >= 0 && pos < (int)modelCount()); + Q_ASSERT(pos >= 0 && pos < static_cast(modelCount())); auto it = d_ptr->data_.begin(); std::advance(it, pos); diff --git a/visualization/motion_planning_tasks/utils/icon.cpp b/visualization/motion_planning_tasks/utils/icon.cpp index 2296d7201..0ee0db95c 100644 --- a/visualization/motion_planning_tasks/utils/icon.cpp +++ b/visualization/motion_planning_tasks/utils/icon.cpp @@ -55,7 +55,7 @@ static QPixmap maskToColorAndAlpha(const QPixmap& mask, const QColor& color) { using MaskAndColor = QPair; using MasksAndColors = QList; -static MasksAndColors masksAndColors(const Icon& icon, int dpr) { +static MasksAndColors masksAndColors(const Icon& icon, int /*dpr*/) { MasksAndColors result; for (const IconMaskAndColor& i : icon) { const QString& file_name = i.first; diff --git a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp index f57023ed6..2f54f9c0b 100644 --- a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp @@ -269,7 +269,7 @@ QModelIndex TreeMergeProxyModel::index(int row, int column, const QModelIndex& p return QModelIndex(); if (!parent.isValid()) { // top-level / group items - if ((size_t)row >= d_ptr->data_.size()) + if (static_cast(row) >= d_ptr->data_.size()) return QModelIndex(); // for group items, internal pointer refers to this model @@ -397,11 +397,11 @@ bool TreeMergeProxyModel::insertModel(const QString& name, QAbstractItemModel* m return false; // model can only inserted once // limit pos to range [0, modelCount()] - if (pos > 0 && pos > (int)modelCount()) + if (pos > 0 && pos > static_cast(modelCount())) pos = modelCount(); if (pos < 0) pos = modelCount() + std::max(pos + 1, -modelCount()); - Q_ASSERT(pos >= 0 && pos <= (int)modelCount()); + Q_ASSERT(pos >= 0 && pos <= static_cast(modelCount())); auto it = d_ptr->data_.begin(); std::advance(it, pos); @@ -440,6 +440,7 @@ std::pair TreeMergeProxyModel::getModel(const const QModelIndex& src_index = d_ptr->mapToSource(index, data); Q_ASSERT(data); + // NOLINTNEXTLINE(clang-analyzer-core.NonNullParamChecker) return std::make_pair(data->model_, src_index); } @@ -459,9 +460,9 @@ bool TreeMergeProxyModel::removeModel(int pos) { pos = modelCount() + pos + 1; if (pos < 0) return false; - if (pos >= (int)modelCount()) + if (pos >= static_cast(modelCount())) return false; - Q_ASSERT(pos >= 0 && pos < (int)modelCount()); + Q_ASSERT(pos >= 0 && pos < static_cast(modelCount())); auto it = d_ptr->data_.begin(); std::advance(it, pos); diff --git a/visualization/package.xml b/visualization/package.xml index 088389009..8a6467fdd 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -1,9 +1,10 @@ moveit_task_constructor_visualization - 0.0.0 + 0.1.3 Visualization tools for MoveIt Task Pipeline BSD + Robert Haschke Robert Haschke Michael Goerner diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index 8c35ec20a..0ae4deb76 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -23,7 +23,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} rviz_ogre_vendor::OgreMain ) target_include_directories(${MOVEIT_LIB_NAME} - PUBLIC include + PUBLIC include ) ament_target_dependencies(${MOVEIT_LIB_NAME} Boost diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp index e7a88cfb0..82bdd7836 100644 --- a/visualization/visualization_tools/src/marker_visualization.cpp +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -74,7 +74,7 @@ bool MarkerVisualization::createMarkers(rviz_common::DisplayContext* context, Og // fetch transform from planning_frame_ to rviz' fixed frame const std::string& fixed_frame = context->getFrameManager()->getFixedFrame(); Ogre::Quaternion quat; - Ogre::Vector3 pos; + Ogre::Vector3 pos = Ogre::Vector3::ZERO; try { auto tf_wrapper = std::dynamic_pointer_cast( @@ -157,7 +157,7 @@ void MarkerVisualization::update(MarkerData& data, const planning_scene::Plannin auto frame_it = ns_it->second.frames_.find(marker.header.frame_id); Q_ASSERT(frame_it != ns_it->second.frames_.end()); // we have created all of them - const Eigen::Quaterniond q = (Eigen::Quaterniond)pose.linear(); + const Eigen::Quaterniond q{ pose.linear() }; const Eigen::Vector3d& p = pose.translation(); frame_it->second->setOrientation(Ogre::Quaternion(q.w(), q.x(), q.y(), q.z())); frame_it->second->setPosition(Ogre::Vector3(p.x(), p.y(), p.z())); diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index d5af657ad..b53d4b4f8 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -257,7 +257,7 @@ void TaskSolutionVisualization::clearTrail() { void TaskSolutionVisualization::changedLoopDisplay() { // restart animation if current_state_ is at end and looping got activated if (displaying_solution_ && loop_display_property_->getBool() && slider_panel_ && slider_panel_->isVisible() && - current_state_ + 1 >= (int)displaying_solution_->getWayPointCount()) { + current_state_ + 1 >= static_cast(displaying_solution_->getWayPointCount())) { current_state_ = -1; slider_panel_->pauseButton(false); } @@ -482,7 +482,7 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) auto idx_pair = displaying_solution_->indexPair(index); scene = displaying_solution_->scene(idx_pair); - if (previous_index < 0 || previous_index >= (int)waypoint_count || + if (previous_index < 0 || previous_index >= static_cast(waypoint_count) || displaying_solution_->indexPair(previous_index).first != idx_pair.first) { // switch to new stage: show new planning scene renderPlanningScene(scene);