├── .clang-format ├── .clang-tidy ├── .github └── workflows │ ├── ci.yaml │ ├── format.yaml │ ├── lsan.suppressions │ └── prerelease.yaml ├── .gitignore ├── .gitmodules ├── .pre-commit-config.yaml ├── LICENSE.txt ├── README.md ├── capabilities ├── CHANGELOG.rst ├── CMakeLists.txt ├── capabilities_plugin_description.xml ├── package.xml └── src │ ├── execute_task_solution_capability.cpp │ └── execute_task_solution_capability.h ├── codecov.yaml ├── core ├── CHANGELOG.rst ├── CMakeLists.txt ├── cmake │ └── pybind11.cmake.in ├── doc │ ├── Doxyfile │ ├── _templates │ │ ├── custom-class-template.rst │ │ └── custom-module-template.rst │ ├── api.rst │ ├── basics.rst │ ├── concepts.rst │ ├── conf.py │ ├── howto.rst │ ├── index.rst │ ├── requirements.txt │ ├── troubleshooting.rst │ └── tutorials │ │ ├── cartesian.rst │ │ ├── first-steps.rst │ │ ├── index.rst │ │ ├── pick-and-place.rst │ │ └── properties.rst ├── include │ └── moveit │ │ ├── python │ │ └── task_constructor │ │ │ └── properties.h │ │ └── task_constructor │ │ ├── container.h │ │ ├── container_p.h │ │ ├── cost_queue.h │ │ ├── cost_terms.h │ │ ├── fmt_p.h │ │ ├── introspection.h │ │ ├── marker_tools.h │ │ ├── merge.h │ │ ├── moveit_compat.h │ │ ├── properties.h │ │ ├── solvers.h │ │ ├── solvers │ │ ├── cartesian_path.h │ │ ├── joint_interpolation.h │ │ ├── multi_planner.h │ │ ├── pipeline_planner.h │ │ └── planner_interface.h │ │ ├── stage.h │ │ ├── stage_p.h │ │ ├── stages.h │ │ ├── stages │ │ ├── compute_ik.h │ │ ├── connect.h │ │ ├── current_state.h │ │ ├── fix_collision_objects.h │ │ ├── fixed_cartesian_poses.h │ │ ├── fixed_state.h │ │ ├── generate_grasp_pose.h │ │ ├── generate_place_pose.h │ │ ├── generate_pose.h │ │ ├── generate_random_pose.h │ │ ├── modify_planning_scene.h │ │ ├── move_relative.h │ │ ├── move_to.h │ │ ├── noop.h │ │ ├── passthrough.h │ │ ├── pick.h │ │ ├── predicate_filter.h │ │ └── simple_grasp.h │ │ ├── storage.h │ │ ├── task.h │ │ ├── task_p.h │ │ ├── trajectory_execution_info.h │ │ ├── type_traits.h │ │ └── utils.h ├── motion_planning_stages_plugin_description.xml ├── package.xml ├── python │ ├── CMakeLists.txt │ ├── bindings │ │ ├── CMakeLists.txt │ │ └── src │ │ │ ├── core.cpp │ │ │ ├── core.h │ │ │ ├── module.cpp │ │ │ ├── properties.cpp │ │ │ ├── solvers.cpp │ │ │ ├── stages.cpp │ │ │ ├── stages.h │ │ │ └── utils.h │ ├── src │ │ └── moveit │ │ │ ├── __init__.py │ │ │ └── task_constructor │ │ │ ├── __init__.py │ │ │ ├── core.py │ │ │ └── stages.py │ └── test │ │ ├── rostest_mps.py │ │ ├── rostest_mtc.py │ │ ├── rostest_mtc.test │ │ ├── rostest_trampoline.py │ │ └── test_mtc.py ├── rosdoc.yaml ├── setup.py ├── src │ ├── CMakeLists.txt │ ├── container.cpp │ ├── cost_terms.cpp │ ├── introspection.cpp │ ├── marker_tools.cpp │ ├── merge.cpp │ ├── properties.cpp │ ├── solvers │ │ ├── cartesian_path.cpp │ │ ├── joint_interpolation.cpp │ │ ├── multi_planner.cpp │ │ ├── pipeline_planner.cpp │ │ └── planner_interface.cpp │ ├── stage.cpp │ ├── stages │ │ ├── CMakeLists.txt │ │ ├── compute_ik.cpp │ │ ├── connect.cpp │ │ ├── current_state.cpp │ │ ├── fix_collision_objects.cpp │ │ ├── fixed_cartesian_poses.cpp │ │ ├── fixed_state.cpp │ │ ├── generate_grasp_pose.cpp │ │ ├── generate_place_pose.cpp │ │ ├── generate_pose.cpp │ │ ├── generate_random_pose.cpp │ │ ├── modify_planning_scene.cpp │ │ ├── move_relative.cpp │ │ ├── move_to.cpp │ │ ├── passthrough.cpp │ │ ├── pick.cpp │ │ ├── plugins.cpp │ │ ├── predicate_filter.cpp │ │ └── simple_grasp.cpp │ ├── storage.cpp │ ├── task.cpp │ └── utils.cpp └── test │ ├── CMakeLists.txt │ ├── gtest_value_printers.cpp │ ├── gtest_value_printers.h │ ├── models.cpp │ ├── models.h │ ├── move_relative.test │ ├── move_to.test │ ├── pick_pa10.cpp │ ├── pick_pa10.test │ ├── pick_pr2.cpp │ ├── pick_pr2.test │ ├── pick_ur5.cpp │ ├── pick_ur5.test │ ├── stage_mockups.cpp │ ├── stage_mockups.h │ ├── test_container.cpp │ ├── test_cost_queue.cpp │ ├── test_cost_terms.cpp │ ├── test_fallback.cpp │ ├── test_interface_state.cpp │ ├── test_mockups.cpp │ ├── test_move_relative.cpp │ ├── test_move_to.cpp │ ├── test_properties.cpp │ ├── test_pruning.cpp │ ├── test_serial.cpp │ ├── test_stage.cpp │ └── test_stage.launch ├── demo ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config │ ├── mtc.rviz │ └── panda_config.yaml ├── include │ └── moveit_task_constructor_demo │ │ └── pick_place_task.h ├── launch │ ├── demo.launch │ └── pickplace.launch ├── package.xml ├── scripts │ ├── alternatives.py │ ├── cartesian.py │ ├── compute_ik.py │ ├── constrained.py │ ├── current_state.py │ ├── fallbacks.py │ ├── fix_collision_objects.py │ ├── fixed_state.py │ ├── generate_pose.py │ ├── merger.py │ ├── modify_planning_scene.py │ ├── multi_planner.py │ ├── pickplace.py │ └── properties.py ├── src │ ├── alternative_path_costs.cpp │ ├── cartesian.cpp │ ├── fallbacks_move_to.cpp │ ├── ik_clearance_cost.cpp │ ├── modular.cpp │ ├── pick_place_demo.cpp │ └── pick_place_task.cpp └── test │ ├── CMakeLists.txt │ ├── pick_place.test │ └── pick_place_test.cpp ├── msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── action │ └── ExecuteTaskSolution.action ├── msg │ ├── Property.msg │ ├── Solution.msg │ ├── SolutionInfo.msg │ ├── StageDescription.msg │ ├── StageStatistics.msg │ ├── SubSolution.msg │ ├── SubTrajectory.msg │ ├── TaskDescription.msg │ ├── TaskStatistics.msg │ └── TrajectoryExecutionInfo.msg ├── package.xml └── srv │ └── GetSolution.srv ├── rviz_marker_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── rviz_marker_tools │ │ └── marker_creation.h ├── package.xml └── src │ └── marker_creation.cpp └── visualization ├── CHANGELOG.rst ├── CMakeLists.txt ├── icons └── classes │ └── Motion Planning Tasks.svg ├── motion_planning_tasks ├── CMakeLists.txt ├── properties │ ├── CMakeLists.txt │ ├── property_factory.cpp │ ├── property_factory.h │ └── property_from_yaml.cpp ├── src │ ├── CMakeLists.txt │ ├── factory_model.cpp │ ├── factory_model.h │ ├── global_settings.ui │ ├── icons.cpp │ ├── icons.h │ ├── icons │ │ ├── add.png │ │ ├── connectarrow.png │ │ ├── downarrow.png │ │ ├── failed.png │ │ ├── generatearrow.png │ │ ├── new-stage.png │ │ ├── settings.png │ │ ├── success.png │ │ ├── tasks.png │ │ ├── uparrow.png │ │ └── updownarrow.png │ ├── job_queue.cpp │ ├── job_queue.h │ ├── local_task_model.cpp │ ├── local_task_model.h │ ├── meta_task_list_model.cpp │ ├── meta_task_list_model.h │ ├── plugin_init.cpp │ ├── pluginlib_factory.h │ ├── remote_task_model.cpp │ ├── remote_task_model.h │ ├── resources.qrc │ ├── task_display.cpp │ ├── task_display.h │ ├── task_list_model.cpp │ ├── task_list_model.h │ ├── task_panel.cpp │ ├── task_panel.h │ ├── task_panel.ui │ ├── task_panel_p.h │ └── task_view.ui ├── test │ ├── CMakeLists.txt │ ├── test_merge_models.cpp │ ├── test_solution_models.cpp │ ├── test_task_model.cpp │ └── test_task_model.launch └── utils │ ├── CMakeLists.txt │ ├── flat_merge_proxy_model.cpp │ ├── flat_merge_proxy_model.h │ ├── icon.cpp │ ├── icon.h │ ├── tree_merge_proxy_model.cpp │ └── tree_merge_proxy_model.h ├── motion_planning_tasks_rviz_plugin_description.xml ├── package.xml └── visualization_tools ├── CMakeLists.txt ├── include └── moveit │ └── visualization_tools │ ├── display_solution.h │ ├── marker_visualization.h │ ├── task_solution_panel.h │ └── task_solution_visualization.h └── src ├── display_solution.cpp ├── marker_visualization.cpp ├── task_solution_panel.cpp └── task_solution_visualization.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | AccessModifierOffset: -3 5 | AlignAfterOpenBracket: Align 6 | AlignEscapedNewlinesLeft: true 7 | AlignTrailingComments: false 8 | AllowAllParametersOfDeclarationOnNextLine: false 9 | AllowShortFunctionsOnASingleLine: Inline 10 | AllowShortIfStatementsOnASingleLine: false 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakBeforeMultilineStrings: false 13 | AlwaysBreakTemplateDeclarations: true 14 | BinPackArguments: true 15 | BinPackParameters: true 16 | BraceWrapping: 17 | AfterClass: true 18 | AfterControlStatement: false 19 | AfterEnum: true 20 | AfterFunction: false 21 | AfterNamespace: false 22 | AfterStruct: true 23 | AfterUnion: true 24 | BeforeCatch: false 25 | BeforeElse: false 26 | IndentBraces: false 27 | SplitEmptyRecord: false 28 | BreakBeforeBinaryOperators: None 29 | BreakBeforeBraces: Custom 30 | BreakBeforeTernaryOperators: false 31 | BreakConstructorInitializersBeforeComma: true 32 | ColumnLimit: 120 33 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 34 | ConstructorInitializerIndentWidth: 2 35 | ContinuationIndentWidth: 4 36 | Cpp11BracedListStyle: false 37 | DerivePointerBinding: false 38 | ExperimentalAutoDetectBinPacking: false 39 | IndentCaseLabels: true 40 | IndentFunctionDeclarationAfterType: false 41 | IndentWidth: 3 42 | MaxEmptyLinesToKeep: 1 43 | NamespaceIndentation: None 44 | ObjCSpaceBeforeProtocolList: true 45 | PenaltyBreakBeforeFirstCallParameter: 19 46 | PenaltyBreakComment: 60 47 | PenaltyBreakFirstLessLess: 1000 48 | PenaltyBreakString: 100 49 | PenaltyExcessCharacter: 1000 50 | PenaltyReturnTypeOnItsOwnLine: 70 51 | PointerBindsToType: true 52 | SortIncludes: false 53 | SpaceAfterCStyleCast: false 54 | SpaceAfterControlStatementKeyword: false 55 | SpaceBeforeAssignmentOperators: true 56 | SpaceBeforeParens: ControlStatements 57 | SpaceInEmptyParentheses: false 58 | SpacesBeforeTrailingComments: 2 59 | SpacesInAngles: false 60 | SpacesInCStyleCastParentheses: false 61 | SpacesInParentheses: false 62 | Standard: Auto 63 | TabWidth: 3 64 | UseTab: ForIndentation 65 | ... 66 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | --- 2 | Checks: 'performance-*, 3 | -performance-noexcept-move-constructor, 4 | -clang-analyzer-core.CallAndMessage, 5 | llvm-namespace-comment, 6 | modernize-redundant-void-arg, 7 | modernize-use-nullptr, 8 | modernize-use-default, 9 | modernize-use-override, 10 | modernize-use-using, 11 | modernize-loop-convert, 12 | readability-named-parameter, 13 | readability-redundant-smartptr-get, 14 | readability-redundant-string-cstr, 15 | readability-simplify-boolean-expr, 16 | readability-container-size-empty, 17 | readability-identifier-naming, 18 | ' 19 | HeaderFilterRegex: '.*/moveit/task_constructor/.*\.h' 20 | CheckOptions: 21 | - key: llvm-namespace-comment.ShortNamespaceLines 22 | value: '10' 23 | - key: llvm-namespace-comment.SpacesBeforeComments 24 | value: '2' 25 | - key: readability-braces-around-statements.ShortStatementLines 26 | value: '2' 27 | # type names 28 | - key: readability-identifier-naming.ClassCase 29 | value: aNy_CasE # CamelCase 30 | - key: readability-identifier-naming.EnumCase 31 | value: CamelCase 32 | - key: readability-identifier-naming.UnionCase 33 | value: CamelCase 34 | # method names 35 | - key: readability-identifier-naming.MethodCase 36 | value: aNy_CasE # camelBack 37 | # variable names 38 | - key: readability-identifier-naming.VariableCase 39 | value: lower_case 40 | - key: readability-identifier-naming.ClassMemberSuffix 41 | value: '_' 42 | # const static or global variables are UPPER_CASE 43 | - key: readability-identifier-naming.EnumConstantCase 44 | value: UPPER_CASE 45 | - key: readability-identifier-naming.StaticConstantCase 46 | value: UPPER_CASE 47 | - key: readability-identifier-naming.ClassConstantCase 48 | value: UPPER_CASE 49 | - key: readability-identifier-naming.GlobalVariableCase 50 | value: UPPER_CASE 51 | ... 52 | -------------------------------------------------------------------------------- /.github/workflows/format.yaml: -------------------------------------------------------------------------------- 1 | # This is a format job. Pre-commit has a first-party GitHub action, so we use 2 | # that: https://github.com/pre-commit/action 3 | 4 | name: Format 5 | 6 | on: 7 | workflow_dispatch: 8 | pull_request: 9 | push: 10 | 11 | jobs: 12 | pre-commit: 13 | name: pre-commit 14 | runs-on: ubuntu-latest 15 | steps: 16 | - uses: actions/checkout@v4 17 | with: 18 | submodules: recursive 19 | - name: Install clang-format-14 20 | run: sudo apt-get install clang-format-14 21 | - uses: rhaschke/install-catkin_lint-action@main 22 | with: 23 | distro: noetic 24 | - uses: pre-commit/action@v3.0.1 25 | id: precommit 26 | - name: Upload pre-commit changes 27 | if: failure() && steps.precommit.outcome == 'failure' 28 | uses: rhaschke/upload-git-patch-action@main 29 | with: 30 | name: pre-commit 31 | -------------------------------------------------------------------------------- /.github/workflows/lsan.suppressions: -------------------------------------------------------------------------------- 1 | leak:class_loader 2 | -------------------------------------------------------------------------------- /.github/workflows/prerelease.yaml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: pre-release 5 | 6 | on: 7 | workflow_dispatch: 8 | inputs: 9 | ROS_DISTRO: 10 | type: string 11 | required: true 12 | description: 'ROS distribution codename:' 13 | default: noetic 14 | 15 | permissions: 16 | contents: read # to fetch code (actions/checkout) 17 | 18 | jobs: 19 | default: 20 | env: 21 | ROS_DISTRO: ${{ inputs.ROS_DISTRO }} 22 | PRERELEASE: true 23 | BASEDIR: ${{ github.workspace }}/.work 24 | 25 | name: "${{ inputs.ROS_DISTRO }}" 26 | runs-on: ubuntu-latest 27 | steps: 28 | - name: "Free up disk space" 29 | run: | 30 | sudo apt-get -qq purge build-essential "ghc*" 31 | sudo apt-get clean 32 | # cleanup docker images not used by us 33 | docker system prune -af 34 | # free up a lot of stuff from /usr/local 35 | sudo rm -rf /usr/local 36 | df -h 37 | - uses: actions/checkout@v4 38 | with: 39 | submodules: recursive 40 | - name: industrial_ci 41 | uses: ros-industrial/industrial_ci@master 42 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | *.pyc 3 | __pycache__/ 4 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "core/python/pybind11"] 2 | path = core/python/pybind11 3 | url = https://github.com/pybind/pybind11 4 | branch = smart_holder 5 | shallow = true 6 | [submodule "core/src/scope_guard"] 7 | path = core/src/scope_guard 8 | url = https://github.com/ricab/scope_guard 9 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To use: 2 | # 3 | # pre-commit run -a 4 | # 5 | # Or: 6 | # 7 | # pre-commit install # (runs every time you commit in git) 8 | # 9 | # To update this file: 10 | # 11 | # pre-commit autoupdate 12 | # 13 | # See https://github.com/pre-commit/pre-commit 14 | 15 | repos: 16 | # Standard hooks 17 | - repo: https://github.com/pre-commit/pre-commit-hooks 18 | rev: v5.0.0 19 | hooks: 20 | - id: check-added-large-files 21 | - id: check-case-conflict 22 | - id: check-merge-conflict 23 | - id: check-symlinks 24 | - id: check-xml 25 | - id: check-yaml 26 | - id: debug-statements 27 | - id: end-of-file-fixer 28 | - id: mixed-line-ending 29 | - id: trailing-whitespace 30 | 31 | - repo: https://github.com/psf/black 32 | rev: 24.10.0 33 | hooks: 34 | - id: black 35 | args: ["--line-length", "100"] 36 | 37 | - repo: local 38 | hooks: 39 | - id: clang-format 40 | name: clang-format 41 | description: Format files with ClangFormat. 42 | entry: clang-format-14 43 | language: system 44 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ 45 | args: ["-fallback-style=none", "-i"] 46 | - id: catkin_lint 47 | name: catkin_lint 48 | description: Check package.xml and cmake files 49 | entry: catkin_lint . --ignore duplicate_cmd 50 | language: system 51 | always_run: true 52 | pass_filenames: false 53 | 54 | - repo: local 55 | hooks: 56 | - id: misspelled-moveit 57 | name: misspelled-moveit 58 | description: MoveIt should be spelled exactly as MoveIt 59 | language: pygrep 60 | entry: Moveit|MoveIt! 61 | exclude: .pre-commit-config.yaml|README.md 62 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2017, Hamburg University 4 | Copyright (c) 2017, Bielefeld University 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MoveIt Task Constructor Framework 2 | 3 | The Task Constructor framework provides a flexible and transparent way to define and plan actions that consist of multiple interdependent subtasks. 4 | It draws on the planning capabilities of [MoveIt](https://moveit.ros.org/) to solve individual subproblems in black-box *planning stages*. 5 | A common interface, based on MoveIt's PlanningScene is used to pass solution hypotheses between stages. 6 | The framework enables the hierarchical organization of basic stages using *containers*, allowing for sequential as well as parallel compositions. 7 | 8 | ## Branches 9 | 10 | This repository provides the following branches: 11 | 12 | - **master**: ROS 1 development 13 | - **ros2**: ROS 2 development, compatible with MoveIt 2 `main` 14 | - **humble**: ROS 2 stable branch for Humble support 15 | 16 | ## Videos 17 | 18 | - Demo video associated with [ICRA 2019 paper](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf) 19 | 20 | [![](https://img.youtube.com/vi/fCORKVYsdDI/0.jpg)](https://www.youtube.com/watch?v=fCORKVYsdDI) 21 | 22 | - [Presentation @ ROSCon 2018 (Madrid)](https://vimeo.com/293432325) 23 | - [Presentation @ MoveIt workshop 2019 (Macau)](https://www.youtube.com/watch?v=a8r7O2bs1Mc) 24 | 25 | ## Tutorial 26 | 27 | We provide a tutorial for a pick-and-place pipeline without bells & whistles [as part of the MoveIt tutorials](https://moveit.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html). 28 | 29 | ## Roadmap 30 | 31 | **Feedback, reports and contributions are very welcome.** 32 | 33 | The current roadmap is to replace MoveIt's old pick&place pipeline and provide a *transparent mechanism* to enable and debug complex motion sequences. 34 | 35 | Further planned features include 36 | 37 | - Entwined planning and execution for early execution, monitoring and code hooks 38 | - Subsolution blending 39 | - Parallel planning 40 | - Iterative solution improvement 41 | 42 | Ideas and requests for other interesting/useful features are welcome. 43 | 44 | ## Citation 45 | 46 | If you use this framework in your project, please cite the associated paper: 47 | 48 | Michael Görner*, Robert Haschke*, Helge Ritter, and Jianwei Zhang, 49 | "MoveIt! Task Constructor for Task-Level Motion Planning", 50 | _International Conference on Robotics and Automation (ICRA)_, 2019, Montreal, Canada. 51 | [[DOI]](https://doi.org/10.1109/ICRA.2019.8793898) [[PDF]](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf). 52 | 53 | 54 | ```plain 55 | @inproceedings{goerner2019mtc, 56 | title={{MoveIt! Task Constructor for Task-Level Motion Planning}}, 57 | author={Görner, Michael* and Haschke, Robert* and Ritter, Helge and Zhang, Jianwei}, 58 | booktitle={IEEE International Conference on Robotics and Automation (ICRA)}, 59 | year={2019} 60 | } 61 | ``` 62 | -------------------------------------------------------------------------------- /capabilities/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package moveit_task_constructor_capabilities 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.3 (2023-03-06) 6 | ------------------ 7 | 8 | 0.1.2 (2023-02-24) 9 | ------------------ 10 | 11 | 0.1.1 (2023-02-15) 12 | ------------------ 13 | 14 | 0.1.0 (2023-02-02) 15 | ------------------ 16 | * Initial release 17 | * Contributors: Michael Görner, Robert Haschke 18 | -------------------------------------------------------------------------------- /capabilities/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(moveit_task_constructor_capabilities) 3 | 4 | find_package(fmt REQUIRED) 5 | find_package(catkin REQUIRED COMPONENTS 6 | actionlib 7 | moveit_core 8 | moveit_ros_move_group 9 | moveit_task_constructor_core 10 | moveit_task_constructor_msgs 11 | pluginlib 12 | std_msgs 13 | ) 14 | 15 | moveit_build_options() 16 | 17 | catkin_package( 18 | LIBRARIES ${PROJECT_NAME} 19 | CATKIN_DEPENDS 20 | actionlib 21 | moveit_task_constructor_msgs 22 | std_msgs 23 | ) 24 | 25 | # Check for availability of moveit::core::MoveItErrorCode::toString 26 | set(CMAKE_REQUIRED_INCLUDES ${catkin_INCLUDE_DIRS}) 27 | set(CMAKE_REQUIRED_LIBRARIES ${catkin_LIBRARIES}) 28 | set(CMAKE_REQUIRED_FLAGS -Wno-error) 29 | include(CheckCXXSymbolExists) 30 | 31 | add_library(${PROJECT_NAME} 32 | src/execute_task_solution_capability.cpp 33 | ) 34 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 35 | target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) 36 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt) 37 | 38 | install(TARGETS ${PROJECT_NAME} 39 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 40 | ) 41 | 42 | install(FILES capabilities_plugin_description.xml 43 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 44 | ) 45 | -------------------------------------------------------------------------------- /capabilities/capabilities_plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Action server to execute solutions generated through the MoveIt Task Constructor. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /capabilities/package.xml: -------------------------------------------------------------------------------- 1 | 2 | moveit_task_constructor_capabilities 3 | 0.1.3 4 | 5 | MoveGroupCapabilites to interact with MoveIt 6 | 7 | 8 | Michael v4hn Goerner 9 | Robert Haschke 10 | 11 | BSD 12 | 13 | catkin 14 | 15 | fmt 16 | moveit_core 17 | moveit_ros_move_group 18 | actionlib 19 | pluginlib 20 | std_msgs 21 | moveit_task_constructor_core 22 | moveit_task_constructor_msgs 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /capabilities/src/execute_task_solution_capability.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Hamburg University. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Hamburg University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* 36 | * Capability to execute trajectories through a ROS action 37 | * that were generated by the MoveIt Task Constructor. 38 | * 39 | * Author: Michael 'v4hn' Goerner 40 | * */ 41 | 42 | #pragma once 43 | 44 | #include 45 | #include 46 | 47 | #include 48 | 49 | #include 50 | 51 | namespace move_group { 52 | 53 | class ExecuteTaskSolutionCapability : public MoveGroupCapability 54 | { 55 | public: 56 | ExecuteTaskSolutionCapability(); 57 | 58 | void initialize() override; 59 | 60 | private: 61 | bool constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, 62 | plan_execution::ExecutableMotionPlan& plan); 63 | 64 | void execCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal); 65 | void preemptCallback(); 66 | 67 | std::unique_ptr> as_; 68 | }; 69 | 70 | } // namespace move_group 71 | -------------------------------------------------------------------------------- /codecov.yaml: -------------------------------------------------------------------------------- 1 | coverage: 2 | precision: 2 3 | round: up 4 | range: "45...70" 5 | status: 6 | project: 7 | default: 8 | target: auto 9 | threshold: 0.5% 10 | patch: off 11 | -------------------------------------------------------------------------------- /core/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package moveit_task_constructor_core 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.3 (2023-03-06) 6 | ------------------ 7 | * MoveRelative: Allow backwards operation for joint-space delta (`#436 `_) 8 | * ComputeIK: Limit collision checking to JMG (`#428 `_) 9 | * Fix: Fetch pybind11 submodule if not yet present 10 | * Contributors: Robert Haschke 11 | 12 | 0.1.2 (2023-02-24) 13 | ------------------ 14 | * Remove moveit/__init__.py during .deb builds to avoid installation conflicts 15 | * MultiPlanner solver (`#429 `_): a planner that tries multiple planners in sequence 16 | 17 | * CartesianPath: Deprecate redundant property setters 18 | * PlannerInterface: provide "timeout" property 19 | * PlannerInterface: provide setters for properties 20 | * JointInterpolation: fix timeout handling 21 | * Contributors: Robert Haschke 22 | 23 | 0.1.1 (2023-02-15) 24 | ------------------ 25 | * Resort to MoveIt's python tools 26 | * Provide ComputeIK.ik_frame as full PoseStamped 27 | * Fixed build farm issues 28 | 29 | * Removed unused eigen_conversions includes 30 | * Fixed odr compiler warning on Buster 31 | * Fixed missing dependency declarations 32 | * Contributors: Robert Haschke 33 | 34 | 0.1.0 (2023-02-02) 35 | ------------------ 36 | * Initial release 37 | * Contributors: Michael Görner, Robert Haschke, Captain Yoshi, Christian Petersmeier, Henning Kayser, Jafar Abdi, Tyler Weaver 38 | -------------------------------------------------------------------------------- /core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(moveit_task_constructor_core) 3 | 4 | find_package(Boost REQUIRED) 5 | find_package(fmt REQUIRED) 6 | find_package(catkin REQUIRED COMPONENTS 7 | roslint 8 | tf2_eigen 9 | geometry_msgs 10 | moveit_core 11 | moveit_ros_planning 12 | moveit_ros_planning_interface 13 | moveit_task_constructor_msgs 14 | roscpp 15 | visualization_msgs 16 | rviz_marker_tools 17 | py_binding_tools 18 | ) 19 | 20 | option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" OFF) # We use our own set of warnings 21 | moveit_build_options() 22 | 23 | catkin_python_setup() 24 | 25 | catkin_package( 26 | LIBRARIES 27 | ${PROJECT_NAME} 28 | ${PROJECT_NAME}_stages 29 | ${PROJECT_NAME}_stage_plugins 30 | INCLUDE_DIRS 31 | include 32 | CATKIN_DEPENDS 33 | geometry_msgs 34 | moveit_core 35 | moveit_task_constructor_msgs 36 | rviz_marker_tools 37 | visualization_msgs 38 | CFG_EXTRAS pybind11.cmake 39 | ) 40 | 41 | add_compile_options(-fvisibility-inlines-hidden) 42 | 43 | set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) 44 | 45 | add_subdirectory(src) 46 | add_subdirectory(python) 47 | add_subdirectory(test) 48 | 49 | install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 50 | PATTERN "*_p.h" EXCLUDE) 51 | install(FILES 52 | motion_planning_stages_plugin_description.xml 53 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 54 | -------------------------------------------------------------------------------- /core/cmake/pybind11.cmake.in: -------------------------------------------------------------------------------- 1 | # pybind11 must use the ROS python version 2 | set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION}) 3 | 4 | if(@INSTALLSPACE@) 5 | include(${CMAKE_CURRENT_LIST_DIR}/pybind11Config.cmake) 6 | else() 7 | # in build space, directly include pybind11 directory 8 | add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../pybind11 ${CMAKE_CURRENT_BINARY_DIR}/pybind11) 9 | endif() 10 | -------------------------------------------------------------------------------- /core/doc/Doxyfile: -------------------------------------------------------------------------------- 1 | PROJECT_NAME = MTC 2 | INPUT = ../include/ ../src/ 3 | RECURSIVE = YES 4 | 5 | GENERATE_HTML = YES 6 | GENERATE_LATEX = NO 7 | GENERATE_XML = NO 8 | HTML_OUTPUT = _cpp 9 | XML_OUTPUT = _doxygenxml 10 | XML_PROGRAMLISTING = YES 11 | 12 | ALIASES = "rst=\verbatim embed:rst" 13 | ALIASES += "endrst=\endverbatim" 14 | 15 | QUIET = YES 16 | WARNINGS = YES 17 | WARN_IF_UNDOCUMENTED = NO 18 | 19 | EXCLUDE_SYMBOLS = *Private 20 | EXCLUDE_SYMBOLS += class_ 21 | EXCLUDE_SYMBOLS += declval* 22 | -------------------------------------------------------------------------------- /core/doc/_templates/custom-class-template.rst: -------------------------------------------------------------------------------- 1 | {{ fullname | escape | underline}} 2 | 3 | .. currentmodule:: {{ module }} 4 | 5 | .. autoclass:: {{ objname }} 6 | :members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __len__, __getitem__, __iter__, __call__, __add__, __mul__ 10 | 11 | {% block methods %} 12 | {% if methods %} 13 | .. rubric:: {{ _('Methods') }} 14 | 15 | .. autosummary:: 16 | :nosignatures: 17 | {% for item in methods %} 18 | {%- if not item.startswith('_') %} 19 | ~{{ name }}.{{ item }} 20 | {%- endif -%} 21 | {%- endfor %} 22 | {% endif %} 23 | {% endblock %} 24 | 25 | {% block attributes %} 26 | {% if attributes %} 27 | .. rubric:: {{ _('Attributes') }} 28 | 29 | .. autosummary:: 30 | {% for item in attributes %} 31 | ~{{ name }}.{{ item }} 32 | {%- endfor %} 33 | {% endif %} 34 | {% endblock %} 35 | -------------------------------------------------------------------------------- /core/doc/_templates/custom-module-template.rst: -------------------------------------------------------------------------------- 1 | {{ fullname | escape | underline}} 2 | 3 | .. automodule:: {{ fullname }} 4 | 5 | {% block attributes %} 6 | {% if attributes %} 7 | .. rubric:: Module attributes 8 | 9 | .. autosummary:: 10 | :toctree: 11 | {% for item in attributes %} 12 | {{ item }} 13 | {%- endfor %} 14 | {% endif %} 15 | {% endblock %} 16 | 17 | {% block functions %} 18 | {% if functions %} 19 | .. rubric:: {{ _('Functions') }} 20 | 21 | .. autosummary:: 22 | :toctree: 23 | :nosignatures: 24 | {% for item in functions %} 25 | {{ item }} 26 | {%- endfor %} 27 | {% endif %} 28 | {% endblock %} 29 | 30 | {% block classes %} 31 | {% if classes %} 32 | .. rubric:: {{ _('Classes') }} 33 | 34 | .. autosummary:: 35 | :toctree: 36 | :template: custom-class-template.rst 37 | :nosignatures: 38 | {% for item in classes %} 39 | {{ item }} 40 | {%- endfor %} 41 | {% endif %} 42 | {% endblock %} 43 | 44 | {% block exceptions %} 45 | {% if exceptions %} 46 | .. rubric:: {{ _('Exceptions') }} 47 | 48 | .. autosummary:: 49 | :toctree: 50 | {% for item in exceptions %} 51 | {{ item }} 52 | {%- endfor %} 53 | {% endif %} 54 | {% endblock %} 55 | 56 | {% block modules %} 57 | {% if modules %} 58 | .. autosummary:: 59 | :toctree: 60 | :template: custom-module-template.rst 61 | :recursive: 62 | {% for item in modules %} 63 | {{ item }} 64 | {%- endfor %} 65 | {% endif %} 66 | {% endblock %} 67 | -------------------------------------------------------------------------------- /core/doc/api.rst: -------------------------------------------------------------------------------- 1 | .. _sec-api: 2 | 3 | API reference 4 | ------------- 5 | 6 | `C++ <_static/index.html>`_ 7 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^ 8 | 9 | Python 10 | ^^^^^^ 11 | 12 | .. autosummary:: 13 | :toctree: _autosummary 14 | :caption: API 15 | :recursive: 16 | :template: custom-module-template.rst 17 | 18 | moveit.task_constructor 19 | pymoveit_mtc.core 20 | pymoveit_mtc.stages 21 | -------------------------------------------------------------------------------- /core/doc/concepts.rst: -------------------------------------------------------------------------------- 1 | .. _sec-concepts: 2 | 3 | Concepts 4 | ------------ 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | 9 | basics 10 | -------------------------------------------------------------------------------- /core/doc/index.rst: -------------------------------------------------------------------------------- 1 | MoveIt Task Constructor (MTC) 2 | ============================= 3 | 4 | The Task Constructor framework provides a flexible and transparent way 5 | to define and plan actions that consist of multiple interdependent subtasks. 6 | It draws on the planning capabilities of MoveIt to solve individual subproblems 7 | in black-box planning stages. 8 | A common interface, based on MoveIt’s PlanningScene is used to pass solution 9 | hypotheses between stages. The framework enables the hierarchical organization of 10 | basic stages using containers, allowing for sequential as well as parallel compositions. 11 | For more details, please refer to the associated `ICRA 2019 publication `_. 12 | 13 | 14 | Organization of the documentation 15 | --------------------------------- 16 | 17 | - :ref:`sec-tutorials` provide examples how to setup your task pipeline. 18 | Start with :ref:`subsec-tut-firststeps` if you are new to MTC. 19 | - :ref:`sec-concepts` discuss the architecture and terminology of MTC on a fairly high level. 20 | - :ref:`sec-howtoguides` help solving specific problems and use cases. 21 | - The :ref:`sec-api` provides quick access to available classes, functions, and their parameters. 22 | 23 | .. toctree:: 24 | :maxdepth: 2 25 | :hidden: 26 | 27 | tutorials/index 28 | concepts 29 | howto 30 | api 31 | troubleshooting 32 | -------------------------------------------------------------------------------- /core/doc/requirements.txt: -------------------------------------------------------------------------------- 1 | furo 2 | lxml 3 | sphinx 4 | sphinx-copybutton 5 | -------------------------------------------------------------------------------- /core/doc/troubleshooting.rst: -------------------------------------------------------------------------------- 1 | .. _sec-troubleshooting: 2 | 3 | Troubleshooting 4 | =============== 5 | 6 | ``Task::init()`` reports mismatching interfaces 7 | ----------------------------------------------- 8 | 9 | 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: 10 | 11 | .. code-block:: c 12 | 13 | try { 14 | task.plan(); 15 | } catch (const InitStageException& e) { 16 | std::cerr << e << std::endl; 17 | throw; 18 | } 19 | 20 | For example, the following pipeline: 21 | 22 | - ↕ current 23 | - ⛓ connect 24 | - ↑ moveTo 25 | 26 | throws the error: ``task pipeline: end interface (←) of 'moveto' does not match mine (→)``. 27 | 28 | 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``. 29 | ``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. 30 | 31 | Obviously, in a ``ParallelContainer`` all (direct) children need to share a common interface. 32 | -------------------------------------------------------------------------------- /core/doc/tutorials/cartesian.rst: -------------------------------------------------------------------------------- 1 | .. _subsec-tut-cartesian: 2 | 3 | Cartesian 4 | --------- 5 | 6 | The following example demonstrates how to compute a simple point to point motion 7 | plan using the moveit task constructor. You can take a look at the full 8 | source code here: 9 | :download:`Source <../../../demo/scripts/cartesian.py>` 10 | 11 | First, lets make sure we specify the planning group and the 12 | end effector that we want to use. 13 | 14 | .. literalinclude:: ../../../demo/scripts/cartesian.py 15 | :language: python 16 | :start-after: [cartesianTut1] 17 | :end-before: [cartesianTut1] 18 | 19 | The moveit task constructor provides different planners. 20 | We will use the ``CartesianPath`` and ``JointInterpolation`` 21 | planners for this example. 22 | 23 | .. literalinclude:: ../../../demo/scripts/cartesian.py 24 | :language: python 25 | :start-after: [cartesianTut2] 26 | :end-before: [cartesianTut2] 27 | 28 | Lets start by initializing a task and adding the current 29 | planning scene state and robot state to it. 30 | This will be the starting state for our motion plan. 31 | 32 | .. literalinclude:: ../../../demo/scripts/cartesian.py 33 | :language: python 34 | :start-after: [cartesianTut3] 35 | :end-before: [cartesianTut3] 36 | 37 | To compute a relative motion in cartesian space, we can use 38 | the ``MoveRelative`` stage. Specify the planning group and 39 | frame relative to which you want to carry out the motion. 40 | the relative direction can be specified by a ``Vector3Stamped`` 41 | geometry message. 42 | 43 | .. literalinclude:: ../../../demo/scripts/cartesian.py 44 | :language: python 45 | :start-after: [initAndConfigMoveRelative] 46 | :end-before: [initAndConfigMoveRelative] 47 | 48 | Similarly we can move along a different axis. 49 | 50 | .. literalinclude:: ../../../demo/scripts/cartesian.py 51 | :language: python 52 | :start-after: [cartesianTut4] 53 | :end-before: [cartesianTut4] 54 | 55 | The ``MoveRelative`` stage also offers an interface to 56 | ``Twist`` messages, allowing to specify rotations. 57 | 58 | .. literalinclude:: ../../../demo/scripts/cartesian.py 59 | :language: python 60 | :start-after: [cartesianTut5] 61 | :end-before: [cartesianTut5] 62 | 63 | Lastly, we can compute linear movements in cartesian space 64 | by providing offsets in joint space. 65 | 66 | .. literalinclude:: ../../../demo/scripts/cartesian.py 67 | :language: python 68 | :start-after: [cartesianTut6] 69 | :end-before: [cartesianTut6] 70 | 71 | If we want to specify goals instead of directions, 72 | we can use the ``MoveTo`` stage. In the following example 73 | we use simple joint interpolation to move the robot to 74 | a named pose. the named pose is defined in the urdf of 75 | the robot configuration. 76 | 77 | .. literalinclude:: ../../../demo/scripts/cartesian.py 78 | :language: python 79 | :start-after: [initAndConfigMoveTo] 80 | :end-before: [initAndConfigMoveTo] 81 | 82 | Lastly, we invoke the planning mechanism that traverses 83 | the task hierarchy for us and compute a valid motion plan. 84 | At this point, when you run this script you are able to 85 | inspect the solutions of the individual stages in the rviz 86 | mtc panel. 87 | -------------------------------------------------------------------------------- /core/doc/tutorials/first-steps.rst: -------------------------------------------------------------------------------- 1 | .. _subsec-tut-firststeps: 2 | 3 | First Steps 4 | ----------- 5 | 6 | The MoveIt Task Constructor package contains several basic examples and 7 | a pick-and-place demo. For all demos you should launch the basic environment: 8 | 9 | .. code-block:: 10 | 11 | roslaunch moveit_task_constructor_demo demo.launch 12 | 13 | Subsequently, you can run the individual demos: 14 | 15 | .. code-block:: 16 | 17 | rosrun moveit_task_constructor_demo cartesian 18 | rosrun moveit_task_constructor_demo modular 19 | roslaunch moveit_task_constructor_demo pickplace.launch 20 | 21 | To inspect the task hierarchy, be sure that you selected the correct solution topic 22 | in the reviz moveit task constructor plugin. 23 | -------------------------------------------------------------------------------- /core/doc/tutorials/index.rst: -------------------------------------------------------------------------------- 1 | .. _sec-tutorials: 2 | 3 | Tutorials 4 | ========= 5 | 6 | The following tutorials take you step by step through the implementation of fundamental examples 7 | of the moveit task constructor. 8 | 9 | .. toctree:: 10 | :caption: Tutorials 11 | 12 | first-steps 13 | cartesian 14 | properties 15 | pick-and-place 16 | -------------------------------------------------------------------------------- /core/include/moveit/python/task_constructor/properties.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) 10 | PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap) 11 | 12 | namespace moveit { 13 | namespace python { 14 | 15 | class PYBIND11_EXPORT PropertyConverterBase 16 | { 17 | public: 18 | using to_python_converter_function = pybind11::object (*)(const boost::any&); 19 | using from_python_converter_function = boost::any (*)(const pybind11::object&); 20 | 21 | protected: 22 | static bool insert(const std::type_index& type_index, const std::string& ros_msg_name, 23 | to_python_converter_function to, from_python_converter_function from); 24 | }; 25 | 26 | /// utility class to register C++ / Python converters for a property of type T 27 | template 28 | class PropertyConverter : protected PropertyConverterBase 29 | { 30 | public: 31 | PropertyConverter() { insert(typeid(T), rosMsgName(), &toPython, &fromPython); } 32 | 33 | private: 34 | static pybind11::object toPython(const boost::any& value) { return pybind11::cast(boost::any_cast(value)); } 35 | static boost::any fromPython(const pybind11::object& po) { return pybind11::cast(po); } 36 | 37 | template 38 | typename std::enable_if::value, std::string>::type rosMsgName() { 39 | return ros::message_traits::DataType::value(); 40 | } 41 | 42 | template 43 | typename std::enable_if::value, std::string>::type rosMsgName() { 44 | return std::string(); 45 | } 46 | }; 47 | 48 | namespace properties { 49 | 50 | /** Extension for pybind11::class_ to allow convienient definition of properties 51 | * 52 | * New method property(const char* name) adds a property getter/setter. 53 | */ 54 | 55 | template 56 | class class_ : public pybind11::classh // NOLINT(readability-identifier-naming) 57 | { 58 | using base_class_ = pybind11::classh; 59 | 60 | public: 61 | // forward all constructors 62 | using base_class_::classh; 63 | 64 | template 65 | class_& property(const char* name, const Extra&... extra) { 66 | PropertyConverter(); // register corresponding property converter 67 | auto getter = [name](type_& self) -> PropertyType& { 68 | moveit::task_constructor::PropertyMap& props = self.properties(); 69 | return const_cast(props.get(name)); 70 | }; 71 | auto setter = [name](type_& self, const PropertyType& value) { 72 | moveit::task_constructor::PropertyMap& props = self.properties(); 73 | props.set(name, boost::any(value)); 74 | }; 75 | base_class_::def_property(name, getter, setter, pybind11::return_value_policy::reference_internal, extra...); 76 | return *this; 77 | } 78 | }; 79 | } // namespace properties 80 | } // namespace python 81 | } // namespace moveit 82 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/fmt_p.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2024, University of Hamburg 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Michael Goerner 36 | Desc: Thin wrapper around fmt includes to provide Eigen formatters for fmt9 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #if FMT_VERSION >= 90000 46 | template 47 | struct fmt::formatter, T>, char>> : ostream_formatter 48 | {}; 49 | #endif 50 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/marker_tools.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace planning_scene { 9 | MOVEIT_CLASS_FORWARD(PlanningScene); 10 | } 11 | namespace moveit { 12 | namespace core { 13 | MOVEIT_CLASS_FORWARD(RobotState); 14 | MOVEIT_CLASS_FORWARD(LinkModel); 15 | } // namespace core 16 | } // namespace moveit 17 | 18 | namespace moveit { 19 | namespace task_constructor { 20 | 21 | /** signature of callback function, passing the generated marker and the name of the robot link / scene object */ 22 | using MarkerCallback = std::function; 23 | 24 | /** generate marker msgs to visualize the planning scene, calling the given callback for each of them 25 | * object_names: set of links to include (or all if empty) */ 26 | void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr& scene, const MarkerCallback& callback, 27 | const std::vector& object_names = {}); 28 | 29 | /** generate marker msgs to visualize robot's collision geometry, calling the given callback for each of them 30 | * link_names: set of links to include (or all if empty) */ 31 | void generateCollisionMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback, 32 | const std::vector& link_names = {}); 33 | void generateCollisionMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback, 34 | const std::vector& link_models); 35 | 36 | /** generate marker msgs to visualize robot's visual geometry, calling the given callback for each of them 37 | * link_names: set of links to include (or all if empty) */ 38 | void generateVisualMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback, 39 | const std::vector& link_names = {}); 40 | void generateVisualMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback, 41 | const std::vector& link_models); 42 | 43 | /** generate marker msgs to visualize the planning scene, calling the given callback for each of them 44 | * calls generateMarkersForRobot() and generateMarkersForObjects() */ 45 | void generateMarkersForScene(const planning_scene::PlanningSceneConstPtr& scene, const MarkerCallback& callback); 46 | } // namespace task_constructor 47 | } // namespace moveit 48 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/moveit_compat.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2021, Hamburg University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Michael 'v4hn' Goerner 36 | Desc: Provide test for MoveIt features 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | 44 | #define MOVEIT_VERSION_GE(major, minor, patch) \ 45 | (MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \ 46 | major * 1'000'000 + minor * 1'000 + patch) 47 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/solvers.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Hamburg University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Michael 'v4hn' Goerner 36 | Desc: collective include of solvers 37 | */ 38 | 39 | #pragma once 40 | 41 | #include "solvers/cartesian_path.h" 42 | #include "solvers/joint_interpolation.h" 43 | #include "solvers/pipeline_planner.h" 44 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/stages.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Hamburg University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Michael 'v4hn' Goerner 36 | Desc: collective include of primitive stages 37 | */ 38 | 39 | #pragma once 40 | 41 | #include "stages/compute_ik.h" 42 | #include "stages/connect.h" 43 | #include "stages/current_state.h" 44 | #include "stages/fix_collision_objects.h" 45 | #include "stages/fixed_cartesian_poses.h" 46 | #include "stages/fixed_state.h" 47 | #include "stages/generate_grasp_pose.h" 48 | #include "stages/generate_place_pose.h" 49 | #include "stages/generate_pose.h" 50 | #include "stages/generate_random_pose.h" 51 | #include "stages/modify_planning_scene.h" 52 | #include "stages/move_relative.h" 53 | #include "stages/move_to.h" 54 | #include "stages/predicate_filter.h" 55 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/stages/current_state.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * Copyright (c) 2017, Hamburg University 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Bielefeld University nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | /* Authors: Michael Goerner, Luca Lach, Robert Haschke 37 | Desc: Generator Stage to fetch the current PlanningScene state 38 | */ 39 | 40 | #pragma once 41 | 42 | #include 43 | 44 | namespace moveit { 45 | namespace task_constructor { 46 | namespace stages { 47 | 48 | /** Fetch the current PlanningScene state via get_planning_scene service */ 49 | class CurrentState : public Generator 50 | { 51 | public: 52 | CurrentState(const std::string& name = "current state"); 53 | 54 | void init(const moveit::core::RobotModelConstPtr& robot_model) override; 55 | bool canCompute() const override; 56 | void compute() override; 57 | 58 | protected: 59 | moveit::core::RobotModelConstPtr robot_model_; 60 | planning_scene::PlanningScenePtr scene_; 61 | }; 62 | } // namespace stages 63 | } // namespace task_constructor 64 | } // namespace moveit 65 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/stages/fix_collision_objects.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Elham Iravani, Robert Haschke 36 | Desc: Fix collisions in input scene 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace moveit { 47 | namespace task_constructor { 48 | namespace stages { 49 | 50 | class FixCollisionObjects : public PropagatingEitherWay 51 | { 52 | public: 53 | FixCollisionObjects(const std::string& name = "fix collisions of objects"); 54 | 55 | void computeForward(const InterfaceState& from) override; 56 | void computeBackward(const InterfaceState& to) override; 57 | 58 | void setDirection(const geometry_msgs::Vector3& dir) { setProperty("direction", dir); } 59 | void setMaxPenetration(double penetration) { setProperty("max_penetration", penetration); } 60 | 61 | private: 62 | SubTrajectory fixCollisions(planning_scene::PlanningScene& scene) const; 63 | }; 64 | } // namespace stages 65 | } // namespace task_constructor 66 | } // namespace moveit 67 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/stages/fixed_cartesian_poses.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Robert Haschke 36 | Desc: Spawn fixed Cartesian poses 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | namespace moveit { 46 | namespace task_constructor { 47 | namespace stages { 48 | 49 | class FixedCartesianPoses : public MonitoringGenerator 50 | { 51 | public: 52 | FixedCartesianPoses(const std::string& name = "generate poses"); 53 | 54 | void reset() override; 55 | bool canCompute() const override; 56 | void compute() override; 57 | 58 | void addPose(const geometry_msgs::PoseStamped& pose); 59 | 60 | protected: 61 | void onNewSolution(const SolutionBase& s) override; 62 | ordered upstream_solutions_; 63 | }; 64 | } // namespace stages 65 | } // namespace task_constructor 66 | } // namespace moveit 67 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/stages/fixed_state.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Robert Haschke 36 | Desc: Generator Stage spawning a pre-defined, fixed planning scene state 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | 43 | namespace moveit { 44 | namespace task_constructor { 45 | namespace stages { 46 | 47 | /** Spawn a pre-defined PlanningScene state */ 48 | class FixedState : public Generator 49 | { 50 | public: 51 | FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr scene = nullptr); 52 | void setState(const planning_scene::PlanningScenePtr& scene); 53 | 54 | void setIgnoreCollisions(bool ignore) { setProperty("ignore_collisions", ignore); } 55 | 56 | void reset() override; 57 | bool canCompute() const override; 58 | void compute() override; 59 | 60 | protected: 61 | planning_scene::PlanningScenePtr scene_; 62 | bool ran_ = false; 63 | }; 64 | } // namespace stages 65 | } // namespace task_constructor 66 | } // namespace moveit 67 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/stages/generate_place_pose.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Robert Haschke 36 | Desc: Generator Stage to place an object at a certain pose 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | 43 | namespace moveit { 44 | namespace task_constructor { 45 | namespace stages { 46 | 47 | /** Simple IK pose generator to place an attached object in a specific pose 48 | * 49 | * The "pose" property, inherited from GeneratePose specifies the target pose 50 | * of the grasped object. This stage transforms this pose into a target pose for the ik_frame */ 51 | class GeneratePlacePose : public GeneratePose 52 | { 53 | public: 54 | GeneratePlacePose(const std::string& name = "place pose"); 55 | 56 | void compute() override; 57 | 58 | void setObject(const std::string& object) { setProperty("object", object); } 59 | 60 | protected: 61 | void onNewSolution(const SolutionBase& s) override; 62 | }; 63 | } // namespace stages 64 | } // namespace task_constructor 65 | } // namespace moveit 66 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/stages/generate_pose.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Hamburg University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Michael Goerner 36 | Desc: Generator Stage for poses 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | namespace moveit { 46 | namespace task_constructor { 47 | namespace stages { 48 | 49 | class GeneratePose : public MonitoringGenerator 50 | { 51 | public: 52 | GeneratePose(const std::string& name = "generate pose"); 53 | 54 | void reset() override; 55 | bool canCompute() const override; 56 | void compute() override; 57 | 58 | void setPose(const geometry_msgs::PoseStamped& pose) { setProperty("pose", pose); } 59 | 60 | protected: 61 | void onNewSolution(const SolutionBase& s) override; 62 | ordered upstream_solutions_; 63 | }; 64 | } // namespace stages 65 | } // namespace task_constructor 66 | } // namespace moveit 67 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/stages/noop.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2024, Sherbrooke University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* Authors: Captain Yoshi */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | 41 | namespace moveit { 42 | namespace task_constructor { 43 | namespace stages { 44 | 45 | /** no-op stage, which doesn't modify the interface state nor adds a trajectory. 46 | * However, it can be used to store custom stage properties, 47 | * which in turn can be queried post-planning to steer the execution. 48 | */ 49 | 50 | class NoOp : public PropagatingEitherWay 51 | { 52 | public: 53 | NoOp(const std::string& name = "no-op") : PropagatingEitherWay(name){}; 54 | 55 | private: 56 | bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& /*trajectory*/, 57 | Interface::Direction /*dir*/) override { 58 | scene = state.scene()->diff(); 59 | return true; 60 | }; 61 | }; 62 | } // namespace stages 63 | } // namespace task_constructor 64 | } // namespace moveit 65 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/stages/passthrough.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Hamburg University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holders nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* Authors: Michael 'v4hn' Goerner */ 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | namespace moveit { 41 | namespace task_constructor { 42 | namespace stages { 43 | 44 | /** Generic Wrapper that passes on a solution 45 | * 46 | * Useful to set a custom CostTransform via Stage::setCostTerm 47 | * to change a solution's cost without loosing the original value 48 | */ 49 | class PassThrough : public WrapperBase 50 | { 51 | public: 52 | PassThrough(const std::string& name = "PassThrough", Stage::pointer&& child = Stage::pointer()); 53 | 54 | void onNewSolution(const SolutionBase& s) override; 55 | }; 56 | } // namespace stages 57 | } // namespace task_constructor 58 | } // namespace moveit 59 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/stages/predicate_filter.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Hamburg University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* Authors: Michael Goerner */ 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | namespace moveit { 41 | namespace core { 42 | MOVEIT_CLASS_FORWARD(RobotState); 43 | } 44 | } // namespace moveit 45 | 46 | namespace moveit { 47 | namespace task_constructor { 48 | namespace stages { 49 | 50 | /** Stage Wrapper to filter generated solutions by custom criteria 51 | * 52 | * All solutions of the wrapped class are passed to predicate. 53 | * Solutions are accepted if predicate(s) == true. 54 | * Rejected solutions are forwarded as failures with an optional comment 55 | */ 56 | class PredicateFilter : public WrapperBase 57 | { 58 | public: 59 | using Predicate = std::function; 60 | 61 | PredicateFilter(const std::string& name, Stage::pointer&& child = Stage::pointer()); 62 | 63 | void init(const moveit::core::RobotModelConstPtr& robot_model) override; 64 | 65 | void onNewSolution(const SolutionBase& s) override; 66 | 67 | void setPredicate(const Predicate& p) { setProperty("predicate", p); } 68 | void setIgnoreFilter(bool ignore) { setProperty("ignore_filter", ignore); } 69 | }; 70 | } // namespace stages 71 | } // namespace task_constructor 72 | } // namespace moveit 73 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/task_p.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Robert Haschke 36 | Desc: Private API of class Task 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | 44 | namespace robot_model_loader { 45 | MOVEIT_CLASS_FORWARD(RobotModelLoader); 46 | } 47 | 48 | namespace moveit { 49 | namespace task_constructor { 50 | 51 | class TaskPrivate : public WrapperBasePrivate 52 | { 53 | friend class Task; 54 | TaskPrivate& operator=(TaskPrivate&& other); 55 | 56 | public: 57 | TaskPrivate(Task* me, const std::string& ns); 58 | 59 | const std::string& ns() const { return ns_; } 60 | const ContainerBase* stages() const; 61 | 62 | private: 63 | std::string ns_; 64 | robot_model_loader::RobotModelLoaderPtr robot_model_loader_; 65 | moveit::core::RobotModelConstPtr robot_model_; 66 | std::atomic preempt_requested_; 67 | 68 | // introspection and monitoring 69 | std::unique_ptr introspection_; 70 | std::list task_cbs_; // functions to monitor task's planning progress 71 | }; 72 | PIMPL_FUNCTIONS(Task) 73 | } // namespace task_constructor 74 | } // namespace moveit 75 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/trajectory_execution_info.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2022, PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PickNik Inc. nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Joe Schornak, Sebastian Jahr */ 36 | 37 | #pragma once 38 | 39 | #include 40 | 41 | namespace moveit { 42 | namespace task_constructor { 43 | using TrajectoryExecutionInfo = moveit_task_constructor_msgs::TrajectoryExecutionInfo; 44 | } // namespace task_constructor 45 | } // namespace moveit 46 | -------------------------------------------------------------------------------- /core/include/moveit/task_constructor/type_traits.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Robert Haschke 36 | Desc: type traits for SFINAE-based templating 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | 43 | namespace moveit { 44 | namespace task_constructor { 45 | 46 | // detect STL-like containers by presence of cbegin(), cend() methods 47 | template 48 | struct is_container : std::false_type 49 | {}; 50 | 51 | template 52 | struct is_container_helper 53 | {}; 54 | 55 | template 56 | struct is_container< 57 | T, std::conditional_t().cbegin()), 59 | decltype(std::declval().cend())>, 60 | void> > : public std::true_type 61 | {}; 62 | } // namespace task_constructor 63 | } // namespace moveit 64 | -------------------------------------------------------------------------------- /core/motion_planning_stages_plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Use the current state of the robot (when starting planning) as a target. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | moveit_task_constructor_core 3 | 0.1.3 4 | MoveIt Task Pipeline 5 | 6 | https://github.com/moveit/moveit_task_constructor 7 | https://github.com/moveit/moveit_task_constructor 8 | https://github.com/moveit/moveit_task_constructor/issues 9 | 10 | BSD 11 | Michael Goerner 12 | Robert Haschke 13 | 14 | catkin 15 | python-setuptools 16 | python3-setuptools 17 | 18 | roscpp 19 | roslint 20 | roscpp 21 | 22 | fmt 23 | tf2_eigen 24 | geometry_msgs 25 | moveit_core 26 | moveit_ros_planning 27 | moveit_ros_planning_interface 28 | moveit_task_constructor_msgs 29 | py_binding_tools 30 | visualization_msgs 31 | rviz_marker_tools 32 | 33 | rosunit 34 | rostest 35 | moveit_resources_fanuc_moveit_config 36 | moveit_planners 37 | moveit_commander 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /core/python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # We rely on pybind11's smart_holder branch imported pybind11 via git submodule 2 | 3 | # pybind11 must use the ROS python version 4 | set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION}) 5 | 6 | # Use minimum-size optimization for pybind11 bindings 7 | add_compile_options("-Os") 8 | 9 | # create symlink to grant access to downstream packages in devel space 10 | add_custom_target(pybind11_devel_symlink ALL COMMAND ${CMAKE_COMMAND} -E create_symlink 11 | ${CMAKE_CURRENT_SOURCE_DIR}/pybind11 12 | ${CATKIN_DEVEL_PREFIX}/share/${PROJECT_NAME}/pybind11) 13 | 14 | # configure pybind11 install for use by downstream packages in install space 15 | set(PYBIND11_INSTALL ON CACHE INTERNAL "Install pybind11") 16 | set(CMAKE_INSTALL_INCLUDEDIR include/moveit/python) 17 | set(PYBIND11_CMAKECONFIG_INSTALL_DIR ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake 18 | CACHE INTERNAL "install path for pybind11 cmake files") 19 | 20 | # source pybind11 folder, which exposes its targets and installs them 21 | if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/pybind11/CMakeLists.txt") 22 | message("Missing content of submodule pybind11: Use 'git clone --recurse-submodule' in future.\n" 23 | "Checking out content automatically") 24 | execute_process(COMMAND git submodule init WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) 25 | execute_process(COMMAND git submodule update WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) 26 | endif() 27 | #catkin_lint: ignore_once subproject duplicate_cmd 28 | add_subdirectory(pybind11) 29 | 30 | # C++ wrapper code 31 | add_subdirectory(bindings) 32 | 33 | if(CATKIN_ENABLE_TESTING) 34 | find_package(rostest REQUIRED) 35 | 36 | # Add folders to be run by python nosetests 37 | if(DEFINED ENV{PRELOAD}) 38 | message(WARNING "Skipping python tests due to asan") 39 | else() 40 | catkin_add_nosetests(test) 41 | 42 | # Add rostests 43 | add_rostest(test/rostest_mtc.test) 44 | endif() 45 | endif() 46 | 47 | roslint_python() 48 | -------------------------------------------------------------------------------- /core/python/bindings/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # catkin_lint cannot detect target declarations in functions, here in pybind11_add_module 2 | #catkin_lint: ignore undefined_target 3 | 4 | # moveit.task_constructor 5 | set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor) 6 | pybind11_add_module(pymoveit_mtc 7 | ${INCLUDES}/properties.h 8 | 9 | src/properties.cpp 10 | src/solvers.cpp 11 | src/core.cpp 12 | src/stages.cpp 13 | src/module.cpp 14 | ) 15 | target_include_directories(pymoveit_mtc PUBLIC $) 16 | target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${TOOLS_LIB_NAME}) 17 | set_target_properties(pymoveit_mtc PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}) 18 | 19 | # install python libs 20 | install(TARGETS pymoveit_mtc 21 | LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION} 22 | ) 23 | -------------------------------------------------------------------------------- /core/python/bindings/src/module.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | namespace moveit { 38 | namespace python { 39 | 40 | void export_properties(pybind11::module& m); 41 | void export_solvers(pybind11::module& m); 42 | void export_core(pybind11::module& m); 43 | void export_stages(pybind11::module& m); 44 | 45 | } // namespace python 46 | } // namespace moveit 47 | 48 | PYBIND11_MODULE(pymoveit_mtc, m) { 49 | auto msub = m.def_submodule("core", "Provides wrappers for core C++ classes. " 50 | "**Import as** :doc:`moveit.task_constructor.core`."); 51 | 52 | moveit::python::export_properties(msub); 53 | moveit::python::export_solvers(msub); 54 | moveit::python::export_core(msub); 55 | 56 | msub = m.def_submodule("stages", "Provides wrappers of standard MTC stages. " 57 | "**Import as** :doc:`moveit.task_constructor.stages`."); 58 | moveit::python::export_stages(msub); 59 | } 60 | -------------------------------------------------------------------------------- /core/python/bindings/src/utils.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace moveit { 4 | namespace python { 5 | namespace { 6 | 7 | // utility function to normalize index: negative indeces reference from the end 8 | size_t normalize_index(size_t size, long index) { 9 | if (index < 0) 10 | index += size; 11 | if (index >= long(size) || index < 0) 12 | throw pybind11::index_error("Index out of range"); 13 | return index; 14 | } 15 | 16 | // implement operator[](index) 17 | template 18 | typename T::value_type get_item(const T& container, long index) { 19 | auto it = container.begin(); 20 | std::advance(it, normalize_index(container.size(), index)); 21 | return *it; 22 | } 23 | 24 | } // namespace 25 | } // namespace python 26 | } // namespace moveit 27 | -------------------------------------------------------------------------------- /core/python/src/moveit/__init__.py: -------------------------------------------------------------------------------- 1 | # https://packaging.python.org/guides/packaging-namespace-packages/#pkgutil-style-namespace-packages 2 | __path__ = __import__("pkgutil").extend_path(__path__, __name__) 3 | -------------------------------------------------------------------------------- /core/python/src/moveit/task_constructor/__init__.py: -------------------------------------------------------------------------------- 1 | from . import core, stages 2 | 3 | __doc__ = "top-level module of MoveIt Task constructor" 4 | -------------------------------------------------------------------------------- /core/python/src/moveit/task_constructor/core.py: -------------------------------------------------------------------------------- 1 | from pymoveit_mtc.core import * 2 | 3 | __doc__ = "Provides wrappers for :doc:`core C++ classes `." 4 | -------------------------------------------------------------------------------- /core/python/src/moveit/task_constructor/stages.py: -------------------------------------------------------------------------------- 1 | from pymoveit_mtc.stages import * 2 | 3 | __doc__ = "Provides wrappers for :doc:`stage C++ classes `." 4 | -------------------------------------------------------------------------------- /core/python/test/rostest_mtc.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import unittest 4 | import rostest 5 | from py_binding_tools import roscpp_init 6 | from moveit.task_constructor import core, stages 7 | from geometry_msgs.msg import PoseStamped 8 | from geometry_msgs.msg import Vector3Stamped, Vector3 9 | from std_msgs.msg import Header 10 | 11 | 12 | def setUpModule(): 13 | roscpp_init("test_mtc") 14 | 15 | 16 | class Test(unittest.TestCase): 17 | PLANNING_GROUP = "manipulator" 18 | 19 | def test_MoveAndExecute(self): 20 | moveRel = stages.MoveRelative("moveRel", core.JointInterpolationPlanner()) 21 | moveRel.group = self.PLANNING_GROUP 22 | moveRel.setDirection({"joint_1": 0.2, "joint_2": 0.4}) 23 | 24 | moveTo = stages.MoveTo("moveTo", core.JointInterpolationPlanner()) 25 | moveTo.group = self.PLANNING_GROUP 26 | moveTo.setGoal("all-zeros") 27 | 28 | task = core.Task() 29 | task.add(stages.CurrentState("current"), moveRel, moveTo) 30 | 31 | self.assertTrue(task.plan()) 32 | self.assertEqual(len(task.solutions), 1) 33 | task.execute(task.solutions[0]) 34 | 35 | def test_Merger(self): 36 | cartesian = core.CartesianPath() 37 | 38 | def createDisplacement(group, displacement): 39 | move = stages.MoveRelative("displace", cartesian) 40 | move.group = group 41 | move.ik_frame = PoseStamped(header=Header(frame_id="tool0")) 42 | dir = Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(*displacement)) 43 | move.setDirection(dir) 44 | move.restrictDirection(stages.MoveRelative.Direction.FORWARD) 45 | return move 46 | 47 | task = core.Task() 48 | task.add(stages.CurrentState("current")) 49 | merger = core.Merger("merger") 50 | merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0])) 51 | merger.insert(createDisplacement(self.PLANNING_GROUP, [0.2, 0, 0])) 52 | task.add(merger) 53 | 54 | task.enableIntrospection() 55 | task.init() 56 | self.assertFalse(task.plan()) 57 | 58 | 59 | if __name__ == "__main__": 60 | rostest.rosrun("mtc", "base", Test) 61 | -------------------------------------------------------------------------------- /core/python/test/rostest_mtc.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /core/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | name: Python API 3 | output_dir: python 4 | sphinx_root_dir: doc 5 | - builder: doxygen 6 | name: C++ API 7 | output_dir: cpp 8 | file_patterns: "*.c *.cpp *.h *.cc *.hh *.dox" 9 | exclude_patterns: "*/core/python/pybind11/* */core/python/bindings/* */test/* " 10 | exclude_symbols: "*Private class_ declval*" 11 | -------------------------------------------------------------------------------- /core/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import find_packages, setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | # list of packages to setup 8 | packages=find_packages("python/src"), 9 | # specify location of root ("") package dir 10 | package_dir={"": "python/src"}, 11 | ) 12 | dist = setup(**d) 13 | 14 | # Remove moveit/__init__.py when building .deb packages 15 | # Otherwise, the installation procedure will complain about conflicting files (with moveit_core) 16 | try: 17 | # installation dir (.../lib/python3/dist-packages) 18 | libdir = dist.command_obj["install_lib"].install_dir 19 | if "/debian/ros-" in libdir and "moveit-task-constructor-core/opt/ros/" in libdir: 20 | import os 21 | import shutil 22 | 23 | os.remove(os.path.join(libdir, "moveit", "__init__.py")) 24 | shutil.rmtree(os.path.join(libdir, "moveit", "__pycache__")) 25 | except AttributeError as e: 26 | pass 27 | -------------------------------------------------------------------------------- /core/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(${PROJECT_NAME} 2 | ${PROJECT_INCLUDE}/container.h 3 | ${PROJECT_INCLUDE}/container_p.h 4 | ${PROJECT_INCLUDE}/cost_queue.h 5 | ${PROJECT_INCLUDE}/introspection.h 6 | ${PROJECT_INCLUDE}/marker_tools.h 7 | ${PROJECT_INCLUDE}/merge.h 8 | ${PROJECT_INCLUDE}/moveit_compat.h 9 | ${PROJECT_INCLUDE}/properties.h 10 | ${PROJECT_INCLUDE}/stage.h 11 | ${PROJECT_INCLUDE}/stage_p.h 12 | ${PROJECT_INCLUDE}/storage.h 13 | ${PROJECT_INCLUDE}/task.h 14 | ${PROJECT_INCLUDE}/task_p.h 15 | ${PROJECT_INCLUDE}/utils.h 16 | 17 | ${PROJECT_INCLUDE}/solvers/planner_interface.h 18 | ${PROJECT_INCLUDE}/solvers/cartesian_path.h 19 | ${PROJECT_INCLUDE}/solvers/joint_interpolation.h 20 | ${PROJECT_INCLUDE}/solvers/pipeline_planner.h 21 | ${PROJECT_INCLUDE}/solvers/multi_planner.h 22 | 23 | container.cpp 24 | cost_terms.cpp 25 | introspection.cpp 26 | marker_tools.cpp 27 | merge.cpp 28 | properties.cpp 29 | stage.cpp 30 | storage.cpp 31 | task.cpp 32 | utils.cpp 33 | 34 | solvers/planner_interface.cpp 35 | solvers/cartesian_path.cpp 36 | solvers/joint_interpolation.cpp 37 | solvers/pipeline_planner.cpp 38 | solvers/multi_planner.cpp 39 | ) 40 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt) 41 | target_include_directories(${PROJECT_NAME} 42 | PUBLIC $ 43 | PUBLIC $ 44 | ) 45 | target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) 46 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 47 | 48 | add_subdirectory(stages) 49 | 50 | install(TARGETS ${PROJECT_NAME} 51 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 53 | -------------------------------------------------------------------------------- /core/src/solvers/planner_interface.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Robert Haschke 36 | Desc: Planner Interface: implementation details shared across different planners 37 | */ 38 | 39 | #include 40 | #include 41 | 42 | using namespace trajectory_processing; 43 | 44 | namespace moveit { 45 | namespace task_constructor { 46 | namespace solvers { 47 | 48 | PlannerInterface::PlannerInterface() { 49 | auto& p = properties(); 50 | p.declare("timeout", std::numeric_limits::infinity(), "timeout for planner (s)"); 51 | p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); 52 | p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); 53 | p.declare("time_parameterization", std::make_shared()); 54 | } 55 | } // namespace solvers 56 | } // namespace task_constructor 57 | } // namespace moveit 58 | -------------------------------------------------------------------------------- /core/src/stages/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(${PROJECT_NAME}_stages 2 | ${PROJECT_INCLUDE}/stages/modify_planning_scene.h 3 | ${PROJECT_INCLUDE}/stages/fix_collision_objects.h 4 | 5 | ${PROJECT_INCLUDE}/stages/current_state.h 6 | ${PROJECT_INCLUDE}/stages/fixed_state.h 7 | ${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h 8 | ${PROJECT_INCLUDE}/stages/generate_pose.h 9 | ${PROJECT_INCLUDE}/stages/generate_random_pose.h 10 | ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h 11 | ${PROJECT_INCLUDE}/stages/generate_place_pose.h 12 | ${PROJECT_INCLUDE}/stages/compute_ik.h 13 | ${PROJECT_INCLUDE}/stages/passthrough.h 14 | ${PROJECT_INCLUDE}/stages/noop.h 15 | ${PROJECT_INCLUDE}/stages/predicate_filter.h 16 | 17 | ${PROJECT_INCLUDE}/stages/connect.h 18 | ${PROJECT_INCLUDE}/stages/move_to.h 19 | ${PROJECT_INCLUDE}/stages/move_relative.h 20 | 21 | ${PROJECT_INCLUDE}/stages/simple_grasp.h 22 | ${PROJECT_INCLUDE}/stages/pick.h 23 | 24 | modify_planning_scene.cpp 25 | fix_collision_objects.cpp 26 | 27 | current_state.cpp 28 | fixed_state.cpp 29 | fixed_cartesian_poses.cpp 30 | generate_pose.cpp 31 | generate_random_pose.cpp 32 | generate_grasp_pose.cpp 33 | generate_place_pose.cpp 34 | compute_ik.cpp 35 | passthrough.cpp 36 | predicate_filter.cpp 37 | 38 | connect.cpp 39 | move_to.cpp 40 | move_relative.cpp 41 | 42 | simple_grasp.cpp 43 | pick.cpp 44 | ) 45 | target_link_libraries(${PROJECT_NAME}_stages PUBLIC ${PROJECT_NAME} ${catkin_LIBRARIES}) 46 | 47 | add_library(${PROJECT_NAME}_stage_plugins 48 | plugins.cpp 49 | ) 50 | target_link_libraries(${PROJECT_NAME}_stage_plugins ${PROJECT_NAME}_stages ${catkin_LIBRARIES}) 51 | 52 | install(TARGETS ${PROJECT_NAME}_stages ${PROJECT_NAME}_stage_plugins 53 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 54 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 55 | -------------------------------------------------------------------------------- /core/src/stages/fixed_state.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Authors: Robert Haschke */ 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | namespace moveit { 43 | namespace task_constructor { 44 | namespace stages { 45 | 46 | FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene) 47 | : Generator(name), scene_(std::move(scene)) { 48 | properties().declare("ignore_collisions", false); 49 | setCostTerm(std::make_unique(0.0)); 50 | } 51 | 52 | void FixedState::setState(const planning_scene::PlanningScenePtr& scene) { 53 | scene_ = scene; 54 | } 55 | 56 | void FixedState::reset() { 57 | Generator::reset(); 58 | ran_ = false; 59 | } 60 | 61 | bool FixedState::canCompute() const { 62 | return !ran_ && scene_; 63 | } 64 | 65 | void FixedState::compute() { 66 | SubTrajectory trajectory; 67 | if (!properties().get("ignore_collisions") && scene_->isStateColliding()) { 68 | trajectory.markAsFailure("in collision"); 69 | } 70 | 71 | spawn(InterfaceState(scene_), std::move(trajectory)); 72 | ran_ = true; 73 | } 74 | } // namespace stages 75 | } // namespace task_constructor 76 | } // namespace moveit 77 | -------------------------------------------------------------------------------- /core/src/stages/passthrough.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Hamburg University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holders nor the names of their 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* Author: Michael 'v4hn' Goerner */ 35 | 36 | #include 37 | 38 | namespace moveit { 39 | namespace task_constructor { 40 | namespace stages { 41 | 42 | PassThrough::PassThrough(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {} 43 | 44 | void PassThrough::onNewSolution(const SolutionBase& s) { 45 | this->liftSolution(s); 46 | } 47 | 48 | } // namespace stages 49 | } // namespace task_constructor 50 | } // namespace moveit 51 | -------------------------------------------------------------------------------- /core/src/stages/plugins.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | /// register plugins to use with ClassLoader 6 | 7 | PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::CurrentState, moveit::task_constructor::Stage) 8 | -------------------------------------------------------------------------------- /core/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############# 2 | ## Testing ## 3 | ############# 4 | 5 | ## Add gtest based cpp test target and link libraries 6 | if (CATKIN_ENABLE_TESTING) 7 | find_package(rostest REQUIRED) 8 | 9 | add_library(gtest_utils gtest_value_printers.cpp models.cpp stage_mockups.cpp) 10 | target_link_libraries(gtest_utils ${PROJECT_NAME}) 11 | 12 | macro(mtc_add_test TYPE) 13 | # Split ARGN into .test file and other sources 14 | set(TEST_FILE ${ARGN}) 15 | set(SOURCES ${ARGN}) 16 | list(FILTER TEST_FILE INCLUDE REGEX "\.test$") 17 | list(FILTER SOURCES EXCLUDE REGEX "\.test$") 18 | # Determine TARGET name from first source file 19 | list(GET SOURCES 0 TEST_NAME) 20 | string(REGEX REPLACE "\.cpp$" "" TEST_NAME ${TEST_NAME}) 21 | string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME}) 22 | # Configure build target 23 | if(TEST_FILE) # Add rostest if .test file was specified 24 | _add_rostest_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${TEST_FILE} ${SOURCES}) 25 | else() 26 | _catkin_add_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) 27 | endif() 28 | target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) 29 | endmacro() 30 | macro(mtc_add_gtest) 31 | mtc_add_test("gtest" ${ARGN}) 32 | endmacro() 33 | macro(mtc_add_gmock) 34 | mtc_add_test("gmock" ${ARGN}) 35 | endmacro() 36 | 37 | mtc_add_gmock(test_mockups.cpp) 38 | mtc_add_gtest(test_stage.cpp) 39 | mtc_add_gtest(test_container.cpp) 40 | mtc_add_gmock(test_serial.cpp) 41 | mtc_add_gmock(test_pruning.cpp) 42 | mtc_add_gtest(test_properties.cpp) 43 | mtc_add_gtest(test_cost_terms.cpp) 44 | 45 | mtc_add_gmock(test_fallback.cpp) 46 | mtc_add_gmock(test_cost_queue.cpp) 47 | mtc_add_gmock(test_interface_state.cpp) 48 | 49 | mtc_add_gtest(test_move_to.cpp move_to.test) 50 | mtc_add_gtest(test_move_relative.cpp move_relative.test) 51 | 52 | # building these integration tests works without moveit config packages 53 | add_executable(pick_ur5 pick_ur5.cpp) 54 | target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages gtest) 55 | 56 | add_executable(pick_pr2 pick_pr2.cpp) 57 | target_link_libraries(pick_pr2 ${PROJECT_NAME}_stages gtest) 58 | 59 | add_executable(pick_pa10 pick_pa10.cpp) 60 | target_link_libraries(pick_pa10 ${PROJECT_NAME}_stages gtest) 61 | 62 | # running these integrations test naturally requires the moveit configs 63 | find_package(tams_ur5_setup_moveit_config QUIET) 64 | if(tams_ur5_setup_moveit_config_FOUND) 65 | add_rostest(pick_ur5.test) 66 | endif() 67 | find_package(pr2_moveit_config_FOUND QUIET) 68 | if(pr2_moveit_config_FOUND) 69 | add_rostest(pick_pr2.test) 70 | endif() 71 | find_package(pa10_shadow_moveit_config QUIET) 72 | if(pa10_shadow_moveit_config_FOUND) 73 | add_rostest(pick_pa10.test) 74 | endif() 75 | endif() 76 | -------------------------------------------------------------------------------- /core/test/gtest_value_printers.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest_value_printers.h" 2 | 3 | using namespace moveit::task_constructor; 4 | 5 | ::std::ostream& operator<<(::std::ostream& os, const InterfaceFlag& flag) { 6 | switch (flag) { 7 | case READS_START: 8 | return os << "READS_START"; 9 | case READS_END: 10 | return os << "READS_END"; 11 | case WRITES_NEXT_START: 12 | return os << "WRITES_NEXT_START"; 13 | case WRITES_PREV_END: 14 | return os << "WRITES_PREV_END"; 15 | default: 16 | return os << "unknown"; 17 | } 18 | } 19 | 20 | ::std::ostream& operator<<(::std::ostream& os, const InterfaceFlags& flags) { 21 | os << "InterfaceFlags("; 22 | bool have_previous = false; 23 | for (InterfaceFlag f : { READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END }) { 24 | if (flags & f) { 25 | os << (have_previous ? ", " : "{") << f; 26 | have_previous = true; 27 | } 28 | } 29 | return os << (have_previous ? "})" : ")"); 30 | } 31 | -------------------------------------------------------------------------------- /core/test/gtest_value_printers.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | ::std::ostream& operator<<(::std::ostream& os, const moveit::task_constructor::InterfaceFlag& flag); 5 | ::std::ostream& operator<<(::std::ostream& os, const moveit::task_constructor::InterfaceFlags& flags); 6 | -------------------------------------------------------------------------------- /core/test/models.cpp: -------------------------------------------------------------------------------- 1 | #include "models.h" 2 | #include 3 | #include 4 | 5 | using namespace moveit::core; 6 | 7 | RobotModelPtr getModel() { 8 | // suppress RobotModel errors and warnings 9 | ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME ".moveit_core.robot_model", ros::console::levels::Fatal); 10 | 11 | // create dummy robot model 12 | moveit::core::RobotModelBuilder builder("robot", "base"); 13 | builder.addChain("base->link1->link2->tip", "continuous"); 14 | builder.addGroupChain("base", "link2", "group"); 15 | builder.addGroupChain("link2", "tip", "eef_group"); 16 | builder.addEndEffector("eef", "link2", "group", "eef_group"); 17 | return builder.build(); 18 | } 19 | 20 | moveit::core::RobotModelPtr loadModel() { 21 | static robot_model_loader::RobotModelLoader loader; 22 | return loader.getModel(); 23 | } 24 | -------------------------------------------------------------------------------- /core/test/models.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace moveit { 6 | namespace core { 7 | MOVEIT_CLASS_FORWARD(RobotModel); 8 | } 9 | } // namespace moveit 10 | 11 | // get a hard-coded model 12 | moveit::core::RobotModelPtr getModel(); 13 | 14 | // load a model from robot_description 15 | moveit::core::RobotModelPtr loadModel(); 16 | -------------------------------------------------------------------------------- /core/test/move_relative.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /core/test/move_to.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /core/test/pick_pa10.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /core/test/pick_pr2.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | [/move_group/fake_controller_joint_states] 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /core/test/pick_ur5.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | [/move_group/fake_controller_joint_states] 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /core/test/test_mockups.cpp: -------------------------------------------------------------------------------- 1 | #include "stage_mockups.h" 2 | #include 3 | 4 | using namespace moveit::task_constructor; 5 | 6 | struct TestGeneratorMockup : public GeneratorMockup 7 | { 8 | InterfacePtr next_starts_; 9 | InterfacePtr prev_ends_; 10 | 11 | using GeneratorMockup::GeneratorMockup; 12 | using GeneratorMockup::init; 13 | 14 | void init() { 15 | next_starts_ = std::make_shared(); 16 | prev_ends_ = std::make_shared(); 17 | 18 | GeneratorMockup::reset(); 19 | GeneratorMockup::init(getModel()); 20 | 21 | GeneratorPrivate* impl = pimpl(); 22 | impl->setNextStarts(next_starts_); 23 | impl->setPrevEnds(prev_ends_); 24 | } 25 | }; 26 | 27 | TEST(GeneratorMockup, compute) { 28 | TestGeneratorMockup gen({ 1.0, 2.0, 3.0 }); 29 | gen.init(); 30 | 31 | for (int i = 0; i < 3; ++i) { 32 | ASSERT_TRUE(gen.canCompute()); 33 | gen.compute(); 34 | } 35 | EXPECT_FALSE(gen.canCompute()); 36 | 37 | EXPECT_COSTS(gen.solutions(), ::testing::ElementsAre(1, 2, 3)); 38 | } 39 | 40 | #define COMPUTE_EXPECT_COSTS(stage, ...) \ 41 | { \ 42 | EXPECT_TRUE(stage.canCompute()); \ 43 | stage.compute(); \ 44 | EXPECT_COSTS(stage.solutions(), ::testing::ElementsAre(__VA_ARGS__)); \ 45 | constexpr auto num_elements = std::initializer_list{ __VA_ARGS__ }.size(); \ 46 | EXPECT_EQ(runs, num_elements); \ 47 | } 48 | 49 | TEST(GeneratorMockup, delayed) { 50 | auto gen = std::make_unique(PredefinedCosts({ 1.0, 2.0, 3.0 })); 51 | gen->init(); 52 | auto& runs = gen->runs_; 53 | 54 | DelayingWrapper w({ 1, 0, 2 }, std::move(gen)); 55 | auto prev_ends = std::make_shared(); 56 | auto next_starts = std::make_shared(); 57 | 58 | WrapperBasePrivate* impl = w.pimpl(); 59 | impl->setPrevEnds(prev_ends); 60 | impl->setNextStarts(next_starts); 61 | 62 | // 1st compute() is delayed by 1 63 | COMPUTE_EXPECT_COSTS(w); 64 | COMPUTE_EXPECT_COSTS(w, 1); 65 | 66 | // 2nd compute() is not delayed 67 | COMPUTE_EXPECT_COSTS(w, 1, 2); 68 | 69 | // 1st compute() is delayed by 2 70 | COMPUTE_EXPECT_COSTS(w, 1, 2); 71 | COMPUTE_EXPECT_COSTS(w, 1, 2); 72 | COMPUTE_EXPECT_COSTS(w, 1, 2, 3); 73 | 74 | EXPECT_FALSE(w.canCompute()); 75 | } 76 | #undef COMPUTE_EXPECT_COSTS 77 | -------------------------------------------------------------------------------- /core/test/test_serial.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "stage_mockups.h" 9 | #include "models.h" 10 | #include 11 | #include 12 | 13 | using namespace moveit::task_constructor; 14 | using namespace planning_scene; 15 | 16 | /* Connect creating solutions with given costs */ 17 | struct Connect : stages::Connect 18 | { 19 | PredefinedCostsPtr costs_; 20 | unsigned int calls_{ 0 }; 21 | static unsigned int id_; 22 | 23 | static GroupPlannerVector getPlanners() { 24 | auto planner = std::make_shared(); 25 | return { { "group", planner }, { "eef_group", planner } }; 26 | } 27 | 28 | Connect(std::initializer_list costs = { 0.0 }, bool enforce_sequential = false) 29 | : stages::Connect("CON" + std::to_string(++id_), getPlanners()) { 30 | costs_ = std::make_shared(costs, false); 31 | setCostTerm(costs_); 32 | if (enforce_sequential) 33 | setProperty("merge_mode", SEQUENTIAL); 34 | } 35 | void compute(const InterfaceState& from, const InterfaceState& to) override { 36 | ++calls_; 37 | stages::Connect::compute(from, to); 38 | } 39 | }; 40 | 41 | unsigned int Connect::id_ = 0; 42 | 43 | struct TestBase : public TaskTestBase 44 | { 45 | TestBase() { Connect::id_ = 0; } 46 | }; 47 | 48 | using ConnectConnect = TestBase; 49 | // https://github.com/moveit/moveit_task_constructor/issues/182 50 | TEST_F(ConnectConnect, SuccSucc) { 51 | add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 })); 52 | add(t, new Connect()); 53 | add(t, new GeneratorMockup({ 10.0, 20.0 })); 54 | add(t, new Connect()); 55 | add(t, new GeneratorMockup({ 0.0 })); 56 | 57 | EXPECT_TRUE(t.plan()); 58 | EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(11, 12, 13, 21, 22, 23)); 59 | } 60 | 61 | // https://github.com/moveit/moveit_task_constructor/issues/218 62 | TEST_F(ConnectConnect, FailSucc) { 63 | add(t, new GeneratorMockup()); 64 | add(t, new Connect({ INF }, true)); 65 | add(t, new GeneratorMockup()); 66 | add(t, new Connect()); 67 | add(t, new GeneratorMockup()); 68 | add(t, new ForwardMockup(PredefinedCosts::constant(0.0), 0)); 69 | 70 | EXPECT_FALSE(t.plan()); 71 | } 72 | 73 | // https://github.com/moveit/moveit_task_constructor/issues/485#issuecomment-1760606116 74 | TEST_F(ConnectConnect, UniqueEnumeration) { 75 | add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 })); 76 | auto con1 = add(t, new ConnectMockup()); 77 | add(t, new GeneratorMockup({ 10.0, 20.0 })); 78 | auto con2 = add(t, new ConnectMockup({ INF, 0.0, 0.0, 0.0 })); 79 | add(t, new GeneratorMockup({ 100.0, 200.0 })); 80 | 81 | EXPECT_TRUE(t.plan()); 82 | EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(121, 122, 123, 211, 212, 213, 221, 222, 223)); 83 | EXPECT_EQ(con1->runs_, 3u * 2u); 84 | EXPECT_EQ(con2->runs_, 2u * 2u); 85 | } 86 | -------------------------------------------------------------------------------- /core/test/test_stage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | -------------------------------------------------------------------------------- /demo/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package moveit_task_constructor_demo 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.3 (2023-03-06) 6 | ------------------ 7 | * Use const reference instead of reference for ros::NodeHandle (`#437 `_) 8 | * Contributors: Robert Haschke 9 | 10 | 0.1.2 (2023-02-24) 11 | ------------------ 12 | * CartesianPath: Deprecate redundant property setters (`#429 `_) 13 | * Contributors: Robert Haschke 14 | 15 | 0.1.1 (2023-02-15) 16 | ------------------ 17 | * Resort to MoveIt's python tools 18 | * Provide ComputeIK.ik_frame as full PoseStamped 19 | * Fixed build farm issues 20 | * Fixed missing dependency declarations 21 | * pick_place_task: monitor last state before Connect 22 | ... to prune solutions as much as possible 23 | * Contributors: Robert Haschke 24 | 25 | 0.1.0 (2023-02-02) 26 | ------------------ 27 | * Initial release 28 | * Contributors: Michael Görner, Robert Haschke 29 | -------------------------------------------------------------------------------- /demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(moveit_task_constructor_demo) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | moveit_core 7 | moveit_task_constructor_core 8 | moveit_ros_planning_interface 9 | rosparam_shortcuts 10 | ) 11 | 12 | moveit_build_options() 13 | 14 | catkin_package( 15 | CATKIN_DEPENDS roscpp 16 | ) 17 | 18 | add_library(${PROJECT_NAME}_pick_place_task src/pick_place_task.cpp) 19 | target_link_libraries(${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES}) 20 | target_include_directories(${PROJECT_NAME}_pick_place_task PUBLIC include) 21 | target_include_directories(${PROJECT_NAME}_pick_place_task SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) 22 | add_dependencies(${PROJECT_NAME}_pick_place_task ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 23 | install(TARGETS ${PROJECT_NAME}_pick_place_task 24 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 25 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 26 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 27 | ) 28 | 29 | # declare a demo consisting of a single cpp file 30 | function(demo name) 31 | add_executable(${PROJECT_NAME}_${name} src/${name}.cpp) 32 | add_dependencies(${PROJECT_NAME}_${name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 33 | target_link_libraries(${PROJECT_NAME}_${name} ${catkin_LIBRARIES}) 34 | target_include_directories(${PROJECT_NAME}_${name} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) 35 | set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "") 36 | install(TARGETS ${PROJECT_NAME}_${name} 37 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 40 | ) 41 | endfunction() 42 | 43 | demo(cartesian) 44 | demo(modular) 45 | demo(alternative_path_costs) 46 | demo(ik_clearance_cost) 47 | demo(fallbacks_move_to) 48 | 49 | demo(pick_place_demo) 50 | target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task) 51 | 52 | install(DIRECTORY launch config 53 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 54 | ) 55 | 56 | add_subdirectory(test) 57 | -------------------------------------------------------------------------------- /demo/README.md: -------------------------------------------------------------------------------- 1 | # moveit_task_constructor_demo 2 | 3 | Description: A simple pick & place demo using MoveIt Task Constructor. This uses the Panda from Franka Emika 4 | 5 | Developed by Henning Kayser & Simon Goldstein at [PickNik Consulting](http://picknik.ai/) 6 | 7 | ## Run 8 | 9 | Run demo 10 | 11 | roslaunch moveit_task_constructor_demo demo.launch 12 | -------------------------------------------------------------------------------- /demo/config/panda_config.yaml: -------------------------------------------------------------------------------- 1 | # Total planning attempts 2 | max_solutions: 10 3 | 4 | # Planning group and link names 5 | arm_group_name: "panda_arm" 6 | eef_name: "hand" 7 | hand_group_name: "hand" 8 | hand_frame: "panda_link8" 9 | 10 | # Poses 11 | hand_open_pose: "open" 12 | hand_close_pose: "close" 13 | arm_home_pose: "ready" 14 | 15 | # Scene frames 16 | world_frame: "world" 17 | table_reference_frame: "world" 18 | object_reference_frame: "world" 19 | surface_link: "table" 20 | 21 | # Collision object for picking 22 | # CYLINDER object specifications 23 | object_name: "object" 24 | object_dimensions: [0.25, 0.02] # [height, radius] 25 | object_pose: [0.5, -0.25, 0.0, 0, 0, 0] 26 | 27 | # Table model 28 | spawn_table: true 29 | table_name: "table" 30 | table_dimensions: [0.4, 0.5, 0.1] # [length, width, height] 31 | table_pose: [0.5, -0.25, 0, 0, 0, 0] 32 | 33 | # Gripper grasp frame transform [x,y,z,r,p,y] 34 | grasp_frame_transform: [0, 0, 0.1, 1.571, 0.785, 1.571] 35 | 36 | # Place pose [x,y,z,r,p,y] 37 | place_pose: [0.6, -0.15, 0, 0, 0, 0] 38 | place_surface_offset: 0.0001 # place offset from table 39 | 40 | # Valid distance range when approaching an object for picking 41 | approach_object_min_dist: 0.1 42 | approach_object_max_dist: 0.15 43 | 44 | # Valid height range when lifting an object after pick 45 | lift_object_min_dist: 0.01 46 | lift_object_max_dist: 0.1 47 | -------------------------------------------------------------------------------- /demo/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /demo/launch/pickplace.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_task_constructor_demo 4 | 0.1.3 5 | demo tasks illustrating various capabilities of MTC. 6 | 7 | Robert Haschke 8 | Simon Goldstein 9 | Henning Kayser 10 | Robert Haschke 11 | 12 | BSD 13 | 14 | catkin 15 | 16 | roscpp 17 | rosparam_shortcuts 18 | 19 | moveit_core 20 | moveit_ros_planning_interface 21 | 22 | moveit_task_constructor_core 23 | 24 | moveit_task_constructor_capabilities 25 | moveit_resources_panda_moveit_config 26 | py_binding_tools 27 | 28 | rostest 29 | moveit_fake_controller_manager 30 | 31 | -------------------------------------------------------------------------------- /demo/scripts/alternatives.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.task_constructor import core, stages 5 | from py_binding_tools import roscpp_init 6 | import time 7 | 8 | roscpp_init("mtc_tutorial") 9 | 10 | # Use the joint interpolation planner 11 | jointPlanner = core.JointInterpolationPlanner() 12 | 13 | # Create a task 14 | task = core.Task() 15 | task.name = "alternatives" 16 | 17 | # Start from current robot state 18 | currentState = stages.CurrentState("current state") 19 | 20 | # Add the current state to the task hierarchy 21 | task.add(currentState) 22 | 23 | # [initAndConfigAlternatives] 24 | # The alternatives stage supports multiple execution paths 25 | alternatives = core.Alternatives("Alternatives") 26 | 27 | # goal 1 28 | goalConfig1 = { 29 | "panda_joint1": 1.0, 30 | "panda_joint2": -1.0, 31 | "panda_joint3": 0.0, 32 | "panda_joint4": -2.5, 33 | "panda_joint5": 1.0, 34 | "panda_joint6": 1.0, 35 | "panda_joint7": 1.0, 36 | } 37 | 38 | # goal 2 39 | goalConfig2 = { 40 | "panda_joint1": -3.0, 41 | "panda_joint2": -1.0, 42 | "panda_joint3": 0.0, 43 | "panda_joint4": -2.0, 44 | "panda_joint5": 1.0, 45 | "panda_joint6": 2.0, 46 | "panda_joint7": 0.5, 47 | } 48 | 49 | # First motion plan to compare 50 | moveTo1 = stages.MoveTo("Move To Goal Configuration 1", jointPlanner) 51 | moveTo1.group = "panda_arm" 52 | moveTo1.setGoal(goalConfig1) 53 | alternatives.insert(moveTo1) 54 | 55 | # Second motion plan to compare 56 | moveTo2 = stages.MoveTo("Move To Goal Configuration 2", jointPlanner) 57 | moveTo2.group = "panda_arm" 58 | moveTo2.setGoal(goalConfig2) 59 | alternatives.insert(moveTo2) 60 | 61 | # Add the alternatives stage to the task hierarchy 62 | task.add(alternatives) 63 | # [initAndConfigAlternatives] 64 | 65 | if task.plan(): 66 | task.publish(task.solutions[0]) 67 | time.sleep(1) 68 | -------------------------------------------------------------------------------- /demo/scripts/cartesian.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from std_msgs.msg import Header 5 | from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3 6 | from moveit.task_constructor import core, stages 7 | from math import pi 8 | import time 9 | 10 | from py_binding_tools import roscpp_init 11 | 12 | roscpp_init("mtc_tutorial") 13 | 14 | # [cartesianTut1] 15 | group = "panda_arm" 16 | # [cartesianTut1] 17 | 18 | # [cartesianTut2] 19 | # Cartesian and joint-space interpolation planners 20 | cartesian = core.CartesianPath() 21 | jointspace = core.JointInterpolationPlanner() 22 | # [cartesianTut2] 23 | 24 | # [cartesianTut3] 25 | task = core.Task() 26 | task.name = "cartesian" 27 | 28 | # start from current robot state 29 | task.add(stages.CurrentState("current state")) 30 | # [cartesianTut3] 31 | 32 | # [initAndConfigMoveRelative] 33 | # move along x 34 | move = stages.MoveRelative("x +0.2", cartesian) 35 | move.group = group 36 | header = Header(frame_id="world") 37 | move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0))) 38 | task.add(move) 39 | # [initAndConfigMoveRelative] 40 | 41 | # [cartesianTut4] 42 | # move along y 43 | move = stages.MoveRelative("y -0.3", cartesian) 44 | move.group = group 45 | move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0))) 46 | task.add(move) 47 | # [cartesianTut4] 48 | 49 | # [cartesianTut5] 50 | # rotate about z 51 | move = stages.MoveRelative("rz +45°", cartesian) 52 | move.group = group 53 | move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0, 0, pi / 4.0)))) 54 | task.add(move) 55 | # [cartesianTut5] 56 | 57 | # [cartesianTut6] 58 | # Cartesian motion, defined as joint-space offset 59 | move = stages.MoveRelative("joint offset", cartesian) 60 | move.group = group 61 | move.setDirection(dict(panda_joint1=pi / 6, panda_joint3=-pi / 6)) 62 | task.add(move) 63 | # [cartesianTut6] 64 | 65 | # [initAndConfigMoveTo] 66 | # moveTo named posture, using joint-space interplation 67 | move = stages.MoveTo("moveTo ready", jointspace) 68 | move.group = group 69 | move.setGoal("ready") 70 | task.add(move) 71 | # [initAndConfigMoveTo] 72 | 73 | if task.plan(): 74 | task.publish(task.solutions[0]) 75 | time.sleep(100) 76 | -------------------------------------------------------------------------------- /demo/scripts/compute_ik.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.task_constructor import core, stages 5 | from geometry_msgs.msg import PoseStamped, Pose, Point 6 | from std_msgs.msg import Header 7 | import time 8 | 9 | from py_binding_tools import roscpp_init 10 | 11 | roscpp_init("mtc_tutorial") 12 | 13 | # Specify the planning group 14 | group = "panda_arm" 15 | 16 | # Create a task 17 | task = core.Task() 18 | task.name = "compute IK" 19 | 20 | # Add a stage to retrieve the current state 21 | task.add(stages.CurrentState("current state")) 22 | 23 | # Add a planning stage connecting the generator stages 24 | planner = core.PipelinePlanner() # create default planning pipeline 25 | task.add(stages.Connect("connect", [(group, planner)])) # operate on group 26 | del planner # Delete PipelinePlanner when not explicitly needed anymore 27 | 28 | # [propertyTut12] 29 | # Add a Cartesian pose generator 30 | generator = stages.GeneratePose("cartesian pose") 31 | # [propertyTut12] 32 | # Inherit PlanningScene state from "current state" stage 33 | generator.setMonitoredStage(task["current state"]) 34 | # Configure target pose 35 | # [propertyTut13] 36 | pose = Pose(position=Point(z=0.2)) 37 | generator.pose = PoseStamped(header=Header(frame_id="panda_link8"), pose=pose) 38 | # [propertyTut13] 39 | 40 | # [initAndConfigComputeIk] 41 | # Wrap Cartesian generator into a ComputeIK stage to yield a joint pose 42 | computeIK = stages.ComputeIK("compute IK", generator) 43 | computeIK.group = group # Use the group-specific IK solver 44 | # Which end-effector frame should reach the target? 45 | computeIK.ik_frame = PoseStamped(header=Header(frame_id="panda_link8")) 46 | computeIK.max_ik_solutions = 4 # Limit the number of IK solutions 47 | # [propertyTut14] 48 | props = computeIK.properties 49 | # derive target_pose from child's solution 50 | props.configureInitFrom(core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"]) 51 | # [propertyTut14] 52 | 53 | # Add the stage to the task hierarchy 54 | task.add(computeIK) 55 | # [initAndConfigComputeIk] 56 | 57 | if task.plan(): 58 | task.publish(task.solutions[0]) 59 | 60 | time.sleep(1) # sleep some time to allow C++ threads to publish their messages 61 | -------------------------------------------------------------------------------- /demo/scripts/constrained.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from std_msgs.msg import Header 5 | from geometry_msgs.msg import Vector3Stamped, Vector3, Pose 6 | from moveit_msgs.msg import CollisionObject, Constraints, OrientationConstraint 7 | from shape_msgs.msg import SolidPrimitive 8 | from moveit.task_constructor import core, stages 9 | import time 10 | 11 | from py_binding_tools import roscpp_init 12 | 13 | roscpp_init("mtc_tutorial") 14 | 15 | group = "panda_arm" 16 | planner = core.PipelinePlanner() 17 | 18 | task = core.Task() 19 | task.name = "constrained" 20 | 21 | task.add(stages.CurrentState("current state")) 22 | 23 | co = CollisionObject() 24 | co.header.frame_id = "world" 25 | co.id = "obstacle" 26 | sphere = SolidPrimitive() 27 | sphere.type = sphere.SPHERE 28 | sphere.dimensions.insert(sphere.SPHERE_RADIUS, 0.1) 29 | 30 | pose = Pose() 31 | pose.position.x = 0.3 32 | pose.position.y = 0.2 33 | pose.position.z = 0.5 34 | pose.orientation.w = 1.0 35 | co.primitives.append(sphere) 36 | co.primitive_poses.append(pose) 37 | co.operation = co.ADD 38 | 39 | mps = stages.ModifyPlanningScene("modify planning scene") 40 | mps.addObject(co) 41 | 42 | co.id = "object" 43 | co.primitives[0].type = SolidPrimitive.BOX 44 | co.primitives[0].dimensions = [0.1, 0.05, 0.03] 45 | pose = co.primitive_poses[0] 46 | pose.position.x = 0.30702 47 | pose.position.y = 0.0 48 | pose.position.z = 0.485 49 | pose.orientation.x = pose.orientation.w = 0.70711 # 90° about x 50 | mps.addObject(co) 51 | mps.attachObjects(["object"], "panda_hand") 52 | 53 | task.add(mps) 54 | 55 | move = stages.MoveRelative("y +0.4", planner) 56 | move.timeout = 5 57 | move.group = group 58 | header = Header(frame_id="world") 59 | move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, 0.4, 0))) 60 | 61 | constraints = Constraints() 62 | oc = OrientationConstraint() 63 | oc.parameterization = oc.ROTATION_VECTOR 64 | oc.header.frame_id = "world" 65 | oc.link_name = "object" 66 | oc.orientation.x = oc.orientation.w = 0.70711 # 90° about x 67 | oc.absolute_x_axis_tolerance = 0.1 68 | oc.absolute_y_axis_tolerance = 0.1 69 | oc.absolute_z_axis_tolerance = 3.14 70 | oc.weight = 1.0 71 | constraints.orientation_constraints.append(oc) 72 | move.path_constraints = constraints 73 | 74 | task.add(move) 75 | 76 | if task.plan(): 77 | task.publish(task.solutions[0]) 78 | time.sleep(100) 79 | -------------------------------------------------------------------------------- /demo/scripts/current_state.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.task_constructor import core, stages 5 | from py_binding_tools import roscpp_init 6 | import time 7 | 8 | roscpp_init("mtc_tutorial") 9 | 10 | # Create a task 11 | task = core.Task() 12 | task.name = "current state" 13 | 14 | # Get the current robot state 15 | currentState = stages.CurrentState("current state") 16 | 17 | # Add the stage to the task hierarchy 18 | task.add(currentState) 19 | 20 | if task.plan(): 21 | task.publish(task.solutions[0]) 22 | time.sleep(1) 23 | -------------------------------------------------------------------------------- /demo/scripts/fallbacks.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.task_constructor import core, stages 5 | from py_binding_tools import roscpp_init 6 | import time 7 | 8 | roscpp_init("mtc_tutorial") 9 | 10 | # use cartesian and joint interpolation planners 11 | cartesianPlanner = core.CartesianPath() 12 | jointPlanner = core.JointInterpolationPlanner() 13 | 14 | # initialize the mtc task 15 | task = core.Task() 16 | task.name = "fallbacks" 17 | 18 | # add the current planning scene state to the task hierarchy 19 | currentState = stages.CurrentState("Current State") 20 | task.add(currentState) 21 | 22 | # [initAndConfigFallbacks] 23 | # create a fallback container to fall back to a different planner 24 | # if motion generation fails with the primary one 25 | fallbacks = core.Fallbacks("Fallbacks") 26 | 27 | # primary motion plan 28 | moveTo1 = stages.MoveTo("Move To Goal Configuration 1", cartesianPlanner) 29 | moveTo1.group = "panda_arm" 30 | moveTo1.setGoal("extended") 31 | fallbacks.insert(moveTo1) 32 | 33 | # fallback motion plan 34 | moveTo2 = stages.MoveTo("Move To Goal Configuration 2", jointPlanner) 35 | moveTo2.group = "panda_arm" 36 | moveTo2.setGoal("extended") 37 | fallbacks.insert(moveTo2) 38 | 39 | # add the fallback container to the task hierarchy 40 | task.add(fallbacks) 41 | # [initAndConfigFallbacks] 42 | 43 | if task.plan(): 44 | task.publish(task.solutions[0]) 45 | time.sleep(1) 46 | -------------------------------------------------------------------------------- /demo/scripts/fix_collision_objects.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.task_constructor import core, stages 5 | from py_binding_tools import roscpp_init 6 | import time 7 | 8 | roscpp_init("mtc_tutorial") 9 | 10 | # Create a task 11 | task = core.Task() 12 | task.name = "fix collision objects" 13 | 14 | # Add the current state to the task hierarchy 15 | task.add(stages.CurrentState("current state")) 16 | 17 | # [initAndConfig] 18 | # check for collisions and find corrections 19 | fixCollisionObjects = stages.FixCollisionObjects("FixCollisionObjects") 20 | 21 | # cut off length for collision fixing 22 | fixCollisionObjects.max_penetration = 0.01 23 | 24 | # Add the stage to the task hierarchy 25 | task.add(fixCollisionObjects) 26 | # [initAndConfig] 27 | 28 | if task.plan(): 29 | task.publish(task.solutions[0]) 30 | time.sleep(1) 31 | -------------------------------------------------------------------------------- /demo/scripts/fixed_state.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.core import planning_scene 5 | from moveit.task_constructor import core, stages 6 | from py_binding_tools import roscpp_init 7 | from moveit.core.planning_scene import PlanningScene 8 | import time 9 | 10 | roscpp_init("mtc_tutorial") 11 | 12 | 13 | # Create a task 14 | task = core.Task() 15 | task.name = "fixed state" 16 | 17 | # [initAndConfigFixedState] 18 | # Initialize a PlanningScene for use in a FixedState stage 19 | task.loadRobotModel() # load the robot model (usually done in init()) 20 | planningScene = PlanningScene(task.getRobotModel()) 21 | 22 | # Create a FixedState stage and pass the created PlanningScene as its state 23 | fixedState = stages.FixedState("fixed state") 24 | fixedState.setState(planningScene) 25 | 26 | # Add the stage to the task hierarchy 27 | task.add(fixedState) 28 | # [initAndConfigFixedState] 29 | 30 | if task.plan(): 31 | task.publish(task.solutions[0]) 32 | 33 | del planningScene # Avoid ClassLoader warning by destroying the RobotModel 34 | time.sleep(1) 35 | -------------------------------------------------------------------------------- /demo/scripts/generate_pose.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.task_constructor import core, stages 5 | from geometry_msgs.msg import PoseStamped 6 | from py_binding_tools import roscpp_init 7 | import time 8 | 9 | roscpp_init("mtc_tutorial") 10 | 11 | # Specify the planning group 12 | group = "panda_arm" 13 | 14 | # Create a task 15 | task = core.Task() 16 | task.name = "generate pose" 17 | 18 | # Get the current robot state 19 | currentState = stages.CurrentState("current state") 20 | task.add(currentState) 21 | 22 | # Create a planner instance that is used to connect 23 | # the current state to the grasp approach pose 24 | pipelinePlanner = core.PipelinePlanner() 25 | pipelinePlanner.planner = "RRTConnectkConfigDefault" 26 | planners = [(group, pipelinePlanner)] 27 | 28 | # Connect the two stages 29 | connect = stages.Connect("connect1", planners) 30 | connect.properties.configureInitFrom(core.Stage.PropertyInitializerSource.PARENT) 31 | task.add(connect) 32 | 33 | # [initAndConfigGeneratePose] 34 | # create an example pose wrt. the origin of the 35 | # panda arm link8 36 | pose = PoseStamped() 37 | pose.header.frame_id = "panda_link8" 38 | 39 | # Calculate the inverse kinematics for the current robot state 40 | generatePose = stages.GeneratePose("generate pose") 41 | 42 | # spwan a pose whenever there is a solution of the monitored stage 43 | generatePose.setMonitoredStage(task["current state"]) 44 | generatePose.pose = pose 45 | 46 | # Add the stage to the task hierarchy 47 | task.add(generatePose) 48 | # [initAndConfigGeneratePose] 49 | 50 | if task.plan(): 51 | task.publish(task.solutions[0]) 52 | time.sleep(1) 53 | -------------------------------------------------------------------------------- /demo/scripts/merger.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.task_constructor import core, stages 5 | from py_binding_tools import roscpp_init 6 | import time 7 | 8 | roscpp_init("mtc_tutorial") 9 | 10 | # use the joint interpolation planner 11 | planner = core.JointInterpolationPlanner() 12 | 13 | # the task will contain our stages 14 | task = core.Task() 15 | task.name = "merger" 16 | 17 | # start from current robot state 18 | currentState = stages.CurrentState("current state") 19 | task.add(currentState) 20 | 21 | # [initAndConfigMerger] 22 | # the merger plans for two parallel execution paths 23 | merger = core.Merger("Merger") 24 | 25 | # first simultaneous execution 26 | moveTo1 = stages.MoveTo("Move To Home", planner) 27 | moveTo1.group = "hand" 28 | moveTo1.setGoal("close") 29 | merger.insert(moveTo1) 30 | 31 | # second simultaneous execution 32 | moveTo2 = stages.MoveTo("Move To Ready", planner) 33 | moveTo2.group = "panda_arm" 34 | moveTo2.setGoal("extended") 35 | merger.insert(moveTo2) 36 | 37 | # add the merger stage to the task hierarchy 38 | task.add(merger) 39 | # [initAndConfigMerger] 40 | 41 | if task.plan(): 42 | task.publish(task.solutions[0]) 43 | time.sleep(1) 44 | -------------------------------------------------------------------------------- /demo/scripts/modify_planning_scene.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.task_constructor import core, stages 5 | from moveit_msgs.msg import CollisionObject 6 | from shape_msgs.msg import SolidPrimitive 7 | from geometry_msgs.msg import PoseStamped 8 | from py_binding_tools import roscpp_init 9 | import time 10 | 11 | roscpp_init("mtc_tutorial") 12 | 13 | # Create a task 14 | task = core.Task() 15 | task.name = "modify planning scene" 16 | 17 | # Add the current state to the task hierarchy 18 | task.add(stages.CurrentState("current state")) 19 | 20 | # [initAndConfigModifyPlanningScene] 21 | # Specify object parameters 22 | object_name = "grasp_object" 23 | object_radius = 0.02 24 | 25 | objectPose = PoseStamped() 26 | objectPose.header.frame_id = "world" 27 | objectPose.pose.orientation.x = 1.0 28 | objectPose.pose.position.x = 0.30702 29 | objectPose.pose.position.y = 0.0 30 | objectPose.pose.position.z = 0.285 31 | 32 | object = CollisionObject() 33 | object.header.frame_id = "world" 34 | object.id = object_name 35 | sphere = SolidPrimitive() 36 | sphere.type = sphere.SPHERE 37 | sphere.dimensions.insert(sphere.SPHERE_RADIUS, object_radius) 38 | 39 | object.primitives.append(sphere) 40 | object.primitive_poses.append(objectPose.pose) 41 | object.operation = object.ADD 42 | 43 | modifyPlanningScene = stages.ModifyPlanningScene("modify planning scene") 44 | modifyPlanningScene.addObject(object) 45 | task.add(modifyPlanningScene) 46 | # [initAndConfigModifyPlanningScene] 47 | 48 | if task.plan(): 49 | task.publish(task.solutions[0]) 50 | time.sleep(1) 51 | -------------------------------------------------------------------------------- /demo/scripts/multi_planner.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.task_constructor import core, stages 5 | from py_binding_tools import roscpp_init 6 | import time 7 | 8 | roscpp_init("mtc_tutorial") 9 | 10 | ompl_planner = core.PipelinePlanner("ompl") 11 | ompl_planner.planner = "RRTConnectkConfigDefault" 12 | pilz_planner = core.PipelinePlanner("pilz_industrial_motion_planner") 13 | pilz_planner.planner = "PTP" 14 | multiPlanner = core.MultiPlanner() 15 | multiPlanner.add(pilz_planner, ompl_planner) 16 | 17 | # Create a task 18 | task = core.Task() 19 | task.name = "multi planner" 20 | 21 | # Start from current robot state 22 | currentState = stages.CurrentState("current state") 23 | 24 | # Add the current state to the task hierarchy 25 | task.add(currentState) 26 | 27 | # The alternatives stage supports multiple execution paths 28 | alternatives = core.Alternatives("Alternatives") 29 | 30 | # goal 1 31 | goalConfig1 = { 32 | "panda_joint1": 1.0, 33 | "panda_joint2": -1.0, 34 | "panda_joint3": 0.0, 35 | "panda_joint4": -2.5, 36 | "panda_joint5": 1.0, 37 | "panda_joint6": 1.0, 38 | "panda_joint7": 1.0, 39 | } 40 | 41 | # goal 2 42 | goalConfig2 = { 43 | "panda_joint1": -3.0, 44 | "panda_joint2": -1.0, 45 | "panda_joint3": 0.0, 46 | "panda_joint4": -2.0, 47 | "panda_joint5": 1.0, 48 | "panda_joint6": 2.0, 49 | "panda_joint7": 0.5, 50 | } 51 | 52 | # First motion plan to compare 53 | moveTo1 = stages.MoveTo("Move To Goal Configuration 1", multiPlanner) 54 | moveTo1.group = "panda_arm" 55 | moveTo1.setGoal(goalConfig1) 56 | alternatives.insert(moveTo1) 57 | 58 | # Second motion plan to compare 59 | moveTo2 = stages.MoveTo("Move To Goal Configuration 2", multiPlanner) 60 | moveTo2.group = "panda_arm" 61 | moveTo2.setGoal(goalConfig2) 62 | alternatives.insert(moveTo2) 63 | 64 | # Add the alternatives stage to the task hierarchy 65 | task.add(alternatives) 66 | 67 | if task.plan(): 68 | task.publish(task.solutions[0]) 69 | time.sleep(1) 70 | -------------------------------------------------------------------------------- /demo/scripts/properties.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | from moveit.task_constructor import core, stages 5 | from geometry_msgs.msg import PoseStamped 6 | import time 7 | 8 | from py_binding_tools import roscpp_init 9 | 10 | roscpp_init("mtc_tutorial") 11 | 12 | # [propertyTut1] 13 | # Create a property 14 | p = core.Property() 15 | 16 | # Set a descriptive string to describe the properties function 17 | p.setDescription("Foo Property") 18 | # [propertyTut1] 19 | 20 | # Set the current and the default value 21 | p.setValue("Bar") 22 | 23 | # [propertyTut2] 24 | # Check if the property is defined 25 | assert p.defined() 26 | # [propertyTut2] 27 | 28 | # [propertyTut3] 29 | # Retrieve the stored value 30 | print(p.value()) 31 | 32 | # Retrieve the default value 33 | print(p.defaultValue()) 34 | 35 | # Retrieve the description 36 | print(p.description()) 37 | # [propertyTut3] 38 | 39 | # [propertyTut4] 40 | # Create a property map 41 | pm = core.PropertyMap() 42 | props = {"prop1": "test", "prop2": 21, "prop3": PoseStamped(), "prop4": 5.4} 43 | pm.update(props) 44 | # [propertyTut4] 45 | 46 | # [propertyTut5] 47 | # Add a property to the property map using the pythonic way 48 | pm["prop5"] = 2 49 | # [propertyTut5] 50 | 51 | # [propertyTut6] 52 | # Return the value of a property 53 | print(pm["prop5"]) 54 | # [propertyTut6] 55 | 56 | # [propertyTut7] 57 | # Return the underlying property object 58 | p2 = pm.property("prop5") 59 | # [propertyTut7] 60 | 61 | # [propertyTut8] 62 | # Iterate through all the values in the property map 63 | print("\n") 64 | for i in pm: 65 | print(i, "\t\t", pm[i]) 66 | print("\n") 67 | # [propertyTut8] 68 | 69 | # [propertyTut9] 70 | # A new property map can also be configured using an existing one 71 | # You can also only use a subset of the properties that should be configured. 72 | pm2 = core.PropertyMap() 73 | pm.exposeTo(pm2, ["prop2", "prop4"]) 74 | # [propertyTut9] 75 | 76 | # Lets test that by printing out our properties 77 | for i in pm2: 78 | print(i, "\t\t", pm2[i]) 79 | print("\n") 80 | 81 | # [propertyTut10] 82 | # Create a stage 83 | stage = stages.CurrentState("Current State") 84 | # Access the property map of the stage 85 | props = stage.properties 86 | # [propertyTut10] 87 | -------------------------------------------------------------------------------- /demo/src/alternative_path_costs.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | using namespace moveit::task_constructor; 16 | 17 | /* FixedState - Connect - FixedState */ 18 | int main(int argc, char** argv) { 19 | ros::init(argc, argv, "mtc_tutorial"); 20 | 21 | Task t; 22 | t.stages()->setName("alternative path costs"); 23 | t.loadRobotModel(); 24 | 25 | assert(t.getRobotModel()->getName() == "panda"); 26 | 27 | auto scene{ std::make_shared(t.getRobotModel()) }; 28 | auto& robot_state{ scene->getCurrentStateNonConst() }; 29 | robot_state.setToDefaultValues(); 30 | robot_state.setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "extended"); 31 | 32 | auto initial{ std::make_unique("start") }; 33 | initial->setState(scene); 34 | t.add(std::move(initial)); 35 | 36 | auto pipeline{ std::make_shared() }; 37 | 38 | auto alternatives{ std::make_unique("connect") }; 39 | { 40 | auto connect{ std::make_unique( 41 | "path length", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) }; 42 | connect->setCostTerm(std::make_unique()); // This is the default for Connect, specified for 43 | // demonstration purposes 44 | alternatives->add(std::move(connect)); 45 | } 46 | { 47 | auto connect{ std::make_unique( 48 | "trajectory duration", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) }; 49 | connect->setCostTerm(std::make_unique()); 50 | alternatives->add(std::move(connect)); 51 | } 52 | { 53 | auto connect{ std::make_unique( 54 | "eef motion", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) }; 55 | connect->setCostTerm(std::make_unique("panda_hand")); 56 | alternatives->add(std::move(connect)); 57 | } 58 | { 59 | auto connect{ std::make_unique( 60 | "elbow motion", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) }; 61 | connect->setCostTerm(std::make_unique("panda_link4")); 62 | alternatives->add(std::move(connect)); 63 | } 64 | 65 | t.add(std::move(alternatives)); 66 | 67 | auto goal_scene{ scene->diff() }; 68 | goal_scene->getCurrentStateNonConst().setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "ready"); 69 | auto goal = std::make_unique("goal"); 70 | goal->setState(goal_scene); 71 | t.add(std::move(goal)); 72 | 73 | try { 74 | t.plan(0); 75 | } catch (const InitStageException& e) { 76 | std::cout << e << '\n'; 77 | } 78 | 79 | ros::spin(); 80 | 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /demo/src/ik_clearance_cost.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | using namespace moveit::task_constructor; 13 | 14 | /* ComputeIK(FixedState) */ 15 | int main(int argc, char** argv) { 16 | ros::init(argc, argv, "mtc_tutorial"); 17 | ros::NodeHandle nh{ "~" }; 18 | 19 | ros::AsyncSpinner spinner{ 1 }; 20 | spinner.start(); 21 | 22 | Task t; 23 | t.stages()->setName("clearance IK"); 24 | 25 | // announce new task (in case previous run was restarted) 26 | ros::Duration(0.5).sleep(); 27 | 28 | t.loadRobotModel(); 29 | assert(t.getRobotModel()->getName() == "panda"); 30 | 31 | auto scene = std::make_shared(t.getRobotModel()); 32 | auto& robot_state = scene->getCurrentStateNonConst(); 33 | robot_state.setToDefaultValues(); 34 | [[maybe_unused]] bool found = 35 | robot_state.setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "extended"); 36 | assert(found); 37 | 38 | moveit_msgs::CollisionObject co; 39 | co.id = "obstacle"; 40 | co.primitives.emplace_back(); 41 | co.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; 42 | co.primitives[0].dimensions.resize(1); 43 | co.primitives[0].dimensions[0] = 0.1; 44 | co.header.frame_id = t.getRobotModel()->getModelFrame(); 45 | co.primitive_poses.emplace_back(); 46 | co.primitive_poses[0].orientation.w = 1.0; 47 | co.primitive_poses[0].position.z = 0.85; 48 | scene->processCollisionObjectMsg(co); 49 | 50 | auto initial = std::make_unique(); 51 | initial->setState(scene); 52 | initial->setIgnoreCollisions(true); 53 | 54 | auto ik = std::make_unique(); 55 | ik->insert(std::move(initial)); 56 | ik->setGroup("panda_arm"); 57 | ik->setTargetPose(Eigen::Translation3d(.3, 0, .35) * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY())); 58 | ik->setTimeout(1.0); 59 | ik->setMaxIKSolutions(100); 60 | 61 | auto cl_cost{ std::make_unique() }; 62 | cl_cost->cumulative = nh.param("cumulative", false); // sum up pairwise distances? 63 | cl_cost->with_world = nh.param("with_world", true); // consider distance to world objects? 64 | ik->setCostTerm(std::move(cl_cost)); 65 | 66 | t.add(std::move(ik)); 67 | 68 | try { 69 | t.plan(0); 70 | } catch (const InitStageException& e) { 71 | std::cout << e << '\n'; 72 | } 73 | 74 | ros::waitForShutdown(); 75 | 76 | return 0; 77 | } 78 | -------------------------------------------------------------------------------- /demo/src/pick_place_demo.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2019 PickNik LLC. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Henning Kayser, Simon Goldstein 34 | Desc: A demo to show MoveIt Task Constructor in action 35 | */ 36 | 37 | // ROS 38 | #include 39 | 40 | // MTC pick/place demo implementation 41 | #include 42 | 43 | constexpr char LOGNAME[] = "moveit_task_constructor_demo"; 44 | 45 | int main(int argc, char** argv) { 46 | ros::init(argc, argv, "mtc_tutorial"); 47 | ros::NodeHandle nh, pnh("~"); 48 | 49 | // Handle Task introspection requests from RViz & feedback during execution 50 | ros::AsyncSpinner spinner(1); 51 | spinner.start(); 52 | 53 | moveit_task_constructor_demo::setupDemoScene(pnh); 54 | 55 | // Construct and run pick/place task 56 | moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh); 57 | if (!pick_place_task.init()) { 58 | ROS_INFO_NAMED(LOGNAME, "Initialization failed"); 59 | return 1; 60 | } 61 | 62 | if (pick_place_task.plan()) { 63 | ROS_INFO_NAMED(LOGNAME, "Planning succeded"); 64 | if (pnh.param("execute", false)) { 65 | pick_place_task.execute(); 66 | ROS_INFO_NAMED(LOGNAME, "Execution complete"); 67 | } else { 68 | ROS_INFO_NAMED(LOGNAME, "Execution disabled"); 69 | } 70 | } else { 71 | ROS_INFO_NAMED(LOGNAME, "Planning failed"); 72 | } 73 | 74 | // Keep introspection alive 75 | ros::waitForShutdown(); 76 | return 0; 77 | } 78 | -------------------------------------------------------------------------------- /demo/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############# 2 | ## Testing ## 3 | ############# 4 | 5 | ## Add gtest based cpp test target and link libraries 6 | if (CATKIN_ENABLE_TESTING) 7 | find_package(rostest REQUIRED) 8 | 9 | add_rostest_gtest(pick_place_test pick_place.test pick_place_test.cpp) 10 | target_link_libraries(pick_place_test ${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES}) 11 | endif() 12 | -------------------------------------------------------------------------------- /demo/test/pick_place.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /demo/test/pick_place_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | using namespace moveit::task_constructor; 17 | 18 | TEST(PickPlaceDemo, run) { 19 | ros::NodeHandle nh, pnh("~"); 20 | 21 | moveit_task_constructor_demo::setupDemoScene(pnh); 22 | 23 | // Construct and run pick/place task 24 | moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh); 25 | 26 | ASSERT_TRUE(pick_place_task.init()); 27 | 28 | ASSERT_TRUE(pick_place_task.plan()); 29 | ASSERT_TRUE(pick_place_task.execute()); 30 | } 31 | 32 | int main(int argc, char** argv) { 33 | testing::InitGoogleTest(&argc, argv); 34 | ros::init(argc, argv, "pick_place_test"); 35 | ros::AsyncSpinner spinner(1); 36 | spinner.start(); 37 | 38 | return RUN_ALL_TESTS(); 39 | } 40 | -------------------------------------------------------------------------------- /msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package moveit_task_constructor_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.3 (2023-03-06) 6 | ------------------ 7 | 8 | 0.1.2 (2023-02-24) 9 | ------------------ 10 | 11 | 0.1.1 (2023-02-15) 12 | ------------------ 13 | 14 | 0.1.0 (2023-02-02) 15 | ------------------ 16 | * Initial release 17 | * Contributors: Michael Görner, Robert Haschke 18 | -------------------------------------------------------------------------------- /msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(moveit_task_constructor_msgs) 3 | 4 | set(MSG_DEPS moveit_msgs visualization_msgs) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | message_generation 8 | ${MSG_DEPS} 9 | ) 10 | 11 | # ROS messages, services and actions 12 | add_message_files(DIRECTORY msg FILES 13 | Property.msg 14 | Solution.msg 15 | SolutionInfo.msg 16 | StageDescription.msg 17 | StageStatistics.msg 18 | SubSolution.msg 19 | SubTrajectory.msg 20 | TaskDescription.msg 21 | TaskStatistics.msg 22 | TrajectoryExecutionInfo.msg 23 | ) 24 | 25 | add_service_files(DIRECTORY srv FILES 26 | GetSolution.srv 27 | ) 28 | 29 | add_action_files(DIRECTORY action FILES 30 | ExecuteTaskSolution.action 31 | ) 32 | 33 | generate_messages(DEPENDENCIES ${MSG_DEPS}) 34 | 35 | catkin_package( 36 | CATKIN_DEPENDS 37 | message_runtime 38 | ${MSG_DEPS} 39 | ) 40 | -------------------------------------------------------------------------------- /msgs/action/ExecuteTaskSolution.action: -------------------------------------------------------------------------------- 1 | # Task solution to execute 2 | Solution solution 3 | 4 | --- 5 | 6 | # result of execution 7 | moveit_msgs/MoveItErrorCodes error_code 8 | 9 | --- 10 | 11 | # finished subtrajectory id / number 12 | uint32 sub_id 13 | uint32 sub_no 14 | -------------------------------------------------------------------------------- /msgs/msg/Property.msg: -------------------------------------------------------------------------------- 1 | string name 2 | string description 3 | string type 4 | string value 5 | -------------------------------------------------------------------------------- /msgs/msg/Solution.msg: -------------------------------------------------------------------------------- 1 | # id of generating task 2 | string task_id 3 | 4 | # planning scene of start state 5 | moveit_msgs/PlanningScene start_scene 6 | 7 | # set of all sub solutions involved 8 | SubSolution[] sub_solution 9 | 10 | # (ordered) sequence of actual trajectories 11 | SubTrajectory[] sub_trajectory 12 | -------------------------------------------------------------------------------- /msgs/msg/SolutionInfo.msg: -------------------------------------------------------------------------------- 1 | # unique id within task 2 | uint32 id 3 | 4 | # associated cost 5 | float32 cost 6 | 7 | # associated comment, usually providing failure hint 8 | string comment 9 | 10 | # id of stage that created this trajectory 11 | uint32 stage_id 12 | 13 | # markers, e.g. providing additional hints or illustrating failure 14 | visualization_msgs/Marker[] markers 15 | -------------------------------------------------------------------------------- /msgs/msg/StageDescription.msg: -------------------------------------------------------------------------------- 1 | # static description of a stage 2 | 3 | # unique id within task 4 | uint32 id 5 | 6 | # parent id, parent_id == id means root 7 | uint32 parent_id 8 | 9 | # name of this stage 10 | string name 11 | 12 | # flags: interface, ... 13 | uint32 flags 14 | 15 | # properties 16 | Property[] properties 17 | -------------------------------------------------------------------------------- /msgs/msg/StageStatistics.msg: -------------------------------------------------------------------------------- 1 | # dynamically changing information for a stage 2 | 3 | # unique id within task 4 | uint32 id 5 | 6 | # successful solution IDs of this stage, sorted by increasing cost 7 | uint32[] solved 8 | 9 | # (optional) failed solution IDs of this stage 10 | uint32[] failed 11 | # number of failed solutions (if failed is empty) 12 | uint32 num_failed 13 | # total computation time in seconds 14 | float64 total_compute_time 15 | -------------------------------------------------------------------------------- /msgs/msg/SubSolution.msg: -------------------------------------------------------------------------------- 1 | # generic solution information 2 | SolutionInfo info 3 | 4 | # IDs of subsolutions 5 | uint32[] sub_solution_id 6 | -------------------------------------------------------------------------------- /msgs/msg/SubTrajectory.msg: -------------------------------------------------------------------------------- 1 | # generic solution information 2 | SolutionInfo info 3 | 4 | # trajectory execution information, like controller configuration 5 | TrajectoryExecutionInfo execution_info 6 | 7 | # trajectory 8 | moveit_msgs/RobotTrajectory trajectory 9 | 10 | # planning scene of end state as diff w.r.t. start state 11 | moveit_msgs/PlanningScene scene_diff 12 | -------------------------------------------------------------------------------- /msgs/msg/TaskDescription.msg: -------------------------------------------------------------------------------- 1 | # unique id of this task 2 | string task_id 3 | 4 | # list of all stages, including the task stage itself 5 | StageDescription[] stages 6 | -------------------------------------------------------------------------------- /msgs/msg/TaskStatistics.msg: -------------------------------------------------------------------------------- 1 | # unique id of generating task 2 | string task_id 3 | 4 | # list of all stages, including the task stage itself 5 | StageStatistics[] stages 6 | -------------------------------------------------------------------------------- /msgs/msg/TrajectoryExecutionInfo.msg: -------------------------------------------------------------------------------- 1 | # List of controllers to use when executing the trajectory 2 | string[] controller_names 3 | -------------------------------------------------------------------------------- /msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | moveit_task_constructor_msgs 3 | 0.1.3 4 | Messages for MoveIt Task Pipeline 5 | 6 | BSD 7 | Robert Haschke 8 | Michael Goerner 9 | 10 | catkin 11 | 12 | message_generation 13 | moveit_msgs 14 | visualization_msgs 15 | 16 | message_runtime 17 | 18 | -------------------------------------------------------------------------------- /msgs/srv/GetSolution.srv: -------------------------------------------------------------------------------- 1 | # ID of solution (as published in Task msg) 2 | uint32 solution_id 3 | 4 | --- 5 | 6 | Solution solution 7 | -------------------------------------------------------------------------------- /rviz_marker_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_marker_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.3 (2023-03-06) 6 | ------------------ 7 | 8 | 0.1.2 (2023-02-24) 9 | ------------------ 10 | * Fix marker creation: allow zero scale for geometric shapes (`#430 `_) 11 | * Contributors: Robert Haschke 12 | 13 | 0.1.1 (2023-02-15) 14 | ------------------ 15 | 16 | 0.1.0 (2023-02-02) 17 | ------------------ 18 | * Initial release 19 | * Contributors: Robert Haschke, Michael Görner 20 | -------------------------------------------------------------------------------- /rviz_marker_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(rviz_marker_tools) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | roscpp 7 | std_msgs 8 | tf2_eigen 9 | visualization_msgs 10 | ) 11 | find_package(Eigen3 REQUIRED) 12 | 13 | # lint ignore is needed to support ROS distributions which also define urdfdom_headers in rosdep (e.g., Debian ROS packages) 14 | #catkin_lint: ignore missing_depend[pkg=urdfdom_headers] 15 | find_package(urdfdom_headers REQUIRED) 16 | 17 | catkin_package( 18 | LIBRARIES 19 | ${PROJECT_NAME} 20 | INCLUDE_DIRS 21 | include 22 | CATKIN_DEPENDS 23 | geometry_msgs 24 | std_msgs 25 | visualization_msgs 26 | DEPENDS 27 | EIGEN3 28 | ) 29 | 30 | set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}) 31 | 32 | set(HEADERS 33 | ${PROJECT_INCLUDE}/marker_creation.h 34 | ) 35 | 36 | add_library(${PROJECT_NAME} 37 | ${HEADERS} 38 | src/marker_creation.cpp 39 | ) 40 | 41 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 42 | target_include_directories(${PROJECT_NAME} PUBLIC $) 43 | target_include_directories(${PROJECT_NAME} SYSTEM PRIVATE 44 | ${catkin_INCLUDE_DIRS} 45 | ${urdfdom_headers_INCLUDE_DIRS} 46 | ${EIGEN3_INCLUDE_DIR} 47 | ) 48 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 49 | 50 | install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 51 | 52 | install(TARGETS ${PROJECT_NAME} 53 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 54 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 55 | -------------------------------------------------------------------------------- /rviz_marker_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rviz_marker_tools 3 | 0.1.3 4 | Tools for marker creation / handling 5 | 6 | BSD 7 | Robert Haschke 8 | Robert Haschke 9 | 10 | catkin 11 | 12 | eigen 13 | eigen 14 | 15 | geometry_msgs 16 | 17 | liburdfdom-headers-dev 18 | 19 | roscpp 20 | roscpp 21 | 22 | std_msgs 23 | 24 | tf2_eigen 25 | tf2_eigen 26 | 27 | visualization_msgs 28 | 29 | -------------------------------------------------------------------------------- /visualization/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package moveit_task_constructor_visualization 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.3 (2023-03-06) 6 | ------------------ 7 | 8 | 0.1.2 (2023-02-24) 9 | ------------------ 10 | 11 | 0.1.1 (2023-02-15) 12 | ------------------ 13 | * Remove unused eigen_conversions includes 14 | * Contributors: Robert Haschke 15 | 16 | 0.1.0 (2023-02-02) 17 | ------------------ 18 | * Initial release 19 | * Contributors: Robert Haschke, Michael Görner 20 | -------------------------------------------------------------------------------- /visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(moveit_task_constructor_visualization) 3 | 4 | find_package(fmt REQUIRED) 5 | find_package(catkin REQUIRED COMPONENTS 6 | moveit_core 7 | moveit_ros_visualization 8 | moveit_task_constructor_core 9 | moveit_task_constructor_msgs 10 | roscpp 11 | rviz 12 | ) 13 | 14 | moveit_build_options() 15 | 16 | # rviz transitively includes OGRE headers which break with `-Wall -Werror` 17 | # so isolate these include dirs and add them as SYSTEM include where needed. 18 | set(rviz_OGRE_INCLUDE_DIRS) 19 | foreach(header IN ITEMS OgreRoot.h OgreOverlay.h) 20 | find_path(include_dir ${header} 21 | HINTS ${catkin_INCLUDE_DIRS} 22 | NO_DEFAULT_PATH) 23 | list(REMOVE_ITEM catkin_INCLUDE_DIRS "${include_dir}") 24 | list(APPEND rviz_OGRE_INCLUDE_DIRS "${include_dir}") 25 | endforeach() 26 | 27 | # definition needed for boost/math/constants/constants.hpp included by Ogre to compile 28 | add_definitions(-DBOOST_MATH_DISABLE_FLOAT128) 29 | 30 | # Qt Stuff 31 | if("${rviz_QT_VERSION}" VERSION_LESS "5") 32 | find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) 33 | include(${QT_USE_FILE}) 34 | macro(qt_wrap_ui) 35 | qt4_wrap_ui(${ARGN}) 36 | endmacro() 37 | else() 38 | find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) 39 | set(QT_LIBRARIES Qt5::Widgets) 40 | macro(qt_wrap_ui) 41 | qt5_wrap_ui(${ARGN}) 42 | endmacro() 43 | endif() 44 | 45 | set(CMAKE_INCLUDE_CURRENT_DIR ON) 46 | set(CMAKE_AUTOMOC ON) 47 | set(CMAKE_AUTORCC ON) 48 | add_definitions(-DQT_NO_KEYWORDS) 49 | 50 | catkin_package( 51 | LIBRARIES 52 | moveit_task_visualization_tools 53 | motion_planning_tasks_utils 54 | INCLUDE_DIRS 55 | visualization_tools/include 56 | CATKIN_DEPENDS 57 | moveit_core 58 | moveit_task_constructor_msgs 59 | roscpp 60 | rviz 61 | ) 62 | 63 | add_subdirectory(visualization_tools) 64 | add_subdirectory(motion_planning_tasks) 65 | 66 | install(FILES 67 | motion_planning_tasks_rviz_plugin_description.xml 68 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 69 | 70 | install(DIRECTORY icons DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 71 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(utils) 2 | add_subdirectory(properties) 3 | add_subdirectory(src) 4 | add_subdirectory(test) 5 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/properties/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MOVEIT_LIB_NAME motion_planning_tasks_properties) 2 | 3 | set(SOURCES 4 | property_factory.cpp 5 | ) 6 | 7 | find_package(PkgConfig REQUIRED) 8 | pkg_check_modules(YAML yaml-0.1) 9 | if (YAML_FOUND) 10 | # Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually 11 | find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS}) 12 | list(APPEND SOURCES property_from_yaml.cpp) 13 | add_definitions(-DHAVE_YAML) 14 | endif() 15 | 16 | add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES}) 17 | 18 | target_link_libraries(${MOVEIT_LIB_NAME} ${QT_LIBRARIES} ${YAML_LIBRARIES}) 19 | target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $) 20 | target_include_directories(${MOVEIT_LIB_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS} ${YAML_INCLUDE_DIRS}) 21 | 22 | install(TARGETS ${MOVEIT_LIB_NAME} 23 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 24 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 25 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MOVEIT_LIB_NAME motion_planning_tasks_rviz_plugin) 2 | 3 | qt_wrap_ui(UIC_FILES 4 | task_panel.ui 5 | task_view.ui 6 | global_settings.ui 7 | ) 8 | 9 | add_library(${MOVEIT_LIB_NAME} 10 | factory_model.cpp 11 | icons.cpp 12 | job_queue.cpp 13 | local_task_model.cpp 14 | meta_task_list_model.cpp 15 | pluginlib_factory.h 16 | plugin_init.cpp 17 | remote_task_model.cpp 18 | task_display.cpp 19 | task_list_model.cpp 20 | task_panel.cpp 21 | task_panel_p.h 22 | 23 | ${UIC_FILES} 24 | 25 | resources.qrc 26 | ) 27 | 28 | set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") 29 | target_link_libraries(${MOVEIT_LIB_NAME} 30 | motion_planning_tasks_utils motion_planning_tasks_properties moveit_task_visualization_tools 31 | ${catkin_LIBRARIES} ${QT_LIBRARIES} 32 | ) 33 | target_include_directories(${MOVEIT_LIB_NAME} 34 | PUBLIC $ 35 | PRIVATE $ 36 | ) 37 | target_include_directories(${MOVEIT_LIB_NAME} SYSTEM PUBLIC 38 | # https://stackoverflow.com/questions/47175683/cmake-target-link-libraries-propagation-of-include-directories 39 | $ 40 | $ 41 | $ 42 | ${catkin_INCLUDE_DIRS} 43 | ${rviz_OGRE_INCLUDE_DIRS} 44 | ) 45 | 46 | install(TARGETS ${MOVEIT_LIB_NAME} 47 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 48 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 49 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/factory_model.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Robert Haschke */ 36 | 37 | #pragma once 38 | 39 | #include 40 | #include 41 | 42 | namespace moveit_rviz_plugin { 43 | 44 | /** Provide a tree model listing all available plugins from the rviz::Factory 45 | * grouped by package name 46 | */ 47 | class FactoryModel : public QStandardItemModel 48 | { 49 | QString mime_type_; 50 | void fillTree(rviz::Factory& factory); 51 | 52 | public: 53 | FactoryModel(rviz::Factory& factory, const QString& mime_type, QObject* parent = nullptr); 54 | 55 | QStringList mimeTypes() const override; 56 | QMimeData* mimeData(const QModelIndexList& indexes) const override; 57 | }; 58 | } // namespace moveit_rviz_plugin 59 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/global_settings.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_rviz_plugin::GlobalSettingsWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 400 10 | 300 11 | 12 | 13 | 14 | Settings 15 | 16 | 17 | 18 | 0 19 | 20 | 21 | 0 22 | 23 | 24 | 0 25 | 26 | 27 | 0 28 | 29 | 30 | 31 | 32 | 0 33 | 34 | 35 | 0 36 | 37 | 38 | 0 39 | 40 | 41 | 0 42 | 43 | 44 | 0 45 | 46 | 47 | 48 | 49 | Global Settings 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | rviz::PropertyTreeWidget 63 | QTreeView 64 |
rviz/properties/property_tree_widget.h
65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons.cpp: -------------------------------------------------------------------------------- 1 | #include "icons.h" 2 | #include 3 | 4 | using namespace moveit_rviz_plugin::utils; 5 | 6 | namespace moveit_rviz_plugin { 7 | namespace icons { 8 | 9 | const Icon CONNECT({ { QLatin1String(":icons/connectarrow.png"), QColor::fromRgba(0xff000000) } }, Icon::TINT); 10 | const Icon FORWARD({ { QLatin1String(":icons/downarrow.png"), QColor::fromRgba(0xff000000) } }, Icon::TINT); 11 | const Icon BACKWARD({ { QLatin1String(":icons/uparrow.png"), QColor::fromRgba(0xff000000) } }, Icon::TINT); 12 | const Icon BOTHWAY({ { QLatin1String(":icons/updownarrow.png"), QColor::fromRgba(0xff000000) } }, Icon::TINT); 13 | const Icon GENERATE({ { QLatin1String(":icons/generatearrow.png"), QColor::fromRgba(0xff000000) } }, Icon::TINT); 14 | } // namespace icons 15 | } // namespace moveit_rviz_plugin 16 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace moveit_rviz_plugin { 6 | namespace icons { 7 | 8 | extern const moveit_rviz_plugin::utils::Icon CONNECT; 9 | extern const moveit_rviz_plugin::utils::Icon FORWARD; 10 | extern const moveit_rviz_plugin::utils::Icon BACKWARD; 11 | extern const moveit_rviz_plugin::utils::Icon BOTHWAY; 12 | extern const moveit_rviz_plugin::utils::Icon GENERATE; 13 | } // namespace icons 14 | } // namespace moveit_rviz_plugin 15 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/add.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/add.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/connectarrow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/connectarrow.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/downarrow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/downarrow.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/failed.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/failed.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/generatearrow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/generatearrow.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/new-stage.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/new-stage.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/settings.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/settings.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/success.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/success.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/tasks.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/tasks.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/uparrow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/uparrow.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/icons/updownarrow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/moveit_task_constructor/0cc398797f56de22e4328f0064ac5d0c53c011a3/visualization/motion_planning_tasks/src/icons/updownarrow.png -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/job_queue.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Robert Haschke */ 36 | 37 | #include "job_queue.h" 38 | #include 39 | 40 | namespace moveit { 41 | namespace tools { 42 | 43 | JobQueue::JobQueue(QObject* parent) : QObject(parent) {} 44 | 45 | void JobQueue::addJob(const std::function& job) { 46 | boost::unique_lock ulock(jobs_mutex_); 47 | jobs_.push_back(job); 48 | } 49 | 50 | void JobQueue::clear() { 51 | jobs_.clear(); 52 | } 53 | 54 | size_t JobQueue::numPending() { 55 | boost::unique_lock ulock(jobs_mutex_); 56 | return jobs_.size(); 57 | } 58 | 59 | void JobQueue::waitForAllJobs() { 60 | boost::unique_lock ulock(jobs_mutex_); 61 | while (!jobs_.empty()) 62 | idle_condition_.wait(ulock); 63 | } 64 | 65 | void JobQueue::executeJobs() { 66 | boost::unique_lock ulock(jobs_mutex_); 67 | while (!jobs_.empty()) { 68 | std::function fn = jobs_.front(); 69 | jobs_.pop_front(); 70 | ulock.unlock(); 71 | try { 72 | fn(); 73 | } catch (std::exception& ex) { 74 | ROS_ERROR("Exception caught executing main loop job: %s", ex.what()); 75 | } 76 | ulock.lock(); 77 | } 78 | idle_condition_.notify_all(); 79 | } 80 | } // namespace tools 81 | } // namespace moveit 82 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/job_queue.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Robert Haschke */ 36 | 37 | #pragma once 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace moveit { 47 | namespace tools { 48 | 49 | /** Job Queue (of std::functions) */ 50 | class JobQueue : public QObject 51 | { 52 | Q_OBJECT 53 | boost::mutex jobs_mutex_; 54 | std::deque > jobs_; 55 | boost::condition_variable idle_condition_; 56 | 57 | public: 58 | explicit JobQueue(QObject* parent = nullptr); 59 | void addJob(const std::function& job); 60 | void clear(); 61 | size_t numPending(); 62 | 63 | void waitForAllJobs(); 64 | void executeJobs(); 65 | }; 66 | } // namespace tools 67 | } // namespace moveit 68 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/plugin_init.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Bielefeld University 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Bielefeld University nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Robert Haschke */ 36 | 37 | #include 38 | #include "task_display.h" 39 | #include "task_panel.h" 40 | 41 | PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskDisplay, rviz::Display) 42 | PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskPanel, rviz::Panel) 43 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/resources.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | icons/add.png 4 | icons/settings.png 5 | icons/failed.png 6 | icons/new-stage.png 7 | icons/success.png 8 | icons/tasks.png 9 | icons/connectarrow.png 10 | icons/downarrow.png 11 | icons/generatearrow.png 12 | icons/uparrow.png 13 | icons/updownarrow.png 14 | 15 | 16 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/src/task_panel.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_rviz_plugin::TaskPanel 4 | 5 | 6 | 7 | 0 8 | 0 9 | 400 10 | 300 11 | 12 | 13 | 14 | Tasks Panel 15 | 16 | 17 | 18 | 0 19 | 20 | 21 | 0 22 | 23 | 24 | 0 25 | 26 | 27 | 0 28 | 29 | 30 | 31 | 32 | 2 33 | 34 | 35 | 36 | 37 | Qt::Horizontal 38 | 39 | 40 | 41 | 40 42 | 20 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | Execute solution 51 | 52 | 53 | Exec 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | Show available stages 64 | 65 | 66 | ... 67 | 68 | 69 | 70 | :/icons/new-stage.png:/icons/new-stage.png 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############# 2 | ## Testing ## 3 | ############# 4 | 5 | ## Add gtest based cpp test target and link libraries 6 | if (CATKIN_ENABLE_TESTING) 7 | find_package(rostest REQUIRED) 8 | 9 | catkin_add_gtest(${PROJECT_NAME}-test-merge-models test_merge_models.cpp) 10 | target_link_libraries(${PROJECT_NAME}-test-merge-models 11 | motion_planning_tasks_utils gtest_main) 12 | 13 | catkin_add_gmock(${PROJECT_NAME}-test-solution-models test_solution_models.cpp) 14 | target_link_libraries(${PROJECT_NAME}-test-solution-models 15 | motion_planning_tasks_rviz_plugin gtest_main) 16 | 17 | add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp) 18 | target_link_libraries(${PROJECT_NAME}-test-task_model 19 | motion_planning_tasks_rviz_plugin ${catkin_LIBRARIES} gtest) 20 | endif() 21 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/test/test_solution_models.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace moveit_rviz_plugin; 7 | 8 | class SolutionModelTest : public ::testing::Test 9 | { 10 | protected: 11 | template 12 | inline std::vector reversed(const std::vector& in) { 13 | return std::vector(in.rbegin(), in.rend()); 14 | } 15 | void validateSorting(QAbstractTableModel& model, int sort_column, Qt::SortOrder sort_order, 16 | const std::vector& expected_id_order) { 17 | SCOPED_TRACE(qPrintable(QString("sorting in %1 %2 order") 18 | .arg(sort_order == Qt::AscendingOrder ? "ascending" : "descending") 19 | .arg(sort_column == 0 ? "creation" : "cost"))); 20 | model.sort(sort_column, sort_order); 21 | std::vector actual_id_order(model.rowCount()); 22 | for (int row = 0; row < model.rowCount(); ++row) 23 | actual_id_order[row] = model.data(model.index(row, 0), Qt::UserRole).toInt(); 24 | EXPECT_THAT(actual_id_order, ::testing::ElementsAreArray(expected_id_order)); 25 | } 26 | void processAndValidate(RemoteSolutionModel& model, const std::vector& success_ids, 27 | const std::vector& failure_ids) { 28 | model.processSolutionIDs(success_ids, failure_ids, failure_ids.size(), 0.0); 29 | 30 | std::vector cost_ordered_ids(success_ids.begin(), success_ids.end()); 31 | std::vector sorted_failure_ids(failure_ids.begin(), failure_ids.end()); 32 | std::sort(sorted_failure_ids.begin(), sorted_failure_ids.end()); 33 | std::copy(sorted_failure_ids.begin(), sorted_failure_ids.end(), std::back_inserter(cost_ordered_ids)); 34 | 35 | std::vector creation_ordered_ids(cost_ordered_ids.size()); 36 | for (size_t i = 0; i < cost_ordered_ids.size(); ++i) 37 | creation_ordered_ids[i] = i + 1; 38 | 39 | validateSorting(model, 0, Qt::AscendingOrder, creation_ordered_ids); 40 | validateSorting(model, 0, Qt::DescendingOrder, reversed(creation_ordered_ids)); 41 | 42 | validateSorting(model, 1, Qt::AscendingOrder, cost_ordered_ids); 43 | validateSorting(model, 1, Qt::DescendingOrder, reversed(cost_ordered_ids)); 44 | } 45 | }; 46 | 47 | #define processAndValidate(...) \ 48 | { \ 49 | SCOPED_TRACE("processSolutionIDs(" #__VA_ARGS__ ")"); \ 50 | processAndValidate(model, __VA_ARGS__); \ 51 | } 52 | 53 | TEST_F(SolutionModelTest, sorting) { 54 | RemoteSolutionModel model; 55 | processAndValidate({ 1, 3 }, { 2 }); 56 | processAndValidate({ 4, 1, 6, 3 }, { 5, 2 }); 57 | } 58 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/test/test_task_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MOVEIT_LIB_NAME motion_planning_tasks_utils) 2 | 3 | set(SOURCES 4 | flat_merge_proxy_model.cpp 5 | tree_merge_proxy_model.cpp 6 | icon.cpp 7 | ) 8 | add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES}) 9 | 10 | target_link_libraries(${MOVEIT_LIB_NAME} ${QT_LIBRARIES}) 11 | target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $) 12 | target_include_directories(${MOVEIT_LIB_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS}) 13 | 14 | install(TARGETS ${MOVEIT_LIB_NAME} 15 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 16 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 17 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks/utils/icon.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | ** 3 | ** Copyright (C) 2016 The Qt Company Ltd. 4 | ** Contact: https://www.qt.io/licensing/ 5 | ** 6 | ** This file is adapted from Qt Creator (replacing theme stuff by QColor) 7 | ** 8 | ** Commercial License Usage 9 | ** Licensees holding valid commercial Qt licenses may use this file in 10 | ** accordance with the commercial license agreement provided with the 11 | ** Software or, alternatively, in accordance with the terms contained in 12 | ** a written agreement between you and The Qt Company. For licensing terms 13 | ** and conditions see https://www.qt.io/terms-conditions. For further 14 | ** information use the contact form at https://www.qt.io/contact-us. 15 | ** 16 | ** GNU General Public License Usage 17 | ** Alternatively, this file may be used under the terms of the GNU 18 | ** General Public License version 3 as published by the Free Software 19 | ** Foundation with exceptions as appearing in the file LICENSE.GPL3-EXCEPT 20 | ** included in the packaging of this file. Please review the following 21 | ** information to ensure the GNU General Public License requirements will 22 | ** be met: https://www.gnu.org/licenses/gpl-3.0.html. 23 | ** 24 | ****************************************************************************/ 25 | 26 | #pragma once 27 | 28 | #include 29 | #include 30 | 31 | class QColor; 32 | class QIcon; 33 | class QPixmap; 34 | class QString; 35 | 36 | namespace moveit_rviz_plugin { 37 | namespace utils { 38 | 39 | using IconMaskAndColor = QPair; 40 | 41 | // Returns a recolored icon with shadow and custom disabled state for a 42 | // series of grayscalemask|Theme::Color mask pairs 43 | class Icon : public QVector 44 | { 45 | public: 46 | enum IconStyleOption : uint8_t 47 | { 48 | NONE = 0, 49 | TINT = 1, 50 | DROP_SHADOW = 2, 51 | PUNCH_EDGES = 4, 52 | 53 | TOOL_BAR_STYLE = TINT | DROP_SHADOW | PUNCH_EDGES, 54 | MENU_TINTED_STYLE = TINT | PUNCH_EDGES 55 | }; 56 | 57 | Q_DECLARE_FLAGS(IconStyleOptions, IconStyleOption) 58 | 59 | Icon(); 60 | Icon(std::initializer_list args, IconStyleOptions style = TOOL_BAR_STYLE); 61 | Icon(const QString& imageFileName); 62 | Icon(const Icon& other) = default; 63 | 64 | QIcon icon() const; 65 | // Same as icon() but without disabled state. 66 | QPixmap pixmap() const; 67 | 68 | // Try to avoid it. it is just there for special API cases in Qt Creator 69 | // where icons are still defined as filename. 70 | QString imageFileName() const; 71 | 72 | // Combined icon pixmaps in Normal and Disabled states from several QIcons 73 | static QIcon combinedIcon(const QList& icons); 74 | 75 | private: 76 | IconStyleOptions m_style = NONE; 77 | }; 78 | } // namespace utils 79 | } // namespace moveit_rviz_plugin 80 | 81 | Q_DECLARE_OPERATORS_FOR_FLAGS(moveit_rviz_plugin::utils::Icon::IconStyleOptions) 82 | -------------------------------------------------------------------------------- /visualization/motion_planning_tasks_rviz_plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel widget to monitor and edit motion planning tasks 7 | 8 | 9 | 12 | 13 | Monitor motion planning tasks and animate their solution trajectories 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | moveit_task_constructor_visualization 3 | 0.1.3 4 | Visualization tools for MoveIt Task Pipeline 5 | 6 | BSD 7 | Robert Haschke 8 | Robert Haschke 9 | Michael Goerner 10 | 11 | catkin 12 | 13 | fmt 14 | qtbase5-dev 15 | moveit_core 16 | moveit_task_constructor_msgs 17 | moveit_task_constructor_core 18 | moveit_ros_visualization 19 | roscpp 20 | rviz 21 | 22 | rosunit 23 | rostest 24 | moveit_resources_fanuc_moveit_config 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /visualization/visualization_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MOVEIT_LIB_NAME moveit_task_visualization_tools) 2 | 3 | set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/visualization_tools) 4 | 5 | set(HEADERS 6 | ${PROJECT_INCLUDE}/display_solution.h 7 | ${PROJECT_INCLUDE}/marker_visualization.h 8 | ${PROJECT_INCLUDE}/task_solution_panel.h 9 | ${PROJECT_INCLUDE}/task_solution_visualization.h 10 | ) 11 | 12 | add_library(${MOVEIT_LIB_NAME} 13 | ${HEADERS} 14 | 15 | src/display_solution.cpp 16 | src/marker_visualization.cpp 17 | src/task_solution_panel.cpp 18 | src/task_solution_visualization.cpp 19 | ) 20 | set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") 21 | 22 | target_link_libraries(${MOVEIT_LIB_NAME} 23 | ${catkin_LIBRARIES} 24 | ${rviz_DEFAULT_PLUGIN_LIBRARIES} 25 | ${OGRE_LIBRARIES} 26 | ${QT_LIBRARIES} 27 | ${Boost_LIBRARIES} 28 | fmt::fmt 29 | ) 30 | target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include) 31 | target_include_directories(${MOVEIT_LIB_NAME} SYSTEM 32 | PRIVATE ${catkin_INCLUDE_DIRS} 33 | PUBLIC ${rviz_OGRE_INCLUDE_DIRS} 34 | ) 35 | add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) 36 | 37 | install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 38 | 39 | install(TARGETS ${MOVEIT_LIB_NAME} 40 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 41 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 42 | -------------------------------------------------------------------------------- /visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017, Yannick Jonetzko 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Yannick Jonetzko */ 36 | 37 | #pragma once 38 | 39 | #ifndef Q_MOC_RUN 40 | #include 41 | #endif 42 | 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | 49 | namespace moveit_rviz_plugin { 50 | class TaskSolutionPanel : public rviz::Panel 51 | { 52 | Q_OBJECT 53 | 54 | public: 55 | TaskSolutionPanel(QWidget* parent = nullptr); 56 | 57 | ~TaskSolutionPanel() override; 58 | 59 | void onInitialize() override; 60 | void onEnable(); 61 | void onDisable(); 62 | void update(int way_point_count); 63 | 64 | // Switches between pause and play mode 65 | void pauseButton(bool check); 66 | 67 | void setSliderPosition(int position); 68 | 69 | int getSliderPosition() const { return slider_->sliderPosition(); } 70 | 71 | bool isPaused() const { return paused_; } 72 | 73 | private Q_SLOTS: 74 | void sliderValueChanged(int value); 75 | void buttonClicked(); 76 | 77 | protected: 78 | QSlider* slider_; 79 | QLabel* maximum_label_; 80 | QLabel* minimum_label_; 81 | QPushButton* button_; 82 | 83 | bool paused_; 84 | bool empty_; 85 | }; 86 | 87 | } // namespace moveit_rviz_plugin 88 | --------------------------------------------------------------------------------